SLAM导航机器人零基础实战系列：（四）差分底盘设计——2.stm32主控软件设计

SLAM导航机器人零基础实战系列：（四）差分底盘设计——2.stm32主控软件设计

摘要                                            

运动底盘是移动机器人的重要组成部分，不像激光雷达、IMU、麦克风、音响、摄像头这些通用部件可以直接买到，很难买到通用的底盘。一方面是因为底盘的尺寸结构和参数是要与具体机器人匹配的；另一方面是因为底盘包含软硬件整套解决方案，是很多机器人公司的核心技术，一般不会随便公开。出于强烈的求知欲与学习热情，我想自己DIY一整套两轮差分底盘，并且将完整的设计过程公开出去供大家学习。说干就干，本章节主要内容：

1.stm32主控硬件设计

2.stm32主控软件设计

3.底盘通信协议

4.底盘ROS驱动开发

5.底盘PID控制参数整定

6.底盘里程计标

---

温馨提示：

本篇文章已经收录在我最新出版的书籍《机器人SLAM导航核心技术与实战》，感兴趣的读者可以购买纸质书籍来进行更加深入和系统性的学习，购买链接如下：

点这里购买：《机器人SLAM导航核心技术与实战》购买链接

---

2.stm32主控软件设计                

上一节搭建好了底盘的stm32主控硬件，现在就来说说怎么开发配套的stm32软件。关于建立stm32工程、使用stm32开发库、stm32软件调试方法等基础知识就不多说了，有需要的可以查阅相关资料学习，我觉得http://www.openedv.com《正点原子》的开发资料写的还可以。我就直接从底盘控制的项目入手，直接进行项目中各个功能需求开始分析讲解，如图11，是我的底盘控制stm32工程项目。

（图11）底盘控制stm32工程项目

2.1.电机控制                

电机控制分为两个部分（电机转向控制、电机转速控制），这些都集成在了电机驱动芯片TB6612FNG里面，所以只需要用单片机的IO口产生控制转向的高低电平和控制转速的PWM波就能实现。

首先，初始化IO口作为输出脚，用于产生高低电平输出来控制转向，实例代码如图12。

（图12）电机转向控制IO口初始化

然后，用通用定时器TIM4的通道CH1和CH2分别产生两路PWM输出用于两个电机的转速控制，定时器默认引脚分配如图13。

（图13）stm32定时器通道默认引脚分配

初始化通用定时器TIM4的通道CH1和CH2为PWM输出，实例代码如14。

（图14）电机转速控制IO口初始化

最后，将电机转向和速度控制的操作封装在一个函数中，便于其它地方调用，实例代码如图15。

（图15）电机转向和速度控制封装

2.2.编码器数据读取           

编码器对底盘来说至关重要，一方面底盘通过编码器的反馈进行PID闭环速度控制，另一方面底盘通过编码器进行航迹推演得到里程计用于后续的定位与导航等高级算法中。这里用到的编码器是正交编码器，所以直接使用通用定时器的输入捕获中的编码器模式来读取编码器。采用通用定时器TIM2的通道CH1和CH2捕获encoder1的A相和B相脉冲，采用通用定时器TIM3的通道CH1和CH2捕获encoder2的A相和B相脉冲。

先初始化TIM2作为编码器encoder1的捕获，实例代码如图16。

（图16）初始化TIM2作为编码器encoder1的捕获

然后，将读取编码器计数值的操作封装在一个函数中，便于其它地方调用，实例代码如图17。

（图17）读取编码器encoder1计数值封装

最后，编写TIM2计数溢出时的中断处理函数，实例代码如图18。

（图18）TIM2计数溢出中断处理函数

同理可得TIM3捕获encoder2的代码实现，这里就不在赘述了。

2.3.串口数据收发            

串口2是数据接口，负责接收上位机发送过来的控制指令，同时将编码器值返回给上位机；串口1是debug接口，负责接收上位机发送过来的版本信息请求、PIDm默认值恢复、PID值设定等调试指令，同时将程序中的debug打印信息返回给上位机。但是在底盘正常工作时，只需要连接串口2；串口1是预留出来给有需要自己动手修改PID参数使用的。

首先，配置串口1，先对串口1的输出进行printf函数打印支持，实例代码如图19。

（图19）串口1的输出进行printf函数打印支持

然后，初始化串口1，实例代码如图20。

（图20）初始化串口1

最后，编写串口1接收中断处理函数，此函数主要进行对上位机发过来的数据进行协议解析，实例代码如图21。

（图21）串口1接收中断处理函数

接下来，介绍串口2，初始化串口2，实例代码如图22。

（图22）初始化串口2

然后，将串口2发送数据的操作封装到函数中，便于其它地方调用，实例代码如图23。

（图23）串口2发送数据封装

最后，编写串口2接收中断处理函数，此函数主要进行对上位机发过来的数据进行协议解析，实例代码如图24。

（图24）串口2接收中断处理函数

到这里，串口有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。

