车式移动机器人运动学模型
问题
车式移动机器人更具有普遍意义。
参数
- v v v：机器人自身线速度
- ω \omega ω：前轮转向角速度
- u \textbf{u} u：输入，包括 v v v， ω \omega ω
- q ˙ \dot{\textbf{q}} q˙：机器人在环境中的位姿描述
- θ \theta θ：机器人纵向轴与x轴之间的夹脚
- ϕ \phi ϕ：前轮转角
- L L L：车身长度
推导1
当知道机器人自身前进速度和前轮转角角速度时，求机器人在环境中的位姿。
q ˙ \dot{\textbf{q}} q˙= f ( u ) f(\textbf{u}) f(u)
定义机器人在环境中位姿：
q
=
[
x
y
θ
ϕ
]
\textbf{q}=\left [ \begin{aligned} x \\ y \\ \theta \\ \phi \end{aligned} \right]
q=⎣⎢⎢⎢⎢⎡xyθϕ⎦⎥⎥⎥⎥⎤
机器人输入，包括自身线速度和前轮转角角速度：
u
=
[
v
ω
]
\textbf{u}=\left [ \begin{aligned} v \\ \omega \\ \end{aligned} \right]
u=[vω]
而
x
˙
=
cos
(
θ
)
v
y
˙
=
sin
(
θ
)
v
ϕ
˙
=
ω
\begin{aligned} \dot{x} &= \cos(\theta) v \\ \dot{y} &= \sin(\theta) v\\ \dot{\phi} &= \omega \end{aligned}
x˙y˙ϕ˙=cos(θ)v=sin(θ)v=ω
对与计算角速度可以参照下图(其他地方扣过来的，注意符号使用不太一样)
就有了
θ
˙
=
tan
ϕ
L
v
\dot{\theta} = \frac{\tan\phi}{L} v
θ˙=Ltanϕv
那么，
q
˙
=
[
cos
(
θ
)
0
sin
(
θ
)
0
tan
ϕ
L
0
0
1
]
u
\dot{\textbf{q}}= \begin{aligned} \begin{bmatrix} \cos(\theta) & 0 \\ \sin(\theta) & 0 \\ \frac{\tan\phi}{L} & 0 \\ 0 & 1 \end{bmatrix} \end{aligned} \textbf{u}
q˙=⎣⎢⎢⎡cos(θ)sin(θ)Ltanϕ00001⎦⎥⎥⎤u
更详细的，即：
[
x
˙
y
˙
θ
˙
ϕ
˙
]
=
[
cos
(
θ
)
0
sin
(
θ
)
0
tan
ϕ
L
0
0
1
]
[
v
ω
]
\left [ \begin{aligned} \dot{x} \\ \dot{y} \\ \dot{\theta} \\ \dot{\phi} \end{aligned} \right]= \begin{aligned} \begin{bmatrix} \cos(\theta) & 0 \\ \sin(\theta) & 0 \\ \frac{\tan\phi}{L} & 0 \\ 0 & 1 \end{bmatrix} \end{aligned} \left [ \begin{aligned} v \\ \omega \\ \end{aligned} \right]
⎣⎢⎢⎢⎢⎡x˙y˙θ˙ϕ˙⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎡cos(θ)sin(θ)Ltanϕ00001⎦⎥⎥⎤[vω]
推导2
另一种情况是，知道机器人的前轮转角角度（即方向盘角度）和加速度（即油门），想要求机器人的状态。
- v v v：机器人自身线速度（前进速度）
- θ \theta θ：机器人纵向轴与x轴之间的夹脚
- u \textbf{u} u：输入，包括 v v v， θ \theta θ
- q ˙ \dot{\textbf{q}} q˙：机器人在环境中的位姿描述
- ϕ \phi ϕ：前轮转角
-
L
L
L：车身长度
机器人的状态
q = [ x y θ v ] T \textbf{q}=\left [ \begin{aligned} x \\ y \\ \theta \\ v \end{aligned} \right]^T q=⎣⎢⎢⎢⎢⎡xyθv⎦⎥⎥⎥⎥⎤T
机器人输入，包括(油门)加速度和(方向盘)前轮转角：
u = [ a ϕ ] \textbf{u}=\left [ \begin{aligned} a \\ \phi \\ \end{aligned} \right] u=[aϕ]
那么，
x ˙ = v ∗ cos ( θ ) y ˙ = v ∗ sin ( θ ) θ ˙ = v ∗ tan ( ϕ ) / L v ˙ = a \begin{aligned} \dot{x} &= v * \cos (\theta)\\ \dot{y} &= v * \sin (\theta)\\ \dot{\theta} &= v * \tan(\phi) / L \\ \dot{v} &= a \end{aligned} x˙y˙θ˙v˙=v∗cos(θ)=v∗sin(θ)=v∗tan(ϕ)/L=a
更进一步单步更新可以表示为：
x t + 1 = x t + v ∗ cos ( θ ) ∗ d t y t + 1 = y t + v ∗ sin ( θ ) ∗ d t θ t + 1 = θ t + v ∗ tan ( ϕ ) / L ∗ d t v t + 1 = v t + a ∗ d t \begin{aligned} x_{t+1} &= x_{t} + v * \cos (\theta) * dt\\ y_{t+1} &= y_{t} + v * \sin (\theta)* dt\\ \theta_{t+1} &= \theta_{t} + v * \tan(\phi) / L*dt\\ v_{t+1} &= v_{t} + a * dt \end{aligned} xt+1yt+1θt+1vt+1=xt+v∗cos(θ)∗dt=yt+v∗sin(θ)∗dt=θt+v∗tan(ϕ)/L∗dt=vt+a∗dt
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
