章一鸣，姚文广，陈海进

(1.南通大学 江苏省专用集成电路设计重点实验室，江苏 南通，226001；2.傲拓科技股份有限公司，江苏 南京，210012)

路径规划是指机器人根据环境或传感器信息并在一系列约束下，规划一条从起点到目标点的无碰撞路径.随着服务型机器人在智能家居应用中的普及，人们对于减少人为干预、提升服务型机器人自主性提出更高要求，广大学者在该领域进行了大量研究并发展出许多可行算法[1-2].室内环境具有动态随机性的特点，局部路径规划算法可以感知外部环境变化的特点，弥补了全局路径规划算法的不足，因此该算法成为学者的研究重点之一.

Molinos 等[3]提出动态窗口树、运动曲线环境自适应以及多算法融合等方法，以改进动态窗口法（dynamic window approach,DWA）缺乏全局指导、局部最优的缺陷；代婉玉等[4]提出沿边转弯、速度与转向角消息转换机制以及区域分级策略等方法，改进时间弹性带法（time elastic band,TEB）控制不稳定的缺陷.相较于DWA 和TEB，人工势场法[5-6]的数学模型更简单、计算复杂度更小，在实际动态环境中的实时性更优，但存在目标不可达、局部极小值陷阱问题.解决传统人工势场法的局部极小值陷阱问题主要采用如下3 种思路.1）改变势场受力情况[7-8]：通过打破受力平衡状态使机器人脱离陷阱.2）多算法融合[9]：切换其他路径规划算法引导机器人脱离陷阱.3）提前预测陷阱[10]：边移动边对外部环境进行预测，或者在机器人移动前提前堵住易陷入局部极小值陷阱的路径.Yao 等[11]基于思路1）、3），提出黑洞势场强化Q 学习的方法，在原有势场中叠加黑洞势场，通过黑洞力打破部分局部极小值陷阱处的受力平衡，同时通过强化学习使机器人自动学习跳出陷阱.该方法在不同环境下具有极强的适应能力，但额外添加的黑洞势场会使力多重叠加，并且需要预先进行大量训练.Min 等[12]基于思路3），提出经验混合人工势场法，将基础案例推理与人工势场法结合，创新性地将障碍物与线型、角度型2 种内置障碍物模型进行配准，并将过去躲避障碍物的经验存储在经验库中.该方法适用于未知与非结构化环境中实时避障，但无论是进行障碍物的检索与匹配，还是进行避障经验的查找都要耗费大量时间.

为了有效解决局部极小值陷阱，避免使用高性能GPU 进行训练，实现动态环境下快速自主避障，本研究结合3 种思路对传统人工势场法进行改进，提出以静态避障为主、动态避障为辅的双机制切向避障.同时，为了获取在亟需避障时及时避障、无须避障时快速返回最优路径的效果，将基于状态决策进行全局规划模块与局部规划模块的高效结合.

1 改进人工势场法

1.1 传统人工势场法

1986 年Khatib[13]提出人工势场法，该方法实现简单，表达式为

式中：U(p) 为合力势场函数，Uatt(p) 为引力势场函数，Urep(p) 为斥力势场函数，η 为引力增益系数，ρ(p,pgoal) 为机器人与目标点的欧氏距离，k为斥力增益系数，ρ(p,pobs) 为机器人与障碍物的欧氏距离，ρo为障碍物的斥力影响范围.机器人对目标点生成引力，对障碍物生成斥力，对引力与斥力势场函数求解负梯度后即可得到引力与斥力的数值.在势场中，机器人通过叠加引力和斥力的方式得到合力，依据合力的大小与方向判断下一步的移动轨迹.传统人工势场法存在以下缺陷：1）陷入局部极小值陷阱，如图1 所示；2）躲避动态障碍物效果差.

图1 传统人工势场法的局部极小值陷阱Fig.1 Local minimum trap of traditional artificial potential field method

1.2 人工势场法下双机制切向避障机制

1.2.1 陷阱预测 机器人在势场中沿梯度下降方向移动，出现局部极小值陷阱的原因：引力与斥力共线反向，导致合力为0，运动陷入停滞.Hessian矩阵是对二阶导数的矩阵描述，可以用来检查梯度下降方向上的点是不是局部极小值点.本研究将通过Hessian 矩阵在全局地图中预测局部极小值陷阱.

