小豆包的学习之旅：里程计运动模型
星球大戰(zhàn)里面有2個(gè)非常出色的機(jī)器人一個(gè)是3PO，一個(gè)是R2。我的機(jī)器人呢就叫小豆包啦，代號(hào)FR2，不許侵犯它的署名哦。
?
常見的機(jī)器人運(yùn)動(dòng)模型包括：速度計(jì)運(yùn)動(dòng)模型，里程計(jì)運(yùn)動(dòng)模型和慣導(dǎo)運(yùn)動(dòng)模型。概率運(yùn)動(dòng)模型是對(duì)機(jī)器人運(yùn)動(dòng)的一種概率描述。
里程計(jì)是比較常見的一種類型。嚴(yán)格來說，里程計(jì)是一種傳感器測(cè)量信息而非控制信息。如果將里程計(jì)視為測(cè)量信息，貝葉斯濾波需要包含速度作為狀態(tài)變量，將增加狀態(tài)空間的維度。因此，為了減少狀態(tài)空間，通常將里程計(jì)數(shù)據(jù)視為控制信號(hào)。
首先是定義：
小豆包在$t-1$時(shí)刻的位姿$x_{t-1}=(x,y,\theta)^T$，$t$時(shí)刻的位姿$x_{t}=(x',y',\theta')^T$運(yùn)動(dòng)控制信息$u_{t}$，概率運(yùn)動(dòng)模型為$P\{x_{t}|u_{t},x_{t-1}\}$。
其中，運(yùn)動(dòng)控制信息$u_{t}=\{\overline?{x}_{t-1},\overline?{x}_{t}\}$。$\overline?{x}_{t-1}=(?\overline x,?\overline y,?\overline \theta)^T$,$\overline?{x}_{t}=(?\overline x',?\overline y',?\overline?\theta')^T$。
將小豆包在$(t-1,t]$時(shí)間間隔內(nèi)的運(yùn)動(dòng)分解為三個(gè)階段：旋轉(zhuǎn)$\delta_{rot1}$，平移$\delta_{trans}$，旋轉(zhuǎn)$\delta_{rot2}$.
轉(zhuǎn)向和平移都存在不確定性，則存在三個(gè)階段的誤差，認(rèn)為三個(gè)階段的誤差相互獨(dú)立，采用高斯分布表達(dá)這種不確定性。
運(yùn)用高斯分布表達(dá)噪聲模型，則運(yùn)動(dòng)模型算法流程如下：
| ?1: Algorithm motion_model_odometry $(x_{t}, u_{t}, x_{t-1})$: ?2: $\delta_{rot1} = atan2(\overline y'- \overline y, \overline x’ - \overline x) - \overline \theta$ ?3: $\delta_{trans} = (\overline x - \overline x')^2 + (\overline y - \overline y')^2$ ?4: $\delta_{rot2} = \overline \theta' - \overline \theta- \delta_{rot1}$?//里程計(jì)讀數(shù)，運(yùn)動(dòng)控制信息$u_{t}$ ?5: $\delta_{rot1} = atan2(y' - y, x' - x) - \theta$ ?6: $\delta_{trans} = (x - x')^2 + (y - y')^2$ ?7: $\delta_{rot2} = \theta' - \theta - \delta_{rot1}$ ?8: $p1 = prob(\delta_{rot1} - \widehat {\delta}_{rot1}, α1\widehat {\delta}_{rot1}^2 + α2\widehat {\delta}_{rot1}^2 )$ ?9: $p2 = prob(\delta_{trans} - \widehat {\delta}_{trans}, α3\widehat {\delta}_{trans}^2 + α4\widehat {\delta}_{rot1}^2 + α4\widehat {\delta}_{rot2}^2 )$ 10: $p3 = prob(\delta_{rot2} - \widehat {\delta}_{rot2}, α1\widehat {\delta}_{rot2}^2 + α2\widehat {\delta}_{trans}^2 )$ 11: return $p1 · p2 · p3 $ |
采用采樣方式表達(dá)運(yùn)動(dòng)模型，主要運(yùn)用于粒子濾波算法中：
| ?1: ?Algorithm Sample_motion_model_odometry $(u_{t}, x_{t-1})$: ?2: ?$\delta_{rot1} = atan2(\overline y'- \overline y, \overline x’ - \overline x) - \overline \theta$ ?3: ?$\delta_{trans} = (\overline x - \overline x')^2 + (\overline y - \overline y')^2$? ?4: ?$\delta_{rot2} = \overline \theta' - \overline \theta- \delta_{rot1}$?//里程計(jì)讀數(shù)，運(yùn)動(dòng)控制信息$u_{t}$ ?5: ?$\widehat {\delta}_{rot1} ?=\delta_{rot1} - sample(α1\widehat {\delta}_{rot1}^2 + α2\widehat {\delta}_{rot1}^2)$ ?6: ?$\widehat {\delta}_{trans}=\delta_{trans}- sample(α3\widehat {\delta}_{trans}^2 + α4\widehat {\delta}_{rot1}^2 + α4\widehat {\delta}_{rot2}^2)$ ?7: ?$\widehat {\delta}_{rot2} ?=\delta_{rot2} - sample(α1\widehat {\delta}_{rot2}^2 + α2\widehat {\delta}_{trans}^2)$ //為運(yùn)動(dòng)控制的變化量增加誤差項(xiàng) ?8: ?$x'= x+\widehat {\delta}_{trans} cos(\theta+\widehat {\delta}_{rot1})$ ?9: ?$y'= y+\widehat {\delta}_{trans} cos(\theta+\widehat {\delta}_{rot1})$ 10: $\theta'= \theta+\widehat {\delta}_{rot1}+\widehat {\delta}_{rot2}$ //加在$t-1$時(shí)刻的位姿上，計(jì)算出$t$時(shí)刻的位姿 11: return $x_{t}=(x',y',\theta')^T$ |
? 里程計(jì)數(shù)據(jù)通過輪編碼器獲取，隨著時(shí)間的推移，誤差會(huì)不斷累積，而且角度誤差累積的更厲害。IMU對(duì)角度的誤差累積比距離累積要小。另外，機(jī)器人也可能存在機(jī)器人“綁架”之類的機(jī)械問題。這些都是運(yùn)動(dòng)模型的局限性，需要對(duì)運(yùn)動(dòng)觀測(cè)數(shù)據(jù)進(jìn)行優(yōu)化校正，提高機(jī)器人的位姿精度。
機(jī)器人“綁架”指的是機(jī)器人在運(yùn)動(dòng)過程中發(fā)生了滑動(dòng),機(jī)器人傳感器觀測(cè)并不知道此情況的發(fā)生。
其他的運(yùn)動(dòng)模型：Probabilistic Motion Models
?
實(shí)例：
轉(zhuǎn)載于:https://www.cnblogs.com/yhlx125/p/5677135.html
《新程序員》：云原生和全面數(shù)字化實(shí)踐50位技術(shù)專家共同創(chuàng)作，文字、視頻、音頻交互閱讀總結(jié)
以上是生活随笔為你收集整理的小豆包的学习之旅：里程计运动模型的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： cloud-init 工作原理 - 每天
- 下一篇： 2016-2017 ACM-ICPC P