机器人的运动模型

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

1. 转弯的情况

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

∠ O^{'} O P 2 + ∠ O O^{'} P 2^{'} + ∠ O^{'} P 2^{'} P 2 + ∠ O P 2 P 2^{'} = 360

$\angle O'OP2 + \angle OO'P2' + \angle O'P2'P2 + \angle OP2P2' = 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 \int_{0}^{2 π} d θ

$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$ :

α = \frac{r - l}{w}

$\alpha = \frac{r-l}{w}$

R = \frac{l w}{r - l}

$R = \frac{lw}{r-l}$
现在已知

P 1 (x_{1}, y_{1}, θ_{1})

$P1(x_1,y_1,\theta_1)$ 、

α

$\alpha$ 、

R

$R$ 和轮子之间的间距

w

$w$ ，如何求

P 2

$P2$ 呢?

假设机器人在转弯的时候是以圆心 $O$ 为中心从 $P1(x_1,y_1,\theta_1)$ 运动到 $P2(x_2,y_2,\theta_2)$ ,
那么圆心的计算公式如下：

x_{0} + (R + \frac{w}{2}) s i n (θ_{1}) = x_{1}

$x_0 + (R+\frac{w}{2})sin(\theta_1) = x_1$

y_{0} - (R + \frac{w}{2}) c o s (θ_{1}) = y_{1}

$y_0 - (R+\frac{w}{2})cos(\theta_1) = y_1$

[\begin{matrix} x_{0} \\ y_{0} \end{matrix}] = [\begin{matrix} x_{1} \\ y_{1} \end{matrix}] + (R + \frac{w}{2}) [\begin{matrix} - s i n (θ_{1}) \\ c o s (θ_{1}) \end{matrix}]

$\left[ \begin{matrix} x_0 \\ y_0 \end{matrix} \right] = \left[ \begin{matrix} x_1 \\ y_1 \end{matrix} \right] +(R+\frac{w}{2})\left[ \begin{matrix}-sin(\theta_1) \\ cos(\theta_1) \end{matrix} \right]$

知道圆心和从 $P1$ 到 $P2$ 之间移动角度大小为 $\theta_1+\alpha$ ,那么 $P2$ 的计算方法为：

[\begin{matrix} x_{0} \\ y_{0} \end{matrix}] + (R + \frac{w}{2}) [\begin{matrix} c o s (θ_{1} + α) \\ - s i n (θ_{1} + α) \end{matrix}] = [\begin{matrix} x_{2} \\ y_{2} \end{matrix}]

$\left[ \begin{matrix} x_0 \\ y_0 \end{matrix} \right] + (R+\frac{w}{2}) \left[ \begin{matrix}cos(\theta_1+\alpha) \\ -sin(\theta_1+\alpha) \end{matrix} \right] = \left[ \begin{matrix} x_2 \\ y_2 \end{matrix} \right]$

所以此时 $P2(x_2,y_2,\theta_2)$ 的坐标为

[\begin{matrix} x_{2} \\ y_{2} \\ θ_{2} \end{matrix}] = [\begin{matrix} (R + \frac{w}{2}) c o s (θ_{1} + α) \\ - (R + \frac{w}{2}) s i n (θ_{1} + α) \\ θ_{1} + α \end{matrix}]

$\left[ \begin{matrix} x_2 \\ y_2 \\ \theta_2\end{matrix} \right] = \left[ \begin{matrix}(R+\frac{w}{2}) cos(\theta_1+\alpha) \\ -(R+\frac{w}{2}) sin(\theta_1+\alpha) \\ \theta_1+\alpha \end{matrix} \right]$

2. 不转弯的情况

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

θ_{2} = θ_{1}

$\theta_2 = \theta_1$
而机器人的位姿会发生的变化如下图所示，

这里写图片描述

[\begin{matrix} x_{2} \\ y_{2} \end{matrix}] = [\begin{matrix} x_{1} \\ y_{1} \end{matrix}] + [\begin{matrix} l c o s (θ_{1}) \\ l s i n (θ_{1}) \end{matrix}]

$\left[ \begin{matrix} x_2 \\ y_2 \end{matrix} \right] = \left[ \begin{matrix} x_1 \\ y_1 \end{matrix} \right] + \left[ \begin{matrix}lcos(\theta_1) \\ lsin(\theta_1) \end{matrix} \right]$