一起自学SLAM算法：11.4 运动控制

连载文章，长期更新，欢迎关注：

---

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

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

        11.1 自主导航发展简史

        11.2 环境感知

        11.3 路径规划

        11.4 运动控制

        11.5 强化学习与自主导航

第12章-典型自主导航系统

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

---

机器人自主导航涉及SLAM、路径规划、运动控制、环境感知等核心技术，这些技术的大致关系如图11-24所示。其实自主导航问题的本质就是图11-1所描述的那3个问题，首先要知道机器人在哪，然后要知道机器人需要到达的目标在哪，最后就是寻找路径并利用控制策略开始导航。关于目标点，由人或者特定程序触发（比如人通过点击地图上的某个点来告诉机器人应该去哪里；或者语音交互程序接收到某条语音控制指令，然后语音控制指令被转换成地图中的相应目标点发送给机器人；或者某些特殊条件触发目标点，例如机器人电量低时充电桩目标点自动发送给机器人）。总之，对于自主导航来说目标点为外部给定的一个已知量。关于起始点，有SLAM定位模块提供。关于寻路和控制策略则比较复杂，包括全局路径规划、局部路径规划、轨迹规划、轨迹跟踪等过程。

图11-24  自主导航技术组成

在自主导航中SLAM主要扮演着提供全局地图和定位信息两大角色，而SLAM主要有两种工作模式。第一种模式，SLAM先运行建图模式构建好环境地图后将地图保存，接着载入已保存的全局地图并启动SLAM重定位模式提供定位信息。第二种模式，SLAM直接运行在线建图模式，建图过程中直接提供地图和定位信息。

全局路径规划以起始点和目标点为输入，利用全局地图描述的障碍物信息为基础规划出一条从起始点到目标点的全局路径，常见的全局路径规划算法包括Dijkstra、A*、PRM、RRT等。由于全局地图一般为离散静态形式，因此规划出来的全局路径也是离散静态形式。因为全局路径由一个个离散路径点连接而成，并且路径只考虑了静态障碍物信息，所以全局路径无法直接用于导航控制。也就是说，全局路径只能做为导航时的宏观参考。

局部路径规划相当于全局路径规划的细化过程，如图11-25所示。局部路径规划以机器人能感知的局部边界上的全局路径点为局部目标点，利用机器人能感知到的局部动态障碍物信息为基础规划出一条从机器人当前实时位置点到局部目标点的局部路径，局部路径通常为连续动态形式。可以发现局部路径并不与全局路径重合，而是尽量跟随着全局路径。不过局部路径也并不能直接用于导航控制，因为局部路径常常在突然出现的动态障碍影响下变化较大，且机器人实际控制误差使得真实行走路径偏离于局部路径。也就是说，局部路径也只能做为导航时的宏观参考。

图11-25  局部路径规划

轨迹规划相当于局部路径规划的细化过程，如图11-26所示。局部路径只考虑了几何约束，而在局部路径上添加运动学约束和动力学约束后就生成了机器人实际能执行的轨迹，这就是所谓的轨迹规划。理想情况下，可以直接取轨迹规划中的各个轨迹点速度信息做为动作量直接输入到执行器（也就是电机）。不过电机控制误差、路面起伏、轮胎打滑等因素，导致这种开环控制策略很难奏效。

图11-26  轨迹规划

轨迹跟踪相当于轨迹规划的细化过程，如图11-27所示。机器人按照轨迹规划出来的参考轨迹开始执行运动，运动一段时间后发现机器人已经偏离到参考轨迹左边了；这时机器人调整运动方向以便能逼近参考轨迹，但机器人由于惯性在逼近参考轨迹之后立马又偏离到参考轨迹右边了；机器人会根据真实轨迹与参考轨迹的偏差不断调整自身运动，轨迹跟踪其实就是基于误差反馈的闭环控制，真实轨迹始终跟随着参考轨迹左右摆动并不断逼近。

可以发现从全局路径到作用在执行器的动作量，就是一个逐步细化的过程。全局路径规划和局部路径规划统称为路径规划，局部路径规划将全局路径分解成各个小片段逐步细化。局部路径规划和轨迹规划统称为运动规划，轨迹规划其实就是在局部路径规划上添加了运动学约束和动力学约束。轨迹规划和轨迹跟踪统称为运动控制，轨迹规划为轨迹跟踪器提供参考轨迹，而轨迹跟踪器生成动作量实现执行器的最终操控。下面介绍几种比较流行的运动控制算法，即PID、MPC和强化学习。

11.4.1 基于PID的运动控制

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

11.4.2 基于MPC的运动控制

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

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

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

源码仓库

• Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation

• Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

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