改进人工势场法的水陆两栖机器人路径规划*

马文宇，许云猛，王玉甲

(哈尔滨工程大学机电工程学院，哈尔滨 150001)

0 引言

水陆两栖机器人能够在海洋、陆地及海陆过渡环境下工作，帮助人类执行搜索、监测、运输等任务，是目前人类在近海域研究领域内的重点之一[1]。由于水陆两栖机器人工作时的复杂环境，其路径规划研究成为两栖机器人智能控制的核心问题之一。

在现有的路径规划算法当中，人工势场法是一种运算量小，算法结构简单的方法，常被用于各种无人平台的路径规划中。但传统人工势场法存在两个问题：目标不可达与局部平衡[2]。姚靖靖等[3]通过对障碍物作用范围进行限制和添加虚拟目标的方法，使机器人规避障碍物，解决到达不了目标点的问题；刘忠等[4]结合模拟退火法解决局部陷入最小值的问题；GEVA等[5]在斥力势场中加入动态的斥力因子来解决目标不可达问题；SUN等[6]为克服目标不可达问题，设置动态步长，减小障碍物斥力影响；ZHANG等[7]利用概率论知识重新建立势场模型来解决目标不可达的问题。相较于单一环境下工作的无人平台，水陆两栖机器人工作在二维叠加三维的复杂环境中，传统人工势场法在处理三维环境路径规划方面还少有研究。

本文提出利用改进人工势场法对水陆两栖机器人进行路径规划，建立改进的人工势场与机器人运动相结合。通过调整斥力势场解决目标不可达问题，通过改变斥力作用方向来突破局部受力平衡。机器人水下运动时，通过制定水下避障策略，将机器人在三维环境中的路径规划转化为平面内的运动研究。最后利用MATLAB对传统与改进两种方法进行仿真对比，验证本文所提方法对于水陆两栖机器人路径规划研究的有效性。

1 环境建模

1.1 二维环境

水陆两栖机器人在陆地上运动属于二维环境中水平面运动，建立其二维笛卡尔坐标系，如图1所示。

将机器人的目标点和障碍物信息在该坐标系中进行描述，机器人位于坐标系原点(0,0)，目的地坐标设为(X0,Y0)；障碍物分布在机器人的任务环境中，其坐标位置设为(Xi,Yi)。

机器人在陆上属于在二维空间中运动，依靠机器人自身携带的双目视觉系统检测障碍物，当检测到前方存在障碍物时，机器人开始减速，绕开障碍物进行避障，通过障碍物后返回原本航线，为保证机器人运动过程中的安全，对机器人和障碍物进行处理，如图2所示。

图1 二维环境坐标系 图2 障碍物膨胀化处理

对障碍物进行膨胀处理是进行路径规划时一种常见的数学建模手段[8]。如果将障碍物以原形进行规划计算，会增加规划时间，降低工作效率且易增加碰撞的风险。文中将障碍物进行圆形处理，将障碍物看做一定尺寸的圆，便于进行数学计算和建立数学模型，在圆形包围处理后得到的工作环境模型中，可将该圆看作是障碍物影响范围。经处理后的机器人和障碍物的尺寸半径为Rr和Ro。考虑到机器人在运动过程中存在运动误差及保证机器人运动安全性，对两个半径乘以一定的安全系数变为Rr1和Ro1，Rd表示机器人与障碍物的中心距离，是最终的障碍物半径，其大小为：

Rd=Rr1+Ro1

(1)

膨胀化处理后的障碍物影响范围是一个以Rd为半径的圆。与此同时，经膨胀化处理后的机器人可以看作是一个点，经规划出来的路径是由一系列点构成的折线。路径规划过程就是规划一系列路径点的过程，并且连续两点之间的连线不能与经膨胀化处理后障碍物圆相交。

1.2 三维环境

利用仿真软件建立三维环境模型，如图3所示。通过仿真软件对水底地貌进行描述，模拟水陆两栖机器人的水下工作环境，水平面将整个工作区域划分为水下部分和陆上部分。路径规划在可行区域内进行，在进行路径规划时要避开障碍物，确保规划出来的路径是安全的。

机器人在水下属于在三维空间中运动，工作环境更为复杂，遇到的障碍物主要可分为小型和大型两种，水下的大型障碍物一般为礁石、大型海底废弃物等，若机器人仍像陆上避障一样选择绕行，会增加机器人的航行距离，增大推进器功耗。因此，当机器人在水下遇到大型障碍物时，采取垂直面避障[9]，选择翻越障碍物，避障示意图如图4所示。

图3 三维环境模型 图4 避障示意图

