机器人笔记(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=[vtwt]
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+wvsin(θ+wΔt)yc+wvcos(θ+wΔt)θ+wΔt⎦⎤=⎣⎡xyθ⎦⎤+⎣⎡−wvsinθ+wvsin(θ+wΔt)wvcosθ−wvcos(θ+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]+[εα1v2+α2w2εα3v2+α4w2]并假设当到达最终位姿时,又带来了 γ ^ = ε α 5 v 2 + α 6 w 2 \hat{\gamma}=\varepsilon_{\alpha_5}v^2+\alpha_6w^2 γ^=εα5v2+α6w2误差
真实运动模型(前向模型)为: [ 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−1xt]是基于机器人内部坐标的相对位置变化。
里程计模型
如上图将 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θ⎦⎤+⎣⎡δ^transcos(θ+δ^rot1)δ^transsin(θ+δ^rot1)δ^rot1+δ^rot2⎦⎤
得到下面采样算法:
2.闭式算法
假设位置 x t x_t xt为实际位置的后验由闭式算法给出。
更多推荐
概率机器人笔记(5):机器人运动模型
发布评论