1,理论模型
1.1 原理
如下图所示,蓝色圆形区域为小车的地盘,左右轮之间的间距为wheel_dist,注意算法中的所有的单位,全部是国际标准单位。移动机器人编码器,安装在左右轮上,机器人地底盘的坐标系为base_link坐标系,里程计坐标系为odom。
![](https://i-blog.csdnimg.cn/blog_migrate/5e2809462c3df0ede4f2d5c0bfa6160b.png)
当移动机器人移动时,假设运动到如下图所示位置。图中的,V为移动小车中心点的运动速度方向,dxy_avg为移动小车中心点的运动距离。
![](https://i-blog.csdnimg.cn/blog_migrate/9607378f9dbdafab13f98b3a69a30b29.png)
根据编码器的差值,可知前一时刻与当前时刻编码器增量的计数脉冲,已知小车车轮行走一个周长的长度的编码器计数脉冲个数,可以推算出,一个计数脉冲的移动距离。代码实现如下:
# (4096 * 7.5) dright is right boot position
dright = (right_enc - enc_right) * pi * D / (4096 * 7.5)
# dleft is left boot positon
dleft = (left_enc - enc_left) * pi * D / (4096 * 7.5)
其中,dright 为右轮的移动距离,dleft 为左轮的移动距离right_enc - enc_right为右轮编码器脉冲个数的增量,left_enc - enc_left为左轮编码器脉冲个数的增量,参数pi * D / (4096 * 7.5),需要依据自己的小车测量出,编码器一个计数脉冲对应的行走距离(单位m)。
1.2 向中心点换算
因为在使用里程计向上层的开发中,往往对于小车模型,通常会考率成中心点的运动模型,即大致认为时一个质点,这是不准确的。算法如下:
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / wheel_track
vxy = dxy_ave / dt
vth = dth / dt
其中, dxy_ave为左右轮的移动距离均值,认为是中心点的移动距离;dth 为通过为中心点的转动角度,通过弧长公式 θ = l / R \theta = l/R θ=l/R,中心点速度V=vxy 为位移对时间的微分 v = d x / d t v= dx/dt v=dx/dt,vth 为角速度,为转动角度对时间的倒数 v t h = θ / d t vth= \theta/dt