移动机器人建图与导航代码实现——3.Path Tracking

Path Tracking

这一部分利用极坐标下的线性控制器完成路径跟踪，主要思路就是把路径拆开，变成p2p问题，即从当前姿态到目标姿态。

极坐标下的线性控制器

设速度(v)，角速度(omega)，则有两轮差速机器人在惯性坐标系下的模型

$$ egin{bmatrix} dot x\ dot y\ dot heta end{bmatrix}= egin{bmatrix} cos heta & 0\ sin heta & 0\ 0 & 1 end{bmatrix} egin{bmatrix} v\ omega end{bmatrix} $$

如图转换到误差极坐标系下

( ho=sqrt{Delta x^2+Delta y^2})
(alpha=- heta+arctan2(Delta y, Delta x))
(eta=- heta-alpha)

$$ egin{bmatrix} dot ho\ dot alpha\ dot eta end{bmatrix}= egin{bmatrix} -cos alpha & 0\ frac{sin alpha}{ ho} & -1\ -frac{sin alpha}{ ho} & 0 end{bmatrix} egin{bmatrix} v\ omega end{bmatrix} $$

设计线性控制器
(v = k_{ ho} ho)
$omega = k_{alpha}alpha + k_{eta}eta $

近似化以后

$$ egin{bmatrix} dot ho\ dot alpha\ dot eta end{bmatrix}= egin{bmatrix} -k_{ ho} & 0 & 0\ 0 & -(k_{alpha}-k_{ ho}) & k_{eta}\ 0 & -k_{ ho} & 0 end{bmatrix} egin{bmatrix} ho \ alpha \ eta end{bmatrix} $$

求出特征多项式((lambda+k_{ ho})[lambda^2+lambda(k_{alpha}-k_{ ho})-k_{ ho}k_{eta}])
从而得到收敛条件
(k_{ ho}ge 0, k_{eta}le 0, k_{alpha}-k_{ ho}ge 0)

实际上，静态的参数容易使机器人陷入局部极值，我们做了动态调参。当( ho)比较大时，我们希望(alpha)影响更大，反之希望(eta)影响更大，同时希望不出现突变，因此设定了一个阈值(th)，将控制器改为

[omega= egin{cases} (frac{ ho}{th})^2 k_{alpha}alpha + k_{eta}eta & ho < th\ k_{alpha}alpha + (frac{th}{ ho})^2 k_{eta}eta & ext{else} end{cases}]