两轮差速小车循线控制原理分析
两轮差速小车循线控制原理分析
本文详细介绍了两轮差速小车循线控制的原理和算法设计，通过超声波传感器实现沿墙行驶的功能。文章从硬件配置、参数设定、位姿计算、速度关系推导到PID控制算法设计，层层递进，为机器人爱好者和相关专业学生提供了一篇详尽的技术参考。
硬件配置说明
小车驱动来自于两个相同的电机，转向依靠两轮差速实现。小车前后左右安装超声波传感器，前后各一个，左右各两个。
参数设定
- 车轮半径：r = 3cm
- 车轮间距：wd = 10cm
- 车身长度：length = 16cm
- 侧边超声波传感器间距：等于车身长度，即16cm
- 小车线速度：v = 0.2m/s
- 距离墙的目标距离：d = 10cm
- 超声波传感器采集频率：10Hz
假设小车出发时方向与墙平行，距离30cm。接下来设计小车控制算法。
位姿计算方法
根据超声波测距结果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和θ。
车轮转速与速度的关系推导
车轮通过电机控制，电机通过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_r = v - omega * wd / 2
- v_l = v + omega * wd / 2
到此为止，我们得到了设定车辆角速度v，线速度omega时，科技计算出车轮线速度，根据线速度和车轮角速度关系，可以计算出上位机应该给车轮下达怎样的指令。
PID控制算法设计
厘清各参数关系之后，我们来实现通过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和系数相关参数的关系来判定系数的符号，然后就根据车辆运行状态来调节各项参数。