参考资料
原理分析
机器人可以基于码盘数据和底盘运动学模型进行航迹推演,从而得到机器人的轨迹。
这里我们以差分驱动机器人为例进行分析。
它的运动学方程很简单,如下:
x
˙
r
=
v
cos
θ
r
y
˙
r
=
v
sin
θ
r
θ
˙
r
=
ω
r
(1)
\tag{1} \begin{aligned} \dot{x}_r &= v \cos_{\theta_r}\\ \dot{y}_r &= v \sin_{\theta_r}\\ \dot{\theta}_r &= \omega_r\\ \end{aligned}
x˙ry˙rθ˙r=vcosθr=vsinθr=ωr(1)
1. 线速度推导
假设对于左右两轮各有一个转速测量单元(码盘),通过转速测量单元可以实时获取小车当前左右两轮的转速
a
l
,
a
r
a_l, a_r
al,ar,单位为(圈/s)。假设轮子的半径为
r
r
r,周长为
S
S
S,进一步得左右轮行进的速度为:
v
l
=
a
l
S
v
r
=
a
r
S
(2)
\tag{2} v_l=a_l S\\ v_r=a_r S
vl=alSvr=arS(2)
我们取左右两轮轴中心那个点的速度作为车体速度,可计算为
v
=
v
l
+
v
r
2
(3)
\tag{3} v=\frac{v_l+v_r}{2}
v=2vl+vr(3)
2. 角速度推导(行进方向)
对于移动机器人来说,假定机器人的初始始位置已知,在一个行驶维度为一维的环境(例如:单行线公路、单行线导轨),机器人可以根据轮子的圈数实时估计自己在这个一维行驶环境的何处。
在二维平面情况下,通过左右轮的转速测量单元可以推知左右轮的转圈数,显然,当左右轮的转圈数不同时,移动机器人会向左或者向右转。例如,当左侧轮子转速低于右侧轮子时,左侧轮子转圈数少于右侧轮子,因此可推知此时机器人往左转。
假设移动机器人此时行驶了一定距离,如上图,左轮走过的距离为
s
l
s_l
sl,右轮走过的距离为
s
r
s_r
sr,车轮间距为
L
L
L。由圆弧的几何关系
s
=
θ
R
s=\theta R
s=θR可知
{
s
l
=
θ
r
R
s
r
=
θ
r
(
R
+
L
)
(4)
\tag{4} \left\{\begin{array}{c} s_{l}=\theta_r R \\ s_{r}=\theta_r(R+L) \end{array}\right.
{sl=θrRsr=θr(R+L)(4)
等式两边对时间求导,进一步得:
{
v
l
=
ω
r
R
v
r
=
ω
r
(
R
+
L
)
(5)
\tag{5} \left\{\begin{array}{c} v_{l}=\omega_r R \\ v_{r}=\omega_r(R+L) \end{array}\right.
{vl=ωrRvr=ωr(R+L)(5)
以移动机器人角速度为
ω
=
v
r
−
v
l
L
(6)
\tag{6} \omega=\frac{v_{r}-v_{l}}{L}
ω=Lvr−vl(6)
综上,通过公式(3)和公式(6),在知道了线速度与角速度后,便可以根据公式(1)得到它在二维平面上的位置与姿态(偏航角)。
这就是轮式里程计的一般性原理。