两轮差速小车循线控制原理分析
硬件资料设定：小车驱动来自于两个相同的电机，转向依靠两轮差速实现，小车前后左右安装超声波传感器，前后各一个，左右各两个；
功能目标：假设小车左侧有墙壁，通过超声波测距实现按指定距离沿墙行驶
参数设定：车轮半径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和系数相关参数的关系来判定系数的符号，然后就根据车辆运行状态来调节各项参数。
若有收获，就点个赞吧