一种结合球树模型的机械手人工势场空间路径规划方法与流程

本发明涉及了一种机器人自动避障方法，尤其是涉及了一种结合球树模型的机械手人工势场空间路径规划方法，涉及工业生产、物流运输等领域中广泛使用的工业机器人与服务型机器人。

背景技术：

路径规划是机器人技术中非常关键的一项技术，工业机器人与服务型机器人在运行中的迅速，精确的定位到目标点，需要使用路径规划来规划最合适的运行路径，规避障碍物，增加机器人运行的安全性，提高机器人的运行效率。环境是未知的，变化的；这就需要动态的路径规划技术，静态的路径规划已经无法满足要求。

面向工业机器人与服务型机器人的动态路径规划，需要高效碰撞检测技术，碰撞检测技术依赖于各种包围盒模型，例如aabb，obb,球包围盒模型等。

在动态路径规划算法的研究上，常用的方法有传统人工势场法、神经网络、随机路标图法、vfh类算法、遗传算法等。但是面对复杂的形体障碍物，模型描述复杂度高，同时会存在震荡现象。选用障碍物的球形包络抽象模型能使计算量得到了简化，但扩大了障碍物区域。针对细长形障碍物，球形包络模型的冗余空间大。

传统的人工势场法其间存在一些不是目标点的空间点，该点处机械手末端受到的合力为零，从而导致运动停滞。该情况被称为局部极小值问题，当机械手末端受到的力处于一条直线时，由于引力与斥力大小的改变，会存在机械手末端在某些点来回震荡而无法跳出的现象，该情况被称为局部震荡区问题，两者统称为目标点不可达问题。

apf陷入局部极小值与局部震荡区的情况得到的轨迹，会有局部轨迹锯齿状波动的现象

技术实现要素：

本发明针对障碍物球形包络模型冗余空间问题，通过构建球树模型来描述障碍物；采用改进的人工势场法模型来解决目标点斥力不为零问题；针对人工势场法在三维空间中存在的局部极小值与路径反复震荡问题，通过临时构造虚拟目标点跳出极小值点和局部震荡区，使机械手能顺利到达目标点。具体技术方案如下：

一种结合球树模型的机械手人工势场空间路径规划方法，采用以下步骤：

步骤一、构造障碍物的球体八叉树包围盒，根据障碍物尺寸，构建一个与当前坐标系对齐的障碍物外接立方体，选定八叉树深度，递归构建球体八叉树包围盒；

步骤二、修剪空球体，修剪空球体后得到的模型具有障碍物的大致形状；

步骤三、修剪冗余球体，修剪冗余球体后得到的模型更能准确反映模型形状，

步骤四、搭建人工势场模型，计算机器人受到目标点的引力，障碍物的斥力以及合力：

步骤五、判断是否陷入局部极小值或震荡区，如果是，建立虚拟目标点，计算当前点受到的虚拟目标点的引力，障碍物的斥力以及合力；

步骤六、迭代计算，计算得到最优解，规划合适的避障路径；

步骤七、轨迹光顺处理。

进一步的，所述步骤四具体如下：

改进的斥力势场函数如式(1)所示：

其中，m为大于零的斥力增益系数，q，qo，qg分别代表机械手末端点，障碍物和目标点的空间位置坐标，ρ(q,qo)代表障碍物和机械手末端点的空间最短距离，ρ(q,qg)代表机械手末端点和目标点的空间距离，ρo为障碍物最大影响距离，根据障碍物与目标点的特定环境而改变；

相应的斥力如式(2)所示：

其中，

斥力由frep1(q)和frep2(q)两部分组成，frep1(q)方向为当前位置点与障碍物的连线从障碍物指向当前位置点，frep2(q)方向为当前位置点与目标点的连线从当前位置点指向目标点；

人工势场法的引力势场函数定义如式(5)所示：

其中，k为大于零的引力增益系数；

引力函数fatt(q)是引力势场函数的负梯度，定义如式(6)所示：

则机械手末端受到的合力为：

