面向未知地图的六足机器人路径规划算法

杨 洋，童东兵*，陈巧玉

(1.上海工程技术大学电子电气工程学院，上海201620; 2.上海立信会计金融学院统计与数学学院，上海201620)

(*通信作者电子邮箱tongdongbing@163.com)

0 引言

近年来，仿生机器人的研究逐渐成为机器人领域的热门话题，六足机器人作为其中的典型对象，有着丰富的步态和良好的地形适应能力，在排爆、探矿、检测等方面有广泛的应用。在实际的应用环境中，由于环境的复杂性和多样性，使用传统的全局路径规划难以适应需求，局部路径规划由于对未知环境具有较强适应性和实时性，逐渐取代了全局路径规划。

文献［1］提出了一种具有三个路径点的多项式插值算法，减少了冲击，保证了机器人运动的平稳性，并减少了转矩突变带来的影响。文献［2］中，针对机器人在视觉导航系统中的鲁棒性受到运动模糊制约的问题，提出一种基于运动模糊特征的实时性异常探测方法。文献［3－5］采用人工势场法对机器人的路径进行规划，文献［3，5］通过对斥力函数进行修改、加入中间目标点等方法，优化了机器人的路径，减少了行走路径震荡;文献［4］通过修改引力函数和斥力函数，引入“逃脱力”的方法来解决路径规划中的局部最小问题。文献［6］利用人工势场法求解多机器人路径规划，机器人之间的轨迹相互排斥，得到最优路径。在文献［7－11］中，使用自反馈生物激励神经网络、自适应模糊神经网络和径向基神经网络来解决路径规划问题。在文献［12］中，提出了一种改进人工势场法，该方法是在引力函数中增加了一个指数项，用于消除奇异值点，避免了行走路径震荡现象的发生，同时，在斥力函数中引入敏感度这个概念，用于克服传统的人工势场法中目标点与障碍物过近，导致无法到达的现象;但是仅仅通过修改或者添加力的手段是很难做到适应大部分地图的，而且人为痕迹明显。文献［13］针对蚁群算法收敛速度慢和人工势场法容易落入陷阱的问题，提出了一种基于栅格地图的局部搜索寻优算法，减少了蚁群算法的计算量，提高了机器人对障碍物的规避能力。在文献［14］中，使用飞行机器人来对地面数据进行扫描，得到地形和海拔等数据，根据数据建立3D模型，在3D模型的基础上进行路径规划。文献［15］提出了一种用于机器人的角速度约束的非线性跟踪控制算法，利用模糊规划，得到了实际避障的路径。在文献［16－19］中，使用粒子群算法对机器人的路径进行规划。文献［20］提出了一种基于演示的最优路径规划方法，用于重型设备的组装。尽管以上方法可以实现机器人的路径规划，但主要侧重于机器人在已知地图上的路径规划，对于未知地图，往往依赖于全局地形检测手段，而且这类手段可能无法得到适用于当前机器人的地形数据。本文使用测距与模糊规则，进行局部地图的随动构建，由此通过采用一种新的人工势场函数和路径钝化操作来对机器人的路径进行规划，避免了障碍物周围的抖动问题，提高了机器人对环境的实时响应能力。

1 基于测距和模糊规则的声源识别算法

1.1 机器人视窗

设定基于超声测距仪的机器人视窗，每个视窗由一组测距仪提供，共6个视窗。如图1所示，使用超声波传感器，测距仪感应角15°。

在前进方向上，设定水平方向测距仪一个，斜向下测距仪一个，如图2所示。

图2 机器人传感器位置Fig.2 Sensor position of robot

对于水平测距仪组，在起始时刻测得机器人到障碍物的距离w，随着机器人的前进，测得下一时刻机器人到障碍物的距离w'，两个传感器之间固定距离为l，那么，起始时刻夹角θ、下一时刻夹角θ'、障碍物与预定路线距离s、剩余距离l'与机器人前进方向之间的关系如图3所示。

图3 水平测距仪组测距Fig.3 Ranging of horizontal range finder group

于是有如下的关系:

解上述方程，可以得到需要的参数:

对相近的离散时刻t1和t2，计算障碍物与路径的距离与这段时间内前进的距离d之比g=Δs/d=(s1－s2)/d为障碍物形状变化率。