当函数定义在实数域且二阶偏导数连续时，矩阵H为实对称矩阵，进行特征分解：

式中：λ1、λ2为特征值；D=[d1,d2]，其中d1、d2为特征值所对应的特征向量，同时也是标准正交向量.函数的方向二阶导数表示为

在二元函数的临界点处，二阶方向导数的正负仅与特征值的正负相关.当特征值均为正时，Hessian矩阵正定，该点为局部极小值点；当特征值均为负时，Hessian 矩阵负定，该点为局部极大值点.假设选取的位置坐标与目标点的距离为x、与障碍物的距离为y、机器人所受合力势场为U(x,y)，那么合力势场函数表达式为

对式（4）求偏导数组成二阶Hessian 矩阵：

将选取的位置坐标代入式（5）进行特征分解后，依据特征值正负判断当前采样点是不是局部极小值点.

1.2.2 栅格地图预处理 栅格地图的分辨率大小影响地图的精细度.当栅格分辨率设置较大，建图时将丢失部分细节；当分辨率设置较小，需要更多的存储空间和计算资源，但表示的地图信息更加详细.为了减轻当全局地图相对较大时陷阱预测的计算压力，在不影响地图精细度的情况下，在对栅格地图进行缩放处理后再进行陷阱预测，如图2 所示.建图时按图2(a)（分辨率为0.05 m）进行栅格映射.地图预处理时，当20×20 栅格内存在占据栅格时，缩放至10×10 栅格后标注为占据栅格，否则为空闲栅格，从而将20×20 栅格内的不规则障碍物缩放为图2（b）侧1 号占据栅格（分辨率为1 m）.经过缩放后的栅格地图资源占用减少0.75，需要遍历的空闲栅格坐标大大减少.

图2 栅格地图金字塔缩放Fig.2 Grid map pyramid scaling

缩放后，栅格地图的右半部分占据栅格密度较大，相较于空闲栅格密度较大的左半部分更易陷入局部极小值陷阱.为了进一步减少陷阱预测的计算量，通过区域搜索，选取存在局部极小值陷阱概率更高的障碍物密集区域，遍历该区域中梯度下降方向上的空闲坐标点，计算Hessian 矩阵特征值检测是否存在局部极小值陷阱，如图3 所示.以缩放后全局地图原点为起始点构建长度为2t+1的搜索框，按从左向右、从下往上的顺序将地图分成数块小区域，计算每块小区域内障碍物的疏密度：

式中：a、b为占据概率与非连通障碍物占比的权重系数，NG为搜索框中占据栅格的数量，RG为栅格分辨率，NNO为搜索框中非连通障碍物的数量，NT为搜索框中连通与非连通障碍物的总数.连通障碍物的判定条件如下.条件1：缩放后的栅格为占据栅格.条件2：缩放后占据栅格的八邻域栅格内存在占据栅格.定义同时满足2 个判定条件的障碍物为连通障碍物（图3 中2、3、5 号障碍物），其他的均为非连通障碍物（图3 中1、4 号障碍物）.得到非连通障碍物坐标后，无论形状大小，都视为以该坐标为圆心、半径固定的虚拟圆，斥力方向跟随机器人位置的变化而变化；连通障碍物是占据区域且形状不固定，因此固定其斥力方向.

如图4 所示，机器人在不同位置仅受到来自一个方向的斥力作用，具体判定规则如下.

图4 连通障碍物斥力方向判定Fig.4 Direction determination of repulsive force for connected obstacles

规则1（条件3 与条件4 与条件5）：机器人所受单个连通障碍物的斥力，方向垂直于优先级最高的占据栅格.条件3：机器人周围仅一个连通障碍物.条件4：机器人位于该连通障碍物的斥力影响范围内.条件5：机器人左侧占据栅格优先级＞右侧占据栅格优先级＞上侧占据栅格优先级＞下侧占据栅格优先级.

规则2（条件6 与条件7 与条件8）：机器人所受多个连通障碍物的斥力，方向来自距离最近的连通障碍物，斥力方向选择遵循规则1.条件6：机器人周围存在多个连通障碍物.条件7：机器人位于多个连通障碍物的斥力影响范围内.条件8：机器人与多个连通障碍物的距离差较大且存在最小值.

