使用ROS开源代码和激光雷达进行小车的定位导航——里程计信息的计算

写在前面：

    之前挖了个坑，大致讲了一下导航定位的思路但是设计到实际的东西还是有很多细节问题需要注意。最近把一个项目做完了，正好有时间可以总结一些技术要点和大家分享一下。

     我采用的是gmapping算法来对环境进行地图的构建，这就需要里程计的信息。首先是gmapping根据我的实际效果在场地不是很大的情况下还是能够较好的完成建图的过程的，下面可以给出一张效果图。

由于gmapping采用粒子滤波的方法，和amcl一样都是对里程计信息要求比较高的，在我实践过程中里程计信息几乎决定了建图的好坏，下面我会详细的介绍一下里程计信息的获取与计算。

一、通信方式

      为什么我要开始就提通信呢？因为我的亲生经历就在通信上花费了大把大把的时间，所以一开始选取一种好的通信方式既能提高里程计发布的频率提高精度又能避免走很多弯路。下面这部分会涉及到一些底层单片机的东西。

  首先我采用的是stm32F407作为底层的驱动控制，然后使用一个w5500模块将spi转换成以太网和电脑上位机通信。（至于w5500是什么可以上某宝搜一下）通信需要考虑到一下几个因素，第一就是速度。在我看来普通的串口通信很难满足gmapping对里程计信息的需求，实际情况测试下延时比较长效果很差，而且容易数据错误。以下是我推荐的通信方式

       1.USB通信，采用USB的HID模式进行通信，不管是速率还是稳定性都能较好的满足，我也曾采用过这种方式。

          缺点：stm32的usb通信较难，很难在短时间内掌握usb通信，网上资料少。

      2.以太网通信，以太网具有10M或100M的高速传输数据的能力，完全满足要求。

         缺点：stm32的以太网代码量较大，可能会导致代码运行速度变慢或者接触不良时卡在中断导致车子疯掉。

     3.w5500模块spi转以太网通信，优点就是只需要关注spi通信，代码少不用理会网络通信的一些问题。

         缺点：每次和电脑连接都要复位一次，不是很方便(电路设计上可以解决这个问题，需要自己设计）

最后，我采用了第三种方式。至于每种方式的代码和资料，不管是下位机还是上位机的程序，需要的可以留言。

     还有一点值得注意，就是我们里程计的信息需要的是浮点型的才能更加精确，但是我们怎么传输浮点型的数据呢？

那就是采用联合结构体的方式。下面给出一个博客，我就不细讲了。

https://blog.youkuaiyun.com/huhuang/article/details/51585009

二、里程计计算

 里程计的计算主要就是下面这个问题：
 给你四个或者两个电机的编码器的数值，计算出小车目前的位置和速度。

（下面部分内容来自深蓝学院的激光slam课程 不是打广告，尊重版权）

首先介绍四个麦克纳姆轮的小车的运动模型，这和我做的是一样的。

可以参考这个博客

 

下面给出我的计算过程，手写的有点丑凑合着看吧。

实际上，ROS大多采用的两轮的差速控制并不是我们这种麦轮结构。

对于两轮差分的运动学计算，可以参考这个博客

https://blog.youkuaiyun.com/u012926144/article/details/80787175

至于其他的比如全向轮的底盘的解算大多大同小异。

现在我们确实是获得了小车速度信息，但是位置信息还没有得到。有人说积分一下不就好了，但是实际上没有那么简单，因为目前的小车的速度是在小车坐标系下的，并不是世界坐标系下面的。

所以我们需要将速度转换到世界坐标系下然后再进行积分，求得小车目前的位置。

将我们利用运动学模型计算出来的值乘上一个旋转矩阵，然后计算出世界坐标系下的位姿，连成一条轨迹。

三、里程计信息的发布

好了，终于到了最后一步。

现在我们成功的计算出了里程计的信息，那怎么发布到ROS上面给slam算法提供数据呢？

其实只需要关注这两个信息的发布便可

ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 600);

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 600);

一个是发布里程计信息，然后另外一个发布机器人和地图的位置的TF转换关系。

好了，今天就到这了有什么问题或者要什么资料都可以留言哦!!!!

代码链接附上，17年做的了，可能有bug，勿喷。

https://github.com/linyicheng1/ICRA-electronics-design-competition