基于模型预测控制的移动机器人户外导航方法

郭明阳， 刘 爽

（ 华东理工大学机械与动力工程学院，上海 200237）

户外巡检、户外保洁、智能农业等应用的广泛兴起使得移动机器人的使用场景由室内转向户外。导航技术是自主机器人研究的核心，目前广泛应用于移动机器人的室内导航系统为机器人操作系统（Robot Operating System，ROS）开源框架：Gmapping和Navigation。Gmapping 是基于粒子滤波算法的激光同步定位与建图（Simultaneous Localization and Mapping，SLAM）方案，能有效利用车轮里程计信息，根据激光设备的输入和姿态数据建立一个基于网格的2D 地图。此方法在构建小场景地图时所需的计算量小且精度较高，但是随着场景增大所需的粒子增加，内存和计算量都会随之增加[1]；此外Gmapping没有回环检测，在回环闭合时会造成地图错位[2]。Navigation 是ROS 的二维导航功能包，导入地图后根据机器人当前位置和目标位置，通过Dijkstra 算法进行全局路径规划，行进过程中使用动态窗口法计算机器人每个周期内应该行驶的线速度和角速度，但是动态窗口法在机器人穿越稠密障碍物时易出现从密集障碍物区域外绕行、轨迹不平滑的问题[3]。

目前在户外导航方法的研究中，常用方法有使用传感器捕获道路信息、在道路上预先布置路径线或者使用深度学习识别道路参数，然后再进行相应的控制以实现自主导航。Asami 等[4]通过摄像头采集机器人周围道路图像，然后使用Canny 算法提取路段信息用于导航。该方法易受光照强度影响，而且当路面存在条状喷涂线或裂痕时将直接影响路段信息提取效果。Zuo 等[5]使用摄像头识别道路内的引导线，并使用PID 控制器控制车辆运动以保持引导线图像在视窗中间区域，实现实时跟踪。与激光导航相比，该方法更具实用性和可靠性，但光强对系统有较大影响，需要预先在道路上设置引导线和标识，灵活性欠佳。Shin 等[6]和Jin 等[7]均使用单个激光测距仪实时检测路缘来提取可行驶区域。该方法能有效识别弯道和障碍物，但在宽阔平坦的道路上很难准确识别路缘特征，进而影响导航效果。García 等[8]使用街道地图和行车视频对基于机器学习的路况识别模型进行训练，该模型可实时识别道路的几何形状和尺寸，由此估计机器人在直行道路和交叉路口所应执行的车速和转向角。该方法具有一定的灵活性，但无法应对道路上的突发情况，复杂环境下容易出错。可见，户外导航仍然面临各种挑战。

机器人导航的关键技术包括环境感知、运动决策、轨迹规划、轨迹跟踪、人机交互等。在这些关键技术中，机器人的轨迹规划和轨迹跟踪是较为重要的两项，在户外导航的安全性和鲁棒性方面也发挥着重要作用。目前主要的轨迹跟踪控制方法包括各种改进的PID 控制、反步控制、滑模控制、模型预测控制等。已有大量文献表明PID 控制在机器人控制方面取得了成功，自适应巡航控制[9]（Adaptive Cruise Control，ACC）和 车 道 保 持 控 制[10]（Lane Keeping Control，LKC）可以使用PID 控制器实现良好的性能。然而PID 控制器的参数较难确定，对于复杂扰动的表现稍显不足。反步控制法要求被控对象具有严格反馈形式或者能够转化为严格反馈形式，在设计过程中还存在子系统Lyapunov 函数不易设定的问题[11]。滑模控制在设计的过程中滑模函数及其参数难以确定，在系统到达阶段还容易受到外部扰动而出现抖振问题[12]。模型预测控制（Model Predictive Control，MPC）是基于预测模型的控制算法，利用已知的参考轨迹和当前时刻的状态信息，通过内部模型预测系统在未来时刻的输出状态，根据给定的二次型性能指标函数和约束条件滚动地求解最优控制序列。此外，模型预测控制算法在处理输入和状态等多条件约束方面有其独特的优势，非常适用于移动机器人轨迹跟踪控制[13-14]。