规则3（条件6 与条件7 与条件9 与条件10）：机器人所受多个连通物的斥力，方向来自优先级最高的连通障碍物，斥力方向选择遵循规则1.条件9：机器人与多个连通障碍物的距离差较小.条件10：左侧连通障碍物优先级＞右侧连通障碍物优先级＞上侧连通障碍物优先级＞下侧连通障碍物优先级.

1.2.3 静态切向避障机制 对于连通障碍物，通过势场法结合斥力方向判定规则，机器人在不同位置仅受到来自一个方向的斥力，减少了因在C 形、U 形连通障碍物区域受到来自不同方向斥力的叠加而陷入局部极小值陷阱的问题.

针对非连通障碍物，提出切向避障的机制，避免机器人面对非连通障碍物时陷入局部极小值陷阱.机器人只需在障碍物影响移动路径时进行避障，为此进行非连通障碍物的重要性评估，将非连通障碍物分成重要障碍物与普通障碍物.当机器人与障碍物的距离小于2 倍 ρo且机器人与障碍物的夹角小于60°时，该障碍物为重要障碍物；当机器人与障碍物的距离大于等于2 倍 ρo或机器人与障碍物的夹角大于等于60°时，该障碍物为普通障碍物.如图5 所示，非连通障碍物1 为重要障碍物，非连通障碍物2 为普通障碍物，在同一时刻仅有一个重要障碍物，机器人围绕非连通障碍物1完成避障后才进行下一轮重要性评估.

图5 非连通障碍物重要性评估Fig.5 Importance evaluation of non-connected obstacles

针对重要障碍物，将其斥力影响范围划分为人工势场区域（宽度r3）、切向避障区域（宽度r2）以及安全距离区域（宽度r1），如图6 所示.陷阱预测后，若存在一组局部极小值点 (xm,ym)，3 个区域的划分宽度为

图6 非连通障碍物斥力影响范围划分Fig.6 Influence range division of repulsive force for non-connected obstacles

式中：robs为非连通障碍物半径；为局部极小值点与周围障碍物最小距离的平均值，为局部极小值点与周围障碍物最大距离的平均值，

若陷阱预测后未检测到局部极小值点，3 个区域的划分宽度为

式中：rinf为非连通障碍物虚拟圆向外膨胀的宽度.通过区域划分，将局部极小值陷阱所在位置限制在切向避障区域内，该区域不受原势场的控制，从而避免陷入局部极小值陷阱.机器人进入安全距离区域后采取紧急制动，避免与障碍物发生碰撞；在人工势场区域保持原人工势场控制；在切向避障区域不断将斥力分力Frep1的方向调整为避障切线方向，如图7 所示.

图7 非连通障碍物静态切向避障Fig.7 Static tangential obstacle avoidance for non-connected obstacles

将斥力分力调整为避障切线方向原因如下：假设机器人为1 个质点，那么完成避障的极限角度为机器人当前所在位置与重要障碍物的切线方向，如图8(c)合力1；当斥力和向逆时针方向旋转，合力与障碍物存在交集或向偏离引力和方向移动，导致与障碍物碰撞或绕行；斥力和方向顺时针旋转，合力贴近引力和方向，实现对重要障碍物的最优避障.当旋转角度小于避障切线方向、大于极限角度，合力方向如图8(e)合力3，机器人与重要障碍物的距离较小；旋转角度大于避障切线方向时，合力方向如图8(f)合力4，与普通障碍物存在碰撞风险.结合上述分析并开展多组仿真实验，斥力分力调整为避障切线方向，如图8(d)合力2，避障效果较优，静态切向避障的过程如图9 所示.

图8 斥力调整方向后的合力方向分析Fig.8 Analysis of resultant force direction after repulsive force adjustment

图9 静态切向避障流程图Fig.9 Flowchart of static tangential obstacle avoidance

1.2.4 动态切向避障机制 传统人工势场法的势场函数仅考虑机器人与障碍物的相对距离，未考虑机器人与动态障碍物间相对速度.由于不同的动态障碍物具有不同的速度，有时动态障碍物的出现与机器人无碰撞风险，有时机器人周围会出现多个动态障碍物.在障碍物斥力影响范围内，将式（1）中斥力势场函数改进为

