基于改进人工势场法的机械臂避障路径规划

,

(浙江工业大学 信息工程学院，杭州 310023)

0 引言

路径规划[1]一直都是机器人领域中的重点难点，是机器人顺利完成指定要求的前提条件，而避障路径规划[2]是指在给定障碍物以及起始点和目标点的环境中，找出一条能到达目标点并且能够安全、无碰撞地到通过所有障碍。

常见的路径规划方法有基于局部信息的人工势场法[3]等和基于全局信息的自由空间法[4]等两大类，其中第一种是一种经典的最优规划算法，它是把环境信息转变为机械臂所受到的势能，并在该合势能的驱动下运动，由于其不需要全局信息，结构简单，生成路径平滑，规划时间短等优点所以非常适合静态环境下的实时路径规划，但由于并不是全局规划算法，所以容易进入局部极小点。针对此问题，很多学者对算法进行了改进，比如修改势能函数[5]消除局部极小点，加入虚拟目标点或者虚拟障碍物[6-7]来改变机械臂周围环境从而跳出局部极小点，结合遗传算法等其他智能算法退出局部极小点[8]，采用沿墙跟踪算法[9]解决局部极小点问题等。

本文以NAO机器人为平台，对运动学正解、碰撞检测和路径规划等方面进行了研究，针对如何提高机械臂的有效空间，提出使用椭球包围盒对障碍物进行建模；针对人工势场法易陷入局部极小点的问题，则采用RRT[10-12]方法对其进行局部改进。

1 机械臂运动学建模与分析

1.1 机械臂运动学模型

本文基于NAO机器人平台进行建模，其机械臂有5个自由度，具体结构如图1所示，肩关节有两个自由度，肘关节有两个自由度，腕关节有一个自由度，因为腕关节不影响机械臂末端位置，因此角度固定为0。

图1 NAO机械臂结构

通过图1所示，运用D-H法[13]可以建立NAO两个连杆的坐标系，如图2所示。

图2 NAO机械臂各连杆坐标系

根据图2建立的NAO两个连杆的坐标系和NAO机器人手臂的各项参数，可以得到NAO手臂的Modified D-H参数，如表1所示。

表1 NAO机械臂MDH参数

其中，αi代表扭角，为常量；ai代表连杆长度，为常量；di代表连杆偏置距离，因为NAO机械臂均为为转动关节，因此为常量；θi代表关节角，因为NAO机械臂均为为转动关节，所以θi均为变量。

1.2 机械臂运动学分析

在获取MDH参数后，我们可以用关节角的变化来确定末端的位姿，这称为机械臂的正运动学分析。在确定好基坐标系之后，我们通过相邻两连杆之间的变换矩阵通式(1)[13]来计算末端执行器相对于基坐标系的位姿。

(1)

将DH参数代入上式(1)，可得连杆之间的齐次变换矩阵：

其中：c1s1是cosθ1sinθ1的简写，Ci-1Si-1是cosαi-1sinαi-1的简写，将以上连杆齐次变换矩阵相乘可得末端相对于基座标的位姿，如式(2)：

0Te=0T11T22T33T44T5

(2)

2 碰撞检测

碰撞检测是避障路径规划的重要部分，想要机械臂在规定时间内找出一条无碰的可行路径，需要对障碍物进行合理建模，之后进行碰撞检测和分析。

2.1 障碍物建模

由于机械臂的工作环境不同，有的机械臂面对的是提前规划好的规则障碍物，例如工业机械臂，然而更多的时候，机械臂运行环境中的障碍物都是不规则形状，想要精确建模需要花费大量时间，因此我们一般采取包围盒法[14]对障碍物进行近似建模。

包围盒法建模有许多种类，例如球体，AABB，OBB，K-Dops等包围盒[15]，但是不同的包围盒的简单性和紧密性是一对矛盾体[16]，由于机械臂是一个高度耦合的非线性系统，越大的工作空间越有利于控制，而且我们又希望对机械臂进行实时控制，复杂的包围盒建立与碰撞检测需要更多的时间，因此综合考虑，我们采用椭球包围盒来近似地对障碍物进行建模，此种方法建模难度虽然和常用的球体包围盒相比增大了，但是紧密性较高，碰撞分析也不复杂。