机器人依靠自身携带的双目视觉系统和避碰声呐来检测障碍物，当检测到障碍物时，机器人上浮到障碍物上方，保持一定航行高度通过障碍物。通过对水下避障策略的制定，可以限制机器人在水下时只有前行、后退及沉浮动作，因此可将对机器人在水下三维环境中的路径规划转换为对机器人在竖直平面内的运动研究。

2 人工势场法

2.1 传统人工势场法

人工势场法被应用到各种无人设备的路径规划研究当中[10]，利用“同性相斥，异性相吸”的物理原理来描述目标点和障碍物在机器人运动过程中对其产生的作用[11]，目标点作为“异性”对机器人具有吸引作用，引力随二者之间距离的增大而增大，引力势场函数可以用式(2)表示：

(2)

式中，k为引力系数且k>0；X为机器人的位置向量；XT为目标点的位置向量。

障碍物作为“同性”对机器人具有排斥作用，斥力随二者之间距离的增大而减小，斥力势场函数用式(3)表示为：

(3)

式中，kr为斥力系数且kr>0；ρ为障碍物影响范围；XO为障碍物的位置向量。

在两个势场函数的作用下机器人的综合势场函数用式(4)表示为：

(4)

机器人在综合势场的作用下不断运动，可由式(5)推导每次运动后的下一个位置。

(5)

式中，(xk,yk)为当前位置；(xk+1,yk+1)为下一步位置；l为运动步长；θ为势场合力与x轴的正向夹角。

2.2 改进人工势场法

(1)目标不可达问题。使用传统人工势场法时，随着机器人不断地向指定目标点运动，引力作用减弱，当存在于目标点周围的障碍物产生的斥力作用高于引力作用时，会导致机器人运动不到目标点[12]。若想解决此问题，需要降低斥力产生的影响[13]，从斥力势场入手，对斥力势场进行改进，改进后的势场函数用式(6)表示为：

(6)

式中，(X-XG)是机器人与目标点之间的相对距离；n是任意正实数。对上式求其负梯度可得到斥力表达式为：

(7)

通过式(7)将斥力分解为2个分量Frep1和Frep2，2个分量的表达式如式(8)所示：

(8)

图5 机器人受力分析

在斥力分解后的两个分量和引力作用下机器人的受力分析如图5所示，由图可看出，新的合力指向目标点，机器人在改进的人工势场作用下可以继续逼近目标点。

改进的斥力势场函数中的n的取值，若0

(9)

随着机器人向目标点的不断运动，相对距离(X-XG)趋于0，引力Fatt和Frep1趋于0，改进斥力势场中机器人指向目标点的分量Frep2趋于无穷大，机器人不断向目标点移动。

若n=1：

(10)

相对距离(X-XG)趋于0，引力Fatt和Frep1趋于0，改进斥力势场中机器人指向目标点的分量Frep2是大于0的正数，合力大于0，机器人不断向目标点移动。

若n>1：

(11)

随着机器人向目标点的逼近，引力Fatt和改进斥力势场下的两个分量Frep1和Frep2均趋于0，机器人朝目标点移动，最后到达目标点。

由此可看出无论n取何值，机器人都能向目标点不断运动，解决目标不可达问题。

(2)局部受力平衡问题。机器人运动过程中，在某些特殊位置会遇到引力与斥力相等的情况，如3者共线或机器人行进过程中存在多个障碍物，此时两栖机器人受到的合力为0，导致机器人局部平衡，停止运动，不能达到指定目标点，即是传统人工势场法存在的局部平衡问题[14-15]。

此时若给机器人施加一个外力，便可打破受力平衡状态，突破局部所受合力为0的局面[13]。通过调整障碍物产生的斥力角度，可改变机器人所受合力，打破原有平衡，进而向目标点运动。在上文所研究的改进后的斥力势场基础上，当机器人陷入局部所受合力为0情况时，通过旋转斥力方向来调整斥力的角度，破坏局部平衡，受力分析如图6所示。

(a) 3者共线情况 (b) 多障碍物情况

如图6a所示，当机器人、障碍物和目标点共线导致机器人局部受力为0时，顺时针旋转90°调整Frep1的方向，机器人在新的不为0的合力作用下继续向目标点运动；当多障碍物导致机器人局部受力为0时，如图6b所示，逆时针90°旋转调整障碍物b的斥力Frepb1和障碍物a的斥力Frepa1的方向，使机器人继续向目标点运动。

3 仿真实验

本节利用MATLAB仿真软件对改进后建立的新人工势场对于水陆两栖机器人的适用性进行验证，并与传统人工势场的结果进行对比。

3.1 目标不可达问题仿真实验