式中：Urep(p,v) 为适用于动态环境的斥力势场；Urep(v) 为障碍物速度产生的斥力势场；kwar为碰撞风险系数，当机器人与障碍物的相对速度小于等于0 m/s，若机器人与障碍物的距离大于等于2 m且合力与障碍物的夹角大于等于60°，设置kwar=-1，为最低避障优先级，代表机器人无需进行避障.当机器人与障碍物的相对速度大于0 m/s，若机器人与障碍物的距离小于2 m 或合力与障碍物的夹角小于60°，设置kwar=1，为次低避障优先级；若机器人与障碍物的距离小于2 m 且合力与障碍物的夹角小于60°，设置kwar=2，为次高避障优先级；若机器人与障碍物的距离小于0.4 m，设置kwar=3，为最高避障优先级.设置kwar=1、2、3 代表机器人需要避障且必须选择目前避障优先级最高的动态障碍物进行避障.

静态切向避障机制是在障碍物静止不动的前提下进行切向避障，对于动态障碍物，该机制须改进，如图10 所示.图中，速度为Vobs的动态障碍物与机器人相向而行且距离较近，存在碰撞风险.因此在原合力与动态障碍物的夹角 γ 的基础上补偿角度 θ，得到新航向角 | |γ|+θ|，机器人沿新航向角进行避障，计算式为

图10 动态切向避障补偿角的补偿原理Fig.10 Compensation principle of compensation angle for dynamic tangential obstacle avoidance

式中：m为新航向角正反偏系数.当夹角 γ 为负角时，新航向角正偏（航向角为正），否则负偏（航向角为负），角度正负关系以逆时针方向为正.补偿角 θ 由模糊控制器依据机器人与动态障碍物间的情况给出，

式中：ε 为避障补偿角模糊控制器的输出，{PB，MD，PS，ZE}为补偿角的模糊语言变量.

2 基于双机制切向避障的混合路径规划系统设计

2.1 系统设计

如图11 所示为本研究方法的系统框架.全局路径规划单元与经过地图预处理后的局部路径规划单元通过状态决策决定控制状态.若规划的全局路径与膨胀后的障碍物仍存在一定距离或未检测到初始静态地图中不存在的障碍物，由全局路径规划单元控制；反之，由局部路径规划单元控制.状态决策的转移过程如图12 所示.

图11 混合路径规划模块系统框图Fig.11 Block diagram of mixed path planning module system

图12 混合路径规划状态决策过程Fig.12 Decision process of mixed path planning state

基于上述状态决策思想统筹全局路径规划与双机制切向避障，实现混合路径规划，流程如图13所示.1）初始化势场法参数：输入起点、终点坐标、小车线速度、角速度等，转至步骤2）.2）全局路径规划：进行一次全局路径与局部路径规划，在该状态下全局路径优先级高于局部路径，机器人沿全局路径移动，移动一段距离后进行优先级判断，转至步骤3）.3）路径优先级判断：计算机器人与全局路径前进方向上障碍物的距离，检测是否存在距离全局路径较近的障碍物或临时出现的静态、动态障碍物，影响机器人移动安全性.若存在这样的障碍物，进入局部路径规划，转至步骤4）；若不存在，则输出全局路径轨迹，转至步骤5）.4）局部路径规划：根据机器人与障碍物的相对速度决定进行静态或动态避障，动态障碍物在势场下进行动态切向避障，非连通静态障碍物在势场下进行静态切向避障，连通静态障碍物在势场下固定斥力方向，优先级调整为局部路径高于全局路径，并以当前位置为起点重新进行一次全局规划；选取全局路径前进方向上一点作为临时目标点，机器人沿局部避障路径向该点移动；输出局部避障轨迹，转至步骤5）.5）判断是否到达临时目标点：若已到达，转至步骤6）；否则转至步骤4）.6）判断机器人是否完成避障：若完成避障，转至步骤7），否则保持局部路径规划状态直至到达临时目标点；如果选取的临时目标点附近存在突发障碍物时，则重新进行一次全局路径规划并选取一点作为新临时目标点.若避障失败，机器人陷入停滞，则复位至步骤1）开始一轮全新的规划.7）结束：完成本轮路径规划.

