【自己动手做一台SLAM导航机器人】第四章：差分底盘设计
本专栏目录
第四章：差分底盘设计
第五章：树莓派3开发环境搭建
第六章：SLAM建图与自主避障导航
第七章：语音交互与自然语言处理
附录A：用于ROS机器人交互的Android手机APP开发
附录B：用于ROS机器人管理调度的后台服务器搭建
附录C：如何选择ROS机器人平台进行SLAM导航入门
视频教程
https://www.bilibili.com/video/BV1jS4y1a7Lz
运动底盘是移动机器人的重要组成部分，不像激光雷达、IMU、麦克风、音响、摄像头这些通用部件可以直接买到，很难买到通用的底盘。一方面是因为底盘的尺寸结构和参数是要与具体机器人匹配的；另一方面是因为底盘包含软硬件整套解决方案，是很多机器人公司的核心技术，一般不会随便公开。出于强烈的求知欲与学习热情，我想自己DIY一整套两轮差分底盘，并且将完整的设计过程公开出去供大家学习。说干就干，本章节主要内容：
1.stm32主控硬件设计
2.stm32主控软件设计
3.底盘通信协议
4.底盘ROS驱动开发
5.底盘PID控制参数整定
6.底盘里程计标定
下面这本书是本篇文章的参考文献，大家有需要可以入手一本：
1.stm32主控硬件设计
完整的stm32主控硬件包括：带霍尔编码器的直流减速电机、电机驱动、stm32单片机开发板等配件。
1.1.带霍尔编码器的直流减速电机
要制作一台机器人底盘，需要一套完整的电机部件，就如图1中看到的一样，需要有轮胎、联轴器、减速箱、电机和编码器，具体选型可以参考这几个方面的因素：
轮胎：直径越大，小车的越障能力越好，但会降低小车爬坡的马力；
联轴器：选择跟轮胎与电机输出轴尺寸相匹配的型号；
减速箱：减速比决定电机输出轴的扭矩，减速比越大，输出轴扭矩越大，但输出轴转速越慢；
电机：一般是12V的电机，直流有刷简单易控制；
编码器：一般为增量式正交编码器，编码线数根据实际需要精度进行选择。
如图2，可以清楚的看到电机的接线端口，其实电路板上也是有丝印标注的。接线分为两类，一类是电机控制（电机线+、电机线-），另一类是编码器（编码器5V、编码器A相、编码器B相、编码器GND）。
1.2.电机驱动电路
了解了电机的构造知识后，就来介绍一下如何将电机驱动起来。如图3所示，TB6612FNG是很流行的一款电机驱动芯片，相比于传统的L298N效率上提升很多，而且体积大幅减小。TB6612FNG是双驱动，也就是可以驱动两个电机；TB6612FNG每通道输出最高1.2 A的连续驱动电流，启动峰值电流达2A/3.2 A（连续脉冲/单脉冲）；4种电机控制模式：正转/反转/制动/停止；PWM支持频率高达100 kHz。
TB6612FNG的引脚定义，如图4所示，引脚分为电源脚、控制输入脚、控制输出脚。
VM：为电机驱动电压，根据实际电机额定电压选择，推荐使用12V供电；
VCC：逻辑电源供电，推荐使用5V供电；
STBY：待机/工作状态切换，低电平待机，高电平工作；
PWMA：A端口电机PWM调速信号输入；
AIN1和AIN2：A电机转向控制信号输入；
PWMB：B端口电机PWM调速信号输入；
BIN1和BIN2：B电机转向控制信号输入；
AO1和AO2：A端口电机驱动信号输出；
BO1和BO2：B端口电机驱动信号输出。
最后，我们来看一下控制信号的逻辑真值表，如图5，输入由单片机IO口给定，再结合PWM信号，便可以实现对电机的正/反转和调速控制了。由于两路电机控制是一模一样的，所以另一路控制信号的逻辑真值表就不重复赘述了。
1.3.霍尔正交编码器原理
如果两个信号相位相差90度，则这两个信号称为正交。由于两个信号相位相差90度，因此可以根据两个信号哪个先哪个后来判断方向。利用单片机的IO口对编码器的A、B相进行捕获，很容易得到电机的转速和转向。霍尔正交编码器原理，如图6。
1.4.stm32单片机最小系统
stm32单片机常用的型号是stm32f103，根据具体需求的Flash容量、RAM容量、IO口数量进行选择，下面是常用的一些型号参数对比，如图7。
考虑到stm32主控只是用于两个电机的控制，资源开销不算大，需要用到的IO口也不是很多，定时器资源也不多，出于性价比考虑推荐stm32f103c8t6这个型号。
如图8所示，stm32f103c8t6最小系统板比较简洁，控制两个电动机，只需要用两个IO口输出2路PWM分别给两个电机调速，用4个IO口分别控制两个电机的方向，另外4个IO口分别接两个电机的正交编码器输入，UART1与UART2跟上位机连接分别用于程序debug与上层指令控制。
1.5.stm32主控硬件整体框图
第一个版本的硬件电路是用飞线连接的各个模块，电路稳定性很差，而且外观极其丑陋。痛定思痛，决心老老实实设计电路板，把各模块集成到一个板子上，经过两次改板打样，终于成功了。如图9，板子简洁美观，而且接插端子布局合理，符合我一向严苛的标准。
好了，有了这个电路板就好办多了。针对这个电路板，讲讲我的设计思路吧。首先需要设计一个电源系统，用于单片机供电、电机供电、外部设备供电，同时还要考虑电源反接、过压、短路等保护；然后需要设计一个stm32单片机最小系统电路；最后围绕stm32最小系统，需要设计电机驱动、UART转USB、编码器信号捕获这些外围电路，同时还要考虑电机堵转保护、电机对系统电源干扰等问题。逐一采坑之后，差不多就完成设计了。stm32主控硬件整体框图，如图10。
2.stm32主控软件设计
上一节搭建好了底盘的stm32主控硬件，现在就来说说怎么开发配套的stm32软件。关于建立stm32工程、使用stm32开发库、stm32软件调试方法等基础知识就不多说了，有需要的可以查阅相关资料学习，我觉得http://www.openedv.com《正点原子》的开发资料写的还可以。我就直接从底盘控制的项目入手，直接进行项目中各个功能需求开始分析讲解，如图11，是我的底盘控制stm32工程项目。
2.1.电机控制
电机控制分为两个部分（电机转向控制、电机转速控制），这些都集成在了电机驱动芯片TB6612FNG里面，所以只需要用单片机的IO口产生控制转向的高低电平和控制转速的PWM波就能实现。
首先，初始化IO口作为输出脚，用于产生高低电平输出来控制转向，实例代码如图12。
然后，用通用定时器TIM4的通道CH1和CH2分别产生两路PWM输出用于两个电机的转速控制，定时器默认引脚分配如图13。
初始化通用定时器TIM4的通道CH1和CH2为PWM输出，实例代码如14。
最后，将电机转向和速度控制的操作封装在一个函数中，便于其它地方调用，实例代码如图15。
2.2.编码器数据读取
编码器对底盘来说至关重要，一方面底盘通过编码器的反馈进行PID闭环速度控制，另一方面底盘通过编码器进行航迹推演得到里程计用于后续的定位与导航等高级算法中。这里用到的编码器是正交编码器，所以直接使用通用定时器的输入捕获中的编码器模式来读取编码器。采用通用定时器TIM2的通道CH1和CH2捕获encoder1的A相和B相脉冲，采用通用定时器TIM3的通道CH1和CH2捕获encoder2的A相和B相脉冲。
先初始化TIM2作为编码器encoder1的捕获，实例代码如图16。
然后，将读取编码器计数值的操作封装在一个函数中，便于其它地方调用，实例代码如图17。
最后，编写TIM2计数溢出时的中断处理函数，实例代码如图18。
同理可得TIM3捕获encoder2的代码实现，这里就不在赘述了。
2.3.串口数据收发
串口2是数据接口，负责接收上位机发送过来的控制指令，同时将编码器值返回给上位机；串口1是debug接口，负责接收上位机发送过来的版本信息请求、PIDm默认值恢复、PID值设定等调试指令，同时将程序中的debug打印信息返回给上位机。但是在底盘正常工作时，只需要连接串口2；串口1是预留出来给有需要自己动手修改PID参数使用的。
首先，配置串口1，先对串口1的输出进行printf函数打印支持，实例代码如图19。
然后，初始化串口1，实例代码如图20。
最后，编写串口1接收中断处理函数，此函数主要进行对上位机发过来的数据进行协议解析，实例代码如图21。
接下来，介绍串口2，初始化串口2，实例代码如图22。
然后，将串口2发送数据的操作封装到函数中，便于其它地方调用，实例代码如图23。
最后，编写串口2接收中断处理函数，此函数主要进行对上位机发过来的数据进行协议解析，实例代码如图24。
到这里，串口有1和串口2的数据发送与接收都编写好了，依据我们定义的usart2数据通信协议和usart1调试通信协议，上位机就可以编写对应的程序来跟底盘的串口2和串口1进行通信了。关于通信协议的具体内容，将在后续做展开。
2.4.电机速度PID控制
我在底盘中采用的是增量型PID算法，编程涉及到的数学表达式有3个，分别是：
e(k) = target_value - current_value
delta_u(k) = Kp*[e(k)-e(k-1)] + Ki*e(k) + Kd*[e(k)-2*e(k-1)+e(k-2)]
u(k) = u(k-1) + delta_u(k)
将这3个数学表达式封装到函数中，便于其它地方调用，实例代码如图25。
电机1与电机2采用同样的PID算法，所以电机2的PID算法代码实现就不赘述了。关于PID参数的整定方法，将在后续做展开。
2.5.周期性控制
通过上面的讲解，各个模块的驱动代码都准备就绪了，现在需要产生一个周期性的过程，在里面实现编码器计数值采样、PID控制等具体实现。这里采用定时器TIM1产生一个周期性的中断，在中断处理函数中实现各模块的具体操作。
首先，配置定时器TIM1，实例代码如图26。
然后，编写中断处理函数，实例代码如图27。
2.6.stm32主控软件整体框图
通过上面的讲解，对底盘控制的stm32程序实现有了一定的了解，接下来就来做一个总结。
先来看看main()函数实现，如图28。
结合上面TIM1中断处理函数，不难发现，整个stm32程序的执行过程：
a.在main()函数中初始化各个模块；
b.TIM1中断处理函数周期性的读取编码器值、反馈获取的编码值、PID控制；
c.剩下的就是串口1和串口2的通信交互。
具体stm32主控软件整体框图如图29。
需要说明的是，在周期性循环体中，要首先读取编码器的值，来保证严格的等间隔采样。
3.底盘通信协议
对于做纯SLAM算法、机器人导航避障、或者别的需要用到移动底盘的应用，其实不需要搞明白底盘的底层硬件原理和软件实现等繁琐的细节，只需要根据底盘通信协议，在上层应用程序中利用串口以收发数据的方式来完成对底盘的操作。也就是说底盘的底层操作细节被封装到基于串口通信的API中了。
先来说说ROS社区提供的rosserial库，rosserial库是为了解决单片机与机器人之间的通信问题，使用rosserial库可以实现单片机与机器人之间透明的ROS主题发布与订阅通信。原理其实很简单，如图30。
单片机中通过包含rosserial.h头文件来引用rosserial库中的数据封装与数据解析方法，这样在单片机上可以直接按照ROS中发布和订阅数据的语法来编写程序，rosserial库会自动完成封装和解析；被rosserial库封装成串口字节流后可以在串口数据线上传输；在机器人上同样通过包含rosserial.h头文件来引用rosserial库中的数据封装与数据解析方法，这样在机器人上直接按照ROS中发布和订阅数据的语法来编写程序，rosserial库会自动完成封装和解析。rosserial协议建立了单片机与机器人之间的透明ROS通信，这个ROS机器人开发这带来了很大的方便。
但是，rosserial协议虽然好，目前rosserial对很多单片机的支持还不是很好，只对少数型号的单片机（比如Arduino系列单片机）有支持，像应用广泛的stm32单片机就没有官方rosserial库的支持；另一个缺点，rosserial协议比较臃肿，这样对通信的资源消耗大并且影响数据实时性。
其实解决rosserial协议这几个缺点很简单，我们借鉴rosserial协议的思想，对rosserial协议中的冗余进行裁剪，我们miiboo机器人底盘自己的通信协议也就应运而生了。miiboo机器人底盘自己的通信协议，如图31。
其实很好理解，miiboo机器人底盘自己的通信协议包含两个部分：DEBUG-uart1和DATA-uart2。DEBUG-uart1用于stam32与机器人之间传输调试打印信息、调试命令；DATA-uart2用于stam32与机器人之间传输速度反馈、速度控制。并且DEBUG-uart1和DATA-uart2两个串口都采用波特率115200进行数据传输。下面就针对DEBUG-uart1和DATA-uart2这两部分的协议进行详细的讲解。
3.1.DEBUG-uart1协议内容
DEBUG-uart1协议内容分为：调试打印信息（stm32单片机==>机器人）、调试命令（stm32单片机<==机器人）。调试打印信息是stm32单片机向机器人发送数据，调试命令是机器人向stm32单片机发送数据。
在机器人端，对从串口获取的字符串数据流，直接用printf()函数就可以解析。
在机器人端，将要下发的调试命令（其实就是请求stm32单片机的版本信息与PID信息命令、请求将stm32中PID恢复为默认值命令、请求将stm32中PID设置为指定值命令）封装成对应的数据帧，然后让串口下发由这15个字节组成的数据帧就行了。
3.2.DATA-uart2协议内容
DATA-uart2协议内容分为：速度反馈（stm32单片机==>机器人）、速度控制（stm32单片机<==机器人）。速度反馈是stm32单片机向机器人发送数据，速度控制是机器人向stm32单片机发送数据。
在机器人端，对从串口获取的字符串数据流，按照这个数据帧格式进行解析，就可以从这11字节组成的数据帧中解析出左轮速度、右轮速度。
在机器人端，将要下发的目标速度控制值（左轮速度、右轮速度），按照这个数据帧格式进行封装，然后让串口下发由这11个字节组成的数据帧就行了。其实不难发现，速度反馈和速度控制遵循同样的数据帧格式，这也是很好理解的。
4.底盘ROS驱动开发
对于做纯SLAM算法、机器人导航避障、或者别的需要用到移动底盘的应用，根据底盘的通信协议，直接使用底盘ROS驱动实现跟底盘的交互。miiboo机器人底盘的ROS驱动代码组织如图36。
整个代码组织是一个完整的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导航等功能的开发。
节点实现源码解析：
底盘控制节点由base_controller.cpp实现。程序主要分为两个过程：订阅topic数据并下发给底盘、从底盘接收数据并发布到topic。
首先，程序订阅/cmd_vel作为用户的控制输入，将控制输入的速度信息转换为通信协议中DATA-uart2规定的格式，然后通过串口下发给底盘,实现对底盘的运动控制。订阅/cmd_vel的回调函数和串口下发函数分别如图38和图39所示。
然后，程序从串口获取底盘的速度反馈，并将速度反馈数据放入航迹推演算法中进行解算，得到里程计，将反馈回来的左轮速度、右轮速度值分别发布到/wheel_left_speed和/wheel_right_speed主题，将解算出来的里程计分别发布到/odom和/tf主题。由于不同的算法对里程计的格式要求不一样，所以将里程计同时发布到/odom和/tf主题，便于不同的算法使用。从串口获取速度反馈并求解里程计和发布反馈速度与里程计到topic分别如图40和图41所示。
基于航迹推演算法的里程计解算：
首先，我们需要了解一下ROS下的机器人坐标系为右手坐标系，如图42，机器人底盘的正前方为x轴正方向、机器人底盘的正上方为z轴正方向、机器人底盘的正左方向为y轴正方向、机器人航向角theta坐标轴以x轴为0度角并逆时针方向增大。一般以机器人底盘上电时刻，机器人底盘的位置建立里程计坐标系，也就是说机器人底盘的起始位姿为原点O，机器人底盘在运动过程中，通过前一时刻的位置和左、右轮位移可以推算出机器人底盘的下一时刻位姿，这就是航迹推演算法。
我们这里值讨论两轮差分底盘的情况，分析如图41，通过前一时刻的位置和左、右轮位移可以推算出机器人底盘的下一时刻位姿。航迹推演的数学模型如图43。
在很短的时间间隔里，前后两个机器人位姿满足一定的关系，具体看图44的推导。
这样，经过进一步的化简，可以得到我们解算里程计的核心公式，如图45。
不难发现，上面程序中解算里程计部分的代码就是这个核心公式的具体编程实现。关于航迹推演算法更详细的推导，请参考：
http://faculty.salina.k-state.edu/tim/robotics_sg/Control/kinematics/odometry.html
4.2.底盘调试节点
接口：
底盘调试节点对下与底盘DEBUG-uart1串口通信，对上通过命令行终端指令交互方式。
节点实现源码解析：
底盘调试节点由pid_set.cpp实现。程序主要分为两个过程：从终端获取调试命令并下发给底盘、从底盘接收应答数据并显示在终端。
首先，程序从终端获取调试命令，用户可输入数字1,2,3，如果用户输入3会再要求输入kp,ki,kd这三个数，将调试命令转换为通信协议中DEBUG-uart1规定的格式，然后通过串口下发给底盘,实现对底盘的调试。调试命令获取与下发如图47。
然后，程序从串口获取底盘的应答信息，这里就比较简单了，直接将获取的应答数据原样打印到终端就行了，如图48。
5.底盘PID控制参数整定
我们的miiboo机器人底盘的stm32控制板中已经内置了整定好的PID参数，如果选用我们提供的控制板和电机，一般情况下是不需要整定PID的。
对于想体验一下PID参数整定过程或将我们的miiboo机器人底盘的stm32控制板应用到其他地方的朋友，这里给出了整定PID的整个操作过程和思路，方便大家学习和更深层次的研究。首先，对PID三个参数定性的分析，先有个感性的认识，如图49。
其次，由于我们的miiboo机器人底盘的stm32控制板中采用的是增量式PID，所以这里对增量式PID参数的特殊性进行一些说明，如图50。
位置型PID的参数整定过程一般是，先整定KP，然后整定KI，最后整定KD；对比位置型PID与增量型PID的数学表达式，可以发现位置型KP和增量型KI一样，位置型KI和增量型KD一样，位置型KD和增量型KP一样，如图51。这样，增量型PID应该先整定KI，然后整定KD，最后整定KP。这一点需要特别注意，弄错顺序的话会发现整定规律完全不适用的。
在机器人上进行具体PID整定操作之前，先对整定原理做一些讲解。下面的表述是针对增量型PID的，即KI为比例参数、KD为积分参数、KP为微分参数。这里使用试凑法对miiboo机器人底盘的增量PID参数进行整定：
第1步：
首先只整定比例部分。比例系数KI由小变大，观察相应的系统响应，直到得到反应快，超调小的响应曲线。系统若无静差或静差已小到允许范围内，并且响应效果良好，那么只须用比例调节器即可。
第2步：
若稳态误差不能满足设计要求，则需加入积分控制。整定时先置KD为较小值，并将经第1步整定得到的KI减小些( 如缩小为原值的0.8倍 )，然后增大KD，并使系统在保持良好动态响应的情况下，消除稳态误差。这种调整可根据响应曲线的状态，反复改变KI及KD，以期得到满意的控制过程。
第3步：
若使用比例-积分调节器消除了稳态误差，但动态过程仍不能满意，则可加入微分环节。在第2步整定的基础上，逐步增大KP，同时相应地改变KI和KD，逐步试凑以获得满意的调节效果。
原理了解后，就要到实际的miiboo机器人上进行整定了，首先需要将底盘的DATA-uart2与DEBUG-uart1串口连接到机器人的主板树莓派3中，并确保被树莓派识别的串口设备号为底盘驱动设置的值，如果串口号不匹配需要先进行匹配，关于这部分内容将在miiboo机器人SLAM导航中做更详细的展开。然后，需要启动底盘控制节点、底盘调试节点、键盘控制节点。
键盘控制节点teleop_twist_keyboard需要通过apt-get命令来安装，rqt_plot是ROS提供的绘图工具，关于这些的具体使用方法将在miiboo机器人SLAM导航中做更详细的展开。
最后，就是通过观察速度曲线，按照试凑法的步骤，在底盘调试节点的终端中输入相应的kp、ki、kd参数，不断重复这个过程直到速度曲线达到一个比较满意的形状。rqt_plot速度曲线的样子如图52所示。
6.底盘里程计标定
机器人底盘运行的精度是衡量底盘的重要指标。底盘精度受里程计的走直线误差和转角误差影响。因此，需要对里程计的走直线和转角进行标定，尽量减小误差。miiboo机器人底盘的ROS驱动中已经写好了相应的标定程序，跟里程计标定有关的文件主要有：
.../miiboo_bringup/launch/check_linear.launch为里程计走直线标定启动文件
.../miiboo_bringup/launch/check_angular.launch为里程计转角标定启动文件
.../miiboo_bringup/launch/minimal.launch为设置标定参数及底盘控制启动文件
下面是标定步骤过程。
第一步：
打开终端，给标定脚本赋予可执行权限
第二步：
连接好底盘DATA-uart2串口，启动底盘
第三步：
设定前进1米的目标，对走直线进行标定
测量底盘停止时实际走的直线距离M，按下面的规则调整里程计直线参数speed_ratio
如果M > 1米，增大speed_ratio
如果M < 1米，减小speed_ratio
里程计的参数存放在miiboo_bringup/launch/minimal.launch文件中，如图53所示。
修改好参数后，需要保存，然后重新启动一下底盘节点，这样参数才能生效。
重复第三步的操作，直到走直线的误差达到我们能接受的范围（比如1%的误差），则进入下一步。
第四步：
设定旋转360度的目标，对转角进行标定
测量底盘停止旋转时实际转过的角度A，按下面的规则调整里程计转角参数wheel_distance
如果A > 360度，减小wheel_distance
如果A < 360度，增大wheel_distance
上面已经讲过，里程计的参数存放在miiboo_bringup/launch/minimal.launch文件中，如图53所示。
修改好参数后，需要保存，然后重新启动一下底盘节点，这样参数才能生效。
重复第四步的操作，直到走转角的误差达到我们能接受的范围（比如1%的误差），则标定完成。
当然，有兴趣的朋友可以阅读miiboo_bringup/scripts/中的标定脚本源码，结合航迹推演算法，理解里程计标定的整个原理。其实wheel_distance这个参数是编码脉冲值与电机轮胎位移值的一个比例系数，简单点说就是电机转过一个编码脉冲，这个时候电机轮胎走过多少距离；wheel_distance这个参数是左右两个轮子的间距。有了这个认识后，我们可以在这两个参数的理论值附近对参数进行微调，标定起来会更快。
后记
为了防止后续大家找不到本篇文章，我同步制作了一份文章的pdf和本专栏涉及的例程代码放在github和gitee方便大家下载，如果下面给出的github下载链接打不开，可以尝试gitee下载链接：
github下载链接：https://github.com/xiihoo/DIY_A_SLAM_Navigation_Robot
gitee下载链接：https://gitee.com/xiihoo-robot/DIY_A_SLAM_Navigation_Robot
技术交流
QQ技术交流群：117698356
参考文献
[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.
