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

本发明专利技术公开了一种结合球树模型的机械手人工势场空间路径规划方法，采用以下步骤：步骤一、构造障碍物的球体八叉树包围盒；步骤二、修剪空球体，修剪空球体后得到的模型具有障碍物的大致形状；步骤三、修剪冗余球体，修剪冗余球体后得到的模型更能准确反映模型形状；步骤四、搭建人工势场模型，计算机器人受到目标点的引力，障碍物的斥力以及合力：步骤五、判断是否陷入局部极小值或震荡区，如果是，建立虚拟目标点，计算当前点受到的虚拟目标点的引力，障碍物的斥力以及合力；步骤六、迭代计算，计算得到最优解，规划合适的避障路径；步骤七、轨迹光顺处理。

A Spatial Path Planning Method for Manipulator Artificial Potential Field Based on Ball Tree Model

The invention discloses a space path planning method of artificial potential field of manipulator combined with spherical tree model, which adopts the following steps: step 1, constructing spherical octree enclosure box of obstacles; step 2, pruning empty sphere, pruning empty sphere, the model obtained after pruning empty sphere has roughly the shape of obstacles; step 3, pruning redundant sphere, pruning redundant sphere, the model obtained after pruning redundant sphere can be more accurate. Step 4: Establish the artificial potential field model, the computer robot is attracted by the gravity of the target point, the repulsion force of the obstacle and the resultant force: Step 5, judge whether it falls into the local minimum or the oscillation area, if so, establish the virtual target point, calculate the gravitation of the virtual target point, the repulsion force of the obstacle and the resultant force of the current point; Step 6, iterate calculation, calculate. The optimal solution is obtained, and the appropriate obstacle avoidance path is planned. Step 7: Trajectory smoothing.

【技术实现步骤摘要】
一种结合球树模型的机械手人工势场空间路径规划方法
本专利技术涉及了一种机器人自动避障方法，尤其是涉及了一种结合球树模型的机械手人工势场空间路径规划方法，涉及工业生产、物流运输等领域中广泛使用的工业机器人与服务型机器人。
技术介绍
路径规划是机器人技术中非常关键的一项技术，工业机器人与服务型机器人在运行中的迅速，精确的定位到目标点，需要使用路径规划来规划最合适的运行路径，规避障碍物，增加机器人运行的安全性，提高机器人的运行效率。环境是未知的，变化的；这就需要动态的路径规划技术，静态的路径规划已经无法满足要求。面向工业机器人与服务型机器人的动态路径规划，需要高效碰撞检测技术，碰撞检测技术依赖于各种包围盒模型，例如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),再分别对已经得到两段路径递归。本专利技术有益效果本专利技术采用了一种基于八叉树空间细分的球面几何近似方法，通过子球体抽象离散障碍物，以此将障碍物拆分成若干个子障碍物，简化计算量的同时也解决了障碍物模型冗余空间过大的问题，对于避障功能有着较好的本文档来自技高网...

【技术保护点】
1.一种结合球树模型的机械手人工势场空间路径规划方法，采用以下步骤：步骤一、构造障碍物的球体八叉树包围盒，根据障碍物尺寸，构建一个与当前坐标系对齐的障碍物外接立方体，选定八叉树深度，递归构建球体八叉树包围盒；步骤二、修剪空球体，修剪空球体后得到的模型具有障碍物的大致形状；步骤三、修剪冗余球体，修剪冗余球体后得到的模型更能准确反映模型形状，步骤四、搭建人工势场模型，计算机器人受到目标点的引力，障碍物的斥力以及合力：步骤五、判断是否陷入局部极小值或震荡区，如果是，建立虚拟目标点，计算当前点受到的虚拟目标点的引力，障碍物的斥力以及合力；步骤六、迭代计算，计算得到最优解，规划合适的避障路径；步骤七、轨迹光顺处理。

【技术特征摘要】
1.一种结合球树模型的机械手人工势场空间路径规划方法，采用以下步骤：步骤一、构造障碍物的球体八叉树包围盒，根据障碍物尺寸，构建一个与当前坐标系对齐的障碍物外接立方体，选定八叉树深度，递归构建球体八叉树包围盒；步骤二、修剪空球体，修剪空球体后得到的模型具有障碍物的大致形状；步骤三、修剪冗余球体，修剪冗余球体后得到的模型更能准确反映模型形状，步骤四、搭建人工势场模型，计算机器人受到目标点的引力，障碍物的斥力以及合力：步骤五、判断是否陷入局部极小值或震荡区，如果是，建立虚拟目标点，计算当前点受到的虚拟目标点的引力，障碍物的斥力以及合力；步骤六、迭代计算，计算得到最优解，规划合适的避障路径；步骤七、轨迹光顺处理。2.如权利要求1所述的结合球树模型的机械手人工势场空间路径规划方法，其特征在于：所述步骤四具体如下：改进的斥力势场函数如式(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)；由式...

【专利技术属性】
技术研发人员：林旭军，王进，泮求亮，陆国栋，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江,33