同样的，斜向下测距仪起始时刻夹角γ、下一时刻夹角γ'、剩余预定路线l″和障碍物与预定路线高度差h″、前进方向之间的关系如图4所示。

图4 斜向下测距仪组测距Fig.4 Ranging of oblique down range finder group

有如下的关系:

同样的，解方程:

则障碍物离地高度为h=h'－h″。

对不同时刻t1和t2，计算障碍物离地高度差与前进距离d之比f=Δh/d=(h1－h2)/d，近似作为地形高度变化的梯度数据。

1.2 障碍物模糊判定

模糊控制器的输入为1.1节算得的形状变化率、近似梯度值，左1与右1测得的距离值，对于前下测距仪的近似梯度值f，记DPD为向下陡坡，HP为缓坡，DPU为向上陡坡，其隶属度函数图像如图5所示。

图5 前下测距仪隶属度函数图像Fig.5 Membership function image of front down range finder

对于前测距仪的形状变化率g，记Convergence为收敛，也就是向靠近预定路线的方向弯曲;MD为近似平直;Divergence为发散，也就是向远离预定路线的方向弯曲。其隶属度函数如图6所示。

图6 前测距仪隶属度函数图像Fig.6 Membership function image of front range finder

对于左1和右1测距仪，记N为近距离，M为中等距离，F为远距离。其隶属度函数如图7所示。

图7 左1、右1测距仪隶属度函数图像Fig.7 Membership function image of left 1 and right 1 range finders

采用Madami规则对障碍物形状进行判定，其中U为楔形走廊，D为喇叭形走廊，Flat为平直走廊，Obstacle为障碍物。

模糊规则共19条:R=Ri，i为规则序号。

由于规则条目较多，很难通过一两条式子表示总模糊关系，这里直接使用逻辑语句来表示，在程序内部也直接表示为条件语句。

这里例举其中Obstacle的部分关系:

1)If(Down is DP_D)then(OP is Obstacle)。

2)If(Down is DP_U)then(OP is Obstacle)。

3)If(Down is HP)and(front is Convergence)and(left 1 is N)and(right 1 is N)then(OP is Obstacle)。

简而言之，对于陡坡，无论下降与否，都认为是无法翻越的障碍物。对于同方向的障碍物，认为是平直，对于方向相反的障碍物，依据其方向的不同分为楔形走廊或者喇叭形走廊。

根据近似梯度对障碍物进行判断，依据其离地高度h和机器人腿长len。若h＜1.3len，则认为满足跨越条件;若h≥1.3len，则认为不能跨越，需要绕行。

两次判断取交集，即可得到在二维地图上的障碍物与非障碍物。

1.3 构建局部地图

在测定点设置激光发生器和相应的舵机，机器人上设置接收器(光敏传感器组)。在初始的时刻需要将激光束对准在传感器组中央。测量时先从测定点射出脉冲激光束，接着光敏传感器组接收到脉冲激光束。通过测量接收激光束的传感器与传感器组中央的传感器之间的距离，来判断舵机转角的大小。若接收激光束的传感器在传感器组中央的传感器的左边，则舵机向右转;反之，向左转。

如图8所示，sd为测定点到目标点的距离，sr为测定点到机器人的距离，测量测定点到机器人与测定点到目标点的夹角为β，显然，可以得到机器人到目标点与测定点到机器人之间的夹角α与距离。

图8 目标点构建示意图Fig.8 Schematic diagram of target point construction

记录从开始到当前的时间内，机器人测得的地形数据，构建局部地图，依照局部地图规划路径逐渐逼近全局路径。

为了方便计算，取机器人形心进行计算，设立测量点为坐标原点，机器人初始位置在横坐标轴上，如图9所示。

图9 障碍检测示意图Fig.9 Schematic diagram of obstacle detection

对于第一个检测到的障碍物，其坐标为:

其中:Sr1为初始时刻机器人到测定点之间的距离，在无阻挡的条件下;α1为初始时刻机器人到目标点与机器人到测定点之间的夹角;θ1为第一个障碍物与预定路线之间的夹角;w'1为第一个障碍物与机器人之间的距离。

对于第二个障碍物，此时机器人的坐标为:

其中:ηi为截止第二次转向前的第i次转向角度;li为截止第二次转向前的第i次转向走过的路程。

此时障碍物的坐标为:

其中，δ2=α1+∑ηi为当前机器人水平方向夹角。

由此，可以推出第m个障碍物的坐标:

