概率机器人笔记(5)：机器人运动模型

机器人笔记(5)：机器人运动模型"/>

概率机器人笔记(5)：机器人运动模型

前言

本节通过机器人运动模型(motion model)实现状态转移概率 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt​∣ut​,xt−1​)，即执行运动控制 u t u_t ut​后机器人状态的后验分布，其中， x t , x t − 1 x_t,x_{t-1} xt​,xt−1​代表机器人的位姿： [ x y θ ] \begin{bmatrix} x \\ y \\ \theta \\ \end{bmatrix} ⎣⎡​xyθ​⎦⎤​《概率机器人》假设 θ = 0 \theta =0 θ=0为 x x x轴正方向， θ = π / 2 \theta =\pi/2 θ=π/2为 y y y轴正方向。

一、速度运动模型

速度运动模型的控制输入： u t = [ v t w t ] u_t=\begin{bmatrix} v_t \\ w_t \\ \end{bmatrix} ut​=[vt​wt​​]

1.速度运动前向模型

我所理解的前向运动是指跟定上一时刻的位姿 x t − 1 x_{t-1} xt−1​和控制输入 u t u_t ut​，计算本时刻位姿的可能状态 x t x_t xt​。
理想运动

这个模型我们可以认为机器人时时刻刻做旋转运动，直线只是当旋转半径无穷大的一种特例，基于上图，我们可以写出如下公式： { v = w ⋅ r x c = x − r ⋅ c o s ( θ − 90 ) y c = y − r ⋅ s i n ( θ − 90 ) \begin{cases}v=w·r&\\ x_c=x-r·cos(\theta-90)&\\y_c=y-r·sin(\theta-90) &\end{cases} ⎩⎪⎨⎪⎧​v=w⋅rxc​=x−r⋅cos(θ−90)yc​=y−r⋅sin(θ−90)​​整理得： { x c = x + r ⋅ s i n θ y c = y − r ⋅ c o s θ \begin{cases}x_c=x+r·sin\theta&\\y_c=y-r·cos\theta &\end{cases} {xc​=x+r⋅sinθyc​=y−r⋅cosθ​​最终得到：
[ x ′ y ′ θ ′ ] = [ x c + v w s i n ( θ + w Δ t ) y c + v w c o s ( θ + w Δ t ) θ + w Δ t ] = [ x y θ ] + [ − v w s i n θ + v w s i n ( θ + w Δ t ) v w c o s θ − v w c o s ( θ + w Δ t ) w Δ t ] \begin{bmatrix} x' \\ y' \\\theta' \end{bmatrix}=\begin{bmatrix} x_c + \frac{v}{w}sin(\theta +w\Delta t) \\y_c + \frac{v}{w}cos(\theta +w\Delta t) \\\theta+w\Delta t \end{bmatrix}=\begin{bmatrix}x \\ y \\ \theta\end{bmatrix}+\begin{bmatrix}-\frac{v}{w}sin\theta+\frac{v}{w}sin(\theta+w\Delta t) \\ \frac{v}{w}cos\theta-\frac{v}{w}cos(\theta+w\Delta t) \\ w\Delta t\end{bmatrix} ⎣⎡​x′y′θ′​⎦⎤​=⎣⎡​xc​+wv​sin(θ+wΔt)yc​+wv​cos(θ+wΔt)θ+wΔt​⎦⎤​=⎣⎡​xyθ​⎦⎤​+⎣⎡​−wv​sinθ+wv​sin(θ+wΔt)wv​cosθ−wv​cos(θ+wΔt)wΔt​⎦⎤​真实运动
由于真实的速度存在一些与给定速度成比例的误差： [ v ^ w ^ ] = [ v w ] + [ ε α 1 v 2 + α 2 w 2 ε α 3 v 2 + α 4 w 2 ] \begin{bmatrix}\hat v\\\hat w\end{bmatrix}=\begin{bmatrix}v\\w\end{bmatrix}+\begin{bmatrix}\varepsilon_{\alpha_1v^2+\alpha_2w^2}\\\varepsilon_{\alpha_3v^2+\alpha_4w^2}\end{bmatrix} [v^w^​]=[vw​]+[εα1​v2+α2​w2​εα3​v2+α4​w2​​]并假设当到达最终位姿时，又带来了 γ ^ = ε α 5 v 2 + α 6 w 2 \hat{\gamma}=\varepsilon_{\alpha_5}v^2+\alpha_6w^2 γ^​=εα5​​v2+α6​w2误差
真实运动模型(前向模型)为： [ x ′ y ′ θ ′ ] = [ x y θ ] + [ − v ^ w ^ s i n θ + v ^ w ^ s i n ( θ + w ^ Δ t ) v ^ w ^ c o s θ − v ^ w ^ c o s ( θ + w ^ Δ t ) w ^ Δ t + γ ^ Δ t ] \begin{bmatrix} x' \\ y' \\\theta' \end{bmatrix}=\begin{bmatrix}x \\ y \\ \theta\end{bmatrix}+\begin{bmatrix}-\frac{\hat{v}}{\hat{w}}sin\theta+\frac{\hat{v}}{\hat{w}}sin(\theta+\hat{w}\Delta t) \\ \frac{\hat{v}}{\hat{w}}cos\theta-\frac{\hat{v}}{\hat{w}}cos(\theta+\hat{w}\Delta t) \\ \hat{w}\Delta t+\hat{\gamma}\Delta t\end{bmatrix} ⎣⎡​x′y′θ′​⎦⎤​=⎣⎡​xyθ​⎦⎤​+⎣⎡​−w^v^​sinθ+w^v^​sin(θ+w^Δt)w^v^​cosθ−w^v^​cos(θ+w^Δt)w^Δt+γ^​Δt​⎦⎤​
采样算法
在粒子滤波中，我们不是要计算状态转移后验的具体数值，而是能够从状态转移中采样，使得上一时刻的粒子转变为临时粒子，根据上面推导的公式，有以下算法：

2.速度运动逆向模型

我所理解的速度运动逆向模型是从上一时刻的位姿 x t − 1 x_{t-1} xt−1​和本时刻的位姿 x t x_t xt​以及旋转误差 γ ^ \hat{\gamma} γ^​，来确定运动参数 u ^ t \hat{u}_t u^t​。具体推导较为复杂，也没有太看明白╮(╯﹏╰）╭，先这么用，后续补上。

二、里程计运动模型

里程计运动模型的输入： u t = [ x ‾ t − 1 x ‾ t ] u_t=\begin{bmatrix}\overline{x}_{t-1}\\\overline{x}_t\end{bmatrix} ut​=[xt−1​xt​​]是基于机器人内部坐标的相对位置变化。
里程计模型
如上图将 u t u_t ut​分解成三个部分：旋转+平移+旋转，根据里程计读数 x ‾ t − 1 , x ‾ t \overline{x}_{t-1},\overline{x}_t xt−1​,xt​，可以求出这三个部分： { δ r o t 1 = a t a n 2 ( y ‾ ′ − y ‾ , x ‾ ′ − x ‾ ) − θ ‾ δ t r a n s = ( x ‾ − x ‾ ′ ) 2 + ( y ‾ − y ‾ ′ ) 2 δ r o t 2 = θ ‾ ′ − θ ‾ − δ r o t 1 \begin{cases}\delta_{rot1}=atan2(\overline{y}'-\overline{y},\overline{x}'-\overline{x})-\overline{\theta}\\\delta_{trans}=\sqrt{(\overline{x}-\overline{x}')^2+(\overline{y}-\overline{y}')^2}\\\delta_{rot2}=\overline{\theta}'-\overline{\theta}-\delta_{rot1}\end{cases} ⎩⎪⎨⎪⎧​δrot1​=atan2(y​′−y​,x′−x)−θδtrans​=(x−x′)2+(y​−y​′)2 ​δrot2​=θ′−θ−δrot1​​上面里程计测量模型将作为真实位姿 x t − 1 , x t x_{t-1},x_t xt−1​,xt​之间变换的估计器使用。
真实模型
真实模型由初始位姿 x t − 1 x_{t-1} xt−1​和假设的最终状态 x t x_t xt​作为输入： { δ ^ r o t 1 = a t a n 2 ( y ′ − y , x ′ − x ) − θ δ ^ t r a n s = ( x − x ′ ) 2 + ( y − y ′ ) 2 δ ^ r o t 2 = θ ′ − θ − δ ^ r o t 1 \begin{cases}\hat{\delta}_{rot1}=atan2(y'-y,x'-x)-\theta\\\hat{\delta}_{trans}=\sqrt{(x-x')^2+(y-y')^2}\\\hat{\delta}_{rot2}=\theta'-\theta-\hat{\delta}_{rot1}\end{cases} ⎩⎪⎨⎪⎧​δ^rot1​=atan2(y′−y,x′−x)−θδ^trans​=(x−x′)2+(y−y′)2 ​δ^rot2​=θ′−θ−δ^rot1​​误差模型
为了建立误差的运动模型，定义里程计模型和真实值模型之间是带有独立噪声的关系： { δ ^ r o t 1 = δ r o t 1 − ε α 1 δ r o t 1 2 + α 2 δ t r a n s 2 δ ^ t r a n s = δ t r a n s − ε α 3 δ t r a n s 2 + α 4 δ r o t 1 2 + α 4 δ r o t 2 2 δ ^ r o t 2 = δ r o t 2 − ε α 1 δ r o t 2 2 + α 2 δ t r a n s 2 \begin{cases}\hat{\delta}_{rot1}=\delta_{rot1}-\varepsilon_{\alpha1\delta_{rot1}^2+\alpha2\delta_{trans}^2}\\\hat{\delta}_{trans}=\delta_{trans}-\varepsilon_{\alpha3\delta_{trans}^2+\alpha4\delta_{rot1}^2+\alpha4\delta_{rot2}^2}\\\hat{\delta}_{rot2}=\delta_{rot2}-\varepsilon_{\alpha1\delta_{rot2}^2+\alpha2\delta_{trans}^2}\end{cases} ⎩⎪⎨⎪⎧​δ^rot1​=δrot1​−εα1δrot12​+α2δtrans2​​δ^trans​=δtrans​−εα3δtrans2​+α4δrot12​+α4δrot22​​δ^rot2​=δrot2​−εα1δrot22​+α2δtrans2​​​

1.采样算法

采样算法不需要计算后验概率 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt​∣ut​,xt−1​)，只需要能根据后验概率采样即可。则根据真实模型、里程计模型和误差模型，实际位置 x t x_t xt​可由下面公式计算： [ x ′ y ′ θ ′ ] = [ x y θ ] + [ δ ^ t r a n s c o s ( θ + δ ^ r o t 1 ) δ ^ t r a n s s i n ( θ + δ ^ r o t 1 ) δ ^ r o t 1 + δ ^ r o t 2 ] \begin{bmatrix}x'\\y'\\\theta'\end{bmatrix}=\begin{bmatrix}x\\y\\\theta\end{bmatrix}+\begin{bmatrix}\hat{\delta}_{trans}cos(\theta+\hat{\delta}_{rot1})\\\hat{\delta}_{trans}sin(\theta+\hat{\delta}_{rot1})\\\hat{\delta}_{rot1}+\hat{\delta}_{rot2}\end{bmatrix} ⎣⎡​x′y′θ′​⎦⎤​=⎣⎡​xyθ​⎦⎤​+⎣⎡​δ^trans​cos(θ+δ^rot1​)δ^trans​sin(θ+δ^rot1​)δ^rot1​+δ^rot2​​⎦⎤​
得到下面采样算法：

2.闭式算法

假设位置 x t x_t xt​为实际位置的后验由闭式算法给出。