（图25）串口2接收中断处理函数

 电机1与电机2采用同样的PID算法，所以电机2的PID算法代码实现就不赘述了。关于PID参数的整定方法，将在后续做展开。

2.5.周期性控制           

通过上面的讲解，各个模块的驱动代码都准备就绪了，现在需要产生一个周期性的过程，在里面实现编码器计数值采样、PID控制等具体实现。这里采用定时器TIM1产生一个周期性的中断，在中断处理函数中实现各模块的具体操作。

首先，配置定时器TIM1，实例代码如图26。

（图26）配置定时器TIM1

然后，编写中断处理函数，实例代码如图27。

（图27）TIM1中断处理函数

2.6.stm32主控软件整体框图

通过上面的讲解，对底盘控制的stm32程序实现有了一定的了解，接下来就来做一个总结。

先来看看main()函数实现，如图28。

（图28）main()函数实现

结合上面TIM1中断处理函数，不难发现，整个stm32程序的执行过程：

a.在main()函数中初始化各个模块；

b.TIM1中断处理函数周期性的读取编码器值、反馈获取的编码值、PID控制；

c.剩下的就是串口1和串口2的通信交互。

具体stm32主控软件整体框图如图29。

（图29）stm32主控软件整体框图

需要说明的是，在周期性循环体中，要首先读取编码器的值，来保证严格的等间隔采样。

后记                

如果大家对博文的相关类容感兴趣，或有什么技术疑问，欢迎加QQ技术交流群（117698356）

参考文献

