全局路径约束的两轮差速机器人改进导航算法

赵师兵， 张志明

（同济大学电子与信息工程学院，上海 200092）

0 引 言

随着人工智能的快速发展，研究机器人在不同场景下的落地应用，已成为国家和社会未来重点关注和发展的方向之一［1-3］。以人工智能和机器人为切入点，面向新工科的本科教育建设已成为热点［4-6］。从人工智能＋机器人专业方向人才培养需求出发，教学实践中涉及跨专业的多学科领域，具有理论和工程实用性兼备的特点［4-6］。

导航技术告诉机器人该如何移动，是服务机器人研究领域的重点内容［1，7］。Leonard 把机器人导航的相关问题表述为对“我在哪”“我要去哪”及“我如何到达那里”3 个问题的回答［8］，其中“我在哪”指的是机器人定位问题，“我如何到达那里”是机器人的路径规划问题。基于地图的路径规划是机器人导航核心技术之一，机器人在获得与环境相关的地图信息后进行路径规划，通过本体运动控制单元在规划路径指引下实现自主导航［7-9］。室内静态环境下常使用忽略高度信息后的二维栅格地图，将整个环境分为若干相同大小的栅格单元。路径规划即要求机器人根据某种优化准则，在地图上寻找一条代价最小的路线，实现从起点行驶到终点的任务，并且需避开障碍物。

两轮差速底盘的建模与控制都较为简单，也是室内服务机器人常采用的平台架构［3，9］。本文以两轮差速机器人为应用对象，分析其在经过障碍物情况下的狭窄路径规划和存在极端路径情况下的运动，提出一种基于全局路径约束的导航路径规划算法优化方案，解决两轮差速机器人在室内环境导航过程中出现的典型问题，如狭窄路径无法通行、靠近障碍物后停止运行等。

1 室内环境导航问题描述

1.1 差速机器人运动模型

在室内环境导航中，通常把机器人建模为理想质点模型，涉及世界坐标系、里程计坐标系和机器人坐标系。机器人坐标系随着机器人移动，全局路径是在世界坐标系下进行解算，在导航过程中需要将机器人的速度信息转换到世界坐标系下［9-10］。本文所用的两轮差速机器人采用分层方式运动控制，如图1 所示，下位机负责接收来自上位机的线速度和角速度控制指令并转化成左／右轮的转速，通过比例-积分-微分（Proportional-Integral-Derivative，PID）控制器调节PWM信号占空比输出给左／右轮驱动电动机，使左／右轮稳定为期望转速运行。

图1 两轮差速机器人运动控制框图

1.2 路径规划

机器人的路径规划可分为全局路径规划和局部路径规划［8-10］。通过全局代价地图规划全局路径，再将全局路径作为一种约束并结合局部代价地图规划局部路径，是一种成熟且通用性较好的做法［11-13］。

全局路径规划有A*、Dijkstra 等经典算法及其改进算法［11-13］。常用的局部路径规划算法中，动态窗口法（Dynamic Window Approach，DWA）的前瞻性不足，且非全局最优，多用于全向模型［14］；时间弹性带（Time Elastic Band，TEB）算法计算复杂度大，且控制不稳定，速度和角速度波动较大［15］；另外两者都有参数烦多调节不易的问题。跟随萝卜法（Follow the Carrot，FTC）会尽可能地贴近全局路径进行局部规划［16］，适用于差速机器人和小房间情况，但无法后退，避障效果差。

1.2.1 加速度参数

DWA算法和TEB 算法中引入有难以确定的参数，如加速度。在差速机器人运动控制过程中，没有对驱动电动机进行动力学建模，只涉及运动学而没有动力学中的加速度控制，算法直接控制速度。室内环境下机器人实际运动速度不快，理论计算和实际测试表明，速度环控制能很快地稳定，调节时间较短，而直接控制加速度比较困难，故算法中加入加速度参数意义不大。

1.2.2 原地自转

DWA和TEB算法并未充分利用差速机器人原地旋转的优势来规划路径。DWA 算法的评价函数与角速度相关，即使在需要自转时，当线速度存在时总会得到更高的评分，所以DWA 算法无法得到原地自转的效果。TEB算法可设置转弯半径（差速机器人的转弯半径为零），但在实际使用时，在刚出发情况下，算法不能规划出转弯半径为零的路径（原地自转），在导航过程中，由于机器人线速度不为零，机器人转弯半径也不能设置为零。TEB 算法的另一个缺点是要求机器人到达目标点的同时方向指向目标，因此会在目标点附近进行调整，可能会导致状态震荡。

1.3 狭窄路径与极端路径的室内环境