在关于移动机器人控制器的文献中，都认为所跟踪的参考轨迹是已知的，这里的轨迹可以是全局轨迹也可以是局部轨迹。对于户外机器人，环境交通流量动态多变，不可控因素多，所跟踪的应是行驶期间实时规划和优化的局部避障轨迹。当前在局部规划领域已经有了大量的成功经验[15-17]。基本思想是在起点和终点之间建立局部地图，然后通过启发式搜索算法获得一系列相邻网格到达目标点。Dijkstra 算法及A*算法是最常见的路径搜索算法，其中A*算法除了简单高效外，还具有可扩展性强的优点，许多团队使用A*及其改进算法在实际应用中取得了很好的效果[18-19]。

针对户外导航对实时性、安全性和灵活性的需求，本文设计了一种“感知建图-A*规划-模型预测控制（MPC）”移动机器人户外导航方法，有效提高了轨迹规划的稳定性和轨迹跟踪的精度，与传统方法相比效率更高、具有更强的环境适应力和稳定性。

1 轨迹规划

轨迹规划部分涉及局部地图构建、规划终点的计算和基于路段走向改进A*算法这3 部分，最终为模型预测控制器提供具有避障作用的局部轨迹。

1.1 地图构建

栅格化是目前最常用的环境建模方法之一，栅格地图容易构建、表示和保存，对于短路径的规划也十分方便，因此局部地图采用八向栅格图。LiDAR理论最大探测半径为200 m，此处建图半径为10 m，栅格单元大小为10 cm × 10 cm。基于实时更新策略，将非地面点云数据投影到机器人坐标系（机器人当前位置为原点O，前方为X轴，左侧为Y轴，上方为Z轴）XOY平面，然后对所形成的栅格边界做1 m的膨胀处理，以补偿机器人外形尺寸对避障的影响。最终所构建的局部栅格图如图1 所示。

图1 局部栅格图Fig. 1 Local grid map

1.2 规划终点

规划涉及到的坐标系除机器人坐标系外还有大地坐标系（机器人初始位置为原点O，东方为X轴，北方为Y轴，上方为Z轴）和里程计坐标系（机器人初始位置为原点O，前方为X轴，左侧为Y轴，上方为Z轴），栅格地图建立在机器人坐标系下，而航迹点处于大地坐标系下，因此需要将航迹点转换至机器人坐标系。

此处定义航迹点由大地坐标系转换到机器人坐标系的转换关系为T，它由姿态变换角度p和平移量q组成，如式（1）所示。

其中：x、y、z分别表示大地坐标系相对机器人坐标系在X轴、Y轴、Z轴的平移量； θ、 φ、 φ分别表示大地坐标系相对于机器人坐标系绕X轴、Y轴、Z轴的旋转角，这3 个角度统称为欧拉角。

其中：R表示由欧拉角组成的旋转矩阵，这里忽略Z方向平移量，并认为 θ 、 φ 均为0，则

航迹点在机器人XOY坐标系下表示为

如图2 所示，连接G与原点Orobot的线段，与以探测范围为半径的圆交于M，则Orobot、M分别为每次局部路径规划的起点和终点。当G处于雷达探测范围以内时，则取G为规划的终点。

图2 局部规划起点和终点Fig. 2 Starting point and ending point of local planning

1.3 改进A＊算法

A*算法原估价函数如下：

A*启发函数如图3 所示，f(s) 为节点Snow的总启发式代价值，g(s) 代表由规划起点Sstart至当前检索点Snow的连通代价值，h(s) 代表由当前检索点Snow至规划终点Sgoal的启发式代价值。

图3 A*启发函数Fig. 3 A* heuristic function

A*算法总是选取具有最小总启发式代价f(s) 的待扩展节点，不断向终点方向搜索，直至到达终点，基于A*的改进算法一直是路径规划领域的研究热点。此处使用路段走向对A*搜索方向进行约束，使得路径结果尽量与路段平行[20]，以减少机器人不必要的转向，保证运行的稳定性。路段方向和搜索方向如图4 所示，Droad为由航迹点计算得出的路段方向向量，Dsearch为由子节点与父节点计算得出的方向向量， α 为两者夹角。

图 4 路段方向与搜索方向Fig. 4 Road direction and search direction

约束后的启发式代价值为

其中：w为夹角约束项权重。

