基于改进势场法的移动机器人路径规划

2020-12-08孙鹏耀黄炎焱潘尧

兵工学报 2020年10期

孙鹏耀，黄炎焱，潘尧

(南京理工大学自动化学院，江苏南京 210094)

0 引言

随着移动机器人技术的不断发展，机器人自主执行作战任务已成为军事领域的一个重要方向。在许多机器人作战任务中，比如排雷、侦察、运送物资等，要想移动机器人顺利完成作战任务，准确可靠的路径规划是必不可少的前提条件之一。目前，针对移动机器人路径规划常用方法有：势场法(PFA)[1-2]、随机树搜索法[3]、粒子群优化算法[4]、遗传算法[5]、蚁群算法[6]、烟花算法[7]等，其中PFA因其模型简单，计算快速与实时性强等优点被广泛研究。但战场障碍环境往往具有复杂性与部分未知性的特点，PFA在复杂障碍环境下的应用仍存在许多问题。现有研究主要将这些问题分为3类：局部极小陷阱、路径振荡、路径不被识别。

局部极小陷阱是PFA最关键的问题，目前解决局部极小陷阱的方法主要可以分为消除法和逃离法两大类[8-9]：

1)消除法[10-12]通过构造特殊势函数来消除局部极小点，从而保证目标点为全局唯一极小点。但消除法需要基于全局环境信息充分已知前提下进行预处理，无法满足对实时性要求高的情况。

2)逃离法的思想是在机器人陷入局部极小陷阱后通过某种方法引导机器人逃离陷阱。常见的逃离法有搜索算法、多PFA、虚拟障碍物法、沿墙行走法等。搜索算法的思想是当陷入局部极小陷阱后，搜索并移动到势场值比当前局部极小值更小的位置点，然后机器人继续按势场梯度下降法移动，目前常用的搜索算法如随机搜索、模拟退火在缺少启发信息时存在搜索效率的不确定性，可能导致搜索效率很低[13]。多PFA[14]的思想是设计多个全局最小点相同但局部极小点不同的势场函数，当机器人陷入局部极小陷阱时通过切换势场函数保证规划继续进行。多PFA需要对全局环境信息提前准确了解并构造势场函数，且构造多个满足要求的势函数难度大，容易使机器人重新陷入已逃离的局部极小陷阱，增加了规划的复杂度与不确定性。虚拟障碍物法[15]的思想是当机器人陷入局部极小陷阱时，在局部极小点附近加入虚拟障碍物，通过改变原有势场分布引导或阻止机器人逃离或进入局部极小陷阱，但在复杂障碍环境下要确定虚拟障碍物的位置较为复杂，且容易导致振荡问题的产生。沿墙行走法[16]的思想是让机器人沿着障碍区的边缘逃离陷阱，简单实用，但现有的一些沿边行走方法过于简单，不能适应过于复杂的障碍环境。

振荡问题的产生与势场参数、机器人步长、障碍物信息等参数密切相关，目前解决振荡问题的方法主要有两类：一类是参数优化，其思想是通过优化算法不断优化各个参数来避免振荡产生，现常用的优化算法有量子粒子群优化算法[17]、遗传算法等，但是这类方法在面对复杂的障碍环境时，需要不断地优化调整参数，计算量较大，不够简洁高效；另一类是后消除方法[18-19]，该类方法的思想是在完成路径规划后找出路径中的振荡，并消除替换为平直路径，该类方法不能边走边处理，不具有实时性。

路径不被识别问题也与各参数息息相关，目前解决该问题的方法也主要分为两类：一类是参数优化[20]；另一类是基于行为的方法[21]，其思想是通过切换不同的机器人行为模式避免该问题，该类方法实用性强，但在复杂环境下，设计行为切换条件是难点。

综上所述，PFA中存在的系列问题已受到研究者的广泛关注[22-23]，但是依然存在一些不足：1)在复杂环境下，现有方法不能兼顾实时性与高效性；2)现有研究将振荡分为障碍物前方和侧方两类，但并未给出准确的分类标准，是一个研究盲点，模糊的分类边界导致在应用具体方法时难以设计准确判据，并且现有方法都集中于前方振荡，而忽视了侧方振荡，不具有一般性；3)现有研究在解决局部极小陷阱、路径振荡、路径不被识别3类问题时，多采用逐个解决的思想，并未系统全面地将这些问题分析、总结与统一，并且所设计的障碍区形状与环境往往比较简单，在多种问题可能同时存在的复杂障碍环境下，这些方法不能够简洁高效地系统解决这些问题。

