基于STM32的ROS智能移动机器人构建地图、定位以及自主导航研发

        好长时间没写博客了，今天继续工作的第一件事情是做一款STM32 的ROS智能移动机器人，构建地图以及自主导航。在这里笔者认为最难的事情是搞机器人底盘，由于之前没有接触过32板，查了一周的文档才清楚，32板是怎么烧录程序的以及与arduino控制电机的区别。

        搭建ROS智能小车，笔者认为可以将移动机器人分为三个部分（上位机、中间层以及下位机），下位机控制电机（直流无刷电机）、水泵、继电器、LED等，最重要的是把里程计数据（X坐标、Y坐标、方向角、线速度、角速度）实时地传递给上位机；中间层负责通信（和下位机制定通信协议、模式的选择（1.速度模式、2.LED模式、3.继电器模式）、数据的发送（将cmd中的线速度、角速度转换成下位机左右轮的速度）、数据的接收（将下位机上传的odom数据以topic的形式发布），这里出现中间层的发布频率和ROS实际接收频率不一致的问题；上位机负责融合下位机传递的里程计数据和外传感器采集的数据，减少累计误差后，实现构建地图、定位、以及导航功能。

        中间层的部分为三步，第一步在PC机架构ROS操作系统（推荐16.04版本）比较稳定，另外不推荐虚拟机。最好是在PC机上安装双系统，可以通过远程控制（ssh）到arduino、树莓派或者工控机为主板的主控机。第二步是在arduino、树莓派或者工控机上架构ROS操作系统，最好是和PC机的版本一致。完成简单的部分后，接下来到第三步，也就是最核心部分：ROS系统和下位机进行通信，通信协议中包括串口、波特率、流量控制、停止位以及字符大小的定义，在后期我会上传串口程序的源代码。

        前段时间在调试串口通信的过程中遇到了一些问题，在博客中先一一列举出来，例如在知乎大神的对话中，地址是：https://www.zhihu.com/question/32120691，下载了STM32的驱动，地址是：https://github.com/spiralray/stm32f1_rosserial，移植在工控机上并调试STM32的串口通信，出现了串口无法识别的问题，没办法由于第一次搞串口通信这块的东西会比较费时间，网上的方法基本上都基本上尝试了一遍没有解决问题，然后又进入ROS官网中下载rosserial包，地址： http://wiki.ros.org/rosserial有了数据类型不匹配的问题，又在尝试各种解决串口的工具，包括添加串口工具，搞了好几天基本上都是在发现问题，解决问题。然后参考其他文章，下定决心自己写一个串口通信协议。在这里推荐一篇文章 ros与下位机通信常用的c++ boost串口应用--22  原文文章来自http://www.cnblogs.com/zxouxuewei/，

       由于笔者安装的Boost的版本是1.58,在boost_node包中的CMakList.txt中添加

       find_package(catkin REQUIRED COMPONENTS
                                                                      roscpp
                                                                      rospy
                                                                      std_msgs
                                                                      genmsg )