A*搜索结果为一栅格序列，此处从中选取6 个路径点，然后结合机器人当前瞬时速度对路径点分段拟合[21]，使得整体轨迹平滑流畅，便于机器人跟踪。

2 模型预测控制器

轨迹跟踪控制器是机器人导航控制的一项核心技术，任务是在得到期望轨迹后，控制机器人跟踪这条轨迹从当前位姿点到达目标位姿点，并尽可能减小运行过程中机器人与期望轨迹之间的误差。模型预测控制可以有效处理各种约束问题，其迭代步骤最终保证最小的误差和最佳的性能，在实现控制指标最优的同时具有一定的鲁棒性。模型预测控制的基本原理描述如图5 所示。

图5 模型预测控制原理图Fig. 5 Model predictive control diagram

模型预测控制器算法的实现由构建预测模型、构建目标函数、滚动优化和反馈矫正[22]这4 部分组成。

2.1 预测模型

对于差速移动机器人，机器人的状态量可由X=[x,y,δ]T表示，其中(x,y)表示机器人在地图中的位置， δ 表示机器人的航向角，如图6 所示。

图6 机器人位姿Fig. 6 Robot pose

移动机器人的运动学模型可表示为[23]

其中：U=[v,w]T表示机器人的控制量；v代表线速度；w代表角速度。在时刻t=kT0（其中T0为预测步长，k∈{0,1,2,3,···} ）离散化后为

将式(8)、(9)抽象化为

记预测步数为Np，控制时域为Nc，则在预测时域内机器人的状态预测模型可表示为

2.2 目标函数

设机器人参考轨迹为

在当前时刻t，以T0为间隔依次在参考轨迹上取Np个 参 考 点X=f(k+i*T0)(i=1,2,3,···,Np) ，记为Xref(k+i|k)(i=1,2,3,···,Np) 。同时考虑预测值与参考值的跟踪误差、控制量和控制增量，为了使三者都尽可能小，目标函数可表示为

式中：W1、W2、W3分别为跟踪误差惩罚权重、控制量惩罚权重、控制量增量惩罚权重。其中跟踪误差惩罚权重的作用是尽可能减少跟踪误差；控制量惩罚权重的作用是保证控制策略更经济；控制量增量惩罚权重的作用是保证控制器较为平稳地控制机器人跟踪参考轨迹，避免频繁地加减速和转向。

2.3 滚动优化

在依据实际情况对机器人状态、控制量及控制量增量设定上下限后，可以将多约束下的模型预测控制问题描述为[24]

为了得到k时刻控制时域Nc内的最优控制序列Uk，采用二次规划方法进行求解，即将带约束的二次最优问题转化为二次规划问题进行数值求解，在每一个控制周期滚动求解后可得到控制序列

2.4 反馈矫正

预测控制中，采用预测模型对系统输出进行估计只是一种思想方式，控制量作用于移动机器人后，由于实际系统存在时变、非线性以及干扰等一些不确定的要素，使得基于固定模型的预测与实际不可能完全一致，此时就需要采用反馈来对模型进行校正，即将实际输出值和预测值做差，再利用此误差来修正模型的预测值，这样才能更好地克服系统的干扰及不确定性。

定义控制时域内误差修正项为ξ(U(i|k))(i=1,2,3,···,Nc)，则修正后的预测模型可表示为

每个控制周期内重复滚动优化和反馈矫正部分，并将求解得到的控制序列中第一个元素U(k|k) 作用于控制对象，即可控制机器人沿局部轨迹行进，实现轨迹跟踪功能。

3 实验与结果分析

为了验证该方法的可行性，在仿真和户外环境下分别使用本导航方法和ROS 开源的Gmapping &Navigation 导航方法进行了对比实验。

Gazebo 是一款3D 动态模拟器，能够准确有效地模拟机器人的物理特性。Rvi 是ROS 针对系统的可视化需求，为用户提供的一款三维可视化工具，以帮助开发者实现所有可监测信息的图形化显示。在Gazebo 环境100 m×50 m 的范围内随机生成100 个边长在2～4 m 的块状障碍物，路段起点GPS 坐标（121.431 389，31.15），终点GPS 坐标（121.432 471，31.15），起点与终点距离100 m，最终布局如图7 所示。

