移动机器人（一）——运动模型

一、简述

一般来说移动机器人的运动模型可分为完整约束和非完整约束。

• 完整约束（Holonomic，控制数=自由度）：可以用一个由位形变量$x,y,\theta$x,y,θ组成的方程来描述。包括全向轮模型。
• 非完整约束（Non-holonomic，控制数<自由度）：只能用位形变量的微分方程描述，无法积分成一个位形变量的约束方程。包括双轮自行车模型和差速模型 。

通常地面机器人的自由度为3，包括x,y与朝向。对于角轮（castor wheels）或者是全向轮（Omni-wheels）的机器人，是holonomic的，因为它能够朝各个方向移动，机器人的总自由度与可控自由度是相等的。

汽车模型可控的自由度为2，包括油门/刹车和方向盘转角，差速模型可控自由度为2，包括X方向和朝向（差速转向）。使得它难以满足任何方向的行驶（除非车辆发生打滑或侧滑），所以是非完整性约束。

相关概念

• 广义坐标：广义坐标是用来描述系统位形所需要的独立参数，或者最少参数。当分析有的问题时（尤其是当有许多约束条件的时候），尽量选择独立的广义坐标。因为这样可以减少代表约束的变量。
• 位形空间（configuration space，C-space）：所有位置形态集合。
• 任务空间（task space）：所有可能的机器人位姿构成的集合。

二、单轮模型（unicycle model）

单轮模型的位形为$(\theta,x,y)$(θ,x,y)，$r$r为车轮半径，$(x,y)$(x,y)为车轮与地面接触点，$\phi$ϕ为前进方向，单轮的规范简化模型为
$\dot{q} = \begin{bmatrix} \dot{\phi} \\ \dot{x} \\ \dot{y} \end{bmatrix}= \begin{bmatrix} 0 & 1 \\ r \cos \phi & 0\\ r \sin \phi & 0 \end{bmatrix} \begin{bmatrix} u_1 \\ u_2 \\ \end{bmatrix}$q˙​=⎣⎡​ϕ˙​x˙y˙​​⎦⎤​=⎣⎡​0rcosϕrsinϕ​100​⎦⎤​[u1​u2​​]

$u = (u_1,u_2)$u=(u1​,u2​)为控制输入，$u_1$u1​为前后行驶时的转动速度，$u_2$u2​为前进方向得到转速，其控制变换为
$u_1 = \frac{v}{r},\ u_2=w(\dot{\phi})$u1​=rv​, u2​=w(ϕ˙​)

三、差速模型（Differentially-drive）

3.1 两轮差速

两轮差速机器人由两个独立驱动的半径为$r$r轮子组成，围绕同一轴线旋转，并带有一个或多个脚轮、球形脚轮或使机器人保持水平的低摩擦滑块。

在实际应用中，车轮的组合可根据机器人对设计重心、转弯半径的要求，将辅助轮和驱动轮按照不同形式布置：

• 优点：结构及电机控制也相对简单，机器人灵活性较强，且算法易控制。
• 缺点：辅助轮（脚轮，万向轮）不适合在户外使用。

3.2 四轮差速

四轮差速车体为浅灰色外轮廓，可类似为深灰色的单轮模型，其运动学模型与两轮一样，因为同侧车轮转速相同。

四轮驱动在直线行走上能力较强，驱动力也比较大，但成本过高，电机控制较为复杂，为防止机器人打滑，需要更精细的结构设计。

小车车轮半径为$r$r，左右轮角速度为$u = (u_L,u_R)$u=(uL​,uR​)，线速度为

$v_L = r \cdot u_l\\ v_R = r \cdot u_R\\$vL​=r⋅ul​vR​=r⋅uR​

