融栅格法和人工势场法的机器人三维路径规划

葛笑寒

（1.河南省高校节能照明工程技术研究中心；2.三门峡职业技术学院，河南三门峡472000）

0 引言

随着技术的进步和经济的发展，机器人技术更加智能化、多元化。工业生产中对其稳定性和可靠性要求越来越高。这就对其导航系统有了更严格的要求。在局部路径规划方面，人工势场法的核心是把机器人在环境的运行轨迹抽象成一种虚拟受力运动[1]。栅格法又称为单元分解建模法，其本质是把机器人的运行空间细分为很多个二维信息的位置，将机器人工作空间划分为具有多个二值信息的区域，也即栅格[2]。文献[3]则以改进人工势场法为主，栅格法为辅的方案来实施避障，使得机器人能够尽快地脱离局部极小并成功地绕过障碍物到达目标点。但建模环境为二维，具有局限性。文献[4]对移动机器人三维空间路径规划问题,提出了一种基于改进蚁群算法的路径规划算法。结果证明了算法的低耗时和实用性。但计算量大，易陷入局部最优解。本文结合栅格和人工势场法，使机器人沿着位势减小的方向，避开障碍物，最终达到目标点。同时利用栅格三维模型把运行轨迹拓展至立体空间。

1 栅格法三维空间建模

将机器人运动的区域成从真实的环境中抽象出三维空间模型，并细分多个三维区域，且每个区域都用三维信息表示，最后通过优化算法实现路径搜索[5]。如图1所示，将左下角顶点作为三维坐标的原点建立三维坐标系统。x轴和y轴分别为纵向横向增加方向，z轴为高度增加方向，这样就组成了三维空间。该区域即为三维路径规划空间，然后把坐标等分，就形成栅格三维空间。在此空间中把目标点、移动机器人和障碍物都看作是一个质点，用x轴、y轴和z轴三个方向表示它在空间的位置。每个质点不能超越分割后的小立体栅格的边界。在该空间中，用黑色圆点代表障碍物，则黑点存在的空间位置表示障碍物所在位置；黑色方格代表目标点位置已经被目标点占据。假设三维栅格地图尺度和坐标不变，各个立体栅格也是静止不变，则要求目标点和障碍物在空间中运行时不能相互碰撞，也即它们任意时刻不能同时占用同一个栅格。它们的速度和运行方向随机生成，检测到边缘栅格时按相反方向运行。

工作流程如图2所示,首先确定起始点、目标点和障碍物，其次搜索机器人的相邻栅格并进一步选出自由栅格。计算自由栅格的中心点与目标栅格中心点的距离，然后推算机器人下一步的行进轨迹，判定机器人质点是否已经到达目标栅格，如果没有则返回继续搜索栅格，如果到达则任务完成。

三维机器人路径规划的数学模型与二维环境下的比较相似，从起点出发，在三维空间以路径最短为目标，规避障碍物到达终点。三维路径规划即找到一条可行的最短区域L。要求机器人行走的每一步只能朝着一个方向移动一步，可以是前后、左右或是上下。当遇到障碍栅格时，须避免碰撞。在机器人移动过程中，需移动到目标点后，方可停歇。移动包括横向、纵向及前后的位置移动，希望机器人以尽量少的总位移达到既定目标。任意时刻，障碍物和目标点的运动方向有6种，如表1所示。

图1 三维空间模型

图2 栅格法流程图

表1 机器人运动方向及坐标

2 栅格势场的建立

人工势场法的本质就是把移动机器人的工作环境虚拟成引力和斥力的合成势场，移动机器人在该势场中受到障碍物的斥力和目标点的引力的双重作用，移动机器人在两种力的作用下最终达到目标点[6]。

假设移动机器人的任意时刻的坐标为X=(x，y，z)，对应目标点的坐标为Xg=(xg，yg,zg)，则引力势场的函数表达式公式为