室内移动机器人的实际应用环境中，经常会遇到存在狭窄路径和极端路径的情况，如图2 所示，黑色实线为固定的墙体。机器人分别在3 个不同的起点S1、S2和S3出发，初始方向如图中箭头所示，终点相同均为E0。

图2 大幅转弯的狭窄路径和极端路径示意图

机器人导航模块规划出3 条路径。S1～E0路径有一个近180°的转弯，机器人在行驶到转弯处时往往会因为运动惯性偏离预设路径。由于局部路径规划用DWA和TEB算法都是速度控制算法，含有速度和加速度约束，其初衷是使机器人速度平滑不会突兀归零，同时保护驱动电动机，故会控制机器人在有一定线速度的情况下进行急转弯（非原地转），以尽快恢复回到路径。由于加速度和速度的约束，机器人可能会与墙壁或障碍物碰撞。急转弯动作会导致里程计误差增大，影响机器人的准确定位。

S2～E0路径理论上的最优方案应该是绿色路径，静止启动机器人先自转后再行驶，但实际规划结果是红色路径，仍需要拐近180°的大弯，同样会出现转弯惯性问题。分析TEB算法，到达目标点的同时要求规定朝向，机器人在启动后即进行转弯动作；在DWA 算法中，评价函数由速度、朝向和障碍物3 部分组成，由于起点S2所在的环境开阔，且初速为零，影响轨迹主要因素由速度评价函数决定，速度越大得分越高，因此局部规划器更倾向于拐大弯而非原地自转。

S3～E0路径中，由于起点S3至终点E0所途径的周边环境较为狭窄，障碍物评价函数起主导作用，此时机器人启动时可先做原地自转，再按预设路径行驶。在狭窄路径中，机器人与障碍物距离很近，需要机器人在路径中心线附近行驶以避免碰撞。

在大幅转弯的狭窄路径和极端路径的环境中，常常还伴随有未在静态地图上出现的静态障碍物和动态障碍物，路径离障碍边界过近（特别是转弯处），需考虑机器人本身物理尺寸因素，因此防碰撞与恢复也是机器人导航中需要考虑的实际问题。

2 算法实现

差速两轮机器人全局路径约束的导航核心算法包括：全局路径的平滑和机器人的导航，算法框架如图3所示。其中，导航分为3 个核心部分：机器人自转控制、机器人路径跟随和遇到障碍时的避障和恢复。机器人自转控制是在导航前将机器人旋转到路径的朝向和路径跟随结束后将机器人旋转到目标朝向；机器人的路径跟随为使用纯跟踪控制算法（Pure Pursuit，PP）进行轨迹跟踪［17］；机器人的避障可通过全局规划路径规划进行，机器人的恢复为根据障碍物的位置和非障碍物区域的位置对机器人进行控制调整到正常工作状态。

图3 两轮差速机器人改进导航算法框架图

2.1 全局路径平滑及平滑处理

全局路径规划采用Dijkstra 算法，计算量较大，但可得到最优路径，同时避免A*算法过于贪婪无法走出U型区域的缺陷。该算法在栅格地图上规划得到路径，当机器人通过狭窄路径，或靠近膨胀区域行驶时，由于机器人激光雷达扫描的障碍物距离存在误差，膨胀区域边缘会扩展或收缩几个栅格点，此时规划的路径就会出现大量往斜方向运动并频繁转向的短路径，整体看来该部分的路径出现锯齿状，平滑程度较差。借鉴文献［17，19］中的方法，引入样条插值函数对路径进行平滑处理，消除尖角和往复路径，其效果如图4 所示。原路径在180°U型大转弯处和部分直线处平滑程度不佳，平滑后路径直线部分的平滑度提高明显，大转弯部分也有较大改善。

图4 路径平滑处理效果图

当路径上每一个点都考察完后，如果此次路径上的点有变动，则从当前位置开始重新再次进行优化，直到一次优化过程中路径上的点不出现变动或超过最大优化次数。

2.2 核心算法实现

改进算法的核心功能为机器人的自转控制、路径跟随和恢复。机器人自转采用位置式PID 控制，路径采用纯跟踪算法，避障和恢复则根据遇到障碍时的障碍物区域和非障碍物区间的位置来控制。

2.2.1 机器人自转控制

PID控制是一种常用的反馈控制器，由比例、积分和微分项组成，常见的形式有位置式和增量式两种。位置式PID控制的优点是静态误差小，溢出影响小，缺点是计算量大，累计误差相对较大，一般结合输出限幅和累计积分清零使用。增量式PID控制的优点是运算量相对较小，且输出与3 次误差有关，当系统出现错误时影响相对较小，缺点是因为没有累计误差，因此会出现静态误差。