f(q)＝fatt(q)+frep1(q)+frep2(q)(7)

已知在三维空间中两点q1＝(x1,y1,z1)和q2＝(x2,y2,z2)，经过q1和q2的直线在笛卡尔直角坐标系中与x轴，y轴，z轴的夹角分别为:

又已知三维空间有一力f(q)，其在笛卡尔直角坐标系中与x轴，y轴，z轴的夹角分别为α，β，γ，则可得f(q)在x轴，y轴，z轴的分量分别为：

在笛卡尔坐标系中的机械手末端受力分析：

假设机械手末端在空间中的位置为q＝(x,y,z)，目标点的位置为qg＝(xg,yg,zg)；由式(8)可得，末端点与目标点的连线与x轴，y轴，z轴的夹角分别为:αg，βg，γg；

由式(9)可得，目标点对机械手末端的引力在x轴，y轴，z轴上的分量分别为：fatt(qx)，fatt(qy)，fatt(qz)；

假设三维空间中存在n个障碍物，其在空间中的位置分别为qoi＝(xoi,yoi,zoi),i∈[0,n)；由式(8)可得，末端点与障碍物的连线与x轴，y轴，z轴的夹角分别为：αoi,βoi,γoi，其中i∈[0,n)；

由式(9)可得，各障碍物对机械手末端的斥力在x轴，y轴，z轴上的分量分别为：frep,i(qx)，frep,i(qy)，frep,i(qz)，其中i∈[0,n)；

由此可得机械手末端受到的合力在x轴，y轴，z轴上的分量分别为：

机械手末端受到的合力与x轴，y轴，z轴的夹角分别为:αres,βres,γres。

假设单位移动步长为a，则下一位置点qnext＝(xnext,ynext,znext)的计算为：

进一步的，所述步骤五具体如下：

(1)构造球切线基点b：由于o点处于障碍物影响斥力球半径范围内，无法构造斥力球与o点的外切线，对o点进行预处理，将o点沿着oag连线方向远离a点偏移斥力球直径(2r)的距离，得到基点b；

(2)斥力球切线的选取：将三维空间投影到二维平面xoy，投影方向可根据实际需求进行自行调整；

(3)构造虚拟目标点：计算得到平面xoy的切点d1与d2后，再将其转化到三维空间点，此时d1与d2的z轴坐标与a点相同；沿着b，d1与b，d2的连线得到v1与v2，使得bv1＝bv2＝bg，其中v1与v2就是构建的虚拟目标点；

虚拟目标点的计算方式如下：

假设o(xo,yo,zo)，a(xa,ya,za)，g(xg,yg,zg)，b(xb,yb,zb)，d1(xd1,yd1,zd1)，d2(xd2,yd2,zd2)，v1(xv1,yv1,zv1)，v2(xv2,yv2,zv2)，o点与a点的连线与x轴，y轴，z轴的夹角分别为:αao，βao，γao，o点与b点的连线长度为2r，由式(12)可得b点空间坐标为

将三维空间投影至二维平面xoy，由式(13)可得基点b与障碍物斥力圆切线斜率k：

得到斜率即可利用式(14)得到圆上切点坐标：

d点的z轴坐标为o点z轴方向值，由此确定d点坐标。由式(8)可得b点与d点的连线与x轴，y轴，z轴的夹角分别为:αbd，βbd，γbd，已知bv＝bg，由此可得v点的空间坐标。

进一步的，所述步骤七具体如下：

采用道格拉斯-普克算法，实现apf轨迹的压缩以提取必要的路径点；apf路径的压缩方式为：将路径的首末顶点连成一条直线(ab),求出路径上的点到该直线的距离d，将最大值dmax与设定阈值相比较,，若dmax≤ε,则只保留首末端点，若dmax>ε，则将路径分为两部分(ac,bc),再分别对已经得到两段路径递归。

本发明有益效果

本发明采用了一种基于八叉树空间细分的球面几何近似方法，通过子球体抽象离散障碍物，以此将障碍物拆分成若干个子障碍物，简化计算量的同时也解决了障碍物模型冗余空间过大的问题，对于避障功能有着较好的促进作用。