针对上述情况，考虑到战场环境的复杂性，为系统全面地解决PFA在复杂障碍环境下存在的一系列问题，保障机器人实现实时路径规划，顺利完成作战任务，本文将PFA存在的问题细分为相近障碍区之间路径不被识别(问题1)、多障碍区导致的振荡(问题2)、多障碍区导致的局部极小陷阱(问题3)、单个障碍区导致的局部极小陷阱(问题4)、单障碍区导致的振荡(问题5)，从问题的必要条件入手，提出一种结合多行为策略与可变影响范围的PFA(CSVR-PFA)：

1)首先，分析问题1～问题3的必要条件，提出可变影响范围算法解决该必要条件，规避问题1～问题3的出现；

2)其次，提出有效步进与无效步进的概念，结合内部振荡与边缘振荡，提出一种新的振荡分类方法，并给出准确分类标准；

3)再次，分析了问题4、问题5的必要条件，把对问题4、问题5的规避转化为对无效步进与有效振荡的规避，提出多行为行动策略，并给出准确的起止条件。机器人根据不同时刻面对的不同状态，通过各行为起止条件之间的衔接，进行行为的切换，以有效规避问题4、问题5；

4)最后，结合上述研究形成CSVR-PFA，通过大量复杂障碍环境下的仿真实验与对比研究，验证了本文所提方法的有效性、可行性与优越性。

1 PFA基本原理

PFA的基本原理是将整个物理环境转化为一个势场模型，目标点产生引力势场吸引机器人移动，障碍产生斥力势场阻止机器人向目标移动，机器人在引力和斥力共同作用下从高势场位置沿势场的负梯度方向逐步向低势场位置运动。文献[1]把机器人与目标点的距离引入到斥力函数中，确保目标点是全局势场中的势能最低点，解决了目标点与障碍过近从而不可达的问题。根据机器人的物理半径对障碍做膨胀计算，机器人可看作质点。现给出势函数与力函数如(1)式～(4)式所示。

引力势能函数：

(1)

斥力势能函数：

(2)

式中：qr、qg分别表示机器人、目标点所在位置；qobs表示障碍区中与机器人当前位置距离最近点的坐标；ξ为引力增益系数；η为斥力增益系数；ρ(qr,qg)=‖qg-qr‖表示机器人所在位置与目标点的直线距离；ρ(qr,qobs)表示机器人与障碍区最小直线距离；ρO为障碍区的影响范围半径。

引力函数为

(3)

斥力函数为

(4)

式中：

(5)

(6)

nOG表示由机器人指向目标点的单位向量；nOR表示由障碍区指向机器人的单位向量；nRG表示由机器人指向目标点的单位向量。

本文以此势函数与力函数为基础，对PFA进行研究。

2 多障碍区问题的分析与规避

2.1 问题分析

2.1.1 问题1 相近障碍区之间路径不被识别

图1 问题1示意图Fig.1 Problem 1

如图1和图2所示，机器人本该从某两个障碍区之间的狭窄路径通过，但由于路径入口处被障碍区群影响范围完全覆盖且入口处任意一点的斥力势场都很大，机器人在入口处所受合力指向障碍区群外侧区域，此时机器人无法识别和进入路径，只能从障碍区群的外侧绕行。

图2 势场值示意图Fig.2 Schematic diagram of potential field value

综上分析，狭窄路径入口处影响范围重叠或相切以及不合适的斥力系数是产生问题1的必要条件。

2.1.2 问题2 多障碍区导致的振荡

该问题有两种场景：1)振荡出现在障碍区之间的狭窄通道中；2)振荡出现在障碍区四周。

场景1又细分为两种：

图3 问题2示意图Fig.3 Problem 2

1)如图3所示，两个障碍区影响范围重叠且合斥力较小，合力指向障碍区之间的路径，此时机器人可以穿过障碍区之间的通道。在通道区域，合势场呈现U形的峡谷形状，如图4所示。机器人在穿过该峡谷通道时，理论上应该沿着谷底的最低势场线(蓝色路径)移动，但是由于计算机计算的离散性和不恰当的斥力系数，机器人很难严格处在这条理论线上，而是交替出现在这条线的两侧(红色路径点)，从而形成振荡，如图5所示。

图5 理论与实际路径对比图Fig.5 Comparison of theoretical and practical paths

2)两个障碍区的影响范围不重叠但斥力极大，且存在某一区域，这两个影响范围的最小距离极小(小于一个步长)。当机器人进入该部分区域时，由于斥力极大且此时两障碍区影响范围极其接近，机器人刚进入1号障碍区影响范围就被排斥出去，紧接着进入2号障碍区的影响范围。接下来，机器人再被2号障碍区排斥出影响范围，紧接进入1号障碍区影响范围。以此往复，形成振荡，具体如图6所示。若最小距离大于1个步长，机器人从1号影响范围被排斥出去后不能立即进入2号障碍区影响范围，此时如果形成振荡，并不是两个障碍区连续排斥造成，只与某一个障碍区有关，不符合问题2的定义。

