机器人的运动模型

机器人的运动模型

这篇文章主要介绍两轮机器人如何根据:
机器人轮子的编码信息和机器人当先位姿(x1,y1,θ)$(x_1,y_1,\theta )$计算出下一时刻机器人的位姿(x2,y2,θ2)$(x_2,y_2,{\theta }_2)$。
即：
输入：(x1,y2,θ1)$(x_1,y_2,{\theta }_1)$、 机器人左右轮子的间距w$w$，码盘的距离信息l$l$和r$r$
输出：(x2,y2,θ2)$(x_2,y_2,{\theta }_2)$
机器人在平面上运动时有两种情况，一种是转弯的情况，另一种是不转弯的情况；
下面分别进行介绍：

1. 转弯的情况

机器人在转弯的时候，其运动的示意图如下所示，其中角∠P1P1′P2′$\mathrm{\angle }P1P{1}^{\prime }P{2}^{\prime }$表示机器人P1点的头朝向，角∠P2P2′基线$\mathrm{\angle }P2P{2}^{\prime }基线$ 表示机器人在P2点的头朝向。需要注意的是，头朝向θ$\theta$ 是以O$O$为圆心，R+w/2$R+w/2$为半径的圆的边界引出一条切线和基线之间的形成的夹角的大小。在图中，通过相似三角形的性质可以知道，∠P1P1′P2′$\mathrm{\angle }P1P{1}^{\prime }P{2}^{\prime }$ = ∠O′OP1$\mathrm{\angle }{O}^{\prime }OP1$ = θ1${\theta }_1$
那么角度θ2$\theta 2$可以通过四边形OO’P2P2’的内角和为360度得到：

∠O′OP2+∠OO′P2′+∠O′P2′P2+∠OP2P2′=360$$\mathrm{\angle }{O}^{\prime }OP2+\mathrm{\angle }O{O}^{\prime }P{2}^{\prime }+\mathrm{\angle }{O}^{\prime }P{2}^{\prime }P2+\mathrm{\angle }OP2P{2}^{\prime }=360$$

即

(θ1+α)+90+(180−θ2)+90=360$$({\theta }_1+\alpha )+90+(180-{\theta }_2)+90=360$$

整理之后，得到，

θ2=θ1+α$${\theta }_2={\theta }_1+\alpha$$

在中小学的时期，我们学到如下计算周长的公式：

C=2πr=r∫2π0dθ$$C=2\pi r=r{\int }_0^{2\pi }d\theta$$

由此可知，当θ$\theta$的值很小的时候，周长=角度*半径。利用这种思想，我们可以构建如下的公式：

αR=l$$\alpha R=l$$

α(R+w)=r$$\alpha (R+w)=r$$

在上述公式中，只有l$l$和r$r$是已经知道的，但是我们可以根据该公式计算出α$\alpha$和R$R$:

α=r−lw$$\alpha =\frac{r-l}{w}$$

R=lwr−l$$R=\frac{lw}{r-l}$$

现在已知P1(x1,y1,θ1)$P1(x_1,y_1,{\theta }_1)$、α$\alpha$、R$R$和轮子之间的间距w$w$，如何求P2$P2$呢?

假设机器人在转弯的时候是以圆心O$O$为中心从P1(x1,y1,θ1)$P1(x_1,y_1,{\theta }_1)$运动到P2(x2,y2,θ2)$P2(x_2,y_2,{\theta }_2)$,
那么圆心的计算公式如下：

x0+(R+w2)sin(θ1)=x1$$x_0+(R+\frac{w}{2})sin({\theta }_1)=x_1$$

y0−(R+w2)cos(θ1)=y1$$y_0-(R+\frac{w}{2})cos({\theta }_1)=y_1$$

[x0y0]=[x1y1]+(R+w2)[−sin(θ1)cos(θ1)]$$\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]+(R+\frac{w}{2})\left[\begin{array}{c}-sin({\theta }_1)\\ cos({\theta }_1)\end{array}\right]$$

知道圆心和从P1$P1$到P2$P2$之间移动角度大小为θ1+α${\theta }_1+\alpha$,那么P2$P2$的计算方法为：

[x0y0]+(R+w2)[cos(θ1+α)−sin(θ1+α)]=[x2y2]$$\left[\begin{array}{c}{x}_0\\ y_0\end{array}\right]+(R+\frac{w}{2})\left[\begin{array}{c}cos({\theta }_1+\alpha )\\ -sin({\theta }_1+\alpha )\end{array}\right]=\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]$$

所以此时P2(x2,y2,θ2)$P2(x_2,y_2,{\theta }_2)$的坐标为

⎡⎣⎢x2y2θ2⎤⎦⎥=⎡⎣⎢⎢(R+w2)cos(θ1+α)−(R+w2)sin(θ1+α)θ1+α⎤⎦⎥⎥$$\left[\begin{array}{c}{x}_2\\ y_2\\ {\theta }_2\end{array}\right]=\left[\begin{array}{c}(R+\frac{w}{2})cos({\theta }_1+\alpha )\\ -(R+\frac{w}{2})sin({\theta }_1+\alpha )\\ {\theta }_1+\alpha \end{array}\right]$$

2. 不转弯的情况

当机器人不转弯的时候，机器人位姿中θ1${\theta }_1$是不会发生变化的，且l=r$l=r$，所以这时

θ2=θ1$${\theta }_2={\theta }_1$$

而机器人的位姿会发生的变化如下图所示，

[x2y2]=[x1y1]+[lcos(θ1)lsin(θ1)]$$\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]=\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]+\left[\begin{array}{c}lcos({\theta }_1)\\ lsin({\theta }_1)\end{array}\right]$$