概率机器人：速度运动模型

机器人运动

概率机器人学将运动方程推广：由控制噪声或者未建模的外源性影响，控制输出是不确定的。

基础概念

运动学构型

运动学（Kinematics）：描述控制行为对机器人构型产生影响的微积分。

• 位姿：能够表示机器人位置和航向的物理量
  • 平面环境下通常限定为三维向量：${\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$
  • 航向：也称方位，用于表示机器人的朝向。
  • 位置：除航向外的部分，称为位置。
• 位姿和环境中对象的位置构成了机器人-环境系统的运动学状态$x_t$

概率运动学

概率运动学模型在移动机器人中起到状态变换模型的作用，即$p(x_t|u_t,x_{t-1})$。

表示在控制$u_t$作用下，前一刻机器人位姿$x_{t-1}$在当前时刻得到机器人位姿$x_t$的概率。为对$x_{t-1}$施加控制$u_t$后，机器人取得的运动学状态的后验分布。实际控制中，$u_t$有时由机器人里程计提供。

速度运动模型

速度运动模型（Velocity Motion Model）认为可通过一个旋转速度和一个平移速度来控制机器人。差分驱动、阿克曼驱动、同步驱动通常使用该方案进行控制。

速度运动模型被用于概率运动规划，用$v_t$表示t时刻机器人的平移速度（Translational Velocity），用${\omega }_t$表示旋转速度（Rotational Velocity），有：
$$u_t=\left[\begin{array}{c}{v}_t\\ {\omega }_t\end{array}\right]$$
规定，机器人逆时针旋转为旋转正方向（${\omega }_t>0$），机器人前向运动为平移正方向（$v_t>0$）。

模型建立

理想模型

首先，考虑无噪声的理想情况并做出如下假定：

• 时刻t的控制量$u_t={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$
• 机器人初始位姿：$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$
• 在时间间隔$\Delta t$间，机器人速度恒定不变。也即机器人的控制量不变
• 运动$\Delta t$时间后，机器人的位姿：$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$

则在世界坐标系$XOY$中，机器人作半径为$r$的圆弧运动，圆弧的中心点为$C$。过机器人中心作其运动方向的切线，交坐标系得航向角$\theta$

机器人平移速度与旋转速度间存在等式：
$$\begin{array}{rl}{v}_t& ={\omega }_t\cdot r\\ r& =\frac{v_t}{{\omega }_t}\end{array}$$
过运动圆弧中心$C$做坐标轴平行线，可由三角函数得：
$$\begin{gathered} X_C=X-r\cdot \cos (\theta -90°)=X-r\cdot \sin \theta \\ {Y}_C=Y-r\cdot \sin (\theta -90°)=Y-r\cdot \cos \theta \end{gathered}$$
将转弯半径带入简化等式：
$$\{\begin{array}{llc}{X}_C& =& X-\frac{v_t}{{\omega }_t}\cdot \sin \theta \\ Y_C& =& Y-\frac{v_t}{{\omega }_t}\cdot \cos \theta \end{array}$$
如图所示，机器人运动$\Delta t$时间后，沿着运动方向前进$v_t\Delta t$，并导致其航向旋转了${\omega }_t\Delta t$。

此时，圆弧中心坐标不变，在此基础上采用三角函数，计算得到当前时刻的位姿：
$$\begin{array}{rlr}\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right]& =& \left[\begin{array}{c}X+\frac{v_t}{{\omega }_t}\sin (\theta +{\omega }_t\Delta t)\\ Y+\frac{v_t}{{\omega }_t}\cos (\theta +{\omega }_t\Delta t)\\ \theta +\omega \Delta t\end{array}\right]\\ \left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right]& =& \left[\begin{array}{c}X\\ Y\\ \theta \end{array}\right]+\left[\begin{array}{c}-\frac{v_t}{{\omega }_t}\sin \theta +\frac{v_t}{{\omega }_t}\sin (\theta +{\omega }_t\Delta t)\\ \frac{v_t}{{\omega }_t}\cos \theta -\frac{v_t}{{\omega }_t}\cos (\theta +{\omega }_t\Delta t)\\ {\omega }_t\Delta t\end{array}\right]\end{array}$$

噪声模型

实际运动中，机器人将受到噪声影响。实际运行速度与期望速度存在一定的偏差。对控制量加入噪声：
$$\left[\begin{array}{c}{\hat{v}}_t\\ {\hat{\omega }}_t\end{array}\right]=\left[\begin{array}{c}{v}_t\\ {\omega }_t\end{array}\right]+\left[\begin{array}{c}{\epsilon }_{{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2}\\ {\epsilon }_{{\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2}\end{array}\right]$$
式中，噪声${\epsilon }_{b^2}$表示均值为0，方差为$b^2$的误差参量，参数${\alpha }_1$~${\alpha }_4$表示机器人里程计的误差参数，用于确定机器人的精确度。机器人越不精确，参数越大。

噪声可用正态分布进行描述：
$${\epsilon }_{b^2}(x)=\frac{1}{\sqrt{2\pi b^2}}exp(-\frac{1}{2}\frac{x^2}{b^2})$$
如图为均值为0，方差为$b^2$的正态分布曲线。正态分布通常用于连续随机过程的噪声建立模型。

噪声也可以用三角形分布进行描述：
ε b 2 ( x ) = max ⁡ { 0 , 1 6 b − ∣ x ∣ 6 b 2 } \varepsilon_{b^2}(x)=\max\{0,\frac{1}{\sqrt 6b}-\frac{|x|}{6b^2}\} εb2​(x)=max{0,6 ​b1​−6b2∣x∣​}
如图为三角形分布曲线，它仅在$(-\sqrt{6b},\sqrt{6b})$区间内大于零。

由此，得到噪声下的机器人位姿：
$$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ \theta \end{array}\right]+\left[\begin{array}{c}-\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\sin \theta +\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\sin (\theta +{\omega }_t\Delta t)\\ \frac{{\hat{v}}_t}{{\hat{\omega }}_t}\cos \theta -\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\cos (\theta +{\omega }_t\Delta t)\\ {\hat{\omega }}_t\Delta t\end{array}\right]$$

最终航向

上述模型建立在机器人围绕半径为$r$的圆弧运动进行建模，但由于环境噪声原因，实际运动轨迹并非圆形。为推广模型，假设机器人抵达最终位姿时，旋转${\hat{\gamma }}_t$:
$$\begin{array}{rlr}{\theta }^{{}^{\prime }}& =& \theta +{\hat{\omega }}_t\Delta t+{\hat{\gamma }}_t\Delta t\\ \hat{\gamma }& =& {\epsilon }_{{\alpha }_5{v}_t^2+{\alpha }_6{\omega }_t^2}\end{array}$$
参量${\alpha }_5$和${\alpha }_6$为机器人里程计噪声参数，由此得到最终的运动模型：
$$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ \theta \end{array}\right]+\left[\begin{array}{c}-\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\sin \theta +\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\sin (\theta +{\omega }_t\Delta t)\\ \frac{{\hat{v}}_t}{{\hat{\omega }}_t}\cos \theta -\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\cos (\theta +{\omega }_t\Delta t)\\ {\hat{\omega }}_t\Delta t+{\hat{\gamma }}_t\Delta t\end{array}\right]$$

闭式算法

算法推导

计算在时间间隔$\Delta t$内，在控制量$u_t={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$的作用下，机器人位姿从初始位姿$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$变为期望位姿$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$的概率密度$p(x_t|u_t,x_{t-1})$

首先，计算将机器人从位姿$x_{t-1}$变换至位置${\left[\begin{array}{cc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}\end{array}\right]}^T$所需的控制量

模型假设机器人在$\Delta t$时间内速度恒定，运动轨迹形成圆弧。则对于圆弧圆心，其与机器人的初始位姿正交，其中$\lambda$为未知量：
$$\left[\begin{array}{c}{X}^{\ast }\\ Y^{\ast }\end{array}\right]=\left[\begin{array}{c}X\\ Y\end{array}\right]+\left[\begin{array}{c}-\lambda \sin \theta \\ \lambda \cos \theta \end{array}\right]$$
同时，圆弧圆心位于点$(X,Y)$和点$(X^{{}^{\prime }},Y^{{}^{\prime }})$的垂直平分线上，其中$\mu$为未知量：
$$\left[\begin{array}{c}{X}^{\ast }\\ Y^{\ast }\end{array}\right]=\left[\begin{array}{c}\frac{X+X^{{}^{\prime }}}{2}+\mu (Y-Y^{{}^{\prime }})\\ \frac{Y+Y^{{}^{\prime }}}{2}+\mu (X^{{}^{\prime }}-X)\end{array}\right]$$
当${\omega }_t=0$时，圆心位于无穷远处，机器人近似直线运动。；当$w_t\ne 0$时联立方程，得唯一解：
$$\mu =\frac{1}{2}\frac{(X-X^{{}^{\prime }})\cos \theta +(Y-Y^{{}^{\prime }})\sin \theta }{(Y-Y^{{}^{\prime }})\cos \theta -(X-X^{{}^{\prime }})\sin \theta }$$
带入等式，得到圆心坐标表达式：
$$\left[\begin{array}{c}{X}^{\ast }\\ Y^{\ast }\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}(X+X^{{}^{\prime }})+\frac{1}{2}\frac{(X-X^{{}^{\prime }})\cos \theta +(Y-Y^{{}^{\prime }})\sin \theta }{(Y-Y^{{}^{\prime }})\cos \theta -(X-X^{{}^{\prime }})\sin \theta }(Y-Y^{{}^{\prime }})\\ \frac{1}{2}(Y+Y^{{}^{\prime }})+\frac{1}{2}\frac{(X-X^{{}^{\prime }})\cos \theta +(Y-Y^{{}^{\prime }})\sin \theta }{(Y-Y^{{}^{\prime }})\cos \theta -(X-X^{{}^{\prime }})\sin \theta }(X^{{}^{\prime }}-X)\end{array}\right]$$
圆弧半径可以采用欧拉距离公式计算得到：
$$r^{\ast }=\sqrt{(X-X^{\ast }{)}^2+(Y-Y^{\ast }{)}^2}$$
由此，得到机器人航向的变化量计算表达式：
$$\Delta \theta =atan2(Y^{{}^{\prime }}-Y^{\ast },X^{{}^{\prime }}-X^{\ast })-atan2(Y-Y^{\ast },X-X^{\ast })$$
函数$atan2(y,x)$表达式如下，大多数语言（如Python中math库）提供了对应API（math.atan2(y,x)）：
$$atan2(y,x)=\{\begin{array}{ll}\arctan (y/x)& x>0\\ sign(y)(\pi -\arctan |\frac{y}{x}|)& x<0\\ 0& x=0,y=0\\ sign(y)\frac{\pi }{2}& x=0,y\ne 0\end{array}$$
式中，函数$sign(x)$为符号函数，表达式如下：
$$sign(x)=\{\begin{array}{llc}& 1& x>0\\ & 0& x=0\\ -& 1& x<0\end{array}$$
从而，得到机器人的平移距离：
$$\Delta dist=r^{\ast }\Delta \theta$$
进而得到控制量的表达式：
$${\hat{u}}_t=\left[\begin{array}{c}{\hat{v}}_t\\ {\hat{\omega }}_t\end{array}\right]=\Delta t^{-1}\left[\begin{array}{c}\Delta dist\\ \Delta \theta \end{array}\right]$$

其次，计算机器人抵达最终航向${\theta }^{{}^{\prime }}$所需的旋转${\hat{\gamma }}_t$
$${\hat{\gamma }}_t=\Delta t^{-1}({\theta }^{{}^{\prime }}-\theta )-{\hat{\omega }}_t$$

最后，计算机器人的概率密度$p(x_t|u_t,x_{t-1})$。

定义运动误差参量用于表示${\hat{u}}_t$和${\hat{\gamma }}_t$相对控制量$u_t={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$和旋转${\gamma }_t=0$的偏差值。
$$\begin{array}{rl}{v}_{err}& =v_t-{\hat{v}}_t\\ {\omega }_{err}& ={\omega }_t-{\hat{\omega }}_t\\ {\gamma }_{err}& ={\hat{\gamma }}_t\end{array}$$
由此，得到噪声误差：
$$\begin{array}{r}{\epsilon }_{{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2}(v_{err})\\ {\epsilon }_{{\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2}({\omega }_{err})\\ {\epsilon }_{{\alpha }_5{v}_t^2+{\alpha }_6{\omega }_t^2}({\gamma }_{err})\end{array}$$
假定，不同来源的误差噪声相互独立，则得到概率密度：
$$p(x_t|u_t,x_{t-1})={\epsilon }_{{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2}(v_{err})\cdot {\epsilon }_{{\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2}({\omega }_{err})\cdot {\epsilon }_{{\alpha }_5{v}_t^2+{\alpha }_6{\omega }_t^2}({\gamma }_{err})$$

算法设计

系统输入: 初始位姿$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$、控制量$u_t={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$、估计后继姿态$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$

系统输出： 后继姿态为$x_t$的概率$p(x_t|u_t,x_{t-1})$

假设条件： 控制算法以固定时间间隔$\Delta t$执行。

用参数${\alpha }_1$ ~ ${\alpha }_6$表示机器人里程计运动噪声系数：

• 里程计平移噪声
  • 参数${\alpha }_1$：平移运动中平移分量的噪声
  • 参数${\alpha }_2$：平移运动中旋转分量的噪声
• 里程计旋转噪声
  • 参数${\alpha }_3$：旋转运动中平移分量的噪声
  • 参数${\alpha }_4$：旋转运动中旋转分量的噪声
• 里程计航向噪声
  • 参数${\alpha }_5$：最终航向中平移分量的噪声
  • 参数${\alpha }_6$：最终航向中旋转分量的噪声

算法实现逻辑如下：
$$\begin{array}{rl}& Algorithmmotion\_model\_velocity(x_t,u_t,x_{t-1}):\\ 1:& \mu =\frac{1}{2}\frac{(X-X^{{}^{\prime }})\cos \theta +(Y-Y^{{}^{\prime }})\sin \theta }{(Y-Y^{{}^{\prime }})\cos \theta -(X-X^{{}^{\prime }})\sin \theta }\\ 2:& X^{\ast }=\frac{1}{2}(X+X^{{}^{\prime }})+\mu (Y-Y^{{}^{\prime }})\\ 3:& Y^{\ast }=\frac{1}{2}(Y+Y^{{}^{\prime }})+\mu (X^{{}^{\prime }}-X)\\ 4:& r^{\ast }=\sqrt{(X-X^{\ast }{)}^2+(Y-Y^{\ast }{)}^2}\\ 5:& \Delta \theta =atan2(Y^{{}^{\prime }}-Y^{\ast },X^{{}^{\prime }}-X^{\ast })-atan2(Y-Y^{\ast },X-X^{\ast })\\ 6:& {\hat{v}}_t=\frac{\Delta \theta }{\Delta t}\cdot r^{\ast }\\ 7:& {\hat{\omega }}_t=\frac{\Delta \theta }{\Delta t}\\ 8:& {\hat{\gamma }}_t=\frac{{\theta }^{{}^{\prime }}-\theta }{\Delta t}-{\hat{\omega }}_t\\ 9:& returnprob(v_t-{\hat{v}}_t,{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2)\cdot prob({\omega }_t-{\hat{\omega }}_t,{\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2)\cdot prob({\hat{\gamma }}_t,{\alpha }_5{v}_t^2+{\alpha }_6{\omega }_t^2)\end{array}$$
函数$prob(x,b^2)$用于计算参量$x$在均值为0、方差为$b^2$下的概率。

可使用正太分布：
$$\begin{array}{rl}& Algorithmprob\_normal\_distribution(x,b^2):\\ & return\frac{1}{\sqrt{2\pi b^2}}\exp (-\frac{1}{2}\cdot \frac{x^2}{b^2})\end{array}$$
或三角形分布：
A l g o r i t h m p r o b _ t r i a n g u l a r _ d i s t r i b u t i o n ( x , b 2 ) : r e t u r n max ⁡ { 0 , 1 6 b − ∣ x ∣ 6 b 2 } \begin{aligned} &Algorithm\quad prob\_triangular\_distribution(x,b^2):\\ &\quad\quad return\quad \max\{0,\frac{1}{\sqrt 6b}-\frac{|x|}{6b^2}\} \end{aligned} ​Algorithmprob_triangular_distribution(x,b2):returnmax{0,6 ​b1​−6b2∣x∣​}​

算法效果

在平面空间XOY下，设置机器人具有相同的初始姿态$x_{t-1}$和控制量$u_t$，则在不同里程计噪声参数下，模型具有不同效果。

当机器人具有中等误差（参数${\alpha }_1$ ~ ${\alpha }_6$），则如图所示：

当机器人具有较大的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较小的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

当机器人具有较小的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较大的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

采样算法

算法推导

针对速度运动模型，采用粒子滤波计算噪声项并带入公式计算即可得到预测位姿。

算法设计

系统输入： 初始位姿$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$、控制量$u_t={\left[\begin{array}{cc}{v}_t& {\omega }_t\end{array}\right]}^T$

系统输出： 估计后继姿态$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$

假设条件： 控制算法以固定时间间隔$\Delta t$执行。

采样算法采用粒子滤波进行，旨在根据给定的控制量$u_t$和初始位姿$x_{t-1}$，采用运动模型$p(x_t|u_t,x_{t-1})$产生一个随机的$x_t$，算法的设计流程如下：
$$\begin{array}{rl}& Algorithmsample\_motion\_model\_velocity(u_t,x_{t-1}):\\ 1:& {\hat{v}}_t=v_t+sample({\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2)\\ 2:& {\hat{\omega }}_t={\omega }_t+sample({\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2)\\ 3:& {\hat{\gamma }}_t=sample({\alpha }_5{v}_t^2+{\alpha }_6{\omega }_t^2)\\ 4:& x^{{}^{\prime }}=x-\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\sin \theta +\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\sin (\theta +{\hat{\omega }}_t\Delta t)\\ 5:& y^{{}^{\prime }}=y+\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\cos \theta -\frac{{\hat{v}}_t}{{\hat{\omega }}_t}\cos (\theta +{\hat{\omega }}_t\Delta t)\\ 6:& {\theta }^{{}^{\prime }}=\theta +{\hat{\omega }}_t\Delta t+{\hat{\gamma }}_t\Delta t\\ 7:& return{x}_t={\left[\begin{array}{ccc}{x}^{{}^{\prime }}& y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T\end{array}$$

首先，1~3行采用运动学模型误差产生控制量的噪声；随后，4~6行使用噪声生成新的机器人位姿。式中函数$sample(b^2)$用于产生以0为均值，$b^2$为方差的随机样本。

可使用（近似）正态分布实现函数：
$$\begin{array}{rl}& Algorithmsample\_normal\_distribution(b^2):\\ & return\frac{1}{2}\sum _{i=1}^{12}rand(-b,b)\end{array}$$
或使用三角形分布实现函数：
$$\begin{array}{rl}& Algorithmsample\_triangular\_distribution(b^2):\\ & return\frac{\sqrt{6}}{2}[rand(-b,b)+rand(-b,b)]\end{array}$$
式中，函数$rand(x,y)$用于在$x$到$y$之间生成一个均匀分布的伪随机数产生器。

算法效果

在平面空间XOY下，设置机器人具有相同的初始姿态$x_{t-1}$和控制量$u_t$，且控制量与初始位姿同闭式计算中相同，则在不同不同里程计噪声参数下，具有不同效果。

当机器人具有中等误差（参数${\alpha }_1$ ~ ${\alpha }_6$），则如图所示：

当机器人具有较大的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较小的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

当机器人具有较小的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较大的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：