移动机器人

1. 运动学模型和约束

1.1 模型概述

移动机器人的运动学模式是一个带有轮子的刚体，在一个二维的平面上运动。在移动机器人中，使用的比较多的一种运动学模型是差速轮模型（如下图所示）由并排的两个标准轮和一个支撑轮组成。机器人的坐标点通常被确定在两标准轮轴线的中点，被称为 P 点。车头的方向为机器人坐标系的 X 轴正方向，由右手定则确定机器人的 Y 轴正方向。机器人的 X 轴正方向与 Y 轴正方向所形成的角度为机器人的方向角 $\theta$

机器人在世界坐标中的位置为：

$$\zeta_I = \begin{bmatrix} x\\ y\\ \theta \end{bmatrix}$$

为了将机器人坐标系与世界坐标系联系起来，我们需要使用旋转矩阵对来表示两者之间的关系，其中旋转矩阵的计算方法为：

$$R(\theta) = \begin{bmatrix} cos\theta & sin\theta & 0\\ -sin\theta & cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}$$

由世界坐标系到机器人坐标系的变化则为：

$$\zeta_R = R(\theta)\zeta_I$$

1.2 正向运动学

在正向运动学中，我们首先需要知道车辆的几何尺寸，如轮子的半径 r，质点 P 的位置，以及质点 P 到轮子之间的距离 l。此外，还需要知道机器人的航向角 $\theta$，每个轮子的转速 $\dot{\psi_1}$, $\dot{\psi_2}$。正向运动学就是将上面的已知条件投影到机器人在世界坐标上的速度，用数学语言来表示就是：

$$\dot{\zeta_I} = \begin{bmatrix} \dot{x}\\ \dot{y} \\ \dot{\theta} \end{bmatrix} = f(l, r, \theta, \dot{\psi_1}, \dot{\psi_2})$$

接下来，我们来分析如何解出这个 f 方程。首先由机器人与世界坐标之间的变换方程入手：

$$\zeta_I = R^{-1}(\theta)\zeta_R$$

对于旋转矩阵来说：

$$R^{-1} = R^T$$

因此我们可以轻松的计算出:

$$R^{-1} = \begin{bmatrix} cos\theta& -sin\theta & 0\\ sin\theta& cos\theta & 0\\ 0 & 0& 1 \end{bmatrix}$$

而对于机器人本体的速度来说，则需要从质点 P 处将机器人分为两个部分来计算，其中右轮的速度为 $\dot{\psi_1}$左轮的速度为 $\dot{\psi_2}$每一个轮都会对机器人的质点 P 提供 x, y, $\theta$方向的速度。对于 X 方向来说：

$$\begin{matrix} x_1 = \frac{r\dot{\psi_1}}{2} \\\\ x_2 = \frac{r\dot{\psi_2}}{2} \\\\ x_r = x_1 + x_2 = \frac{r(\psi_1 + \psi_2)}{2} \end{matrix}$$

对于 Y 方向来说，因为机器人并不会发生侧滑，因此轮子的转动并不会提供 Y 方向的速度，因此为 0. 最后是航向角的速度，假设逆时针方向为正，则该速度为：

$$\begin{matrix} \omega_1 = \frac{r\dot{\psi_1}}{2l} \\\\ \omega_2 = -\frac{r\dot{\psi_2}}{2l} \\\\ \omega_r = \omega_1 + \omega_2 = \frac{r(\psi_1 - \psi_2)}{2l} \end{matrix}$$

因此，机器人的运动就可以表示为：

$$\zeta_R = \begin{bmatrix} \frac{r(\psi_1 + \psi_2)}{2}\\0\\ \frac{r(\psi_1 - \psi_2)}{2l} \end{bmatrix}$$

那么，在世界坐标系中的速度为：

$$\zeta_I = \begin{bmatrix} cos\theta& -sin\theta & 0\\ sin\theta& cos\theta & 0\\ 0 & 0& 1 \end{bmatrix} \begin{bmatrix} \frac{r(\psi_1 + \psi_2)}{2}\\0\\ \frac{r(\psi_1 - \psi_2)}{2l} \end{bmatrix}$$

1.3 运动约束

在讨论运动约束之前，我们首先需要做出一些假设，假设轮子在运动过程中始终垂直于地面，并且轮子与地面之间始终只有一个点接触。因此，任何轮子存在仅绕轴转动，不滑动的运动学约束。此外不同类别的轮子，其运动学约束是不一样的。

对于标准轮来说，其模型图如下所示：

使用数学语言表示则为：

Rolling Constrain:

$$[sin(\alpha + \beta)\qquad -cos(\alpha+\beta)\qquad -l\,cos\beta]R(\theta)\dot{\zeta_I}-r\dot{\psi} = 0$$

Non-Sliding Constrain:

$$[cos(\alpha + \beta)\qquad sin(\alpha+\beta)\qquad lsin\beta]R(\theta)\dot{\zeta_I} = 0$$

其中：

$$R(\theta)\dot{\zeta_I} = \dot{\zeta_R} = \begin{bmatrix} \dot{x_r}\\ \dot{y_r}\\ \dot{\theta} \end{bmatrix}$$

当标准轮作为主动轮的时候，这两个约束都必须满足。但作为随动轮的时候，由于转动量是一个自由量，因此无需考虑这两个约束。