本发明引入了虚拟目标点来替代原目标点使其合力不为零，这样机械手就能跳出局部极小值与局部震荡区，继续向目标点前进。虚拟目标点尽可能与原目标点保持相近特性，保证了机械手朝着原目标点以尽可能小的偏离方向及更快的速度到达目标点。

本发明采用道格拉斯-普克算法，实现apf轨迹的压缩以提取必要的路径点，选用ferguson曲线来对路径进行光顺处理，以此实现apf路径的光顺。

附图说明

图1是本发明方法的流程框图；

图2是本发明中构建球体八叉树包围盒的示意图；

图3是本发明中修剪空球体的示意图；

图4是本发明中修剪冗余球体的示意图；

图5a是局部极小值示意图；

图5b是局部震荡区示意图；

图6a是三维空间基点构造示意图；

图6b是二维平面斥力球切点构造示意图；

图6c是三维空间虚拟目标点构造示意图；

图7是apf路径的压缩示意图；

图8a-8c是避障仿真效果的示意图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，本发明的结合球树模型的机械手人工势场空间路径规划方法，采用以下步骤：

步骤一、如图2所示，构造障碍物的球体八叉树包围盒，根据障碍物尺寸，构建一个与当前坐标系对齐的障碍物外接立方体，选定八叉树深度，递归构建球体八叉树包围盒；

步骤二、修剪空球体，空球体是指球体不与障碍物相交或者包含，即球体与障碍物分离。修剪空球体后得到的模型具有障碍物的大致形状，如图3所示；

步骤三、修剪冗余球体，冗余球体是指若该球体被移除，其他球体仍然能够完全覆盖当前障碍物。修剪冗余球体后得到的模型更能准确反映模型形状，如图4所示；

步骤四、搭建人工势场模型，计算机器人受到目标点的引力，障碍物的斥力以及合力：

改进的斥力势场函数如式(1)所示：

其中，m为大于零的斥力增益系数，q，qo，qg分别代表机械手末端点，障碍物和目标点的空间位置坐标，ρ(q,qo)代表障碍物和机械手末端点的空间最短距离，ρ(q,qg)代表机械手末端点和目标点的空间距离，ρo为障碍物最大影响距离，它的值可根据障碍物与目标点的特定环境而改变。一般该值应小于所有障碍物之间的最短距离。相应的斥力如式(2)所示：

其中，

斥力由frep1(q)和frep2(q)两部分组成，frep1(q)方向为当前位置点与障碍物的连线从障碍物指向当前位置点，frep2(q)方向为当前位置点与目标点的连线从当前位置点指向目标点。

人工势场法的引力势场函数定义如式(5)所示：

其中，k为大于零的引力增益系数。引力函数fatt(q)是引力势场函数的负梯度，定义如式(6)所示：

则机械手末端受到的合力为：

f(q)＝fatt(q)+frep1(q)+frep2(q)(7)

已知在三维空间中两点q1＝(x1,y1,z1)和q2＝(x2,y2,z2)，经过q1和q2的直线在笛卡尔直角坐标系中与x轴，y轴，z轴的夹角分别为:

又已知三维空间有一力f(q)，其在笛卡尔直角坐标系中与x轴，y轴，z轴的夹角分别为α，β，γ，则可得f(q)在x轴，y轴，z轴的分量分别为：

现给出定义在笛卡尔坐标系中的机械手末端受力分析：

假设机械手末端在空间中的位置为q＝(x,y,z)，目标点的位置为qg＝(xg,yg,zg)。由式(8)可得，末端点与目标点的连线与x轴，y轴，z轴的夹角分别为:αg，βg，γg。

由式(9)可得，目标点对机械手末端的引力在x轴，y轴，z轴上的分量分别为：fatt(qx)，fatt(qy)，fatt(qz)。