随着机器人的运动，上述算法不停地录入地形数据，一步一步地构建局部地图，从而实现从局部地图到全局地图的构建。

2 人工势场法路径规划

人工势场法，假设空间中存在着虚拟的力场，机器人在力场中受力运动。这里的力场包括:源自目标点的引力场，即目标点对机器人产生引力，引导机器人向目标点运动;源自障碍物的斥力场，即障碍物对机器人产生斥力，避免机器人与障碍物发生碰撞。

人工势场法特点是机器人的运动由其当前受到的合力决定，相比其他路径算法计算更加简单，实时性更强。但是一般的势场法存在一定的问题，如容易落入陷阱、靠近障碍物发生路径震荡、存在局部最小等。

为了应对上述问题，这里使用一种改进的斥力函数:

其中:Frep1是从障碍物指向机器人的斥力;Frep2从机器人指向目标点的斥力;ρ为斥力影响距离;P为机器人的位置;Pobs为障碍物的位置;Pgoal为目标点的位置;ΔPobs为机器人到障碍物的距离;ΔPgoal为机器人到目标点的距离;krep为斥力增益系数;a为斥力影响因子。

那么，机器人受到的总斥力Frep为Frep=Frep1+Frep2。

机器人受到的引力Fatt为珔Fatt=katt|P－Pgoal|，方向从机器人指向目标点，katt为引力增益系数。

于是，机器人受到的合力Ftotal为Ftotal=Frep+Fatt，受力情况如图10所示。

图10 机器人所受合力Fig.10 Resultant force of robot

实际上，由于局部地图存在障碍物探测不全、障碍物探测有误的局限性和在一些情况下路径存在一定的震荡等问题，这里采用“钝化”操作，是指机器人不需要严格依照规划的路径运动。在已经规划完毕的路径上，依照规划路径前进延迟时间为:Tload(Tload＜Lmax/(8v))。其中:Lmax为最大探测距离;v为机器人前进速度，依照新探测到的地图重新进行路径规划，再前进Tload，如此往复，直至到达目标点。

本文提出的算法流程如图11所示。

图11 本文算法流程Fig.11 Flow chart of proposed algorithm

首先进行的是初始位置检测，以建立坐标系;在设定完目标点之后，机器人通过声源检测来构建局部地图，在此基础上进行局部的路径规划并前进一段路程，然后进行局部路径规划，如此往复，直到到达目标点

3 实验仿真与分析

设定引力增益系数为15，斥力增益系数为5，斥力影响因子为0.5，斥力影响距离为4，延迟时间为3，机器人初始位置为(－10，0)，使用两幅地图进行仿真，结果如图12所示。

图12(b)中虚线为采用了钝化操作后的随机障碍物下机器人的路径。将图12(a)与图12(b)相对比可以看到，改进型势场法与传统的势场法相比，不会出现与障碍物相碰撞的情况。

图12(c)描述的是在存在凹凸障碍物的情况下，使用改进型势场法，机器人的行走路径。机器人首先需要经过两个圆形障碍物，随后接近凹凸障碍物，受到障碍物上边沿的斥力，路径将稍许下偏，进一步前进时受到来自障碍物底部的斥力，随后在引力的作用下沿着墙壁前进，最后绕过障碍物，绕行过程中存在一定路径震荡，采用钝化操作，以消除大部分路径震荡的影响。

图12 不同障碍物时使用不同方法机器人的路径Fig.12 Robot's path of different methods with different obstacles

4 结语

本文针对未知地图情况下六足机器人的路径规划，提出一种声源识别算法，用于构建局部地图;使用改进的人工势场法，实现六足机器人的路径规划。本文主要工作如下:

1)考虑六足机器人的行动能力和对地形的适应能力，通过构建机器人视窗，设计测距仪组，结合模糊算法，解决了对于普通移动机器来说是障碍物，对六足机器人而言不是障碍物的问题。

2)基于1)构建适用于六足机器人的局部地图，随着机器人的前进，不停录入数据，提高了机器人对地形变化的反应能力和实时性。

3)在人工势场法中，使用新的斥力函数，通过结合钝化操作，有效解决了传统的势场函数在障碍物附近的路径震荡问题，对障碍物的兼容性更好。

未来将考虑对于路径钝化的延迟时间和引力函数的优化。同时，由于测距仪不可避免地会存在精度问题，需要设计算法以应对探测盲区。