硬件资料设定：小车驱动来自于两个相同的电机，转向依靠两轮差速实现，小车前后左右安装超声波传感器，前后各一个，左右各两个；

功能目标：假设小车左侧有墙壁，通过超声波测距实现按指定距离沿墙行驶

参数设定：车轮半径r=3cm，车轮间距wd=10cm，车身长length=16cm，侧边超声波传感器前后各一个，假设一个在车头一个在车尾，间距=车身=length=16cm，小车线速度恒定v = 0.2m/s，距离墙d=10cm，假设超声波传感器以10Hz的频率采集距离。

在以上条件下，设定小车出发点，方向与墙平行，距离30cm，根据以上参数，设计小车控制算法。

1、根据超声波测距结果f_dist和b_dist计算车辆与墙的夹角θ与距离dist

由上图关系可知：

dist = f_dist_cos(θ)

tan(θ) = (b_dist-f_dist)/length

—> θ=arctan(b_dist-f_dist)/length)

—> dist = f_dist_cos(arctan(b_dist-f_dist)/length))

到这里，我们通过超声波测距对小车的位姿进行了计算，得到了dist和θ

2、计算线速度v、角速度omega，与车轮转速的关系

车轮通过电机控制，电机通过PWM控制，上位机可以控制的是电机PWM控制的占空比，再进一步，就是上位机可以直接控制车轮的转速，假设左右车轮转速分别为omega_l和omega_r，那么对应两个轮子的线速度分别是v_l和v_r，二者关系为：

v_l = 2_pi_r_(omega_l/2/pi) = omega_l_r

同理：v_r = omega_r_r

小车通过两轮差速驱动，车轮速度一致时走直线，不一致时走弧线，走弧线时，两个轮子所走弧线以及车实际的轨迹是3个同心圆，而单位时间内走过的轨迹如下图：

上图中，v_l和v_r是单位时间内两个轮走过的轨迹，v是车辆实际速度，R是内车轮轨迹半径，wd为两个轮子的距离，omega为单位时间内车辆转过额角度，实际就是车辆角速度，这些参数有以下关系：

v_r/R = v_l/(R+wd)=v/(R+wd/2)=omega

—> v_r = R_omega

v_l = R_omega+wd_omega

v = omega_R + wd_omega/2

—> omega = (v_l-v_r)/wd = (v-v_r)_2/wd

v在上文我们设定为固定值0.2m/s，那么：

—> v_r = v - omega _wd/2

v_l = v + omega _wd/2

到此为止，我们得到了设定车辆角速度v，线速度omega时，科技计算出车轮线速度，根据线速度和车轮角速度关系，可以计算出上位机应该给车轮下达怎样的指令。

3、厘清各参数关系之后，我们来实现通过pid实现闭环控制小车，也就是通过第一步计算出的θ和dist，目标距离d，计算小车控制参数v（固定值0.2m/s）和omega：

首先看一下控制逻辑，下图控制逻辑是靠传感器数据驱动的，也可以设置为定时驱动，传感器数据采集以后放到固定位置，程序设置定时循环驱动计算和控制：

在上图中，dist和θ是即时计算出来的，

d_err = dist-d

phi_err = θ

另设：

Δt：车辆控制命令发送间隔

d_I_err：横向偏差积分项，每一轮+=d_err _Δt 需要设定范围，避免积分项太大

phi_I_err：航向角度偏差积分项，每一轮+=phi_err _Δt 需要设定范围，避免积分项太大

d_K：横向偏差比例系数

d_I：横向偏差积分系数

phi_K：航向角度偏差比例系数

phi_I：航向角度偏差积分系数

那么，在每一次控制循环中需要进行以下计算：
d_err = dist-d

phi_err = θ

d_I_err += d_err _Δt

判断积分项是否在范围内，超出范围则设为范围边界值

phi_I_err += phi_err _Δt

判断积分项是否在范围内，超出范围则设为范围边界值

omega = d_K_d_err+d_I_d_I_err+phi_K_phi_err +phi_I*phi_I_err

其中4个比例系数都需要通过设其它比例系数为0的情况下，计算omega和系数相关参数的关系来判定系数的符号，然后就根据车辆运行状态来调节各项参数。

​

若有收获，就点个赞吧