[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.

 购书链接：https://item.jd.com/13041503.html

下载更多资料：www.xiihoo.com

QQ技术讨论群： 117698356

B站视频教程：https://space.bilibili.com/66815220

Github源码：https://github.com/xiihoo/Books_Robot_SLAM_Navigation

Gitee源码（国内访问速度快）：https://gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

序

前言

编程基础篇

第1章 ROS入门必备知识

1.1 ROS简介 2

1.1.1 ROS的性能特色 2

1.1.2 ROS的发行版本 3

1.1.3 ROS的学习方法 3

1.2 ROS开发环境的搭建 3

1.2.1 ROS的安装 4

1.2.2 ROS文件的组织方式 4

1.2.3 ROS网络通信配置 5

1.2.4 集成开发工具 5

1.3 ROS系统架构 5

1.3.1 从计算图视角理解ROS架构 6

1.3.2 从文件系统视角理解ROS架构 7

1.3.3 从开源社区视角理解ROS架构 8

1.4 ROS调试工具 8

1.4.1 命令行工具 9

1.4.2 可视化工具 9

1.5 ROS节点通信 10

1.5.1 话题通信方式 12

1.5.2 服务通信方式 15

1.5.3 动作通信方式 19

1.6 ROS的其他重要概念 25

1.7 ROS 2.0展望 28

1.8 本章小结 28

第2章 C++编程范式

2.1 C++工程的组织结构 29

2.1.1 C++工程的一般组织结构 29

2.1.2 C++工程在机器人中的组织结构 29

2.2 C++代码的编译方法 30

2.2.1 使用g++编译代码 31

2.2.2 使用make编译代码 32

2.2.3 使用CMake编译代码 32

2.3 C++编程风格指南 33

2.4 本章小结 34

第3章 OpenCV图像处理

3.1 认识图像数据 35

3.1.1 获取图像数据 35

3.1.2 访问图像数据 36

3.2 图像滤波 37

3.2.1 线性滤波 37

3.2.2 非线性滤波 38

3.2.3 形态学滤波 39

3.3 图像变换 40

3.3.1 射影变换 40

3.3.2 霍夫变换 42

3.3.3 边缘检测 42

3.3.4 直方图均衡 43

3.4 图像特征点提取 44

3.4.1 SIFT特征点 44

3.4.2 SURF特征点 50

3.4.3 ORB特征点 52

3.5 本章小结 54

硬件基础篇

第4章 机器人传感器

4.1 惯性测量单元 56

4.1.1 工作原理 56

4.1.2 原始数据采集 60

4.1.3 参数标定 65

4.1.4 数据滤波 73

4.1.5 姿态融合 75

4.2 激光雷达 91

4.2.1 工作原理 92

4.2.2 性能参数 94

4.2.3 数据处理 96

4.3 相机 100

4.3.1 单目相机 101

4.3.2 双目相机 107

4.3.3 RGB-D相机 109

4.4 带编码器的减速电机 111

4.4.1 电机 111

4.4.2 电机驱动电路 112

4.4.3 电机控制主板 113

4.4.4 轮式里程计 117

4.5 本章小结 118

第5章 机器人主机

5.1 X86与ARM主机对比 119

5.2 ARM主机树莓派3B+ 120

5.2.1 安装Ubuntu MATE 18.04 120

5.2.2 安装ROS melodic 122

5.2.3 装机软件与系统设置 122

5.3 ARM主机RK3399 127

5.4 ARM主机Jetson-tx2 128

5.5 分布式架构主机 129

5.5.1 ROS网络通信 130

5.5.2 机器人程序的远程开发 130

5.6 本章小结 131

第6章 机器人底盘

6.1 底盘运动学模型 132

6.1.1 两轮差速模型 132

6.1.2 四轮差速模型 136

6.1.3 阿克曼模型 140

6.1.4 全向模型 144

6.1.5 其他模型 148

6.2 底盘性能指标 148

6.2.1 载重能力 148

6.2.2 动力性能 148

6.2.3 控制精度 150

6.2.4 里程计精度 150

6.3 典型机器人底盘搭建 151

6.3.1 底盘运动学模型选择 152

6.3.2 传感器选择 152

6.3.3 主机选择 153

6.4 本章小结 155

SLAM篇

第7章 SLAM中的数学基础

7.1 SLAM发展简史 158

7.1.1 数据关联、收敛和一致性 160

7.1.2 SLAM的基本理论 161

7.2 SLAM中的概率理论 163

7.2.1 状态估计问题 164

7.2.2 概率运动模型 166

7.2.3 概率观测模型 171

7.2.4 概率图模型 173

7.3 估计理论 182

7.3.1 估计量的性质 182

7.3.2 估计量的构建 183

7.3.3 各估计量对比 190

7.4 基于贝叶斯网络的状态估计 193

7.4.1 贝叶斯估计 194

7.4.2 参数化实现 196

7.4.3 非参数化实现 202

7.5 基于因子图的状态估计 206

7.5.1 非线性最小二乘估计 206

7.5.2 直接求解方法 206

7.5.3 优化方法 208

7.5.4 各优化方法对比 218

7.5.5 常用优化工具 219

7.6 典型SLAM算法 221

7.7 本章小结 221

第8章 激光SLAM系统

8.1 Gmapping算法 223

8.1.1 原理分析 223

8.1.2 源码解读 228

8.1.3 安装与运行 233

8.2 Cartographer算法 240

8.2.1 原理分析 240

8.2.2 源码解读 247

8.2.3 安装与运行 258

8.3 LOAM算法 266

8.3.1 原理分析 266

8.3.2 源码解读 267

8.3.3 安装与运行 270

8.4 本章小结 270

第9章 视觉SLAM系统

9.1 ORB-SLAM2算法 274

9.1.1 原理分析 274

9.1.2 源码解读 310

9.1.3 安装与运行 319

9.1.4 拓展 327

9.2 LSD-SLAM算法 329

9.2.1 原理分析 329

9.2.2 源码解读 334

9.2.3 安装与运行 337

9.3 SVO算法 338

9.3.1 原理分析 338

9.3.2 源码解读 341

9.4 本章小结 341

第10章 其他SLAM系统

10.1 RTABMAP算法 344

10.1.1 原理分析 344

10.1.2 源码解读 351

10.1.3 安装与运行 357

10.2 VINS算法 362

10.2.1 原理分析 364

10.2.2 源码解读 373

10.2.3 安装与运行 376

10.3 机器学习与SLAM 379

10.3.1 机器学习 379

10.3.2 CNN-SLAM算法 411

10.3.3 DeepVO算法 413

10.4 本章小结 414

自主导航篇

第11章 自主导航中的数学基础

11.1 自主导航 418

11.2 环境感知 420

11.2.1 实时定位 420

11.2.2 环境建模 421

11.2.3 语义理解 422

11.3 路径规划 422

11.3.1 常见的路径规划算法 423

11.3.2 带约束的路径规划算法 430

11.3.3 覆盖的路径规划算法 434

11.4 运动控制 435

11.4.1 基于PID的运动控制 437

11.4.2 基于MPC的运动控制 438

11.4.3 基于强化学习的运动控制 441

11.5 强化学习与自主导航 442

11.5.1 强化学习 443

11.5.2 基于强化学习的自主导航 465

11.6 本章小结 467

第12章 典型自主导航系统

12.1 ros-navigation导航系统 470

12.1.1 原理分析 470

12.1.2 源码解读 475

12.1.3 安装与运行 479

12.1.4 路径规划改进 492

12.1.5 环境探索 496

12.2 riskrrt导航系统 498

12.3 autoware导航系统 499

12.4 导航系统面临的一些挑战 500

12.5 本章小结 500

第13章 机器人SLAM导航综合实战

13.1 运行机器人上的传感器 502

13.1.1 运行底盘的ROS驱动 503

13.1.2 运行激光雷达的ROS驱动 503

13.1.3 运行IMU的ROS驱动 504

13.1.4 运行相机的ROS驱动 504

13.1.5 运行底盘的urdf模型 505

13.1.6 传感器一键启动 506

13.2 运行SLAM建图功能 506

13.2.1 运行激光SLAM建图功能 507

13.2.2 运行视觉SLAM建图功能 508

13.2.3 运行激光与视觉联合建图功能 508

13.3 运行自主导航 509

13.4 基于自主导航的应用 510

13.5 本章小结 511

附录A Linux与SLAM性能优化的探讨

附录B 习题