图13 混合路径规划算法流程图Fig.13 Flowchart of mixed path planning algorithms

2.2 局部规划模糊控制器设计

本研究设计的双机制切向避障通过判断机器人与障碍物的距离、夹角后确定是否避障，这与模糊控制不需要过程的精确模型的特点较为贴合，因此将模糊控制与双机制切向避障相结合来实现局部规划模糊控制器设计.Mamdani 法是机器人控制中常用的模糊方法，中心去模糊法是重心法与最大隶属度法去模糊的折中，因此本研究采用这2 种方法进行模糊化与解模糊.

将切向避障模糊控制器设计成三输入一输出，模糊变量语义对照如表1 所示.将机器人与障碍物的距离d、合力与障碍物的角度ω、引力与调整后斥力分力Frep1的夹角 δ作为输入，共制定30 条模糊规则.机器人根据输出 output 判断在势场中受人工势场控制、切向避障、紧急制动，如当距离d为近（DN）、角度ω 为负（NS）、夹角 δ 为大（QB）时，机器人进行向右切向避障（RQM）.避障补偿角模糊控制器以机器人与动态障碍物的距离d、夹角 |γ| 以及相对速度Vrel为输入，以动态补偿角θ的大小 ε 作为输出，共制定125 条模糊规则，模糊变量语义对照如表2 所示.

表1 切向避障语义对照表Tab.1 Semantic comparison table of tangential obstacle avoidance

表2 避障角补偿语义对照表Tab.2 Semantic comparison table of obstacle avoidance angle compensation

3 仿真验证与实物平台测试

3.1 仿真环境下算法有效性验证

为了验证所提算法的性能，开展机器人使用不同算法在易陷入局部极小值陷阱、狭窄可行区域避障的对比实验，参与对比的算法包括本研究算法、沿边走法、虚拟目标点法和虚拟障碍法.如图14 所示，搭建60 m×60 m 的二维实验环境，起点与目标点分别在（0,0）和（50,50），引力增益系数与斥力增益系数设置为20、30，机器人的移动步长为0.7 m，非连通障碍物的斥力影响范围设置为障碍物的半径加5 m.在如图14(a)的实验环境中，4 种算法都到达目标点，但本研究方法路径更短、更平滑.如图14(b)的实验环境为易陷入局部极小值陷阱与产生路径震荡现象的狭长U 形区域.虚拟目标点法[14]规划失败，在U 形陷阱内陷入循环状态；沿边走法[15]进入U 陷阱后，沿着连通障碍物的边界移动，逃脱了局部较小值陷阱，路径明显较长；虚拟障碍物法[16]未陷入局部极小值陷阱，但在连通障碍物间狭窄通道存在路径震荡现象；本研究方法未产生其他参与对比算法的缺陷.在不同复杂度的实验环境中统计4 种算法的规划结果，如表3 所示，表中，C为规划成功次数，T为完成规划运行时间，L为规划路径的总长，S为规划路径中的拐点数.可以看出，在不同环境复杂度下，本研究方法完成规划的稳定性较高，平均运行时间相较于沿边走法、虚拟目标点法、虚拟障碍物法分别提高21%、20%、19%；在复杂环境下的提升效果尤佳，平均运行时间分别提高33%、23%、20%.

表3 不同环境复杂度时不同算法的规划结果统计Tab.3 Statistics of different algorithm planning results for different environmental complexity

图14 不同仿真环境下改进人工势场法性能对比Fig.14 Performance comparison of improved artificial potential field method under different simulation environments

在Gazebo 仿真平台搭建如图15(a)所示仿真环境，其中t=2.0、η=10.0、k=6.0、ρo=1.8，算法与机器人的其他相关参数如表4 所示.利用Rviz 观测机器人运动状态.机器人接收到目标点后，进入全局规划状态，浅色为全局路径，深色为局部路径.当全局路径与障碍物距离较小时，考虑到机器人转向半径，路径安全性较低，须增大与障碍物的距离，进入局部避障状态，此时局部路径优先级高于全局路径，机器人沿局部路径进行避障，如图15（b）所示.

