里程计运动模型

速度运动模型采用机器人的速度来计算位姿的后验概率。里程计运动模型（Odometry motion model）则以里程计测量为基础来计算机器人随时间的运动。

通常，通过编码器可以获得机器人的里程信息。相比而言，里程计虽仍存在误差，但比之速度信息更为精确。

模型建立

理想模型

里程计信息实际上是对传感器的读取测量值，而非控制量。为建立作为测量的里程计模型且保持状态空间较小，将里程计测量值认为控制信号。

在时刻ttt，机器人确切的位姿由随机变量xtx_txt建立，里程计用于估计该位姿信息。由于里程计存在漂移、打滑情况，机器人里程计坐标系与世界坐标系间不存在固定的变换关系。

里程计模型使用由机器人内部里程计测量的相对运动信息。即在时间间隔(t−1,t](t-1, t](t−1,t]中，机器人位姿由xt−1x_{t-1}xt−1前进至xtx_txt，里程计反馈了xˉt−1=[XˉYˉθˉ]T\bar x_{t-1}=\begin{bmatrix}\bar X&\bar Y&\bar \theta\end{bmatrix}^Txˉt−1=[XˉYˉθˉ]T到xˉt=[Xˉ′Yˉ′θˉ′]T\bar x_t=\begin{bmatrix}\bar X^{'}&\bar Y^{'}&\bar \theta^{'}\end{bmatrix}^Txˉt=[Xˉ′Yˉ′θˉ′]T的相对前进。采用上标“ˉ\bar{\quad}ˉ”表示为机器人坐标系下坐标，该坐标系与世界坐标系转换关系未知。

由此，机器人的控制量ut=[xˉt−1xˉt]Tu_t=\begin{bmatrix}\bar x_{t-1}&\bar x_t\end{bmatrix}^Tut=[xˉt−1xˉt]T，可将任意两相邻时刻机器人位姿的相对差可通过三个基本运动串联得到，如图所示。

初始旋转：δrot1=atan2(Yˉ′−Yˉ,Xˉ′−Xˉ)−θˉ\delta_{rot1}=atan2(\bar Y^{'}-\bar Y,\bar X^{'}-\bar X)-\bar\thetaδrot1=atan2(Yˉ′−Yˉ,Xˉ′−Xˉ)−θˉ
平移：δtrans=(Xˉ−Xˉ′)2+(Yˉ−Yˉ′)2\delta_{trans}=\sqrt{(\bar X-\bar X^{'})^2+(\bar Y-\bar Y^{'})^2}δtrans=(Xˉ−Xˉ′)2+(Yˉ−Yˉ′)2
二次旋转：δrot2=θˉ′−θˉ−δrot1\delta_{rot2}=\bar \theta^{'}-\bar \theta-\delta_{rot1}δrot2=θˉ′−θˉ−δrot1

由此，机器人由初始位姿xt−1=[XYθ]Tx_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^Txt−1=[XYθ]T沿着运动轨迹方向（下图中紫线所示）运动至当前时刻位姿xt=[X′Y′θ′]Tx_{t}=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^Txt=[X′Y′θ′]T可拆分为机器人首先旋转角度δrot1\delta_{rot1}δrot1再沿着此方向平移δtrans\delta_{trans}δtrans，随后再次旋转δrot2\delta_{rot2}δrot2抵达目标位姿，如图所示：

上图中，由三角函数化简可以得到如下表达式：
[X′Y′θ′]=[XYθ]+[δtrans⋅cos⁡(θ+δrot1)δtrans⋅sin⁡(θ+δrot1)δrot1+δrot2]\begin{bmatrix} X^{'}\\Y^{'}\\\theta^{'}\\ \end{bmatrix}=\begin{bmatrix} X\\Y\\\theta \end{bmatrix}+\begin{bmatrix} \delta_{trans}\cdot\cos(\theta+\delta_{rot1})\\ \delta_{trans}\cdot\sin(\theta+\delta_{rot1})\\ \delta_{rot1}+\delta_{rot2} \end{bmatrix} ⎣⎡X′Y′θ′⎦⎤=⎣⎡XYθ⎦⎤+⎣⎡δtrans⋅cos(θ+δrot1)δtrans⋅sin(θ+δrot1)δrot1+δrot2⎦⎤

噪声模型

在真实环境下，机器人运动及里程计的测量存在噪声。定义里程计的测量值减去噪声得到的为实际里程值：
δ^rot1=δrot1−εα1δrot12+α2δtrans2δ^trans=δtrans−εα3δtrans2+α4δrot12+α4δrot22δ^rot2=δrot2−εα1δrot22+α2δtrans2\begin{aligned} \hat \delta_{rot1}&=\delta_{rot1}-\varepsilon_{\alpha_1\delta_{rot1}^2+\alpha_2\delta_{trans}^2}\\ \hat \delta_{trans}&=\delta_{trans}-\varepsilon_{\alpha_3\delta_{trans}^2+\alpha_4\delta_{rot1}^2+\alpha_4\delta_{rot2}^2}\\ \hat \delta_{rot2}&=\delta_{rot2}-\varepsilon_{\alpha_1\delta_{rot2}^2+\alpha_2\delta_{trans}^2} \end{aligned} δ^rot1δ^transδ^rot2=δrot1−εα1δrot12+α2δtrans2=δtrans−εα3δtrans2+α4δrot12+α4δrot22=δrot2−εα1δrot22+α2δtrans2
则，噪声下机器人的里程计运动模型如下所示：
[X′Y′θ′]=[XYθ]+[δ^trans⋅cos⁡(θ+δ^rot1)δ^trans⋅sin⁡(θ+δ^rot1)δ^rot1+δ^rot2]\begin{bmatrix} X^{'}\\Y^{'}\\\theta^{'}\\ \end{bmatrix}=\begin{bmatrix} X\\Y\\\theta \end{bmatrix}+\begin{bmatrix} \hat\delta_{trans}\cdot\cos(\theta+\hat\delta_{rot1})\\ \hat\delta_{trans}\cdot\sin(\theta+\hat\delta_{rot1})\\ \hat\delta_{rot1}+\hat\delta_{rot2} \end{bmatrix} ⎣⎡X′Y′θ′⎦⎤=⎣⎡XYθ⎦⎤+⎣⎡δ^trans⋅cos(θ+δ^rot1)δ^trans⋅sin(θ+δ^rot1)δ^rot1+δ^rot2⎦⎤

闭式算法

算法推导

在真实环境下，通过里程计测量值减去实际值可以得到里程计误差：
δrot1,err=δrot1−δ^rot1δtrans,err=δtrans−δ^transδrot2,err=δrot2−δ^rot2\begin{aligned} \delta_{rot1,err}&=\delta_{rot1}-\hat \delta_{rot1}\\ \delta_{trans,err}&=\delta_{trans}-\hat \delta_{trans}\\ \delta_{rot2,err}&=\delta_{rot2}-\hat \delta_{rot2}\\ \end{aligned} δrot1,errδtrans,errδrot2,err=δrot1−δ^rot1=δtrans−δ^trans=δrot2−δ^rot2
由此，得到各项噪声的概率密度计算如下：
p1=εα1δrot12+α2δtrans2(δrot1,err)p2=εα3δtrans2+α4δrot12+α4δrot22(δtrans,err)p3=εα1δrot22+α2δtrans2(δrot2,err)\begin{aligned} p_1&=\varepsilon_{\alpha_1\delta_{rot1}^2+\alpha_2\delta_{trans}^2}(\delta_{rot1,err})\\ p_2&=\varepsilon_{\alpha_3\delta_{trans}^2+\alpha_4\delta_{rot1}^2+\alpha_4\delta_{rot2}^2}(\delta_{trans,err})\\ p_3&=\varepsilon_{\alpha_1\delta_{rot2}^2+\alpha_2\delta_{trans}^2}(\delta_{rot2,err}) \end{aligned} p1p2p3=εα1δrot12+α2δtrans2(δrot1,err)=εα3δtrans2+α4δrot12+α4δrot22(δtrans,err)=εα1δrot22+α2δtrans2(δrot2,err)
假设各项噪声相互独立，则由xt−1x_{t-1}xt−1状态得到xtx_txt状态的概率密度：
p(xt∣ut,xt−1)=p1⋅p2⋅p3p(x_t|u_t,x_{t-1})=p_1\cdot p_2\cdot p_3 p(xt∣ut,xt−1)=p1⋅p2⋅p3

算法设计

系统输入： 初始位姿xt−1=[XYθ]Tx_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^Txt−1=[XYθ]T、控制量ut=[xˉt−1xˉt]Tu_t=\begin{bmatrix}\bar x_{t-1}&\bar x_t\end{bmatrix}^Tut=[xˉt−1xˉt]T、估计后继姿态xt=[X′Y′θ′]Tx_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^Txt=[X′Y′θ′]T

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

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

用参数α1\alpha_1α1 ~ α4\alpha_4α4表示机器人里程计运动噪声系数：

里程计平移噪声
- 参数α1\alpha_1α1：平移运动中平移分量的噪声
- 参数α2\alpha_2α2：平移运动中旋转分量的噪声
里程计旋转噪声
- 参数α3\alpha_3α3：旋转运动中平移分量的噪声
- 参数α4\alpha_4α4：旋转运动中旋转分量的噪声

算法实现逻辑如下：
Algorithmmotion_model_odometry(xt,ut,xt−1):1:δrot1=atan2(Yˉ′−Yˉ,Xˉ′−Xˉ)−θˉ2:δtrans=(Xˉ−Xˉ′)2+(Yˉ−Yˉ′)23:δrot2=θˉ′−θˉ−δrot14:δ^rot1=atan2(Y′−Y,X′−X)−θ5:δ^trans=(X−X′)2+(Y−Y′)26:δ^rot2=θ′−θ−δ^rot17:p1=prob(δrot1−δ^rot1,α1δ^rot12+α2δ^trans2)8:p2=prob(δtrans−δ^trans,α3δ^trans2+α4δ^rot12+α4δ^rot22)9:p3=prob(δrot2−δ^rot2,α1δ^rot22+α2δ^trans2)10:returnp1⋅p2⋅p3\begin{aligned} &Algorithm\quad motion\_model\_odometry(x_t,u_t,x_{t-1}): \\ 1:&\qquad \delta_{rot1}=atan2(\bar Y^{'}-\bar Y,\bar X^{'}-\bar X)-\bar\theta \\ 2:&\qquad \delta_{trans}=\sqrt{(\bar X-\bar X^{'})^2+(\bar Y-\bar Y^{'})^2}\\ 3:&\qquad \delta_{rot2}=\bar \theta^{'}-\bar \theta-\delta_{rot1}\\ \\ 4:&\qquad \hat\delta_{rot1}=atan2(Y^{'}-Y,X^{'}-X)-\theta \\ 5:&\qquad \hat\delta_{trans}=\sqrt{(X-X^{'})^2+(Y-Y^{'})^2}\\ 6:&\qquad \hat\delta_{rot2}=\theta^{'}-\theta-\hat\delta_{rot1}\\ \\ 7:&\qquad p_1=prob(\delta_{rot1}-\hat\delta_{rot1},\alpha_1\hat\delta_{rot1}^2+\alpha_2\hat\delta_{trans}^2)\\ 8:&\qquad p_2=prob(\delta_{trans}-\hat\delta_{trans},\alpha_3\hat\delta_{trans}^2+\alpha_4\hat\delta_{rot1}^2+\alpha_4\hat\delta_{rot2}^2)\\ 9:&\qquad p_3=prob(\delta_{rot2}-\hat\delta_{rot2},\alpha_1\hat\delta_{rot2}^2+\alpha_2\hat\delta_{trans}^2)\\ \\ 10:&\qquad return\quad p_1\cdot p_2\cdot p_3 \end{aligned} 1:2:3:4:5:6:7:8:9:10:Algorithmmotion_model_odometry(xt,ut,xt−1):δrot1=atan2(Yˉ′−Yˉ,Xˉ′−Xˉ)−θˉδtrans=(Xˉ−Xˉ′)2+(Yˉ−Yˉ′)2δrot2=θˉ′−θˉ−δrot1δ^rot1=atan2(Y′−Y,X′−X)−θδ^trans=(X−X′)2+(Y−Y′)2δ^rot2=θ′−θ−δ^rot1p1=prob(δrot1−δ^rot1,α1δ^rot12+α2δ^trans2)p2=prob(δtrans−δ^trans,α3δ^trans2+α4δ^rot12+α4δ^rot22)p3=prob(δrot2−δ^rot2,α1δ^rot22+α2δ^trans2)returnp1⋅p2⋅p3
其中，1~3行从里程计测量值得到相对运动参数值：[δrot1δtransδrot2]T\begin{bmatrix}\delta_{rot1}&\delta_{trans}&\delta_{rot2}\end{bmatrix}^T[δrot1δtransδrot2]T

4~6行由机器人初始位姿xt−1x_{t-1}xt−1及预计位姿xtx_txt计算得到实际相对运动参数：[δ^rot1δ^transδ^rot2]T\begin{bmatrix}\hat\delta_{rot1}&\hat\delta_{trans}&\hat\delta_{rot2}\end{bmatrix}^T[δ^rot1δ^transδ^rot2]T

7~9行计算各自噪声引起的概率密度p1、p2、p3p_1、p_2、p_3p1、p2、p3，由于各噪声相互独立，最后返回累乘值p1⋅p2⋅p3p_1\cdot p_2\cdot p_3p1⋅p2⋅p3。

函数prob(x,b2)prob(x,b^2)prob(x,b2)用于实现x上均值为0，方差为b2b^2b2的概率分布，可用正态分布或三角形分布实现，同速度运动模型，不在赘述。

应注意，所有角度差值应保持在区间[−π,π][-\pi,\pi][−π,π]内，即函数prob(x,b2)prob(x,b^2)prob(x,b2)中参数xxx属于该区间。

算法效果

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

当机器人具有中等误差（参数α1\alpha_1α1 ~ α4\alpha_4α4），则如图所示：

当机器人具有较大的平移误差（参数α1、α2\alpha_1、\alpha_2α1、α2），但具有较小的角度误差（参数α3、α4\alpha_3、\alpha_4α3、α4），则如图所示：

当机器人具有较小的平移误差（参数α1、α2\alpha_1、\alpha_2α1、α2），但具有较大的角度误差（参数α3、α4\alpha_3、\alpha_4α3、α4），则如图所示：

当两次测量间时间较短时，里程计运动模型同速度运动模型的闭式算法效果差不多。也即，一个机器人如果置信度经常更新（0.1s更新一次），则运动模型间的不同差异不大。

采样算法

算法推导

采用粒子滤波用于采样，与闭式算法不同。该算法通过给定的初始姿态xt−1x_{t-1}xt−1和控制量ut=[xˉt−1xˉt]Tu_t=\begin{bmatrix}\bar x_{t-1}&\bar x_t\end{bmatrix}^Tut=[xˉt−1xˉt]T，估计出一个当前位姿xtx_txt

算法设计

系统输出： 估计后继姿态xt=[X′Y′θ′]Tx_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^Txt=[X′Y′θ′]T

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

算法实现逻辑如下：
Algorithmsample_motion_model_odometry(xt,ut,xt−1):1:δrot1=atan2(Yˉ′−Yˉ,Xˉ′−Xˉ)−θˉ2:δtrans=(Xˉ−Xˉ′)2+(Yˉ−Yˉ′)23:δrot2=θˉ′−θˉ−δrot14:δ^rot1=δrot1−sample(α1δ^rot12+α2δ^trans2)5:δ^trans=δtrans−sample(α3δ^trans2+α4δ^rot12+α4δ^rot22)6:δ^rot2=δrot2−sample(α1δ^rot22+α2δ^trans2)7:X′=X+δ^transcos⁡(θ+δ^rot1)8:Y′=Y+δ^transsin⁡(θ+δ^rot1)9:θ′=θ+δ^rot1+δ^rot210:returnxt=xt=[X′Y′θ′]T\begin{aligned} &Algorithm\quad sample\_motion\_model\_odometry(x_t,u_t,x_{t-1}): \\ 1:&\qquad \delta_{rot1}=atan2(\bar Y^{'}-\bar Y,\bar X^{'}-\bar X)-\bar\theta \\ 2:&\qquad \delta_{trans}=\sqrt{(\bar X-\bar X^{'})^2+(\bar Y-\bar Y^{'})^2}\\ 3:&\qquad \delta_{rot2}=\bar \theta^{'}-\bar \theta-\delta_{rot1}\\ \\ 4:&\qquad \hat\delta_{rot1}=\delta_{rot1}-sample(\alpha_1\hat\delta_{rot1}^2+\alpha_2\hat\delta_{trans}^2) \\ 5:&\qquad \hat\delta_{trans}=\delta_{trans}-sample(\alpha_3\hat\delta_{trans}^2+\alpha_4\hat\delta_{rot1}^2+\alpha_4\hat\delta_{rot2}^2)\\ 6:&\qquad \hat\delta_{rot2}=\delta_{rot2}-sample(\alpha_1\hat\delta_{rot2}^2+\alpha_2\hat\delta_{trans}^2)\\ \\ 7:&\qquad X^{'}=X+\hat\delta_{trans}\cos(\theta+\hat\delta_{rot1})\\ 8:&\qquad Y^{'}=Y+\hat\delta_{trans}\sin(\theta+\hat\delta_{rot1})\\ 9:&\qquad \theta^{'}=\theta+\hat\delta_{rot1}+\hat\delta_{rot2}\\ \\ 10:&\qquad return\quad x_t=x_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^T \end{aligned} 1:2:3:4:5:6:7:8:9:10:Algorithmsample_motion_model_odometry(xt,ut,xt−1):δrot1=atan2(Yˉ′−Yˉ,Xˉ′−Xˉ)−θˉδtrans=(Xˉ−Xˉ′)2+(Yˉ−Yˉ′)2δrot2=θˉ′−θˉ−δrot1δ^rot1=δrot1−sample(α1δ^rot12+α2δ^trans2)δ^trans=δtrans−sample(α3δ^trans2+α4δ^rot12+α4δ^rot22)δ^rot2=δrot2−sample(α1δ^rot22+α2δ^trans2)X′=X+δ^transcos(θ+δ^rot1)Y′=Y+δ^transsin(θ+δ^rot1)θ′=θ+δ^rot1+δ^rot2returnxt=xt=[X′Y′θ′]T
其中，1~3行从里程计测量值得到相对运动参数值：[δrot1δtransδrot2]T\begin{bmatrix}\delta_{rot1}&\delta_{trans}&\delta_{rot2}\end{bmatrix}^T[δrot1δtransδrot2]T

4~6行由机器人初始位姿xt−1x_{t-1}xt−1及采样样本计算得到实际相对运动参数值：[δ^rot1δ^transδ^rot2]T\begin{bmatrix}\hat\delta_{rot1}&\hat\delta_{trans}&\hat\delta_{rot2}\end{bmatrix}^T[δ^rot1δ^transδ^rot2]T

7~9行根据实际相对运动参数值，预测当前时刻机器人位姿xtx_txt

式中，函数sample(b2)sample(b^2)sample(b2)用于实现均值为0，方差为b2b^2b2的随机样本生成，同速度运动模型中采样算法，不在赘述。

算法效果

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

当机器人具有中等误差（参数α1\alpha_1α1 ~ α4\alpha_4α4），则如图所示：

当机器人具有较大的平移误差（参数α1、α2\alpha_1、\alpha_2α1、α2），但具有较小的角度误差（参数α3、α4\alpha_3、\alpha_4α3、α4），则如图所示：

当机器人具有较小的平移误差（参数α1、α2\alpha_1、\alpha_2α1、α2），但具有较大的角度误差（参数α3、α4\alpha_3、\alpha_4α3、α4），则如图所示：

当机器人不断进行移动时，不确定性将增加，样本将遍布越来越大的空间，如下图所示机器人沿着实现不断移动：

概率机器人：里程计运动模型相关推荐

《概率机器人》里程计运动模型gmapping中代码解析
里程计运动模型(odometery motion model)用距离测量代替控制.实际经验表明虽然里程计虽然仍存在误差,但通常比速度运动模型更加的精确.相比于速度运动模型运动信息utu_t由 (x¯t ...
小豆包的学习之旅：里程计运动模型
星球大战里面有2个非常出色的机器人一个是3PO,一个是R2.我的机器人呢就叫小豆包啦,代号FR2,不许侵犯它的署名哦. 常见的机器人运动模型包括:速度计运动模型,里程计运动模型和惯导运动模型.概率运动 ...
ROS机器人里程计校准
ROS机器人里程计校准麦克纳姆轮机器人里程计标定前提是你的机器人已经输入了正确的运动学模型并可以按照你给定的运动速度有正确的运动方向. 一.启动初始化节点首先打开第一个终端,打开你的机器人的初始 ...
里程计运动模型及标定
文章目录 1 里程计运动模型 1.1 两轮差分底盘的运动模型 1.2 三轮全向底盘的运动学模型 1.3 航迹推算(Dead Reckoning) 2 里程计标定 2.1 线性最小二乘的基本原理 2.2 ...
《概率机器人》速度运动模型gmapping中代码解析
一个刚性移动机器人的构型通常用6个变量来描述:他的三维直角坐标系,以及相对外部坐标系的三个欧拉角(RPY 横滚 ,俯仰,偏航),所以那么在平面环境中一般用三个变量既可以描述,称之为位姿. 所以一般而平 ...
ROS机器人里程计模型
3.5 移动机器人系统模型相信大家的机器人平台STM32端底层控制和机器人urdf建模都已经顺利完成了,在正式开始ros端编写机器人启动功能包之前,我们还不得不学习一些必要的理论知识.别担心数学不好 ...
轮式移动机器人里程计分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达来源丨混沌无形编辑丨3D视觉工坊点击进入->3D视觉工坊学习交流群摘要:本文主要分析轮式移 ...
移动机器人——里程计矫正
一.问题分析机器人可以基于码盘数据和底盘运动学模型进行航迹推演,得到机器人的轨迹,但实际轨迹与推演轨迹存在误差,分析原因如下: 底盘实际尺寸与理论模型存在偏差,如轮子半径,两轮间距等: 底盘在运动过 ...
autoware之轮式里程计计算
这部分代码主要是接收了底盘的can消息然后计算一个轮速里程计.具体的: 1.can_status_translator节点 // setup subscribersub1_ = nh_.subscr ...

概率机器人：里程计运动模型

里程计运动模型

模型建立

理想模型

噪声模型

闭式算法

算法推导

算法设计

算法效果

采样算法

算法推导

算法设计

算法效果

概率机器人：里程计运动模型相关推荐

最新文章

热门文章