第四天：SLAM智能小车DIY乐趣-小车控制ROS驱动包开发

Posted 2021-01-07 hiram-zhang

小车控制ROS驱动包开发

####写在前面####

对于做纯SLAM算法和应用功能开发的朋友来说，其实不需要搞明白小车的底层硬件原理和软件实现等繁琐的细节，只需要通过上层API调用来实现小车的控制等操作就可以了。因此，本文开始讲解用于提供上层API调用的ROS驱动包，本文主要内容：

1) 通信协议简介（stm32与ROS驱动包通信）

2) ROS接口（topic订阅与发布）

3) ROS驱动包程序解析

4) 基于航迹推演的里程计解算

####正文####

1.通信协议简介（stm32与ROS驱动包通信）

    通信协议都基于串口通信，通信数据帧由串口通信的单个字节组合而成，stm32的串口2由“小车控制-ROS驱动包”驱动，stm32的串口1由“小车debug-ROS驱动包”驱动，两个串口都使用115200的波特率。下面分别介绍两个驱动包的通信协议：

stm32与小车控制ROS驱动包通信：

不难发现，其实“小车控制-ROS驱动包”发送和接收数据遵循同样的协议。

stm32与小车debug-ROS驱动包通信：

  

stm32发送过来的调试信息时纯字符串流，可以通过printf()直接来查看；

小车debug-ROS驱动发送给stm32的kp、ki、kd是一个扩大10000倍后的整数，除以10000后使用。

2. ROS接口（topic订阅与发布）

ROS接口为上层应用提供API调用，便于SLAM算法和应用功能开发。

订阅topic:

/cmd_vel  (geometry_msgs::Twist)

发布topic:

/enc_l    (msgs::Int16)

/enc_r    (msgs::Int16)

/odom    (nav_msgs::Odometry)

/tf        (odom->base_footprint)

3.ROS驱动包程序解析

了解了前面的通信协议和ROS接口后，接下来就来看看ROS程序是如何实现的。首先程序订阅/cmd_vel作为用户的控制输入，将控制输入的速度信息转换为通信协议中规定的格式，然后通过串口下发给stm32,实现对小车的运动控制。

下图是订阅/cmd_vel的回调函数：

下图是串口下发函数：

知道了怎么控制小车运动，接下来介绍获取小车反馈回来的码盘信息，并将反馈回来的码盘信息进行里程计解算。

如下图是串口读取和里程计解算函数：

如下图是里程计和相关topic发布：

 

4.基于航迹推演的里程计解算

 首先通过下面的一张图来了解一下里程计解算的大致原理，下图包含两个坐标分别对应图A、图B，请看图：

在图A中，设定机器人初始位置与坐标系原点重合，机器人的正前方为坐标系x轴正方向，机器人方向角定义为由坐标系x轴正方向逆时针方向(即图中theta轴所示)。利用极限的思想，机器人在很短的时间delta_t内，由A位置运动到B位置，机器人绕p转动一个微小的角度，其中delta_d_l与delta_d_r分别为左轮和右轮的位移量，可以通过编码反馈值计算得；delta_theta是方向角变化量，由等腰三角形的关系可以得出delta_theta与图中各角的关系；L是两轮之间的轴距。有了这个理论基础，下面接着介绍如何解算出里程计。

在图B中，主要说明了如何通过迭代来计算机器人在坐标系中的姿态，从而得到我们需要的里程计。通过图A中介绍的原理，不难得出delta_d、delta_theta、delta_x、delta_y各变化量，然后通过过迭代的方法便可以求出里程计中的机器人位置(x,y,theta)和机器人的速度(v,w)。

有了里程计解算的理论基础后，接下来谈谈具体程序实现，如下图：

 

程序的输入是在采样周期内得到的左右轮编码值(delta_encode_left和delta_encode_right)，这个编码值可以通过速度系数speed_ratio便可以换算成实际的位移量，速度系数speed_ratio可以通过后续的标定得到；两个轮子的轴距wheel_distance也是通过后续的标定得到，采样周期encode_sampling_time在程序中是一个可选的参数。有了delta_encode_left、delta_encode_right的值，便可以推算出delta_d、delta_theta，继而可以推算出delta_x、delta_y，继续往下使用迭代可以计算出最终的里程计值position_x、position_y、oriention、velocity_linear、velocity_angular。最后通过ROS将里程计发布出去就ok了。