假设三维空间中存在n个障碍物，其在空间中的位置分别为qoi＝(xoi,yoi,zoi),i∈[0,n)。由式(8)可得，末端点与障碍物的连线与x轴，y轴，z轴的夹角分别为：αoi,βoi,γoi，其中i∈[0,n)。

由式(9)可得，各障碍物对机械手末端的斥力在x轴，y轴，z轴上的分量分别为：frep,i(qx)，frep,i(qy)，frep,i(qz)，其中i∈[0,n)。

由此可得机械手末端受到的合力在x轴，y轴，z轴上的分量分别为：

机械手末端受到的合力与x轴，y轴，z轴的夹角分别为:αres,βres,γres。

假设单位移动步长为a，则下一位置点qnext＝(xnext,ynext,znext)的计算为：

步骤五、判断是否陷入如图5a所示的局部极小值或如图5b所示的震荡区，如果是，就建立虚拟目标点，计算当前点受到的虚拟目标点的引力，障碍物的斥力以及合力。具体如下：

(1)构造球切线基点b。由于o点处于障碍物影响斥力球半径范围内，无法构造斥力球与o点的外切线，因此需对o点做一个预处理。将o点沿着oag连线方向远离a点偏移斥力球直径(2r)的距离，得到基点b，如图6a所示。

(2)斥力球切线的选取。将三维空间投影到二维平面xoy，将问题转换为求点到圆的切线，以此将无数条切线缩减为只有2条切线。投影方向可根据实际需求进行自行调整。如图6b所示。

(3)构造虚拟目标点。计算得到平面xoy的切点d1与d2后，再将其转化到三维空间点，此时d1与d2的z轴坐标与a点相同。沿着b，d1与b，d2的连线得到v1与v2，使得bv1＝bv2＝bg，其中v1与v2就是构建的虚拟目标点，如图6c所示。

虚拟目标点的计算方式如下：

假设o(xo,yo,zo)，a(xa,ya,za)，g(xg,yg,zg)，b(xb,yb,zb)，d1(xd1,yd1,zd1)，d2(xd2,yd2,zd2)，v1(xv1,yv1,zv1)，v2(xv2,yv2,zv2)，o点与a点的连线与x轴，y轴，z轴的夹角分别为:αao，βao，γao，o点与b点的连线长度为2r，由式(12)可得b点空间坐标为

将三维空间投影至二维平面xoy，由式(13)可得基点b与障碍物斥力圆切线斜率k：

得到斜率即可利用式(14)得到圆上切点坐标：

d点的z轴坐标为o点z轴方向值，由此确定d点坐标。由式(8)可得b点与d点的连线与x轴，y轴，z轴的夹角分别为:αbd，βbd，γbd。已知bv＝bg，由此可得v点的空间坐标。

步骤六、迭代计算，计算得到最优解，规划合适的避障路径；

步骤七、轨迹光顺处理，由于apf(人工势场法)陷入局部极小值和局部震荡区时，会有局部轨迹锯齿状波动的现象。因此，本发明采用道格拉斯-普克算法，实现apf轨迹的压缩以提取必要的路径点。apf路径的压缩方式为：将路径的首末顶点连成一条直线(ab),求出路径上的点到该直线的距离d,将最大值dmax与设定阈值相比较,若dmax≤ε,则只保留首末端点；若dmax>ε，则将路径分为两部分(ac,bc),再分别对已经得到两段路径递归。如图7所示。选用ferguson曲线来对路径进行光顺处理，以此实现apf路径的光顺。

实施例

机器人初始位置为(100,500,350)；目标位置为(400,150,350)，障碍物1坐标(200,450,350)，影响直径65；障碍物2坐标(350,450,350)，影响直径65；障碍物3坐标(225,250,350)，影响直径75；障碍物4坐标(350,250,350)，影响直径75。

仿真效果：

如图8a-8c所示，从避障仿真效果图中可以看出，本发明的方法可以实现机器人在复杂障碍物，未知环境中进行有效的避障和较优路径的选取，在面对局部震荡或局部极小点时，能够实现对轨迹的光顺处理，实现轨迹的优化。