$\{B\}${B}为小车的坐标系，其原点为小车的中心，X轴为小车前进方向，绕ICR旋转的角速度为：
$\dot{\theta} = \frac{v_L}{R_L} = \frac{v_R}{R_R}$θ˙=RL​vL​​=RR​vR​​
因为$R_R = R_L +W$RR​=RL​+W（$W$W为左右两轮间距），因此$R_L$RL​为：
$\frac{v_L}{R_L} = \frac{v_R}{R_L+W}\\ v_L \cdot R_L+v_L \cdot W = v_R \cdot R_L\\ R_L = \frac{v_L \cdot W}{v_R-v_L}$RL​vL​​=RL​+WvR​​vL​⋅RL​+vL​⋅W=vR​⋅RL​RL​=vR​−vL​vL​⋅W​
将$R_L$RL​带入方程可求得角速度（小车坐标系$\{B\}${B}相对于世界坐标系$\{O\}${O}）为：
$\dot{\theta} = \frac{v_R-v_L}{W}$θ˙=WvR​−vL​​
小车的线速度为：
$v = \frac{v_R+v_L}{2}$v=2vR​+vL​​
小车的运动方程为：
$\dot{x} = v \cos{\theta}\\ \dot{y} = v \sin{\theta}\\ \dot{\theta} = \frac{v_R-v_L}{W}$x˙=vcosθy˙​=vsinθθ˙=WvR​−vL​​
规范简化模型为：
$\dot{q}= \begin{bmatrix} \dot{\theta}\\ \dot{x}\\ \dot{y} \end{bmatrix}= \begin{bmatrix} -\frac{r}{w} & \frac{r}{w}\\ \frac{r}{2} \cos \theta & \frac{r}{2} \cos \theta\\ \frac{r}{2} \sin \theta & \frac{r}{2} \sin \theta\\ \end{bmatrix} \begin{bmatrix} u_L\\ u_R \end{bmatrix}$q˙​=⎣⎡​θ˙x˙y˙​​⎦⎤​=⎣⎡​−wr​2r​cosθ2r​sinθ​wr​2r​cosθ2r​sinθ​⎦⎤​[uL​uR​​]
差速模型可以原地旋转，但不代表其为完整约束，本质上小车只有X轴移动和旋转两个控制自由量。

设$w = \dot{\theta},d = \frac{W}{2}$w=θ˙,d=2W​，小车的控制变换为：
$u_L= \frac{v-wd}{r},\ u_R= \frac{v+wd}{r}$uL​=rv−wd​, uR​=rv+wd​
$wd$wd为旋转时在离旋转中心$d$d距离处产生的切向线速度，$v-wd$v−wd和$v+wd$v+wd为切向加上角速度产生的线速度的影响后的实际速度，其正负与固定在小车上的坐标系有关。

3.3 旋转运动分析

运动状态

• 当$v_l > v_r || v_l < v_r$vl​>vr​∣∣vl​<vr​时，机器做圆弧运动；
• 当$v_l = v_r$vl​=vr​时，机器做直线运动；
• 当$v_l =- v_r$vl​=−vr​时，机器以左右轮中心点做原地旋转；

力分析

1. 原地旋转

差速模型在转动时，作用在车子上的为一对等大，方向相反的力，称为力偶。

由两个等值、反向、不共线的（平行）力组成的力系称为力偶，记作$(\vec{F},\vec{F}^{,})$(F,F,)

• 力偶中两力所在平面称为力偶作用面
• 力偶两力之间的垂直距离称为力偶臂

力偶在小车上形成一个力偶矩
$M = \pm F \cdot d$M=±F⋅d

力矩 = 转动惯量 x 角加速，力偶矩带动小车旋转。

2. 圆弧运动

当作用小车两侧速度不一致时，在小车上产生一个力偶和沿小车前进方向的力，使其旋转的同时前进产生圆弧运动。

四、汽车模型（Car-Like Mobile）

该模型也称为双轮自行车模型，常被误称为阿克曼（Ackeman）模型，因为汽车的转向使用阿克曼转向几何。

使用阿克曼转向的汽车，在转向时，两个前轮的转向角不同，使得所有车轮做无滑动的纯滚动（即车轮前进方向垂直于车轮于ICR之间的连线）

$\{O\}${O}为世界坐标系。