不论是导航前将机器人旋转到路径的朝向还是在结束后将机器人旋转到目标朝向，目的都是使机器人的朝向与目标朝向一致，重要的是要尽量减小静态误差，故采用位置式PID进行自转控制。ROS 中可通过订阅里程计话题／Odometry／filtered 获取机器人的位姿，目标朝向与机器人朝向做差后得到角度差err。比较，选择绝对值较小的值作为控制器的角度误差输入，计算机器人旋转角速度并对角速度输出进行限幅。

2.2.2 导航路径跟随

使用全局路径作为被跟踪路径的优势在于：由于全局路径是实时计算的，每次计算全局路径时都会使用机器人当前坐标点进行计算，有延时时仍可认为机器人在路径上，没有横向误差，因此只需计算机器人方向误差控制量。本文采用纯路径跟随算法控制机器人方向运动，相关原理如图5 中所示。

图5 纯跟踪算法原理

图中：点（x，y）为路径上的预瞄点；θ 为机器人在前进与机器人方向的夹角；d为与机器人的距离。机器人到达预瞄点的路径可近似看成一段圆弧［20］，该段圆弧的半径

机器人行驶的线速度

路径预瞄点坐标（x，y）可从全局路径中获取，再变换到odom坐标系下，距离d和θ夹角可通过机器人odom坐标系下的位姿跟踪坐标和先前的坐标进行计算。预瞄点的选取与前馈距离有关，d和θ 都由预瞄点决定，影响式（1）的独立变量只有速度和前馈距离，相对于TEB 算法需要调节81 个参数，纯路径跟随算法更加简洁与方便。

室内两轮差速机器人一般是在低速环境下行驶，机器人的线速度和前馈距离可保持一个定值，即使环境中存在没有出现在地图上的静态障碍物和动态障碍物，激光雷达传感器也能及时扫描到并放入代价地图，全局路径规划器会重新规划路径。

2.2.3 避障与恢复机制

纯跟踪算法可获得较好的跟踪轨迹，而ROS 导航包计算的全局路径自带避障功能，故在有静态障碍物和简单动态障碍物的环境中，二者结合可获得较好的导航效果。面对实际行驶过程中的突发情况，机器人需要一定的策略恢复到正常运动状态。

一种情况是前方突然出现障碍物时，机器人规划的全局路径与机器人前进方向夹角过大，或者此时需要机器人后退导致二者方向相反；当机器人与障碍物距离过近时，可能会出现无法计算规划出全局路径的情况。

为应对第1 种情况，可在机器人行驶过程中计算路径方向与前进方向的夹角，当夹角过大（如超过60°）时，机器人暂停路径跟随，原地自转恢复到路径方向，再恢复到正常行驶。对于第2 种情况，当检测到无法规划路径时，可以根据局部代价地图，计算机器人邻域8 个方向地图代价的下降速度，选择代价下降最快且最接近机器人朝向直线的方向，控制机器人前进或后退往该方向行驶，直到机器人坐标的地图代价值为零后再重新导航。

2.3 算法涉及的参数

本文所提算法的参数涉及全局路径、机器人旋转、机器人路径跟随和机器人恢复，见表1。全局路径部分的参数主要作用是提高路径的更新频率，以增强机器人的避障能力；机器人旋转和导航相关参数的作用在原理部分已经详细说明，这里不再赘述；机器人恢复相关参数主要作用是将机器人的方向调整为和路径方向一致，避免出现机器人角度大幅度偏移路径的情况。

表1 本文算法需要调整的参数

3 实验测试与结果分析

测试环境为：Ubuntu18.04 操作系统，Intel 2.60 GHz i7-9750H CPU，16GB内存，仿真实测编程环境为ROS Melodic 和Gazebo9.0。机器人操作系统（Robot Operating System，ROS）能极大的简化机器人开发过程，已广泛应用到机器人领域。分别在小型场地和大型场地的Gazebo仿真环境下验证导航算法的有效性，并与目前常用的局部路径规划算法进行对比。

仿真测试使用的机器人是Turtlebot差速移动机器人，机器人自带的传感器为Kinect 深度相机，不同算法测试过程中均使用相同的全局规划器Global Planner，里程计数据来源于Gazebo 自带的模型里程计，全局定位数据通过ROS 功能包的AMCL 算法提供。关键参数的参考值已在上表1 中列举。

3.1 小型场景导航实验

3.1.1 场景介绍

小型场地布置为7.2 m×7.2 m的室内仿真环境，如图6 所示，以实体墙分隔开不同空间，检测不同算法的导航和避障性能。小型场地较为狭窄，不设置动态障碍物，锥桶作为静态障碍物分布在场地内不同位置，保证机器人能够在锥桶和墙体之间通行。导航目标是让机器人从起点出发行驶到终点，多次实验观察机器人的导航完成度，并比较不同算法的行驶路径、运行时间、不同速度下的行驶情况。