图6 影响范围过近导致的振荡问题示意图Fig.6 Oscillation caused by close influence range

场景2：机器人在经过多障碍区影响范围的重叠区域时，与问题1类似，由于计算的离散性和不恰当的斥力系数，机器人不能完全贴合理想路线行进，合力出现突变从而导致振荡。

综上分析，障碍区影响范围重叠或过于接近(小于一个步长)和不合适的斥力系数是产生问题2的必要条件。

2.1.3 问题3 多障碍区导致的局部极小陷阱

所谓局部极小陷阱，原理上，即在合势场中存在某处，其合势场强度为0，机器人在该处受到的合力为0，从而无法进行下一步方向判断，停滞不前，路径中断。局部极小陷阱是由PFA本身的设计引起的，是不可避免的。在实际应用中，由于计算机计算的离散性，当机器人陷入局部极小陷阱时，其表现为在围绕着该局部极小陷阱的某一个较小区域内徘徊，无法逃出该区域。具体表现如图7所示。

图7 问题3示意图Fig.7 Problem 3

显然，障碍区的影响范围存在重叠区域，并且在重叠区域存在合势场强度为0的局部极小点是产生问题3的必要条件。

2.2 可变影响范围算法

通过上述分析可以发现，形成问题1～问题3的共同必要条件是各影响范围之间存在最小距离小于1个步长的部分(最小距离小于0即为重叠)。因此，只要各个障碍区的影响范围之间的最小距离大于1个步长，消除产生问题1～3的必要条件，即可提前避免问题1～问题3的产生。对此，本文提出一种障碍区可变影响范围算法，对已知障碍区位置信息进行处理，处理后各障碍区影响范围之间最小距离不小于1个步长。算法步骤如下：

2) 计算每两个障碍区间的最小距离，获得对称的距离矩阵Dn×n，矩阵元素dij=min [ρ(S(i),S(j))]；

3)计算可变率矩阵Cn×n，矩阵元素cij=(ρO(i)+ρO(j))÷(dij-Ls)。

上述算法步骤中，S(n)表示已知构成障碍区Obs(n)的点集合，Ls表示机器人行进步长，ε为一个微小的正值。可变率c反映了某一对障碍区影响范围调整的紧急程度，c≥1时表示影响范围间最小距离小于1个步长，需要进行调整，c越大表示越紧急，c<1表示不需要调整。考虑到一次调整后会导致多组障碍区的c发生变化，故每次只调整最紧急的一对障碍区，然后重新计算可变率矩阵，再选择下一对要调整的障碍区，直到任意一对障碍区c<1，从而达到算法目的。

在实际应用中，障碍区的位置信息可能不完全已知，需要在机器人行进过程中进行探测。当探测到新的障碍区或障碍区新的信息时，将探测到的内容加入步骤1中的初始信息，重新计算即可。

3 单障碍区问题的分析与规避

3.1 问题分析

3.1.1 问题4 单障碍区局部极小陷阱

在分析单障碍区局部极小陷阱前，需要引入有效步进与无效步进的概念，现给出有效与无效步进定义为：有步进qr(s)qr(s+1),若ρ(qr(s+1),qg)<ρ(qr(s),qg)，则称步进qr(s)qr(s+1)为有效步进；否则为无效步进。

单障碍区局部极小陷阱的表现为障碍区挡在机器人与目标点之间，机器人陷入局部极小点周围的极小区域内无法逃离，具体表现形式为机器人围绕着局部极小点运动，这一过程中包含无效步进。无效步进为构成单障碍区局部极小陷阱表现形式的必要条件。

3.1.2 问题5 单障碍区振荡

从PFA的原理看，振荡问题本不该出现，但由于实际计算的离散性，机器人每次行进固定步长，所以往往不能严格准确地沿着理论上的路径移动，通常会或多或少地超出理论路径点，导致合力的突变。此时机器人实际路径点会交替出现在理论路线的两侧，形成振荡。

在PFA应用中，初始参数如引力系数、斥力系数、障碍区影响范围、步长等设置的不同，振荡出现的位置与形式也不相同。振荡问题产生原因的多样性导致了振荡问题表现形式的复杂性，要准确有效地处理振荡问题，需要对振荡问题进行准确分类与预判。现有的振荡分类方法为障碍区前方与侧方之分，这种方法的分类条件模糊，应用时难以准确选择判据。本文提出新的单障碍区振荡分类方法，将其分为影响范围边缘振荡与影响范围内部振荡两类，具体分析如下：

