基于差速模型的pure pursuit算法仿真

本文的前提是规划器已经为轮式移动机器人(WMR)规划出了满意的路径，倘若建立了路径与时间确切的对应关系，则称之为轨迹。本文重点讨论如何实现对于确定轨迹的跟踪问题。

机器人坐标系

在平面上，该机器人底盘总的维数是3个：2个为平面中的位置；1个为沿垂直轴的方向的转动，它与平面正交。把机器人的底盘看作刚体，忽略机器人和它的轮子间内在的关联和自由度。随动轮仅在运动过程中起支撑作用，其在运动学模型中的影响可忽略不计。假设WMR的左右轮均为刚体，只能沿着车轮滚动的方向前进，无侧向滑动。为了确定机器人在平面中的位置，如下图所示，建立了平面全局参考坐标系和机器人局部参考坐标系之间的关系。

将平面上任意一点选为原点$O$，相互正交的$X$轴和$Y$轴建立全局参考坐标系。为了确定机器人的位置，选择机器人两轮连轴的轴心 $o$ 作为它的位置参考点。基于 { x , y } \{x, y\} { x,y}定义机器人底盘上相对于 $o$ 的两个轴，其中，$y$轴位于两轮连线上并指向左轮，$x$轴与$y$轴正交，从而定义了机器人的局部参考坐标系。在全局参考坐标系上，$O$的位置由坐标$x$和$y$确定，全局和局部参考坐标系之间的角度差由机器人坐标系的$x$轴与全局坐标系$X$轴的夹角$\theta$给定。因此可以将机器人的位姿描述为具有这3个元素的矢量。
$${\xi }_1=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]$$

双轮差速移动机器人运动学模型

全局坐标系和机器人坐标系之间的变换关系

为了根据分量的移动描述机器人的移动，就需要构造全局参考坐标系和机器人局部参考坐标系的运动映射。

先定义一些接下来会用到的量：

• $R$ 移动机器人轮子的半径；
• $w_l,w_r$ 分别是左右轮的角速度；
• $v_l,v_r$ 分别是左右轮的线速度；
• $w_0,v_0$ 分别是质心$o$的角速度和线速度；
• $l$ 两轮的轴距

现在考虑在极短时间$\Delta t$内，机器人位姿的变化：在$\Delta t$时间内，可以认为机器人朝$x$轴方向匀速运动，匀速转动。因此，

全局坐标系下，横坐标改变量为

$$\Delta x=v(t)\Delta t\cos \theta$$

全局坐标系下，纵坐标改变量为

$$\Delta y=v(t)\Delta t\sin \theta$$

$\theta$的改变量为

$$\Delta \theta =w(t)\Delta t$$

写成矩阵形式：
$$\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta \theta \end{array}\right]=\left[\begin{array}{c}v(t)\Delta t\cos \theta \\ v(t)\Delta t\sin \theta \\ w(t)\Delta t\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v(t)\\ w(t)\end{array}\right]\Delta t$$

于是
$$\left[\begin{array}{c}\frac{\Delta x}{\Delta t}\\ \frac{\Delta y}{\Delta t}\\ \frac{\Delta \theta }{\Delta t}\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right][\begin{array}{c}v(t)\\ w\end{array}$$