首先给出椭球的表达式(3)：

(3)

其中：(x0,y0,z0)为椭球的中心点，a,b,c为椭球的三个轴长，有了这6个参数，我们便可以建立障碍物的椭球包围盒模型。为了获取上述6个参数，我们将障碍物分别在xoy，yoz和xoz投影，从而获取障碍物在x，y，z轴上的投影的最大值和最小值xmin,xmax,ymin,ymax,zmin,zmax，通过计算我们可以得出椭球中心点如下式(4)：

(4)

根据获取的障碍物数据，我们可以任选一个点，例如点(xmax,ymax,zmax)，利用拉格朗日求极值方法获取包围障碍物的体积最小的椭球的剩余三个参数a,b,c。

首先我们列出计算椭球的体积公式(5)：

(5)

然后将我们任取的一点带入椭球方程得式(6)：

(6)

此时，只需要求出参数a,b,c就可以计算出椭球的完整参数方程，这里通过拉格朗日求极值方法，我们可以写出拉格朗日函数(7)：

L=V+k·F

(7)

对式(7)求出a,b,c三个参数的偏导数，并令其都为0，同时我们令式(6)也为0，联立方程如下式(8)：

(8)

通过式(8)我们可以求出椭球的剩余3个参数a,b,c。至此，我们已经完全求出了包围障碍物的最小的椭球参数方程。

2.2 碰撞检测

进行碰撞检测的时候，我们要将机械臂的半径也加在障碍物模型中，使其简化为一条线段，并通过DH参数法计算出机械臂连杆两端的坐标，从而确定线段的参数方程(9)。

(9)

将上式带入到椭球的方程(3)中可得式(10)：

(10)

我们对式(10)进行求解，可以求得k的值，通过对其值进行分析，可以判断连杆和障碍物是否碰撞，具体情况如表2。

表2 碰撞检测分析

3 改进人工势场法

3.1 人工势场法基本原理

人工势场法是一种虚拟力法，在机械臂的工作环境中由目标点产生虚拟的引力势场，由障碍物产生一个虚拟的斥力势场，其中的引力势场和斥力势场函数可以由研究人员根据不同情况自己设定。机械臂在合势能的驱动下，自动向合势能的下降方向运动。传统引力和斥力函数分别为式(11)和式(12)。

(11)

(12)

引力势场的作用领域为全局，斥力势场的作用领域为局部，其中ka为引力系数，kr为斥力系数，p为机械臂末端位置，pg为目标点位置，d为椭球到机械臂关节的距离，d0为斥力势场的作用范围。对于移动机器人来说，如果可以将其看作质点的话，某个障碍物只会对其产生一个排斥力，但是对于机械臂来说，每个关节都会受到一个障碍物的产生的斥力，本文实验平台为NAO机器人机械臂，其手臂为仿人形机械臂，所以会有肘部和腕部两个关节受到斥力，因此斥力势能为两个关节受到的斥力势能之和，如式(13)：

Urep=Urepw+Urepe

(13)

其中Urepw,Urepe分别是腕(wrist)部和肘部(elbow)关节受到的斥力势能，则整个机械臂的合势能为(14)：

U=Uatt+Urep

(14)

推断出合势能公式之后，我们搜寻一条合势能下降最快的方向，驱动机械臂运动，图3为其原理图。

图3 人工势场法原理图

由于人工势场法是一种局部搜索方法，很容易就陷入局部极小点或者在某点附近振荡，在使用的时候需要格外关注这种目标不可达的情况。针对这种缺陷，本文采用了添加虚拟目标点的思想，在全局寻找路径的时候采用人工势场法，当机械臂陷入到局部极小点的时候，转而采用RRT方法，自动生成一个新的目标点，从而通过改变合势场大小和方向，达到跳出局部极小点的目的。

3.2 RRT方法基本原理

RRT方法是LaValle教授于1998年提出的一种基于随机采样的运动规划算法，基本思想用在机械臂上就是将其末端的初始位置作为根节点，对其工作空间进行随机采样，通过特定的扩展方式不停地增加叶子节点，直到到达目标点区域后停止，随后按照一定的评价函数在生成的树中确定一条从初始节点到目标点的路径，这个评价函数可以是距离最近，步数最少或者时间最短等。