图8 边缘振荡示意图Fig.8 Edge oscillation

1) 障碍区影响范围边缘振荡。当影响范围边缘斥力强度非常大时，如图8所示：①机器人刚刚进入障碍区范围后，所受斥力极大，机器人立刻被排斥出障碍区影响范围；②机器人刚被排斥出来，此时只受到引力作用，再次进入障碍区影响范围边缘；③上述两个步骤交替出现，形成振荡。

设机器人当前位于路径点qr(s)，在计算出下一路径点qr(s+1)后，预判是否会产生边缘振荡的方法如图9所示。

图9中，ρ(qr(s+1)qg,qobs(n))表示路径点qr(s+1)指向目标点qg的向量与障碍区Obs(n)的最近距离，ρO(n)表示障碍区Obs(n)的影响范围。ρ(qr(s+1)qg,qobs(n))-ρO(n)<0表示向量qr(s+1)qg与障碍区Obs(n)有交点。

图10 内部振荡示意图Fig.10 Internal oscillation

2)障碍区影响范围内部振荡。当影响范围边缘斥力强度较小时，如图10所示，机器人可以进入影响范围内部：①机器人所受斥力较小，合力指向影响范围内部，机器人朝向障碍区行进，越来越靠近障碍区，斥力的增长率急速增大，引力的变化平缓；②由于计算离散性，机器人所受斥力急剧增大，引起合力突变，排斥机器人远离障碍区；③机器人与障碍区距离增大后斥力急剧下降，再次引起合力突变，吸引机器人接近障碍区；④步骤2与步骤3行为交替出现，从而形成振荡。

设机器人现位置为qr(s)，在计算出下一路径点qr(s+1)后，预判是否会产生内部振荡的方法如图11所示。

图11 内部振荡预判方法流程图Fig.11 Flow chart of internal oscillation prediction

图11中：〈m,n〉表示向量n旋转到m方向的角度，逆时针为正，顺时针为负；λ为一个小于0的较小数值，一是用来判定是否产生振荡，二是用来消除一些微弱干扰和误差。

至此，振荡可准确分为内部振荡与边缘振荡。接下来，结合有效、无效步进，引入有效振荡与无效振荡的概念

由有效步进构成的振荡称为有效振荡，由无效步进与有效步进交替构成或者全都由无效步进构成的振荡称为无效振荡。PFA中的振荡可以分割成这两种振荡的组合。

无效振荡发生时，由于无效步进使机器人远离目标点，会导致机器人的行进效率较低，从而降低整个PFA的收敛速度甚至导致无法收敛，同时机器人处于无效振荡时，每次步进的转向角很大，容易造成姿态不稳，严重危害机器人的行进安全。有效振荡对算法收敛速度与机器人行进安全的影响相对较小。

结合内部、边缘振荡与有效、无效振荡两种分类方式，最终将振荡细分为4种：内部- 有效振荡、内部- 无效振荡、边缘- 有效振荡、边缘- 无效振荡。

对于内部- 无效振荡和边缘- 无效振荡而言，由其定义可知无效步进是构成这两种振荡的必要条件，这与构成问题4单障碍区局部极小陷阱表现形式的必要条件相同。故内部- 无效振荡、边缘- 无效振荡以及问题4的解决可以转化为对无效步进的处理。而对于内部- 有效振荡和边缘- 有效振荡，只要处理好有效振荡即可。为此，本文设计多行为行动策略。

3.2 多行为行动策略

本文的解决思路是：由于无效步进或有效振荡是构成问题4、问题5的必要条件，当判定下一步进出现无效步进或有效振荡时，机器人跳出传统PFA，根据所处环境状态选择其他行为方式，沿特定路线行动驶离问题区域，直至障碍物影响范围外，以完成对问题4、问题5的规避，然后回到传统PFA. 对此，本文设计了一种多行为方式组合的机器人行动策略，该策略包含沿等距线、直行和传统PFA 3种行为方式及其起止条件。

直行行为针对预判出无效步进或有效振荡时，机器人当前位置与目标点的连线段不会穿过所在影响范围对应障碍区的情况(简称情况1)；沿等距线行为针对预判出无效步进或有效振荡时，机器人当前位置与目标点的连线段会穿过所在影响范围对应障碍区的情况(简称情况2)；传统PFA行为作为整个算法的基础行为，针对前两种行为之外的其他所有情况(简称情况3)。

当针对的情况出现时，机器人需要开始进入对应的行为，故各行为的起始条件即是其所针对情况的预判条件。当机器人采用新的行为时，原本的行为结束，故原行为的中止条件即为新行为的起始条件。通过各行为起止条件之间的衔接，完成行为切换。起止条件应满足以下要求：1)各行为的起始条件间无交集(不冲突)，且并集包含所有情况；2)新行为的起始条件作为原行为的中止条件，必须确保原行为所针对的问题情况已消失。