如图7所示为目标不可达问题仿真实验结果，机器人位于(0 m,0 m)，向目标点(10 m,10 m)运动，在目标点周围设置一个障碍物，位置为(9.5 m,10 m)，其障碍物圆的影响半径为2 m，设置安全距离0.3 m以保障机器人自身安全。由图7a使用传统方法路径规划的仿真结果可看出，机器人受障碍物影响并未运动到指定目标点，过大的斥力导致目标不可达问题；由图7b使用改进方法路径规划的仿真结果可看出改变斥力势场后，机器人最终可到达指定目标点。

(a) 传统方法路径规划 (b) 改进方法路径规划

3.2 局部合力为0仿真实验

如图8所示为机器人、障碍物、目标点3者共线导致机器人局部所受合力为0问题的仿真实验结果，机器人位于(0 m,0 m)，向目标点(10 m,10 m)运动，障碍物位于(9 m,9 m)，其障碍物圆的影响半径为2 m，设置安全距离0.3 m以保障机器人自身安全，由图8a使用传统方法路径规划的仿真结果可看出，3者共线时，机器人受局部合力为0影响，运动过程中便停止运动，未能抵达指定目标点；调整斥力角度后的仿真结果见图8b，机器人未出现局部受合力为0停止运动的情况，最终到达指定目标点。

(a) 传统方法路径规划 (b) 改进方法路径规划

如图9所示为两障碍物导致机器人局部所受合力为0问题的仿真实验结果，障碍物坐标分别为(7.5 m,8.5 m)和(8.5 m,7.5 m)，分布在机器人与目标点连线两侧，由图9a利用传统方法进行路径规划的仿真结果可看出，在两个障碍物的影响下出现局部合力为0停止运动的情况；调整斥力角度后，机器人能够穿越障碍物继续运动，仿真结果如图9b所示。

(a) 传统方法路径规划 (b) 改进方法路径规划

为进一步验证改进后的人工势场对水陆两栖机器人路径规划的适用性，随机增加障碍物的数量，且位置随机布置，仿真结果如图10所示，可以看出，当存在多个随机位置的障碍物时，改进后的人工势场仍适用于机器人的路径规划，未出现局部合力为0导致机器人中途停止的情况。

(a) 4障碍物情况 (b) 7障碍物情况

3.3 三维环境仿真实验

水陆两栖机器人除在陆地上二维环境中运动，进入水中后还要在三维环境中运动，针对改进人工势场法对其三维环境路径规划进行仿真实验。依靠水陆两栖机器人自身携带的视觉系统和避碰声呐进行障碍物检测，采用遇小型障碍物时绕行避障，遇礁石等大型障碍物时上浮翻越避障的策略，机器人由陆地向水下运动时，机器人先在水面行驶一段时间，随即开始下潜到一定深度，避免水面波浪影响。当运动到近海岸时，机器人与海岸开始接触，履带模块开始工作，通过自身履带模块沿海岸行驶出水面。在三维模型中，整个空间的大小为1000×1000×30 m，z=0为水平面，起始点坐标(100 m,900 m,0 m)，目标点坐标(900 m,100 m,3 m)，水下航行时设置定深5 m，仿真结果如图11所示。

(a) 障碍物与水平面相距较远情况 (b) 障碍物与水平面相距较近情况

由图11a可看出当机器人检测到前方存在障碍物时，机器人开始上浮，上浮到一定高度翻越障碍物后再次回到定深航向轨迹中，最后沿着海岸通过履带模块驶出水面；当障碍物与水平面相距较近时，机器人则浮出水面，在水平面上行驶通过障碍物，随后再潜入水面，返回定深航线，如图11b所示。

根据仿真实验结果可知，改进人工势场法可有效应用于水陆两栖机器人水下三维环境中运动，能够规划出一条有效且合理的路径。

4 结论

为满足水陆两栖机器人在陆上及水下能够规避各类障碍物并到达指定目标点的需求，本文提出利用改进人工势场法对其进行路径规划。在传统方法的基础上重新建立斥力势场，使机器人所受合力的方向始终指向目标点，解决目标不可达问题；旋转斥力角度，改变其作用方向，使机器人不受障碍物位置及数量影响，解决局部平衡问题。同时，通过制定机器人水下避障策略，将其在水下三维环境中的路径规划转换为在二维垂直平面内的运动研究。最后，利用MATLAB对改进人工势场法进行仿真实验，并与传统方法的仿真结果进行对比，实验结果表明本文所研究方法可有效规划水陆两栖机器人在二维及三维环境中的无碰路径，目前此方法已成功应用于实验室自制水陆两栖机器人中，可为今后水陆两栖机器人的开发及其路径规划研究提供有益参考。