公式中X与Xg指的是机器人当前位置和目标值在在三维空间中的位置，k为比例位置增益，ρ2(X,Xg)是机器人坐标到目标的距离。

引力势场函数的负梯度，则定义引力公式为

斥力场常见函数

斥力函数实用公式为

当移动机器人接近目标时，如果目标点周围存在障碍物，则斥力场远高于引力场，移动机器人将不能运动到目标。可引入调和函数来解决[7]，调和函数是可微的，故可以保障机器人运动的轨迹的正确性。在计算出空间每个位置位势值后，机器人从起始点出发，每一步都应沿着最小位势值方向行进，一直到达目标点。如果环境突然变化，则应重新规划路径。

使用人工势场法时，三维环境中的每个坐标都有一个位势值，代表移动机器人前进一步与障碍物碰撞概率的大小。取目标点坐标的位势为0，障碍物的位势值为1。其他位置的位势与时间有关。任意时间，空间某位置的位势值是该坐标在下一时刻所能够到达的所有空间坐标位置的位势的均值。它的含义是移动机器人从空间某位置坐标随机运动一步与障碍物发生碰撞的概率。值越小，代表碰撞的概率越低。

P(x,y,z,t)表示t时刻位置P(x,y,z)的位势值。S(x,y,z)指(t+1)时刻，机器人达到所有位置的可能性。在三维空间环境中，机器人每个步长可以有7种位置：向上(N)，向下(S)，向左(W)，向右（E），向前(F)，向后(B)以和停止(T)。p(x,y,z,t+1)表示机器人从P(x,y,z)出发，在(t+1)时刻，所有位置的位势值之和。

根据上式，知道(t+1)时刻的位势后，才能推算t时刻空间某个位置的位势时，即必须逆向计算。为了计算方便，需确定一个时间步长。步长L的推算流程如图3所示。建立一个空节点队列，把起始位置入列。取三维空间模型构建对角线，按照步长推算流程，找到所有对角线中按照此流程确定的的最大值作为系统的步长，然后计算三维空间中任意位置任意时间的位势。则移动机器人从起始点出发，沿位势减少的方向前进，最后避开障碍物抵达目标。

图3 确定步长流程

3 仿真及结果分析

使用MATLAB7.0建立一个边长9的立体三维栅格，并把地图9等分，这样就形成了729个小立体栅格地图。把移动机器人、目标点和障碍物看成质点，使用x,y,z代表质点在空间中的位置。系统的工作流程如图4所示，利用对角线原则及推算流程确定最大时间步长，然后计算空间所有位置任意时刻的位势。则移动机器人从始点开始，沿位势值降低的方向行走，最后到达目标。

图4 机器人行进流程

图5 三维环境下机器人的运行轨迹

在三维空间环境中机器人起始点坐标(2,1,1),目标点(8,9,9)，运动方向在七个方向中随机产生，系统中设30个障碍物，方向和速度随机。据对角线确定步长L=16，在该空间中，用黑色圆点代表障碍物，黑方格代表目标位置点。行走方向由速度矢量决定，行进至空间边界时，速度大小恒定，反方向运动。大黑圆圈代表移动机器人任意时刻的行走位置，把这些点用平滑导线连接就是机器人的行进路线。机器人在仿真条件下的运行轨迹如图5所示，从图5可以看出，机器人从起始位置开始，沿着位势降低的方向行走，此方向发生碰撞的概率最低最小，最后移动机器人到达目标点。

4 结论

通过栅格法建立了机器人的三维空间模型，对人工势场算法做了分析。在空间中确定了栅格的信息，并且根据对角线原则确定了达到目标点的时间步长。然后计算三维空间位置每个时刻的位势值。机器人从原点出发沿着位势降低的方向最终到达目标点。最后在MATLAB环境下，建立三维空间模型。仿真结果表明该方法在三维空间能找到最优的路径，且避开障碍物到达目标点。