add_executable(boost_node src/boost_node.cpp)
target_link_libraries(boost_node ${catkin_LIBRARIES}

        此外，读者如果没有boost 库，需要在编译时链接自己boost库所在的位置。可以在boost_node包中新建一个build,然后成功编译后，在catkin_ws中用catkin_make编译，在lib中生成一个可执行文件boost_node，执行rosrun boost_node boost_node，会出现buffer[]的返回值，当然也可以多定义buf[]={1,2};最后返回buf[0]的值，打印出来，可以完成ROS与STM32的简单通信。

        如果想更深层次的了解ROS和与STM32的串口通信，请大家参考大神的这篇博客中的base_controller ,可以借鉴博客中串口的收发数据的类型与格式，https://blog.csdn.net/Forrest_Z/article/details/55002484   其实串口通信部分就是实现数据正常传输，我们在写ROS和与STM32的串口通信里面添加了模式，为了区分在不同模式下的数据发送；中间层的数据接收部分现在遇到了发送odom的频率和ROS中显示的频率不一致的问题，很糟心，刚开始以为是串口的问题，在别的小车上试了之后，显然不是这个问题，有可能是下位机数据传送数据太慢导致的，当然是猜测，马上要去尝试。有哪位大神解决了这个问题，可以留言，在此谢过。

        九月份一直在看tf坐标转化这块的内容，在做激光SLAM或者视觉SLAM中有几个坐标系会用到tf坐标转换，例如base_laser和base_footprint之间，在底盘这块会用到base_footprint坐标和odom坐标之间的转换，其实最终判断误差大小是根据map和odom之间的重合度来判定的。tf转换会在ROS中调用tf包，数据直接转换，很赞。想弄清tf之间的转换关系直接在RVIZ中跑机器人仿真模型，然后可视化坐标系，会发现中间会有两个重要的角度，方向角和偏航角。

        偏航角是机器人的初始位置和当前位置存在的角度，是累加角度，而方向角是通过转弯角速度w和时间t计算出来.  偏航角以ROS中大量用到的四元数形式表示。其中在三维可视化RVIZ中有两个坐标odom和map，机器人原点位置时，这两个坐标是完全重合的，而当机器人运动后，odom数据会因为轮子出现打滑，空转以及其他问题出现累计误差，而误差体现在odom坐标和map坐标的重合程度。在做boost_node这块，base_footprint坐标是子坐标，odom是父坐标，读者可以模拟一个odom数据（在ROS 机器人程序设计第二版的第八章有具体的模拟方法），可以在RVIZ中观测到虚拟的机器人使用假的odom数据做圆周运动。关于odom数据的发布中会涉及到机器人的位姿信息，也就包括位置和姿态的计算。

       odom数据包括（X、Y坐标，方向角，线速度，角速度）mm，mm,rad,mm/s,rad/s。没有接触底盘时，不太清楚控制电机的方式，可以参考移动中间层的解释，个人觉得两个电机同正时向前，同负向后运动这种方式会好点。因为后期会根据编码器数据计算里程计数据，这样计算会准确点，另外这里会牵扯到偏航角的计算，个人建议先用编码器计算机器人相对初始位置逐渐累加的方向角信息,也就是下文出现的th=th+。X、Y坐标是机器人相对初始位置经过逐渐累加计算，在odom数据中的''线速度、角速度"与通常理解的线速度、角速度不同，odom中的线速度决定机器人的前进速度，角速度决定机器人的转弯角速度。

       机器人转弯现在基本上是三种转弯方法：1.车轮的左右轮分别正传和反转。2.左右轮正传，左右轮的速度一致。3.原地转弯，其中一个轮有速度，另外一个轮速度为0。在ROS机器人转弯基本上都采用的是差速转弯方式，在这里笔者推荐一篇大神的博客：https://blog.csdn.net/forrest_z/article/details/55001231

  移动机器人中间层解释

  1.串口发送

（1）内容：左右轮速度，单位为mm/s

（2）格式：5个字节,[帧头0x68,0x01  2字节][模式1字节] [帧尾0x0d,0x0a  2字节]

  串口下发数据包括模式选择，当然一般都是速度模式，但是其他模式也是上位机通过串口往下发送。可以选择线速度或者角速度中的一个值判别。

选择01 速度模式时

串口发送	右轮速度(4字节)	左轮速度(4字节)
单位	mm/s	mm/s

选择02 LED模式时 

选择03 计算器模式

 

  2.串口接收

（1）内容：小车X,Y坐标(mm)，方向角(rad)，线速度(mm/s)，转弯角速度(rad/s)

（2）格式：24字节，[帧头0x68,0x01  2字节] [Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][ 帧尾0x0d,0x0a  2字节]

串口接收	X坐标（position_x=0）	Y坐标 (position_y=0)	方向角 (oriention=0)	线速度（表示机器人平移速度） （vel_linear=0）	角速度（表示机器人的转弯速度） （vel_angular=0）
单位	mm	mm	Rad	mm/s	Rad/s

        小车控制思想控制电机转动。

        1. 电机的控制我们分为两部分，一部分为电机转动方向的控制，另一个为电机转速的控制。电机转动的方向我们用两个MCU引脚来控制，假如PIN_A=1，PIN_B=0 时，电机正转；PIN_A=0，PIN_B=1 时，电机反转；PIN_A=0，PIN_B=0 时，电机停止。电机速度的控制则需要一个PWM输出引脚，我们通过控制输出不同的PWM值来控制电机转动的速度。

        2. PID控制。如果我们想控制小车以一米每秒的速度做直线，但由于地面的摩擦阻力的影响，会造成左右轮速度与我们想控制的速度不同，所以会走不直，这时我们就需要加入PID控制，PID控制的思想就是我实时的把轮子真正的速度采集回来和控制的速度对比，差则补，多则减。这样基本就可以实现理想控制。

        3. 小车转弯控制。一般我们要是想控制小车以多少的速度前进或者后退，我们只需要PID控制两个轮子的速度一致就可以基本做到。但如果要想控制小车以多少的角速度转弯，我们需要做一定的计算。   

        在ROS中，机器人的位置[position: x，y，z]和方向[orientation: x，y，z，w]被定义为姿态。该位置由x、y和z三个向量描述，而方向使用四元数形式的x、y、z和w。

                                                      图1 导航推测所需要的信息(中心位置(x，y)，车轮间距离D，车轮半径r)

                                                                             图2 导航推测(dead reckoning)

          当有如图1的移动机器人在运动过程中，设D是车轮之间的距离，r是车轮的半径。如图2所示，当机器人在时间内移动很短距离时，利用左右电机旋转量(当前编码器值、和之前的编码器值 和)来计算出左右车轮的转速（）,如式1-1和1-2所示。                                                            

          如式1-3和1-4求出左右轮的移动速度（V），并如式1-5和1-6求出机器人的平移速度（linear velocity: ）和旋转速度（angular velocity：）。

                    

 

 

注：下位机通过串口上传的X、Y坐标和方向角，都是从初始时刻到当前时刻经过不断累加后得到的数据。 

       我们底盘最大转速是3000转/min, 减速比是1:30，驱动轮直径是D=125mm,轴间距是370mm，通过计算得Vmax=0.65m/s, 0.5Vmax=0.325m/s。我将移动机器人分为三层，下位机控制电机以及对把里程计数据传递给上位机（我们使用的是直流无刷电机），中间层负责通信（和下位机制定通信协议、模式的选择、数据的转换并以topic的形式发布），上位机负责融合下位机传递的里程计数据和外传感器采集的数据，构建地图、定位、以及导航功能。

     构建地图是首先采用开源的是hector_slam,结果发现构建的地图根本不能用，然后想到的是gmapping_slam，这种方法比较依赖于里程计的数据，现在还在测试阶段，另外的是cartgraph_slam,这种方法只是跑了网上的数据集，增加了回环检测的功能，构建的地图比较清晰，目前在看这些方法的源代码，后期再说体会。

     目前遇到的问题有：1.上位机和中间层发布的频率不一致。

                                         2.转弯方式的制定（差速转弯）

                                         3.数据拼接问题

                                         4.下位机最大速度是0.65,百分制问题

                                         5.四元数计算问题

                                         6.对tf坐标的理解

补充：

    ubuntu下的串口工具是cutecom,亲测可用，不用再去win7系统下进行测试底盘

1.  安装cutecom 

    sudo apt-get install cutecom

2. 运行cutecom                                       

    sudo cutecom                                        

.

3.将/dev/ttys0改成/dev/ttyUSB0等