图7 仿真环境Fig. 7 Simulation environment

本导航方法设定终点后所生成的期望轨迹和实际行驶轨迹如图8 所示，路段起点至终点生成的轨迹平滑流畅，可有效躲避障碍物，最大跟踪误差出现在轨迹跟踪启动阶段和障碍物附近，平直路段和曲率半径较大的路段，跟踪误差极小。MPC 控制器参数设置如表1 所示。

图8 改进A*规划和MPC 跟踪效果Fig. 8 Improved A* planning and MPC tracking results

表1 MPC 控制器相关参数Table 1 Related parameters of MPC controller

同样环境布局下，在Gmapping 所建立的地图中使用Navigation 功能包进行导航，Dijkstra 规划及动态窗口法跟踪路径如图9 所示。生成的期望路径可有效躲避障碍物，最大跟踪误差出现在障碍物附近的转角处，平直路段跟踪误差较小。

图9 Gmapping & Navigation 导航效果Fig. 9 Gmapping & Navigation results

改进的A*算法和Dijkstra 算法路径规划结果及耗时参数见表2。MPC 和动态窗口法实际跟踪误差如图10 所示。由于Dijkstra 是基于广度优先策略的搜索方法，会有大量冗余的搜索操作，而A*的启发式搜索策略极大提高了搜索效率，使搜索耗时更短。本文在A*基础上加入的路段走向引导信息减少了路径的转向次数，而对路径的长度没有较大影响，这更有利于机器人的户外行驶。由图10 可见，跟踪过程中，动态窗口法稳态误差在1.8 cm 左右，最大跟踪误差为10.3 cm；MPC 法稳态误差在0.5 cm 左右，拐角处误差较大，最大值为6.6 cm，跟踪精度优于动态窗口法。动态窗口法在复杂路况下（图9 中2、3、4 区域）为了保证避障效果而速度较慢，整体任务用时281 s，耗时较长；而MPC 算法的高精度轨迹跟踪特性使得机器人在复杂路况下不必过多损失行驶速度也能保证避障的有效性，任务用时243 s，耗时更短。

表2 规划及耗时参数Table 2 Planning and time consuming parameters

图10 跟踪误差Fig. 10 Tracking error

最后，在室外进行了对比实验。如图11 所示，首先拾取序号为0～6 的7 个GPS 航迹点，导航任务可表示为：0-1-2-3-4-5-6-1-0，机器人运行至航迹点后驻停，旋转至相应角度后继续行进。图示4 处位置设置了障碍物，以测试避障效果。

图11 GPS 航迹点及障碍物Fig. 11 GPS waypoints and obstacles

ROS 开源方案需要预先建立导航环境地图，然后依次发送任务航迹点以实现导航任务，其中建图耗时20'，导航耗时9'24''，整体耗时29'24''。本文导航方法将航迹点转化至局部栅格图后以5 s 为周期不断计算局部避障轨迹，使用模型预测控制器跟踪轨迹以到达指定的航迹点，由于无需预先建立地图，任务整体耗时6'43''，效率明显优于ROS 开源方案。机器人户外实际行驶轨迹如图12所示。

图12 户外导航轨迹Fig. 12 Outdoor navigation trajectory

回放MPC 轨迹跟踪数据包，复现机器人行驶至4 号障碍物时避障场景如图13 所示。避障过程中机器人与障碍物及路缘距离在1 m 以上，保证了导航的安全。

4 结束语

本文设计的“感知建图-A*规划-MPC”移动机器人户外导航方法无需预先建立全局地图，依靠激光雷达实时建立局部地图并规划避障路径，节省了内存空间，实时性强，大大提高了使用的灵活性。针对户外导航提出的基于路段走向改进的A*算法提升了搜索的稳定性，在保证避障的同时减少了机器人行驶过程中的转向次数；所设计的差速模型预测轨迹跟踪控制器适用于所有差速移动平台，跟踪精度高，轨迹跟踪耗时更短。户外实验表明，机器人在收到GPS 航迹点后，能够自主避障依次到达航迹点，行驶过程中与路缘和障碍物距离在1 m 以上，可安全有效地完成导航任务，解决了户外导航过程中的建图、规划、避障和控制等问题，满足户外大场景导航的任务需求。