对于脚轮来说，则略有差异：

Rolling Constrain:

$$[sin(\alpha + \beta)\qquad -cos(\alpha+\beta)\qquad -l\,cos\beta]R(\theta)\dot{\zeta_I}-r\dot{\psi} = 0$$

Non-Sliding Constrain:

$$[cos(\alpha + \beta)\qquad sin(\alpha+\beta)\qquad lsin\beta]R(\theta)\dot{\zeta_I} + d\dot{\beta}= 0$$

如果脚轮是作为随动轮，那么 $\dot{\beta}$ 是一个自由量，则无侧滑约总能满足，同样转速也是一个自由量，因此转动约束也总能满足。但是作为主动轮时，则控制过程中需要满足无侧滑约束。

2. 机动性

机动性是用于分析不同底盘运动性能的一种分析值。它是由移动性和转向性组成的。用数学语言表示就是：

$$\sigma_M = \sigma_m + \sigma_s$$

计算机动性的方法有很多，但由于该问题已经被彻底研究，所以这里直接给出常见底盘的移动性参数：

另外自行车跟使用阿克曼底盘的四轮车由于其只有前轮提供转向，后轮固定。因此在转向过程中始终会围绕着一个圆心，ICR，转动。因此机器人的可移动为 1. 转动度也为 1. 机动性为 2.

3. 案例分析

逆运动学是在已知机器人速度的情况下，计算机器人每个轮子的速度。接下来以差分驱动机器人为例来分析其逆运动学模型：

在计算之前，我们首先写出机器人的运动学约束：

$$[sin(\alpha + \beta)\qquad -cos(\alpha+\beta)\qquad -l\,cos\beta]\dot{\zeta_R}-r\dot{\psi} = 0$$

$$[cos(\alpha + \beta)\qquad sin(\alpha+\beta)\qquad lsin\beta]\dot{\zeta_R} = 0$$

从差分机器人的机械机构可以看出：

$$\text{left wheel: }\alpha=\frac{\pi}{2},\, \beta=0,\,l=b$$

$$\text{right wheel: }\alpha=\frac{\pi}{2},\, \beta=0,\,l=b$$

因此，运动学约束可以被写为：

Rolling constarin:

$$\begin{bmatrix} 1 & 0 & b\\ 1 & 0 & -b \end{bmatrix}\dot{\zeta_R}= \begin{bmatrix} r & 0\\ 0 & r \end{bmatrix} \begin{bmatrix} \dot{\psi_r}\\ \dot{\psi_l} \end{bmatrix}$$

Non-sliding constrain:

$$\begin{bmatrix} 0&-1&0\\ 0&-1&0 \end{bmatrix}\dot{\zeta_R}= \begin{bmatrix} 0\\ 0 \end{bmatrix}$$

将两个约束堆叠可以得到机器人速度与轮转速之间的关系：

$$\begin{bmatrix} 1 & 0 & b\\ 1 & 0 & -b\\ 0&-1&0\\ 0&-1&0 \end{bmatrix}\dot{\zeta_R}= \begin{bmatrix} r & 0\\ 0 & r\\ 0&0\\ 0&0 \end{bmatrix} \begin{bmatrix} \dot{\psi_r}\\ \dot{\psi_l} \end{bmatrix}$$

接下来，就只需要移项便可完成得到正/逆运动学的模型。但因为两边矩阵都不是正方向矩阵，因此只能计算矩阵的伪逆矩阵。为了简化书写，我们称：

$$A = \begin{bmatrix} 1 & 0 & b\\ 1 & 0 & -b\\ 0&-1&0\\ 0&-1&0 \end{bmatrix} \qquad B = \begin{bmatrix} r & 0\\ 0 & r\\ 0&0\\ 0&0 \end{bmatrix} \qquad \dot{\psi} = \begin{bmatrix} \psi_r\\ \psi_l \end{bmatrix}$$

然后就得到了简化后的式子：

$$A\dot{\zeta_R} = B\dot{\psi}$$

接下来，移项得到正运动学模型：

$$\dot{\zeta_R} = (A^TA)^{-1}A^TB\dot{\psi}$$

将 A 和 B 的值分别带入上式后，便可得到差分驱动底盘的正向运动学模型：

$$\begin{bmatrix} \dot{x}\\ \dot{y} \\ \dot{\theta} \end{bmatrix}= \begin{bmatrix} \frac{r}{2} & \frac{r}{2}\\ 0 & 0\\ \frac{r}{2b} & -\frac{r}{2b} \end{bmatrix} \begin{bmatrix} \dot{\psi_r} \\ \dot{\psi_l} \end{bmatrix}$$

逆向运动学模型为：

$$\dot{\psi} = (B^TB)B^TA \dot{\zeta_R}$$

同样，将值带入后，得到：

$$\begin{bmatrix} \dot{\psi_r}\\\dot{\psi_l} \end{bmatrix}= \begin{bmatrix} \frac{1}{r} & 0 & \frac{b}{r}\\ \frac{1}{r}& 0 & -\frac{b}{r} \end{bmatrix} \begin{bmatrix} \dot{x}\\ \dot{y} \\ \dot{\theta} \end{bmatrix}$$