3.2.1 直行行为

当情况1出现时，此时机器人朝目标直行行驶不会与对应障碍物发生碰撞，机器人保持目标方向直行至该影响范围之外或抵达目标点。

定理1机器人直行驶至障碍物影响范围外时，原可能发生的问题4、问题5被规避。

证明设机器人在经过障碍区Obs(n)影响范围的部分连通域H时预判出现无效步进或有效振荡，此时可能发生问题4、问题5. 机器人朝目标方向直线穿过H行驶至影响范围边界之外后，回到传统PFA行为，此时机器人不受Obs(n)影响，只受目标点引力作用，会继续向目标点直行，不再进入H，故H中预判的无效步进或有效振荡不会出现，即原可能出现的问题4、问题5被规避。

起始条件：

ρ(qr(s),qobs(n))≤ρO(n)∧
{q|q∈qr(s)qg,q∈Obs(n)}=∅，

(7)

式中：q为任意一点坐标。

中止条件：

ρ(qr(s),qobs(n))>ρO(n)∨|qr(s)qg|

(8)

起始条件表示机器人进入Obs(n)的影响范围后在目标点方向上不会与障碍物发生碰撞。该起始条件包含了上述中预判出无效步进或有效振荡的情况；另一方面，如果机器人正常避障不会出现问题，但也会因为避障导致一定程度的绕行，而机器人本可以朝目标直行，这种多余的绕行可以避免。合并这两种情况的预判条件即为直行行为起始条件。

中止条件表示机器人驶离Obs(n)的影响范围或抵达目标点。此时机器人已经不受Obs(n)的影响，原可能发生的问题4、问题5被规避，所以结束直行行为。

根据直行行为过程可得该行为下路径点计算公式为

(9)

式中：第1个公式确定方向一致性；第2个公式确定步长一致性。

3.2.2 沿等距线行为

当情况2出现时，此时机器人必须按特定路线进行规避绕行，机器人先按目标方向直行，抵达等距线限定区域，然后沿着等距线行驶，直至切换至直行行为，离开问题区域。

本文所述的等距线，即在影响范围内，一条由到障碍区最短距离相等的点构成且可以遍历障碍区边缘的闭合曲线。用点集E(ρe)n表示：

E(ρe)n={q|ρ(q,qobs(n))=ρe,ρe≤γexp≤ρO(n)}，

(10)

式中：ρe为等距线上的点要满足的距离值。对于凸障碍区和一般凹障碍区，等距线是障碍区边界线的放大，但是对于一些缺口较窄的凹障碍区，ρe过大会导致所得的等距线不能穿过缺口到达障碍区包围的内部区域，机器人无法搜索该内部区域，如图12所示。而且在实际中可能部分障碍区信息是未知的，无法预知缺口的大小，故参数ρe应越小越好，理论上可以为0，但考虑到实际中存在误差，所以引入一个误差经验参数γexp对ρe进行限定，确保在安全的前提下使ρe尽可能小，机器人能够遍历障碍物轮廓。

图12 不同ρe的等距线示意图Fig.12 Schematic diagram of isometric lines with different ρe

定理2障碍区Obs(n)边界线上必存在一点，该点与目标点的连线段不穿过Obs(n)，即Obs(n)边界线上存在点O与目标点G的有向连线段OG，除起点O外必与Obs(n)没有交点。