其中的扩展方式的基本方法是随机选取一个工作空间中的点设为qtarget，然后计算树中节点与qtarget欧式距离最近的点称为qnearest，从qnearest开始向qtarget方向扩展L距离，设为qnew，判断qnearest到qnew这段新生成的树枝是否和障碍物碰撞，碰撞则舍弃，重新选取qtarget，否则加入到快速树的节点中。

RRT方法可以对机械臂进行完整的避障路径规划，并且只要参数设置合理，并不会出现陷入局部极小点的现象，但是由于其采用随机采样生成快速随机扩展树，所以扩展会过于平均，整个算法的效率较低。而两种方法相结合，既克服了人工势场法容易陷入局部极小点或振荡点的缺陷，又没有加入RRT方法效率低下的缺点。

3.3 改进人工势场法

整个算法的基本思想是在使用人工势场法陷入极小点或振荡点的时候，将此点设置为RRT树的初始节点，开始进行有限次数的随机扩展，然后计算出生成的随机扩展树中节点距障碍物欧氏距离最远的一个叶子节点作为目标点再次使用人工势场法进行接下来的路径规划。

下面是具体步骤：

1)使用第1，2节的方法分别建立机械臂和障碍物的模型。

2)设置目标点。

3)遍历机械臂的相邻关节角，分别为θi-λ,θi,θi+λ，其中θi为当前关节角，λ为步进步长。

4)根据第1节的正运动学分析，分别获取腕关节和肘关节的位置。

5)使用第2节的方法对NAO机器人的机械臂进行碰撞检测，如果碰撞了则舍弃并跳回到步骤3)；如若没有，则分别求出腕关节和肘关节距障碍物的欧氏距离，以及腕关节到目标点的欧氏距离。

6)将步骤5)求得的距离信息带入式(14)中求合势能，记录遍历中的最小合势能的机械臂末端位置。

7)将步骤6)求得的末端位置与前5次遍历求得的机械臂末端位置队列进行比较，如果出现在队列中，证明进入了极值点或者目标点，此时进入步骤8)，否则进入步骤9)。

8)判断末端执行器位置是否为最终目标点，如果是则算法结束，否则判断是否是产生的虚拟目标点；如果是，我们将算法中的目标点替换为最终目标点，并跳回至步骤2，如果以上两者都不满足，则进入局部极小点处理流程，进入步骤10)。

9)机械臂运动到合势能最小的位置。

10)将得到的局部极小点或振荡点设为qinit，即为RRT树的初始根节点。

11)设置循环次数N，迭代N次后停止算法。

12)结合机械臂的工作空间和障碍物位置并使用随机函数产生空间随机点，并设为qtarget。

13)求出RRT树中距qtarget距离最近的节点qnear，给定欧式距离L，求出qnear与qtarget连线上距qnear节点长为L的点，设为qnew。

14)判断qnear到qnew的空间线段是否和障碍物有碰撞，有碰撞则舍弃并跳回到步骤12，否则将qnew节点加到树中。

15)判断迭代是否完成，未完成则跳转到步骤12)，完成则求出随机树节点中除根节点以外的距障碍物最远的节点，将其设置为人工势场法的虚拟目标点，即跳回到步骤2)。

其中qnew点扩展公式如式(15)：

(15)

图4是算法的流程图。

图4 算法流程图

4 仿真实验

本文使用Matlab的数值仿真来模拟机械臂运动情况，进行两组数值实验，第一组是验证使用椭球包围盒的优越性，第二组是验证能否成功跳出局部极小点或振荡点。

第一组实验具体参数设置如表3。

表3 实验参数设置

机械臂初始关节角设定如下：

(θ1,θ2,θ3,θ4,θ5)=

(1.000°,0.500°,0.500°,-1.000°,0°)

此时机械臂成下垂状态。

目标点设定如下：

(x,y,z)=(12 cm,8 cm,1 cm)

障碍物位置如下：

(xmin,xmax,ymin,ymax,zmin,zmax)=

