SLAM导航机器人零基础实战系列:(四)差分底盘设计——4.底盘ROS驱动开发
摘要
运动底盘是移动机器人的重要组成部分,不像激光雷达、IMU、麦克风、音响、摄像头这些通用部件可以直接买到,很难买到通用的底盘。一方面是因为底盘的尺寸结构和参数是要与具体机器人匹配的;另一方面是因为底盘包含软硬件整套解决方案,是很多机器人公司的核心技术,一般不会随便公开。出于强烈的求知欲与学习热情,我想自己DIY一整套两轮差分底盘,并且将完整的设计过程公开出去供大家学习。说干就干,本章节主要内容:
4.底盘ROS驱动开发
对于做纯SLAM算法、机器人导航避障、或者别的需要用到移动底盘的应用,根据底盘的通信协议,直接使用底盘ROS驱动实现跟底盘的交互。miiboo机器人底盘的ROS驱动代码组织如图36。
(图36)miiboo机器人底盘的ROS驱动代码组织
整个代码组织是一个完整的ROS功能包,功能包名为miiboo_bringup,功能包中包含两个ROS节点源码(base_controller.cpp和pid_set.cpp),不难看出这两个节点正是对底盘通信协议中的DATA-uart2与DEBUG-uart1的具体实现。base_controller.cpp负责对底盘控制驱动的具体实现,pid_set.cpp负责对底盘调试驱动的具体实现。本节重点对这两个节点进行讲解,至于功能包名下的其他内容将放在后面的miiboo机器人SLAM导航实战中具体展开。
4.1.底盘控制节点
接口:
底盘控制节点对下与底盘DATA-uart2串口通信,对上开放ROS接口为应用层提功能数据发布与订阅,便于SLAM导航等功能的开发。
(图37)底盘控制节点接口
节点实现源码解析:
底盘控制节点由base_controller.cpp实现。程序主要分为两个过程:订阅topic数据并下发给底盘、从底盘接收数据并发布到topic。
首先,程序订阅/cmd_vel作为用户的控制输入,将控制输入的速度信息转换为通信协议中DATA-uart2规定的格式,然后通过串口下发给底盘,实现对底盘的运动控制。订阅/cmd_vel的回调函数和串口下发函数分别如图38和图39所示。
(图38)订阅/cmd_vel的回调函数
(图39)串口下发函数
然后,程序从串口获取底盘的速度反馈,并将速度反馈数据放入航迹推演算法中进行解算,得到里程计,将反馈回来的左轮速度、右轮速度值分别发布到/wheel_left_speed和/wheel_right_speed主题,将解算出来的里程计分别发布到/odom和/tf主题。由于不同的算法对里程计的格式要求不一样,所以将里程计同时发布到/odom和/tf主题,便于不同的算法使用。从串口获取速度反馈并求解里程计和发布反馈速度与里程计到topic
分别如图40和图41所示。
(图40)从串口获取速度反馈并求解里程计
(图41)发布反馈速度与里程计到topic
基于航迹推演算法的里程计解算:
首先,我们需要了解一下ROS下的机器人坐标系为右手坐标系,如图42,机器人底盘的正前方为x轴正方向、机器人底盘的正上方为z轴正方向、机器人底盘的正左方向为y轴正方向、机器人航向角theta坐标轴以x轴为0度角并逆时针方向增大。一般以机器人底盘上电时刻,机器人底盘的位置建立里程计坐标系,也就是说机器人底盘的起始位姿为原点O,机器人底盘在运动过程中,通过前一时刻的位置和左、右轮位移可以推算出机器人底盘的下一时刻位姿,这就是航迹推演算法。
(图42)机器人右手坐标系与航迹推演
我们这里值讨论两轮差分底盘的情况,分析如图41,通过前一时刻的位置和左、右轮位移可以推算出机器人底盘的下一时刻位姿。航迹推演的数学模型如图43。
(图43)航迹推演的数学模型
在很短的时间间隔里,前后两个机器人位姿满足一定的关系,具体看图44的推导。
(图44)短时间内两个机器人位姿的约束关系
这样,经过进一步的化简,可以得到我们解算里程计的核心公式,如图45。
(图45)解算里程计核心公式
不难发现,上面程序中解算里程计部分的代码就是这个核心公式的具体编程实现。关于航迹推演算法更详细的推导,请参考:
12.1.1. Using Odometry to Track Robot Movement — Robotics Programming Study Guide
4.2.底盘调试节点
接口:
底盘调试节点对下与底盘DEBUG-uart1串口通信,对上通过命令行终端指令交互方式。
(图46)底盘调试节点接口
节点实现源码解析:
底盘调试节点由pid_set.cpp实现。程序主要分为两个过程:从终端获取调试命令并下发给底盘、从底盘接收应答数据并显示在终端。
首先,程序从终端获取调试命令,用户可输入数字1,2,3,如果用户输入3会再要求输入kp,ki,kd这三个数,将调试命令转换为通信协议中DEBUG-uart1规定的格式,然后通过串口下发给底盘,实现对底盘的调试。调试命令获取与下发如图47。
(图47)调试命令获取与下发
然后,程序从串口获取底盘的应答信息,这里就比较简单了,直接将获取的应答数据原样打印到终端就行了,如图48。
(图48)应答数据原样打印
后记
如果大家对博文的相关类容感兴趣,或有什么技术疑问,欢迎加QQ技术交流群(117698356)
参考文献
[1] 张虎,机器人SLAM导航核心技术与实战[M]. 机械工业出版社,2022.
QQ技术讨论群: 117698356