航迹推演(Odometry)_由左右轮速度v_l,v_r推导车辆的线速度v，角速度w，运动半径r

忘是亡心i 2022-09-10 05:15 12阅读 0赞

做机器人底层程序的时候，经常用到航迹推演（Odometry），无论是定位导航还是普通的方向控制。航迹推演中除了对机器人位姿进行估计，另一个很重要的关系是移动机器人前进速度、转向角速度与左轮速度、右轮速度之间的转换。

在[机器人局部路径规划算法DWA][DWA]解析一文中，是在假设已知机器人前进线速度和角速度的情况下，对机器人航迹推演的位姿进行推导了，然而缺少如何通过左右轮速度得到、，因此本文将补上这个空缺。

下图是移动机器人在两个相邻时刻的位姿，其中是两相邻时刻移动机器人绕圆弧运动的角度，是两相邻时刻移动机器航向角（朝向角head）的变化量。是左右轮之间的间距，是右轮比左轮多走的距离。是移动机器人圆弧运动的半径。

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_20_color_FFFFFF_t_70_g_se_x_16][]

移动机器人前进速度等于左右轮速度的平均，这个好理解。

![20210905105017108.png][]

现在来推导机器人航向角如何计算，以及如何计算角速度。如图所示，把两个时刻的机器人位置叠加在一起，可以清楚的看到移动机器人航向角变化量是。从图中的几何关系可以得到：

![20210905105028372.png][]

也就是说移动机器人航向角变化了多少角度，它就绕其运动轨迹的圆心旋转了多少角度。这句话很好验证，我们让机器人做圆周运动，从起点出发绕圆心一圈回到起点处，在这过程中机器人累计的航向角为360度，同时它也确实绕轨迹圆心运动了360度，说明机器人航向角变化多少度，就绕圆心旋转了多少度。而这三个角度中，很容易计算出来，由于相邻时刻时间很短，角度变化量很小，有下面的近似公式：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_12_color_FFFFFF_t_70_g_se_x_16][]

所以可以得到机器人绕圆心运动的角速度，它也是机器人航向角变化的速度：

![20210905105047506.png][]

线速度、角速度都有了，因此可以推出移动机器人圆弧运动的半径：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_11_color_FFFFFF_t_70_g_se_x_16][]

从公式（3）可以发现当左轮速度等于右轮速度时，半径无穷大，即直线运动。最后将三个公式综合起来，可以得到左右轮速度和线速度角速度之间的关系如下，：

![watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_13_color_FFFFFF_t_70_g_se_x_16][]

**参考：**https://blog.csdn.net/heyijia0327/article/details/47021861

[DWA]: https://blog.csdn.net/heyijia0327/article/details/44983551
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_20_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/a81fa5f820394981a5ce5b11e37f16c7.png
[20210905105017108.png]: /images/20220829/daffa3b9bafe4e0ab93e2a9d17a71449.png
[20210905105028372.png]: /images/20220829/bbb576f8b8c54d79aa4d51ae0082b4d4.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_12_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/d80c238362b64b688782a6b1a8fcbf86.png
[20210905105047506.png]: /images/20220829/aa5b58f7f61045928ff3c77e3d6695ea.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_11_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/89e0d44f4c024b16a887d2a60d7acfa3.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBA5oOK6bi_5LiA5Y2a_size_13_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/2216b6016189459f9539bfd85f0f92fe.png