(3 cm,7 cm,4 cm,8 cm,0 cm,10 cm)

图5展示了采用球体包围盒时机械臂的运行情况。

图5 使用球体包围盒时机 图6 使用椭球包围盒时机械臂运行情况展示 械臂运行情况展示

由图5可以看出使用球体包围盒时目标点在障碍物模型中，此时会因为算法中针对这种情况的报错而直接停止。图6是相同的障碍物设置情况下使用椭球包围盒进行障碍物建模时的机械臂运动情况。

如图所示，机械臂准确地到达了目标点，误差在10-4数量级，而且仅步进了166步，满足了机械臂控制的精确性和实时性，如果对控制精度要求不高，对实时性的要求比较高的情况下，可以增加步进步长，这样可以大幅提高规划速度，但是要牺牲一定的精确度。

由此可见相同障碍物的情况下，使用椭球包围盒所占用的体积远远小于球体包围盒，此时机械臂可以到达目标点，而使用球体包围盒却不能到达目标点。

第二组实验是算法进入极小点的情况，实验参数设置如表3。

机械臂初始关节角设定如下：

(θ1,θ2,θ3,θ4,θ5)=(0.5°,-90°,0°,-45°,0°)

目标点设定如下：

(x,y,z)=(14.46 cm,5 cm,3.96 cm)

障碍物设定如下：

(xmin,xmax,ymin,ymax,zmin,zmax)=

(14 cm,15 cm,1.5 cm,2.5 cm,3 cm,5 cm)

图7是机械臂此时的状态和障碍物还有目标点的位置。

图7 机械臂陷入局部极小点

进入局部极小点之后直接跳转到RRT算法，快速计算出一个虚拟目标点，具体情况如图8所示。

图8 RRT算法产生虚拟目标点

在由RRT算法产生虚拟目标点之后，我们将其设置为虚拟目标点，本次实验中获取的虚拟目标点如下：

(x,y,z)=(12.89 cm,0.44 cm,2.04 cm)

图9 机械臂末端运行轨迹

由图9可知，机械臂的末端从局部极小点开始向寻找到的虚拟目标点运动，运动到虚拟目标点之后将最终的目标点设定为算法中的目标点之后，机械臂继续向着最终目标点运动，已经跳出了局部极小点，图10是机械臂整体运行情况的展示。

如图10所示，机械臂从局部极小点经由RRT产生的虚拟目标点最终到达真正的目标点，从局部极小点到虚拟目标点仅需要34步，从虚拟目标点到真正目标点仅需46步，误差同样保持在10-4数量级，整个过程快速精确，可以满足机械臂的实时控制。

为了验证本文方法的适用范围，针对正常情况和陷入极值点情况分别做了10组实验，实验参数设置均能使机械臂在工作空间内与障碍物不碰撞的情况下到达目标点，正常情况下的10次实验均规划成功，步进步数随着障碍物和目标点距离初始点的远近不等，最少步进步数为132步，最多步进步数为268步；进入局部极小点情况下10次实验均规划成功，其中有1次实验进行了2次RRT算法寻找虚拟目标点的过程，没有实验进行3次及以上RRT算法过程，步进步数随着障碍物和目标点距离初始点的远近不等，最少步进步数为80步，最多步进步数为197步，其中从局部极小点到虚拟目标点最少步进步数为34，最多步进步数仅为91。

5 结束语

本文针对工作空间狭小的机械臂提出了使用椭球包围盒建模方法，使机械臂能在有限的工作空间内尽量正常工作；针对人工势场法这种算法容易陷入局部极小点或振荡点的缺陷，提出了采用RRT算法来动态产生虚拟目标点的方法，通过此点来改变机械臂的周围环境来跳出局部极小点的目的，相对于通过几何方法算出虚拟目标点而言，采用动态虚拟目标点，不需要进行虚拟目标点的计算，整个过程快速准确，随着障碍物的变动，不需要重新计算虚拟目标点，算法的适应性更强，更加适合机械臂的实时控制。本文下一步的研究方向则是针对多障碍物和移动障碍物进行实时避障路径规划，进一步提高本方法的适用范围，同时提高规划速度。