$\{B\}${B}为小车的坐标系，其原点为小车的后轮中心，X轴为小车前进方向，Y轴垂直于两轮中心线，在该坐标系下的小车速度为：
$^{V}\dot{x} = v, ^{V}\dot{y} = 0$Vx˙=v,Vy˙​=0
虚线的交点为旋转瞬心（ICR），小车上的参考点（$\{B\}${B}的原心）将绕该瞬心做圆弧运动，$\theta$θ为前进方向，其角速度为：
$\dot{\theta} = \frac{v}{R_B}$θ˙=RB​v​
转弯半径为：
$R_B = \frac{L}{\tan{\gamma}}$RB​=tanγL​
$\gamma$γ为前轮转向角度

小车的完整运动方程为
$\dot{q}= \begin{bmatrix} \dot{\theta}\\ \dot{x}\\ \dot{y}\\ \dot{\gamma} \end{bmatrix}= \begin{bmatrix} \frac{\tan \gamma}{L}&0\\ \cos \theta &0\\ \sin \theta&0\\ 0&1 \end{bmatrix} \begin{bmatrix} v\\ w\\ \end{bmatrix}$q˙​=⎣⎢⎢⎡​θ˙x˙y˙​γ˙​​⎦⎥⎥⎤​=⎣⎢⎢⎡​Ltanγ​cosθsinθ0​0001​⎦⎥⎥⎤​[vw​]

若转向控制为转向角$\gamma$γ而不是其速率$\dot{\gamma}$γ˙​，则可以简化运动方程，其规范简化模型为：

当转向速率$\dot{\gamma}$γ˙​足够高，使得转向角几乎为瞬时改变，则该假设合理，在这种情况下，$\dot{\gamma}$γ˙​作为状态变量被消除，小车位形为$q=(\theta,x,y)$q=(θ,x,y)

$\dot{q}= \begin{bmatrix} \dot{\gamma}\\ \dot{x}\\ \dot{y} \end{bmatrix}= \begin{bmatrix} 0&1\\ \cos \theta & 0\\\ \sin \theta&0 \end{bmatrix} \begin{bmatrix} v\\ w\\ \end{bmatrix}$q˙​=⎣⎡​γ˙​x˙y˙​​⎦⎤​=⎣⎡​0cosθ sinθ​100​⎦⎤​[vw​]

在当前条件下，当控制输入为$(v,w)$(v,w)时，$v$v为汽车前进速度，$w$w为旋转速率，其控制变换为：
$v = v\\ \gamma = \tan^{-1}(\frac{Lw}{v})$v=vγ=tan−1(vLw​)

在世界坐标系$\{O\}${O}中，Y方向的速度约束为：
$\dot{y} \cos{\theta} - \dot{x} \sin{\theta} \equiv 0$y˙​cosθ−x˙sinθ≡0

$\equiv$≡为恒等号，参变量恒为一个常数或恒定表达式时，总等于关系与变量无关。例如函数$f(x)\equiv k$f(x)≡k表示该函数的值始终为k而与x的值无关。

根据不同情况，该模型存在不同的约束

• simple car model
  $|v| \leqslant v_{max}, |\theta| \leqslant \theta_{max} < \frac{\pi}{2}$∣v∣⩽vmax​,∣θ∣⩽θmax​<2π​

• Reeds & Shepp’s car（前进、后退和停止）
  $v \in \left \{-v_{max},v_{max}\right \}, |\theta| \leqslant \theta_{max} < \frac{\pi}{2}$v∈{−vmax​,vmax​},∣θ∣⩽θmax​<2π​

• Dubin’s car（前进和停止）
  $v = v_{max}, |\theta| \leqslant \theta_{max} < \frac{\pi}{2}$v=vmax​,∣θ∣⩽θmax​<2π​

参考

《Robotics, Vision and Control: Fundamental Algorithms in MATLAB Second Edition》——4 Mobile Robot Vehicles

《现代机器人学：机构、规划与控制》——13 轮式移动机器人

Kinematics for Wheeled Systems

图解差速机器人的三种运动学模型

两轮差速移动机器人运动分析、建模和控制

无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型