图6 不同算法下导航效果对比

3.1.2 算法导航效果对比选取3 种目前最常用的局部路径规划算法（TEB、DWA、MPC）和本文所提算法做比较。其中TEB 和DWA 算法分别使用teb_local_planner 和dwa_local_planner功能包实现，模型预测控制（Model Predictive Control，MPC）算法将本文所提算法中的路径跟随控制器从PP算法替换为MPC控制器。设置机器人参考速度为0.5 m／s，每种算法在相同仿真环境中各运行10 次，记录机器人行驶路径，在图6 中展示不同算法下机器人行程最远一次的路径，在图7 中给出导航过程中机器人位姿的线速度变化。图6 中黄线为机器人实际行驶路径，黄线上的红色箭头指向为机器人行驶到该位置时机器人的实际朝向，部分路径上箭头比较密集，说明机器人在该处停留的时间较久。

图7 导航过程中机器人线速度变化

如表2 所示为记录机器人成功到达终点的次数、导航过程中碰撞到障碍物的次数等结果。

表2 导航过程中的数据记录

3.1.3 算法效果分析

分析图6 中机器人成功导航的行驶路径，本文所用算法与TEB、MPC 算法的结果相差不大，都能使机器人导航到终点，而DWA算法效果欠佳。DWA 算法效果分图中，箭头较密集的路径集中在180°转弯路径范围和障碍物附近的路径，说明DWA 算法在遇到极端路径和存在障碍物时的路径时导航效果不是很好。而MPC算法在正常导航的转弯过程中多次使用原地自转方式来修正行进路线，行驶过程中实际效果不如本文算法和TEB算法那样稳定。

由图7 可见，导航过程中机器人路径跟随线速度变化的效果相差较大。本文算法的速度变化较为稳定，与参考速度基本保持一致；TEB 算法在大部分采样点上速度稳定，但在转弯路径和障碍物较多的路径上速度不稳定；DWA和MPC算法的速度稳定性较差，速度变化波动幅度较大。

由表2 可见，本文算法最佳，导航成功率为100%，且碰撞次数为零；MPC 算法次之，导航成功率为100%，碰撞次数为1；TEB 算法的导航成功率为90%，碰撞次数为2，略低于MPC 算法和本文算法；DWA算法导航效果最差，成功率只有10%，且碰撞次数为最多的16 次。

结合实验结果可见，在没有动态障碍物的狭窄场景中，本文算法能使机器人速度保持稳定，且碰撞次数少，计算量也较小，机器人的导航效果最好。

3.2 大中型场景下导航实验

大中型场景布置为55 m×25 m的医院环境，该环境中有病房、病床、桌椅、柜台、轮椅和清洁车等元素，并添加位置固定的候诊人员作为静态障碍物，环境空间较为空旷，但环境中元素较为复杂。机器人的行驶路线为：从地图上方的起点S 开始，依次进入3 个病房，其顺序如下，先进入地图左侧中央的大病房，然后通过中间过道进入右侧中央大病房，接着进入地图左下方的病房，最后回到起点。图中的蓝色实线为机器人行驶的实际路线，红色箭头指向为机器人行驶到该位置的实际朝向，箭头越密集说明机器人在该段路径停留的时间越长。

由图8 可见，DWA算法在进入第一个病房的180°拐弯处时即与墙壁发生碰撞而未能完成导航，另外3种算法导航效果差距不大，都能完成导航，且行驶路径较为平滑，且能避开障碍物，可顺利通过狭窄的路径、90°弯道和180°弯道等区域中的转弯。

图8 大中型场景下导航效果

从对比实验可以得出结论，在大的开阔环境中，本文所用算法、TEB和MPC算法的导航情况都能获得较好的效果，而DWA 算法在遇到180°的弯道时效果较差，会与障碍物发生碰撞。

4 结 语

针对差速两轮机器人本身的物理运动特性，设计一种基于全局路径约束的导航改进算法，实现机器人自转控制、路径跟随和遇到障碍时的避障和恢复。在极端路径和狭窄路径的小环境中，相较于DWA 和TEB算法，本文所提算法应用于路径规划和导航时效果更优，导航路径更安全平滑，可有效降低因速度跳变导致的行驶震荡幅度，机器人行驶过程更加平稳。此外，在大型开放环境中，本文所提算法的效果也优于DWA算法，与TEB和MPC 算法的效果相差不大。该算法可方便地部署到机器人教学实践平台，有助于培养本科生理论结合实践的工程能力。