表4 仿真环境下混合路径规划模块实验参数Tab.4 Experimental parameters of mixed path planning module in simulation environment

图15 Gazebo 仿真环境的避障验证Fig.15 Obstacle avoidance verification of Gazebo simulation environment

3.2 实物平台测试

为了进一步验证所提算法在真实环境中的性能，搭建全向移动平台.机器人的激光雷达数据由镭神N10P 提供；机器人操作系统主要平台为树莓派4B 系统核心板；采用底盘控制器主控（型号为STM32F407ZET6）接收并解析上位机发布的速度指令驱动电机转动.

3.2.1 室内环境下静态障碍物避障测试 在不同的实验环境中对全向移动平台开展避障测试.图16（a）、（b）中，机器人在地图1 中由于固定斥力方向，受到优先级最高的上侧连通障碍物1 与左侧连通障碍物2 的斥力作用，固定斥力方向垂直向下与垂直向右，不再叠加其他连通障碍物斥力，深色避障路径相较于浅色全局路径，明显与障碍物保持充裕的安全距离，避免发生碰撞.图16（c）、（d）中，在地图2 中以易产生受力平衡的C 形、U 形区域为起点进行规划，机器人未陷入停滞状态并成功到达目标点.图16（e）、（f）中机器人对存在影响的非连通障碍物2 与4 进行静态切向避障，对不存在影响的非连通障碍物3 不进行避障，并且未在多个非连通障碍物间陷入停滞.测试结果表明，本研究的静态避障机制在不同环境中实现了自主避障，在几种典型的易陷入局部极小值陷阱的连通或非连通障碍物环境中，也有效避免了机器人陷入停滞状态.

图16 室内环境下静态避障结果Fig.16 Static obstacle avoidance results in indoor environment

将局部规划器采用DWA、TEB 在图16 的3 幅地图中进行25 次多点导航测试，并逐步向地图中添加障碍物改变环境复杂度，结果如图17（a）所示，TN为平均导航耗时，c为路径规划成功次数.在起点与目标点相同的情况下，本研究方法成功完成路径规划73 次，比DWA 多22 次，比TEB 多2 次；平均导航耗时比DWA 提升了39%、比TEB 提升了22%.图17（b）中，TOA为平均完成一次避障的用时.将3 种算法完成对连通、非连通障碍物以及易陷入局部极小值陷阱的U 形障碍物区域的平均避障时间进行对比，本研究方法所耗费的时间最小，比DWA 提升了55%，比TEB 提升了40%.

图17 全向移动平台测试结果比对Fig.17 Test results comparison of omnidirectional mobile platforms

3.2.2 室内环境下动态障碍物避障测试 如图18所示，进行本研究方法躲避行人的有效性验证实验.实验结果表明，机器人进行避障角补偿后能够躲避行人.可以看出，1 号人员与机器人的相对距离较近、夹角较小，风险系数为次高避障优先级，2 号人员与机器人的夹角较大，风险系数为次低避障优先级，对1 号人员进行避障角补偿符合规则.

图18 室内环境下动态避障结果Fig.18 Dynamic obstacle avoidance results in indoor environment

4 结语

本研究提出基于人工势场法的双机制切向避障算法.所提算法将障碍物分成连通与非连通障碍物，并针对阻碍实现自主避障的局部极小值陷阱采取不同解决策略.采用人工势场法并固定连通障碍物斥力方向，减少了不同方向力的叠加，避免机器人陷入停滞；通过陷阱预测得到的局部极小值点，对非连通障碍物的斥力影响范围进行划分，将局部极小值陷阱限制在切向避障区域内，机器人进入该区域后进行切向避障避免陷入停滞.在此基础上，针对室内环境中可能出现的动态障碍物，通过避障补偿角实现动态切向避障.实验结果表明，在不同的地图中，相比主流算法，所提算法能够在不同环境复杂度下有效避免陷入局部极小值陷阱，在避障耗时与整体导航耗时上有较大提升，在复杂环境中提升效果尤佳.同时，实物测试平台的避障自主性较优，减少了人为干预.本研究须在规划前对已完成建模的地图环境进行陷阱预测，而在实际应用中地图往往是未知的，未来计划在本研究的基础上进行未知环境下的实时陷阱预测研究.