证明假设Obs(n)边界线上不存在这样的点，即Obs(n)边界线上任意一点O与目标点G的有向连线段OG，除起点O外必与Obs(n)有交点。设该连线段由K个点组成，则该连线段可以表示为点集：{P1,P2,…,Pj,…,PK}，其中P1=O，PK=G，由上述假设得，必有Pj1∈Obs(n)的边界线(1

定理3对于障碍区Obs(n)边界线与等距线之间区域的任意一点P，必存在两端端点O、Q分别位于Obs(n)边界线与等距线上，且经过该点的一条线段OQ，OQ不穿过Obs(n)，即线段与Obs(n)除端点O外没有交点。

证明首先，根据本文等距线的定义可知，等距线是遍历Obs(n)边界且闭合包围的，连接O、P并延长，与等距线必有交点Q，故必有经过点P的线段OQ；接着，假设任意情况下的OQ，除端点O外与Obs(n)必有交点。设线段OQ由K个点组成，则该连线段可以表示为点集：{P1,P2,…,Pj,…,PK}，其中P1=O，PK=Q，根据假设必有Pj1属于Obs(n)的边界线(1

定理4在上述情况下，等距线上必存在一点，该点与目标点的连线段不穿过障碍区Obs(n)，即满足直行行为的起始条件。

证明当G位于等距线外侧，即ρ(Obs(n),G)≥ρe时，满足定理2的连线段OG上的点到Obs(n)的距离范围为[0,ρ(Obs(n),G)]，OG上有点P满足ρ(Obs(n),P)=ρe，即P也位于等距线上，OG与Obs(n)无交点，故PG与Obs(n)无交点。

当G位于等距线内侧，即ρ(Obs(n),G)<ρe时，将G看作定理3中的点P，则有满足定理3的过点G的线段OQ，因为OQ除点O外与Obs(n)无交点，故GQ与Obs(n)无交点。

综合上述两类讨论，定理4得证，即机器人必定能够从沿等距线行为切换为直行行为，直至驶出影响范围。再根据定理1可知，原本H中预判的无效步进或有效振荡不会出现，即原可能出现的问题4、问题5被规避。

起始条件：

ρ(qr(s),qobs(n))≤
ρO(n)∧{q|q∈qr(s)qg,q∈Obs(n)}≠∅∧
qr(s)qr(s+1)⟹PROBLEM，

(11)

中止条件：

ρ(qr(s),qobs(n))≤ρO(n)∧
{q|q∈qr(s)qg,q∈Obs(n)}=∅.

(12)

起始条件中的PROBLEM表示出现无效步进或有效振荡。起始条件即对情况2的判定；由于等距线位于影响范围内部，为使机器人能够行驶到Obs(n)影响范围外，将沿等距线行为与直行行为衔接起来，所以中止条件即为直行的起始条件。

根据沿等距线行为过程得该行为下路径点计算公式：

当ρ(qr(s),qobs(n))≤γexp时，

(13)

当ρ(qr(s),qobs(n))>γexp时，

(14)

3.2.3 传统PFA行为

传统PFA行为作为整个算法的基础行为，在除去情况1、情况2以及多余绕行的其他所有情况下，机器人按照传统PFA，在势场力的作用下移动。

起始条件：

(15)

中止条件：

ρ(qr(s),qobs(n))≤
ρO(n)∧{q|q∈qr(s)qg,q∈Obs(n)}=∅∨
(ρ(qr(s),qobs(n))≤
ρO(n)∧{q|q∈qr(s)qg,q∈Obs(n)}≠∅∧
qr(s)qr(s+1)⟹PROBLEM)∨|qr(s)qg|

(16)

该行为下路径点计算公式为

(17)

根据3种行为各自的功能与特点，可以得到他们的切换路线图，如图13所示。

图13 行为切换路线图Fig.13 Roadmap of behavior switching

归纳得到行为切换有两种路线：方式1，传统PFA⟹直行⟹传统PFA；方式2，传统PFA⟹沿等距线⟹直行⟹传统PFA. 由定理1和定理2可知这两种路线均可以使机器人规避原来可能出现的问题4、问题5.

至此，本文分析了问题1～问题3的共同必要条件与问题4、问题5的共同必要条件，并提出对应的可变影响范围算法与多行为行动策略，结合传统PFA，形成改进后的CSVR-PFA. 问题分析思路总图与完整算法流程图分别如图14和图15所示。

图14 问题分析思路总图Fig.14 General diagram of problem analysis ideas

图15 CSVR-PFA完整算法流程图Fig.15 Flow chart of CSVR-PFA

4 仿真实验及结果分析

为了验证本文方法的有效性和可行性，在数学仿真软件MATLAB 2017b平台上进行了仿真实验。机器人简化为半径为0.5 m的圆以便于膨胀计算。同时，为了让仿真接近现实，设定机器人探测得到的距离和角度误差为5%.

4.1 U形空间仿真研究

在相同尺寸的U形空间中，通过改变机器人的探测范围大小以及U形空间缺口大小，模拟机器人在多种尺度U形空间中的状态。此U形陷阱环境中机器人的起点为(40 m,30 m)，目标点为(70 m,65 m)，引力系数ξ=1，斥力系数η=2，步长Ls=1 m，抗干扰系数λ=-0.1，等距线限定参数γexp=1.5 m，障碍区影响范围ρO=10 m，探测范围R分别为2 m、10 m和50 m.

图16和图17分别是U形空间在大缺口和小缺口下，探测范围分别为2 m、10 m和50 m时的仿真结果。由仿真结果可知，在模拟的各种尺度的U形空间中，本文所提算法都能够成功规划出一条较好的可行路径，使机器人顺利抵达目标点，验证了所提方法在多尺度U形空间内的有效性。

图16 不同R下的大缺口U形空间路径对比图Fig.16 Comparison diagram of U-shaped spatial path with large gap and different R

图17 不同R下的小缺口U形空间路径对比图Fig.17 Comparison diagram of U-shaped spatial path with narrow gap and different R

4.2 参数不敏感性研究

在PFA中，引力系数ξ、斥力系数η和障碍区影响范围ρO这3个核心参数的改变往往直接影响算法结果。要想通过PFA获得较好的规划结果，需要多次调整参数，较为繁琐复杂。而本文所提的CSVR-PFA从问题的必要条件入手规避问题1～问题5，从而对这3个核心参数具有不敏感性。设置4组参数进行仿真对比实验，以验证本文方法的参数不敏感性，仿真对比实验结果如图18所示。

图18 不同ξ、η和ρO下的仿真对比图Fig.18 Simulation contrast diagrams with different ξ, η and ρO

从仿真对比结果中可明显看出，PFA在参数差异性较大时，规划中出现的问题以及规划结果差异较大。相比之下，本文所提方法在各种参数条件下均能够顺利规划出较优的路径结果，同时各结果之间差异性很小，本文方法对引力系数ξ、斥力系数η和障碍区影响范围ρO这3个核心参数具有很好的不敏感性。

4.3 综合仿真研究

机器人在执行军事任务时面对的障碍环境往往较为复杂，为验证本文所提算法在复杂环境中的有效性，说明并验证算法中各模块对各自针对的问题的作用，本节在130 m×130 m的区域内设置多个形状范围不一的障碍区，模拟构建复杂战场环境。命令机器人从起点出发，分别使用PFA、可变影响范围PFA(VR-PFA)以及CSVR-PFA自行规划路径(纵向对比)。机器人穿过该复杂区域抵达目标点视作完成作战任务。

设定起点为(0 m,0 m)，目标点为(110 m,110 m)，引力系数ξ=1，斥力系数η=1，各障碍区的初始影响范围为ρO(1)=…=ρO(7)=10 m，步长Ls=1 m，抗干扰系数λ=-0.1，算法最大可计算次数为500.

4.3.1 传统PFA仿真结果

采用PFA进行路径规划，结果如图19和图20所示。

图19 PFA仿真结果Fig.19 Simulated results of PFA

图20 PFA计算过程中的收敛趋势Fig.20 Convergence trend in the calculation process of PFA

由图19可以看出：细节1中出现问题2的场景一现象，机器人虽然顺利从Obs(1)与Obs(2)之间的通道穿过，但在Obs(1)与Obs(2)的共同影响下出现了严重的振荡；细节2中出现问题1现象以及问题2中的场景2现象，机器人本应从Obs(3)与Obs(4)之间通道穿过，此时只能从两个障碍区的外侧绕行，同时，机器人在绕行过程中路径发生严重振荡；细节3中出现问题3现象，机器人被困于Obs(5)与Obs(6)影响范围重叠区的某一极小区域无法逃脱。

如图20所示，当达到最大计算次数时，算法没有收敛至0，此时机器人无法抵达目标点，任务失败。

4.3.2 VR-PFA仿真结果

将可变影响范围算法与PFA结合，形成VR-PFA. 机器人路径规划结果如图21和图22所示。

图21 VR-PFA仿真结果Fig.21 Simulated results of VR-PFA

图22 VR-PFA计算过程中的收敛趋势Fig.22 Convergence trend in the calculation process of VR-PFA

观察图21的细节1～细节3可发现，原本存在由多障碍区导致的问题1～问题3已被规避，但此时出现大量单障碍区振荡，即问题5. 细节1中，存在轻微的内部- 有效振荡，因其对算法收敛速度影响甚小，所以在图22中并不能明显反映该处振荡。细节2中，机器人刚进入Obs(3)的影响范围就被排斥出去且出现无效步进，紧接着，与3.1.2中的分析相同，形成大量边缘- 无效振荡。细节3中也产生了一个边缘- 无效振荡。细节4中，机器人在躲避Obs(7)“5”字形复杂凹形障碍区时陷入单障碍区局部极小陷阱，无法逃离，同时产生大量无效步进，即问题4.

如图22所示，当达到最大计算次数时，算法没有收敛至0，此时机器人无法抵达目标点，任务失败。但同时，本小节结果验证了本文提出的可变影响范围算法对规避问题1～问题3的有效性。

4.3.3 CSVR-PFA仿真结果

将多行为行动策略与可变影响范围算法、PFA结合，最终形成本文提出的CSVR-PFA. 机器人路径规划结果如图23和图24所示。

图23 CSVR-PFA仿真结果Fig.23 Simulated results of CSVR-PFA

图24 CSVR-PFA计算过程中收敛趋势Fig.24 Convergence trend in the calculation process of CSVR-PFA

观察细节1与细节3，机器人分别进入Obs(2)、Obs(5)的影响范围后，判定采用直行行为，从而规避了原有的内部- 有效振荡与边缘- 无效振荡。细节2中，机器人进入Obs(3)影响范围后，判定出下一步进为无效步进且不可直行，采用沿等距线行为。行进一段距离后，机器人判定可以直行，转为直行行为，直到离开Obs(3)的影响范围，规避了原有的边缘- 无效振荡。细节4中，机器人进入Obs(7)影响范围后，判定不可直行且不存在问题4、问题5的必要条件，此时采用PFA行为。在即将陷入单障碍区局部极小陷阱时，机器人判定将出现无效步进，转用沿等距线行为直到判定可以直行，从而规避了该陷阱。最终机器人直行抵达目标点，任务成功。此时算法收敛至0，计算次数(步数)为290次。

本节的仿真结果表明，在复杂凹凸障碍环境下，本文提出的可变影响范围算法能够使机器人有效规避问题1～问题3；多行为行动策略能够有效规避问题4、问题5. 最终，本文提出的以传统PFA为基础，结合多行为行动策略与可变影响范围算法的CSVR-PFA算法，能够有效综合规避问题1～问题5，使机器人顺利实现实时路径规划。

4.4 算法对比仿真研究

图25 PFA、DWA、CSVR-PFA仿真结果对比图Fig.25 Comparison of simulated results of PFA, DWA and CSVR-PFA

为进一步验证本文方法的有效性与优越性，在复杂障碍环境下，将本文所提方法CSVR-PFA与PFA、动态窗口法(DWA)、A*算法、快速随机树(RRT) 算法进行了20次仿真对比实验。设有一个100 m×100 m的机器人移动空间，起点坐标(0 m,0 m)，目标点坐标100 m×100 m，机器人需沿途规避多个凹凸障碍区域抵达目标点完成任务。在此环境下，各算法仿真结果如图25、图26和表1所示。

图26 A*算法、RRT算法、CSVR-PFA仿真结果对比图Fig.26 Comparison of simulated results of A* algorithm, RRT algorithm and CSVR-PFA

表1 对比算法仿真结果Tab.1 Comparation of the algorithms’ simulated results

图25是本文所提算法与PFA、DWA两种经典局部路径规划算法的对比结果；图26是本文所提算法与A*、RRT两种经典全局路径规划算法的对比结果；表1是20次仿真实验后各算法的平均规划时间与平均路径长度。

由图25可以看出：PFA算法的结果中包含大量振荡，所得路径曲折严重，DWA算法的结果十分平滑，但这两个算法在复杂凹凸障碍环境下皆陷入局部极小陷阱，机器人无法抵达目标点；与之相比，本文提出的CSVR-PFA算法在传感器存在一定的测量误差情况下也能成功为机器人规划出一条到达目标点的路径，且所得路径较为平直光滑，具有较好的规划性能与稳定性。

由图26看出，RRT算法和A*算法都成功规划出到达目标点的路径，但两者的路径都较为曲折，不够平直光滑。同时，由于RRT算法和A*算法需要对地图进行栅格化处理，所以对于一些形状的障碍(如圆形)，栅格化的地图对这些障碍的描述精度较低，这也导致RRT算法和A*算法的规划结果在路径精度方面的局限性。仿真中发现，若提高栅格化分辨率，虽然能够提升规划路径的精度，但同时也会导致算法花费时间的急剧提高。与之相比，本文的CSVR-PFA算法规划出的路径更加平直光滑，并且由于不需要对地图栅格化处理，对路径点位置没有限制，可以充分探测到各种形状的障碍区边缘，规划出的路径精度更好。结合表1可看出，本文算法在规划时间与路径长度上都明显优于RRT和A*算法(栅格分辨率为1 m)，验证了本文算法的优越性。

延长最右侧长条形障碍，模拟出口十分狭窄的障碍环境。对RRT算法、A*算法和CSVR-PFA进一步仿真对比，结果如图27和图28所示。

图27 A*算法和CSVR-PFA结果对比图Fig.27 Comparison of simulated results of A* algorithm and CSVR-PFA

图28 RRT算法搜索过程图Fig.28 Search process of RRT algorithm

对于RRT算法，由于出口过于狭窄，被搜索到的概率大大降低，最终经过15 000次搜索后，未能寻找到抵达目标点的路径，规划失败。另一方面，与先前的仿真结果类似，A*算法能够规划出抵达目标点的路径，但依旧存在曲折不够平滑、精度低、长度大等缺点。本文算法在出口狭窄的障碍环境下，依然能够为机器人快速地规划出一条较为平直光滑、精度高、长度较短的可行路径。