纯跟踪控制算法的拓展——滑动跟踪算法

背景

阿克曼底盘机器用纯跟踪算法跟踪轨迹时候，是底盘后轮中心跟踪轨迹，有时候希望机器是车中心跟踪轨迹或者车头跟踪轨迹，甚至是变化的跟随点去跟踪轨迹。此时就需要能满足任意点跟随的跟踪算法，从纯跟踪算法拓展开，得到滑动跟踪算法。

原理

原理图说明

阿克曼机器后轮中心 O ，前轮 S，S' 为前轮朝向。射线 robotYaw(OS) 表示了机器朝向，射线 steerYaw(SS') 表示前轮角度。点 C 是阿克曼机器转向的旋转中心，点 P 是射线 OS 上的滑动点，曲线 trajectory 是点 P 在机器转向中的轨迹， 点 T 是想要跟踪的轨迹点。

滑动跟踪算法的目标是，控制机器前轮转向，使得机器中轴线上的滑动点 P 经过轨迹点 T。

公式

• 设线段 OS 为 l，线段 OC 为 m，前轮转角是 steerYaw 与 robotYaw 的夹角，设为角 θ。有方程 (1) :

• 设线段 OP 为 h，线段 PC 为点 P 的转弯半径，设 PC 为 r。过点 P 作转弯半径的切线，交机器于点 D，交后轮轴于点 A。角 α 为滑动点 P 运动方向与机器朝向 robotYaw 的夹角。有方程 (2) :

• 设线段 PT 为 e，调整好前轮转向，使点 T 在点 P 的转弯圆弧轨迹上时候，TP 在以点 C 为圆心的圆上。过点 C 作 CB 垂直于 PT，垂足为 B。此时角 DPT 和角 PCB 相同，设角 DPT 为 γ，设角 SPT 为 β。有方程 (3) :

• 点 OPC 形成直角三角形，有方程 (4) :

联立方程 (2) 和 (4) ，得到方程 (5) :

联立方程 (2) 和 (3)，得到方程 (6) :

根据三角函数的两角和差公式，(6) 可以化为

联立方程 (1) (5) (7)，得到滑动点 P 跟踪轨迹点 T 时候的前轮控制角 θ：

其中 l 是线段 OS，对应阿克曼底盘后轮中心到前轮的距离，即 wheel_base；

角 β 是滑动点 P 和跟踪点 T 与机器当前航向角的夹角；

e 是线段 PT，滑动点 P 和跟踪点 T 的距离；

h 是线段 PO，滑动点 P 到后轮中心 O 的距离；