CN110919661B - 一种手套箱封闭空间内机械臂的运动规划方法 - Google Patents

一种手套箱封闭空间内机械臂的运动规划方法 Download PDF

Info

Publication number
CN110919661B
CN110919661B CN201911363088.2A CN201911363088A CN110919661B CN 110919661 B CN110919661 B CN 110919661B CN 201911363088 A CN201911363088 A CN 201911363088A CN 110919661 B CN110919661 B CN 110919661B
Authority
CN
China
Prior art keywords
path
obstacle
mechanical arm
node
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911363088.2A
Other languages
English (en)
Other versions
CN110919661A (zh
Inventor
刘金国
佟玉闯
刘晓源
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenyang Institute of Automation of CAS
Original Assignee
Shenyang Institute of Automation of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenyang Institute of Automation of CAS filed Critical Shenyang Institute of Automation of CAS
Priority to CN201911363088.2A priority Critical patent/CN110919661B/zh
Publication of CN110919661A publication Critical patent/CN110919661A/zh
Application granted granted Critical
Publication of CN110919661B publication Critical patent/CN110919661B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及一种手套箱封闭空间内机械臂的运动规划方法，包括以下步骤：判断环境中是否有障碍物；如果无障碍物，采用基于IB‑RRT算法的路径规划方法进行路径规划；如果有障碍物，采用冗余机械臂运动学避障方法进行路径规划。本发明能够有效解决RRT*算法规划成功率低、收敛速度慢的问题。不仅能够有效地解决机械臂多个杆件靠近障碍物时引起的震荡问题，还解决了机械臂末端经过障碍物时不能成功通过的问题及避障参数难以调节问题。当机械臂靠近障碍物时，机械臂的优先级从主运动会向附加运动平滑过渡，从而有效地解决机械臂多个杆件靠近障碍物时引起的震荡问题。本发明具有计算精确，收敛速度快，路径平滑，避免震荡等优点。

Description

一种手套箱封闭空间内机械臂的运动规划方法
技术领域
本发明属于空间机器人领域，具体地说是一种手套箱封闭空间内机械臂的运动规划方法。
背景技术
我国载人空间站的建设将为空间科学实验与前沿科技探索提供理想的平台，特别是国家级太空实验室的建立，使我国具备长期空间微重力实验条件，为空间科学的探索提供有力保障。空间生命科学实验设备是开展空间生命科学与生物技术研究的重要组成部分之一，其功能和技术指标决定了空间生命科学与生物技术研究的深度和广度，对实验任务的完成情况和完成能力起着至关重要的作用。在载人航天科学实验研究的整个发展过程中，手套箱在航天飞机、空间实验室、空间站等舱内进行生命科学、生物技术、材料科学、航天医学等科学实验中被广泛应用；同时，根据实际需求和空间平台的支持能力，手套箱的功能和性能不断提升。随着超冗余度机械臂在航天航空制造、核电设备检修和地震灾后救援等领域的应用不断深入，机械臂运动规划及避障算法逐渐受到学者们的关注。机械臂在有障碍空间运动时，如何规划一条无碰撞平滑的路径，对空间机械臂在空间站上安全运动至关重要。本发明涉及一种手套箱封闭空间内机械臂的运动规划方法，所研究的空间手套箱机械臂平台为封闭空间内基于圆弧滑动导轨的机械臂。本发明在机械臂运动模型的基础上，研究其障碍物环境下的路径规划及避障问题，为我国空间站机械臂技术的发展提供理论支持。本发明具有计算精确，收敛速度快，路径平滑，避免震荡等优点。
发明内容
针对上述问题，本发明的目的在于提供一种计算精确，收敛速度快，路径平滑，避免震荡的手套箱封闭空间内机械臂的运动规划方法，用来规划其障碍物环境下的路径规划及避障问题，为我国空间站机械臂技术的发展提供理论支持。
本发明的目的是通过以下技术方案来实现的：一种手套箱封闭空间内机械臂的运动规划方法，包含以下步骤：
一种手套箱封闭空间内机械臂的运动规划方法，包括以下步骤：
判断环境中是否有障碍物；
如果无障碍物，采用基于IB-RRT算法的路径规划方法进行路径规划；
如果有障碍物，采用冗余机械臂运动学避障方法进行路径规划。
所述采用基于IB-RRT算法的路径规划方法进行路径规划，包含如下步骤：
(1)用双向搜索策略搜出一条路径；
(2)根据路径直接以随机点为新节点进行局部优化处理；
(3)对于优化后的路径，根据两颗树距离选择最优树；
(4)对最优树上的节点基于三角不等式进行优化；
(5)基于贝塞尔曲线扩展得到最终的路径。
所述采用冗余机械臂运动学避障方法进行路径规划，包含如下步骤：
(1)对机械臂进行碰撞检测；
(2)对机械臂梯度投影法求解附加运动的最优解；
(3)使用附加背离速度法避障算法得到含有机械臂主运动和附加运动的最终解；
(4)进行障碍物避障。
所述用双向搜索策略搜出一条路径，包括以下步骤：
首先用初始构形x_start初始化搜索树T，从起始构形x_start目标构形x_goal构造两棵树；然后执行Extend_Tree(T)扩展函数，并判断扩展函数是否返回状态Reached；如果返回Reached状态，扩展过程中有一节点落在了目标区域，则停止循环并返回搜索树T；如果返回Trapped状态，则继续循环，直到达到循环次数的上限。
所述根据路径直接以随机点为新节点进行局部优化处理，包括以下步骤：
Cost(x)表示在已经扩展的树中，从起始点到x点的路径代价；Line(X1,X2)表示通过直线连接X1,X2的路径代价；X1,X2分别表示路径中直接随机生成的新节点；
5-1：碰撞检测：给定点x,x'∈X，{Y:y∈Y,y＝x+t·(x'-x),t∈[0,1]}，其中，点x,x'为产生的新节点中的随机点，X为产生的新节点的集合，Y为随机点x,x'所连线段上的任意点的集合，t为Y中的点在x,x'所连线段上分割出的两条线段的比例；如果
Figure GDA0003888669340000021
Xfree为在障碍物外的点的集合，也成为自由空间，则函数CollisionFree(x,x')返回值True，即无碰撞；否则，返回值False，即有碰撞；
5-2：从新节点x_new的相邻节点x_near中，寻找使Cost(x_new)最小的节点作为x_new的父节点；
5-3：如果经过新节点x_new到相邻节点x_near的路径代价小于节点x_near之前的路径代价，则将x_near的父节点改为节点x_new。
所述对于优化后的路径，根据两颗树距离选择最优树，包含如下步骤：
6-1：NearVertices(x,Ta,Tb,r)表示找到距离x点为r的所有Ta,Tb上的节点，其中，Ta,Tb分别表示首先用起始构形x_start和目标构形x_goal构造两棵树；NearestVertices(x,Ta,Tb)表示寻找x到所有Ta,Tb上距离最近的节点xnearest；GetSortedList(x,xnearest)表示xnearest内所有节点到x点路径消耗从小到大顺序排列；GetBestTreeParent(L1,L2,Connection)表示从由GetSortedList(x,Y)生成的最小消耗路径L1,L2中选择路径最小消耗点和由路径最小消耗点组成的最优树，并得到消耗最小的点为xmin，并返回判断此最优树是否与障碍物碰撞；InsertVertex(x,xmin,T)将新节点x插入最优树上；RewireVertices(x,L,T)表示修整最优树上的相邻节点，使相邻节点在加入新节点后保证每个节点上路径消耗最小；其中L为从由GetSortedList(x,Y)生成的最小消耗路径L1,L2；
6-2：最优父节点：首先把
Figure GDA0003888669340000031
和
Figure GDA0003888669340000032
中各自最小消耗点找出来，其中
Figure GDA0003888669340000033
和
Figure GDA0003888669340000034
分别表示从随机节点和附近节点中选出的路径消耗最小的路径，也是6-1中的L1,L2，再比较由L1,L2生成的两个树上各自路径消耗最小的那个节点
Figure GDA0003888669340000035
如果
Figure GDA0003888669340000036
则赋值
Figure GDA0003888669340000037
给Xmin，如果
Figure GDA0003888669340000038
则赋值
Figure GDA0003888669340000039
给Xmin，Xmin表示由L1,L2生成的两个树上路径消耗最小的节点中较小值，用于在InsertVertex(x,Y,T)步骤中，将随机节点插入在最优树后形成新的树，然后判断是否两棵树相遇，即判断两棵树是否有交点，并返回状态σf，σf为最优父节点；
6-3：判断连接：比较节点(σa,σb)的路径消耗之和与原来σf的路径消耗，若前者大于后者，则选择σf的路径，反之选择节点(σa,σb)的路径，最终，返回状态σf；然后判断是否两棵树相遇，即判断两棵树是否有交点，若不相遇，则重新规划节点(σa,σb)并进行判断，若相遇，则结束规划。
所述对最优树上的节点基于三角不等式进行优化，包含如下步骤：
7-1：对于最优树上的相邻的三个节点X1,X2,X3，X2-X1||+||X3-X2||＞||X3-X1||；当X1到X3的路径可行即机械臂与障碍物无碰撞，则直接连接X1到X3的路径，跳过X2，这样X1到X3的路径代价小于之前的路径代价；
7-2：对最优树上的每相邻三个节点进行一次步骤7-1的优化，直到优化前后的路径节点个数没有变化。
所述基于贝塞尔曲线扩展得到最终的路径，包含如下步骤：
8-1：n阶贝塞尔曲线为：
Figure GDA0003888669340000041
式中：Pi称为控制点，为第i个顶点的坐标值，
Figure GDA0003888669340000042
为伯恩斯坦Bernstein多项式；其定义如下：
Figure GDA0003888669340000043
ti表示第i个顶点的参数值；
n＝3时，得到三次贝塞尔曲线的参数方程：
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3。
8-2：应用三次贝塞尔曲线的属性，每个路径段Gp被构建成为P0＝qinit与P3＝qgoal，即优化后的最优树在扩展的时候每一个扩展的端点qinit可以定位P0，而每一个扩展的目标点qgoal可以定位为P3；进而得到最终的路径。
所述碰撞检测，包含如下步骤：
9-1：障碍物处理：采用球体包络的方法表示障碍物，用圆柱体包络法表示机械臂；把机械臂简化成线段连杆，把机械臂的实际连杆厚度叠加到障碍物上；
9-2：A表示
Figure GDA0003888669340000044
路径上的左端点，B表示
Figure GDA0003888669340000045
路径上的右端点，P表示
Figure GDA0003888669340000046
路径外一点，即机械臂连杆上与障碍物最短距离点；向量
Figure GDA0003888669340000047
为
Figure GDA0003888669340000048
在
Figure GDA0003888669340000049
方向上的投影向量，C为投影点；
定义
Figure GDA00038886693400000410
根据r值的不同，机械臂连杆与障碍物的最短距离d为：
Figure GDA00038886693400000411
9-3：当机械臂连杆与障碍物的最短距离d小于阈值，则表示机械臂连杆进入障碍物的安全区内，碰撞几率大，反之，机械臂连杆未进入障碍物的安全区内，无碰撞。
所述对机械臂梯度投影法求解附加运动的最优解，具体如下:
Figure GDA00038886693400000412
其中N＝(I-J+J)，J+是雅可比矩阵的广义逆，J+＝JT(JJT)-1，I为单位矩阵；机器人任务空间的操作速度
Figure GDA0003888669340000051
m表示空间自由度个数，关节空间的速度
Figure GDA0003888669340000052
n表示机械臂关节个数，
Figure GDA0003888669340000053
表示附加运动；v是关节速度矢量。
所述使用附加背离速度法避障算法得到含有机械臂主运动和附加运动的最终解，具体如下：
Figure GDA0003888669340000054
其中
Figure GDA0003888669340000055
左边一项表示保证终端速度
Figure GDA0003888669340000056
的伪逆解，即最小范数解，也就是说主运动；右边一项表示了满足避障要求而不引起终端变化的自由度，即附加运动；其中，关节空间的速度
Figure GDA0003888669340000057
J+是雅可比矩阵的广义逆，dm为安全区半径，
Figure GDA0003888669340000058
式中
Figure GDA0003888669340000059
表示沿
Figure GDA00038886693400000510
方向的单位向量，J0是避障点的雅可比矩阵，
Figure GDA00038886693400000511
是设定的避障点的速度，根据实际环境信息确定；di表示第i个机械臂连杆与障碍物的最短距离，N＝(I-J+J),λ是齐次解增益,是关于障碍物距离d0的函数，称为避障增益。
所述进行障碍物避障，包含如下步骤:
存在一个障碍物时，采用带权重因子加权求和的方法：
Figure GDA00038886693400000512
J+是雅可比矩阵的广义逆，
Figure GDA00038886693400000513
表示终端速度，也就是主运动；n0是进入障碍物安全区连杆的个数，wi是权重因子，λ(d0)h,i是速益，
Figure GDA00038886693400000514
是第i个进入障碍物安全区的连杆对应的齐次解；权重因子wi如下：
Figure GDA00038886693400000515
dm为安全区半径，d0,i为第i个连杆距离障碍物的最短距离；
Figure GDA0003888669340000061
表示沿
Figure GDA0003888669340000062
方向的单位向量，J0是避障点的雅可比矩阵，同时存在多个障碍物时，采用带权重因子求和的方法：
Figure GDA0003888669340000063
p是处于危险状态的障碍物的个数，即连杆与障碍物的最短距离小于安全区；尽管这时避障点的实际速度不是理想速度，但是当距离机械臂最近的障碍物远远比其他障碍物近时，这时的权重因子接近1，也就是说非常接近理想背离速度，dj,i表示为第i个连杆距离第j个障碍物的最短距离；λ(d0)j,i表示第i个连杆与第j个障碍物最短距离的避障增益；
Figure GDA0003888669340000064
表示第i个连杆与第j个障碍物的角速度。
本发明的优点与积极效果为：
(1)本发明提出改进的IB-RRT算法，在RRT*算法的基础上引入了双向搜索策略和基于三角不等式的路径规划优化方法，能够有效解决RRT*算法规划成功率低、收敛速度慢的问题。
(2)本发明针对路径不光滑问题，提出了基于贝塞尔样条曲线的路径平滑算法，使路径更加平滑。
(3)本发明提出了一种改进的冗余机械臂运动学避障算法，该方法不仅能够有效地解决机械臂多个杆件靠近障碍物时引起的震荡问题，还解决了机械臂末端经过障碍物时不能成功通过的问题及避障参数难以调节问题。
(4)本发明对冗余机械臂多个杆靠近同一个障碍物进行了避障算法改进，使多个杆件均匀的靠近障碍物。
(5)本发明对机械臂的主运动和附加运动进行了优先级平滑过渡，当机械臂靠近障碍物时，机械臂的优先级从主运动会向附加运动平滑过渡，从而有效地解决机械臂多个杆件靠近障碍物时引起的震荡问题。
附图说明
图1为本发明算法总流程示意图；
图2为本发明新节点扩展过程示意图；
图3为本发明路径规划算法示意图；
图4为本发明基于三角不等式的路径优化原理图；
图5为本发明贝塞尔曲线路径处理原理图；
图6为本发明机械臂与障碍物简化关系示意图；
图7为本发明安全区示意图；
图8a为本发明矢量算法求点到线段的最短距离原理图一；
图8b为本发明矢量算法求点到线段的最短距离原理图二；
图8c为本发明矢量算法求点到线段的最短距离原理图三；
图9为本发明附加背离速度法避障原理图；
图10为本发明七自由度机械臂单障碍物运动规划示意图；
图11为本发明七自由度机械臂多障碍物运动规划示意图。
具体实施方式
下面结合附图对本发明作进一步详述。
本发明涉及一种手套箱封闭空间内机械臂的运动规划方法，所研究的空间手套箱机械臂平台为封闭空间内基于圆弧滑动导轨的机械臂。本发明在机械臂运动模型的基础上，研究其障碍物环境下的路径规划及避障问题，为我国空间站机械臂技术的发展提供理论支持。本发明具体内容如下：(1)提出改进的IB-RRT算法，在RRT*算法的基础上引入了双向搜索策略和基于三角不等式的路径规划优化方法，能够有效解决RRT*算法规划成功率低、收敛速度慢的问题。(2)针对路径不光滑问题，提出了基于贝塞尔样条曲线的路径平滑算法，使路径更加平滑。(3)提出了一种改进的冗余机械臂运动学避障算法，该方法不仅能够有效地解决机械臂多个杆件靠近障碍物时引起的震荡问题，还解决了机械臂末端经过障碍物时不能成功通过的问题及避障参数难以调节问题。(4)对冗余机械臂多个杆靠近同一个障碍物进行了避障算法改进，使多个杆件均匀的靠近障碍物。(5)对机械臂的主运动和附加运动进行了优先级平滑过渡，当机械臂靠近障碍物时，机械臂的优先级从主运动会向附加运动平滑过渡，从而有效地解决机械臂多个杆件靠近障碍物时引起的震荡问题。本发明具有计算精确，收敛速度快，路径平滑，避免震荡等优点。
步骤1：判断环境中是否有障碍物；
如果无障碍物，采用基于IB-RRT算法的路径规划方法进行路径规划；
如果有障碍物，采用冗余机械臂运动学避障方法进行路径规划。
步骤2：所述采用基于IB-RRT算法的路径规划方法进行路径规划，包含如下步骤：
(1)用双向搜索策略搜出一条路径；
(2)根据路径直接以随机点为新节点进行局部优化处理；
(3)对于优化后的路径，根据两颗树距离选择最优树；
(4)对最优树上的节点基于三角不等式进行优化；
(5)基于贝塞尔曲线扩展得到最终的路径。
所述用双向搜索策略搜出一条路径，包括以下步骤：首先用初始构形x_start初始化搜索树T，从起始构形x_start目标构形x_goal构造两棵树；然后执行Extend_Tree(T)扩展函数，并判断扩展函数是否返回状态Reached；如果返回Reached状态，扩展过程中有一节点落在了目标区域，则停止循环并返回搜索树T；如果返回Trapped状态，则继续循环，直到达到循环次数的上限。
所述根据路径直接以随机点为新节点进行局部优化处理，包括以下步骤：Cost(x)表示在已经扩展的树中，从起始点到x点的路径代价；Line(X1,X2)表示通过直线连接X1,X2的路径代价；X1,X2分别表示路径中直接随机生成的新节点；
5-1：碰撞检测：给定点x,x'∈X，{Y:y∈Y,y＝x+t·(x'-x),t∈[0,1]}，其中，点x,x'为产生的新节点中的随机点，X为产生的新节点的集合，Y为随机点x,x'所连线段上的任意点的集合，t为Y中的点在x,x'所连线段上分割出的两条线段的比例；如果
Figure GDA0003888669340000081
Xfree为在障碍物外的点的集合，也成为自由空间，则函数CollisionFree(x,x')返回值True，即无碰撞；否则，返回值False，即有碰撞；
5-2：从新节点x_new的相邻节点x_near中，寻找使Cost(x_new)最小的节点作为x_new的父节点；
5-3：如果经过新节点x_new到相邻节点x_near的路径代价小于节点x_near之前的路径代价，则将x_near的父节点改为节点x_new。
所述对于优化后的路径，根据两颗树距离选择最优树，包含如下步骤：
6-1：NearVertices(x,Ta,Tb,r)表示找到距离x点为r的所有Ta,Tb上的节点，其中，Ta,Tb分别表示首先用起始构形x_start和目标构形x_goal构造两棵树；NearestVertices(x,Ta,Tb)表示寻找x到所有Ta,Tb上距离最近的节点xnearest；GetSortedList(x,xnearest)表示xnearest内所有节点到x点路径消耗从小到大顺序排列；GetBestTreeParent(L1,L2,Connection)表示从由GetSortedList(x,Y)生成的最小消耗路径L1,L2中选择路径最小消耗点和由路径最小消耗点组成的最优树，并得到消耗最小的点为xmin，并返回判断此最优树是否与障碍物碰撞；InsertVertex(x,xmin,T)将新节点x插入最优树上；RewireVertices(x,L,T)表示修整最优树上的相邻节点，使相邻节点在加入新节点后保证每个节点上路径消耗最小；其中L为从由GetSortedList(x,Y)生成的最小消耗路径L1,L2；
6-2：最优父节点：首先把
Figure GDA0003888669340000091
和
Figure GDA0003888669340000092
中各自最小消耗点找出来，其中
Figure GDA0003888669340000093
和
Figure GDA0003888669340000094
分别表示从随机节点和附近节点中选出的路径消耗最小的路径，也是6-1中的L1,L2，再比较由L1,L2生成的两个树上各自路径消耗最小的那个节点
Figure GDA0003888669340000095
如果
Figure GDA0003888669340000096
则赋值
Figure GDA0003888669340000097
给Xmin，如果
Figure GDA0003888669340000098
则赋值
Figure GDA0003888669340000099
给Xmin，Xmin表示由L1,L2生成的两个树上路径消耗最小的节点中较小值，用于在InsertVertex(x,Y,T)步骤中，将随机节点插入在最优树后形成新的树，然后判断是否两棵树相遇，即判断两棵树是否有交点，并返回状态σf，σf为最优父节点；
6-3：判断连接：比较节点(σa,σb)的路径消耗之和与原来σf的路径消耗，若前者大于后者，则选择σf的路径，反之选择节点(σa,σb)的路径，最终，返回状态σf；然后判断是否两棵树相遇，即判断两棵树是否有交点，若不相遇，则重新规划节点(σa,σb)并进行判断，若相遇，则结束规划。
所述对最优树上的节点基于三角不等式进行优化，包含如下步骤：
7-1：对于最优树上的相邻的三个节点X1,X2,X3，||X2-X1||+||X3-X2||＞||X3-X1||；当X1到X3的路径可行即机械臂与障碍物无碰撞，则直接连接X1到X3的路径，跳过X2，这样X1到X3的路径代价小于之前的路径代价；
7-2：对最优树上的每相邻三个节点进行一次步骤7-1的优化，直到优化前后的路径节点个数没有变化。
所述基于贝塞尔曲线扩展得到最终的路径，包含如下步骤：
8-1：n阶贝塞尔曲线为：
Figure GDA00038886693400000910
式中：Pi称为控制点，为第i个顶点的坐标值，
Figure GDA00038886693400000911
为伯恩斯坦Bernstein多项式；其定义如下：
Figure GDA00038886693400000912
ti表示第i个顶点的参数值；
n＝3时，得到三次贝塞尔曲线的参数方程：
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3。
8-2：应用三次贝塞尔曲线的属性，每个路径段Gp被构建成为P0＝qinit与P3＝qgoal，即优化后的最优树在扩展的时候每一个扩展的端点qinit可以定位P0，而每一个扩展的目标点qgoal可以定位为P3；进而得到最终的路径。
步骤3：所述采用冗余机械臂运动学避障方法进行路径规划，包含如下步骤：
(1)对机械臂进行碰撞检测；
(2)对机械臂梯度投影法求解附加运动的最优解；
(3)使用附加背离速度法避障算法得到含有机械臂主运动和附加运动的最终解；
(4)进行障碍物避障。
所述碰撞检测，包含如下步骤：
9-1：障碍物处理：采用球体包络的方法表示障碍物，用圆柱体包络法表示机械臂；把机械臂简化成线段连杆，把机械臂的实际连杆厚度叠加到障碍物上；
9-2：A表示
Figure GDA0003888669340000101
路径上的左端点，B表示
Figure GDA0003888669340000102
路径上的右端点，P表示
Figure GDA0003888669340000103
路径外一点，即机械臂连杆上与障碍物最短距离点；向量
Figure GDA0003888669340000104
为
Figure GDA0003888669340000105
在
Figure GDA0003888669340000106
方向上的投影向量，C为投影点；
定义
Figure GDA0003888669340000107
根据r值的不同，机械臂连杆与障碍物的最短距离d为：
Figure GDA0003888669340000108
9-3：当机械臂连杆与障碍物的最短距离d小于阈值，则表示机械臂连杆进入障碍物的安全区内，碰撞几率大，反之，机械臂连杆未进入障碍物的安全区内，无碰撞。
所述对机械臂梯度投影法求解附加运动的最优解，具体如下:
Figure GDA0003888669340000109
其中N＝(I-J+J)，J+是雅可比矩阵的广义逆，J+＝JT(JJT)-1，I为单位矩阵；机器人任务空间的操作速度
Figure GDA00038886693400001010
m表示空间自由度个数，关节空间的速度
Figure GDA00038886693400001011
n表示机械臂关节个数，
Figure GDA00038886693400001012
表示附加运动；v是关节速度矢量。
所述使用附加背离速度法避障算法得到含有机械臂主运动和附加运动的最终解，具体如下：
Figure GDA00038886693400001013
其中
Figure GDA0003888669340000111
左边一项表示保证终端速度
Figure GDA0003888669340000112
的伪逆解，即最小范数解，也就是说主运动；右边一项表示了满足避障要求而不引起终端变化的自由度，即附加运动；其中，关节空间的速度
Figure GDA0003888669340000113
J+是雅可比矩阵的广义逆，dm为安全区半径，
Figure GDA0003888669340000114
式中
Figure GDA0003888669340000115
表示沿
Figure GDA0003888669340000116
方向的单位向量，J0是避障点的雅可比矩阵，
Figure GDA0003888669340000117
是设定的避障点的速度，根据实际环境信息确定；di表示第i个机械臂连杆与障碍物的最短距离，N＝(I-J+J),λ是齐次解增益,是关于障碍物距离d0的函数，称为避障增益。
所述进行障碍物避障，包含如下步骤:
存在一个障碍物时，采用带权重因子加权求和的方法：
Figure GDA0003888669340000118
J+是雅可比矩阵的广义逆，
Figure GDA0003888669340000119
表示终端速度，也就是主运动；n0是进入障碍物安全区连杆的个数，wi是权重因子，λ(d0)h,i是速益，
Figure GDA00038886693400001110
是第i个进入障碍物安全区的连杆对应的齐次解；权重因子wi如下：
Figure GDA00038886693400001111
dm为安全区半径，d0,i为第i个连杆距离障碍物的最短距离；
Figure GDA00038886693400001112
表示沿
Figure GDA00038886693400001113
方向的单位向量，J0是避障点的雅可比矩阵，
同时存在多个障碍物时，采用带权重因子求和的方法：
Figure GDA00038886693400001114
p是处于危险状态的障碍物的个数，即连杆与障碍物的最短距离小于安全区；尽管这时避障点的实际速度不是理想速度，但是当距离机械臂最近的障碍物远远比其他障碍物近时，这时的权重因子接近1，也就是说非常接近理想背离速度，dj,i表示为第i个连杆距离第j个障碍物的最短距离；λ(d0)j,i表示第i个连杆与第j个障碍物最短距离的避障增益；
Figure GDA0003888669340000121
表示第i个连杆与第j个障碍物的角速度。
如图1所示，在具体应用实例中，一种手套箱封闭空间内机械臂的运动规划方法，本发明的详细步骤为：
步骤1：判断环境中是否有复杂障碍物
步骤2：如果无复杂障碍物，采用本发明提出的路径规划方法进行路径规划；
所述路径规划方法，包含如下步骤：
(1)用双向搜索策略搜出一条可行路径；
(2)直接以随机点为新节点进行局部优化处理；
(3)根据距离两颗树距离选择最优树；
(4)基于三角不等式的后处理优化；
(5)基于贝塞尔曲线来完成改进的IB-RRT*算法执行后的后续工作，创建一条更加圆滑的路径。
所述双向搜索策略搜出一条可行路径，算法中首先用初始构形x_start初始化搜索树T，从起始构形x_start目标构形x_goal构造两棵树。然后执行Extend_Tree(T)扩展函数，并判断扩展函数是否返回状态Reached；如果返回Reached状态，扩展过程中有一点落在了目标区域，则停止循环并返回搜索树T；如果返回Trapped状态，则继续循环，直到达到循环次数的上限N。包含如下步骤：
4-1：从起始构形x_start目标构形x_goal构造两棵树，引入Swap(G_a,G_b)函数，使每棵树向着对方交替扩展，直到两颗树相遇，因而不再是随机的扩展方式，而是带有启发性目的的扩展方式。
4-2：使用Random(X)函数从构形空间中产生一个随机点x_rand,使用Nearest_Neighbor T(x_rand)函数从树T上找到距离x_rand最近的一点x_near，使用Steer(x_rand,x_near)函数表示x_near沿x_rand方向前进一个步长step，并产生一个新节点x_new。然后把新节点添加到随机树中，检查是否到达目标点，若到达目标点返回状态Reached，停止扩展。否则返回状态Trapped，继续扩展。
4-3：最近节点：找出随机扩展树Tk中距离随机点x_rand最近的节点x_nearest。并返回最近节点x_nearest。
4-4：前进一个步长算法中，Dis(q_(near,)q_rand)算法表示q_near到q_rand的欧式距离，分别取q_near与q_rand的坐标。若距离小于步长，则q_rand作为要扩展的新节点q_new，若距离大于步长，则算出q_new的坐标。然后判断q_new是在q_near与q_rand的连线上还是反向延长线上。若在连线上则返回q_new；如果在反向延长线上则计算出q_new的坐标。
所述以随机点为新节点进行局部优化处理算法中，Cost(x)表示在已经扩展的树中，从起始点到x点的路径代价。每一个新添加的节点，都要计算其对应的路径代价。Line(x_1,x_2)表示通过直线连接x_1,x_2的路径代价。算法没有把新产生的点x_new直接与它的最近节点x_nearest相连，而是经过两个阶段的处理。
步骤3：如果有复杂障碍物，采用本发明提出的避障方法进行路径规划。
所述避障方法，包含如下步骤：
(1)碰撞检测；
(2)梯度投影法求解最优解；
(3)使用附加背离速度法避障算法进行计算；
(4)障碍物避障。
5-0：碰撞检测：给定点x、x'∈X，{Y:y∈Y,y＝x+t·(x'-x),t∈[0,1]}，如果
Figure GDA0003888669340000131
则函数CillisionFree(x,x')返回值True，即无碰撞；否则，返回值False，即有碰撞。
5-1：从x_new的附近节点x_near相中，寻找使Cost(x_new)最小的节点作为x_new的父节点，保证了新添加的节点x_new对应的路径代价有局部最小值。
5-2：如果经过节点x_new到附近节点x_near的路径代价小于节点x_near之前的路径代价，则将x_near的父节点改为节点x_new。
所述根据距离两颗树距离选择最优树算法，包含如下步骤：
6-1：NearVertices(x,Ta,Tb,r)表示找到距离x点为r的所有Ta,Tb上的节点。NearestVertices(x,Ta,Tb)表示寻找x到所有Ta,Tb上距离最近的节点。GetSortedList(x,Y)表示Y内所有节点到x点路径消耗从小到大顺序排列。GetBestTreeParent(L1,L2,Connection)表示从L1,L2中选择路径最小消耗点和最优树，并返回是否碰撞。InsertVertex(x,Y,T)将新节点x插入最优树T上。RewireVertices(x,L,T)修整最优树T上球内附近节点，使附近节点在加入新节点后保证每个节点上路径消耗是最小的。
6-2：最优父节点：GetBestTreeParent算法中，首先把
Figure GDA0003888669340000132
和
Figure GDA0003888669340000133
中各自最小消耗点找出来，再比较两个树上各自最小中最小的那个节点，如果
Figure GDA0003888669340000134
则赋值
Figure GDA0003888669340000135
给Xmin，如果
Figure GDA0003888669340000136
则赋值
Figure GDA0003888669340000137
给Xmin，然后判断是否两棵树相遇，并返回状态σf。
6-3：判断连接：ConnectTrees算法中，比较节点(σa,σb)的路径消耗之和与原来的路径消耗比较，然后判断是否两棵树相遇，并返回状态σf。
所述基于三角不等式的后处理优化，包含如下步骤：
7-1：相邻的三个节点X1,X2,X3，由三角不等式知，||X2-X1||+||X3-X2||＞||X3-X1||。如果X1到X3的路径可行(与障碍物无碰撞)，则直接连接X1到X3的路径，跳过X2，这样X1到X3的路径代价小于之前的路径代价。
7-2：算法中，card(·)表示集合中元素的个数，xvc-parent-to-parent表示节点xvc的父节点的父节点。每相邻三个节点进行一次优化，重复这一过程，直到优化前后的路径节点个数没有变化，这也说明路径中没有可以优化的节点。采用这样的优化措施，不仅可以加快算法收敛最优路径的速率，减小了路径代价，而且在一定程度上减小了轨迹的抖动。
所述基于贝塞尔曲线来完成改进的IB-RRT*算法执行后的后续工作，包含如下步骤：
8-1：贝塞尔曲线依赖于确定该曲线的控制点个数，N+1个顶点可以定义N次多项式的曲线。一般来说n阶贝塞尔曲线可以定义为以下形式：
Figure GDA0003888669340000141
式中：Pi称为控制点，为第i个顶点的坐标值，
Figure GDA0003888669340000142
为伯恩斯坦Bernstein多项式；其定义如下：
Figure GDA0003888669340000143
主要考虑n＝3的情况，也就是三次贝塞尔多项式Bp(t)。随着伯恩斯坦多项式的扩展，得到贝塞尔曲线的参数方程。
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
8-2：应用三次贝塞尔曲线的第一个属性，路径段Gp可以被构建成为P0＝qinit与P3＝qgoal，即随即树在扩展的时候每一个扩展的端点qinit可以定位P0，而每一个扩展的目标点qgoal可以定位为P3。这样随机树就可以按照三次贝塞尔曲线的形式进行扩展了。
所述碰撞检测，包含如下步骤:
9-1：障碍物处理：在本系统中采用球体包络和叠加的方法表示障碍物，用圆柱体包络法表示机械臂。在计算过程中，再把机械臂简化成线段连杆，把机械臂的实际连杆厚度叠加到障碍物上。障碍物经过处理之后,研究机械臂与障碍物的碰撞问题就简化成点(障碍物中心或机械臂末端)到线段(机械臂连杆)的距离计算问题。
9-2：由于矢量具有方向性，故一些方向的判断直接根据其正负号就可以得知，
Figure GDA0003888669340000151
上式中的
Figure GDA0003888669340000152
表示
Figure GDA0003888669340000153
方向上的单位向量，其意义是给所求向量确定方向。
Figure GDA0003888669340000154
是的两个向量的内积，且
Figure GDA0003888669340000155
其中θ为向量
Figure GDA0003888669340000156
与
Figure GDA0003888669340000157
之间的夹角，
Figure GDA0003888669340000158
是向量长度。
那么
Figure DA00038886693451188095
不带有方向性。此数值与上述表征方向的
Figure DA00038886693451197099
整体构成有大小、有方向的新向量
Figure DA00038886693451204849
即为
Figure GDA00038886693400001510
在
Figure GDA00038886693400001511
方向上的投影向量，C为投影点。
这里定义
Figure GDA00038886693400001512
特殊情况如点P在线段上、在线段延长线上等的情况全部适用于此公式，只是作为特殊情况出现，无需另作讨论。这也是矢量算法思想的优势所在。
故根据r值的不同，最短距离d可以计算为：
Figure GDA00038886693400001513
9-3：安全区概念：对于每一个机械臂连杆可达到的障碍物，都被定义了一个虚拟的人工安全区。当机械臂连杆进入某个障碍物的安全区内，并不意味着机械臂连杆会与障碍物发生碰撞，而是表示机械臂与障碍物碰撞的几率增大。机械臂上的任意一点进入安全区后会使障碍物对机械臂连杆产生一个虚拟法向力，使得机械臂沿着法向路径离开障碍物，当连杆离开障碍物安全区后，虚拟力消失。避障的目的不是机械臂连杆距离障碍物越远越好，而是避开障碍物，安全区的建立正好保证了这一点。
所述梯度投影法求解最优解，包含如下步骤:
机器人任务空间的速度,即操作速度
Figure GDA00038886693400001514
与关节空间的速度
Figure GDA0003888669340000161
之间的关系为
Figure GDA0003888669340000162
冗余度机器人的雅可比矩阵是长方矩阵，无论是否满秩都没有常规逆，只有广义逆。因此目前绝大多数对冗余度机器人的研究都是基于广义逆法。
由上式可知机器人末端速度与关节速度的关系，根据线性代数理论
Figure GDA0003888669340000163
其中,J+是雅可比矩阵的广义逆,在J满秩的情况下，J+＝JT(JJT)-1。
Figure GDA0003888669340000164
的第一项
Figure GDA0003888669340000165
是方程组
Figure GDA0003888669340000166
的最小范数解，它落在雅可比矩阵的行空间R(JT)中，即在n维实空间Rn的m维子空间中。第二项
Figure GDA0003888669340000167
是方程组
Figure GDA0003888669340000168
的通解，它位于雅可比矩阵的零空间N(J)中。
Figure GDA0003888669340000169
和
Figure GDA00038886693400001610
护卫正交向量。v是n维实空间Rn的任意矢量，即任意的关节速度矢量。(I-J+J)是零空间投影矩阵，
Figure GDA00038886693400001619
是v在零空间N(J)中的投影。冗余度机器人的第二任务就是通过选择不同的v来实现的。为了调节方便，通常把v乘上一个放大系数k，这样上式就变成
Figure GDA00038886693400001611
这就是解决冗余度机器人逆运动学问题的基本方法，它提供了满足终端运动要求所需的关节速度。在梯度投影法中，放大系数k直接影响机器人自运动的速度，k越大，优化的能力越大。但是放大系数k取得过大，会引起机器人关节速度超限，甚至使机器人控制不稳定而引起机器人关节运动震荡。因此，合理的选择放大系数k是非常重要的。
为了方便表示，我们可以把上式进一步化简表示为：
Figure GDA00038886693400001612
其中N＝(I-J+J)。
所述使用附加背离速度法避障算法进行计算，包含如下步骤:
为使冗余度机器人能躲避障碍物，需要选择合适的
Figure GDA00038886693400001613
使得最近点A0有远离障碍物的速度矢量
Figure GDA00038886693400001614
这就是冗余机器人的附加运动。联立机器人的主运动有
Figure GDA00038886693400001618
Figure GDA00038886693400001615
以上两式分别代表了机器人的主运动和附加运动。
Figure GDA00038886693400001616
是设定的避障点的速度，J0是避障点的雅可比矩阵。由以上两式得：
Figure GDA00038886693400001617
由上式给出
Figure GDA0003888669340000171
的最终解
Figure GDA0003888669340000172
最终解方程是考虑了避障的冗余度机器人运动学方程，它具有明显的物理意义。左边一项表示保证终端速度
Figure GDA0003888669340000173
的伪逆解，即最小范数解，也就是说主运动；右边一项表示了满足避障要求而不引起终端变化的自由度，即附加运动。其中
Figure GDA0003888669340000174
是避障速度，一般根据实际环境信息确定。
这里的J0N不仅取决于A0的位置，还取决于A0与障碍物的距离向量
Figure GDA0003888669340000175
所以要把方向考虑进去，用
Figure GDA0003888669340000176
表示如下：
Figure GDA0003888669340000177
式中
Figure GDA0003888669340000178
表示沿
Figure GDA0003888669340000179
方向的单位向量。
避障算法的效率取决于
Figure GDA00038886693400001710
代表遇到障碍物时，机械臂逃离障碍物的速度，这里定义:
Figure GDA00038886693400001711
αv为速度系数，v0为理想逃离速度，因为当障碍物离机械臂在安全区内时才会远离，当在安全区外时不会做出反应，所以定义dm为安全区半径，其表示如下：
Figure GDA00038886693400001712
为了有效地控制避障过程，保证机器人末端跟踪期望的运行轨迹，避免关节速度的不连续，需要对公式进行适当改写，形式如下：
Figure GDA00038886693400001713
其中N＝(I-J+J)，
Figure GDA00038886693400001714
表示障碍物躲避速度的单位矢量，αv是障碍物躲避速度的增益，αh是齐次解增益。αv，αh是关于障碍物距离d0的函数，统称避障增益。
这里由于主运动和附加运动同时进行，主运动是根据末端轨迹运动的，且恒定不变，附加运动时根据距离障碍物远近产生的，我们希望当机械臂靠近障碍物时主运动速度减缓，也就是说主运动优先级减小，附加运动优先级增大，这个过程平滑过渡。这里我们引入优先级调节参数λ(d0)改进上式为如下形式：
Figure GDA00038886693400001715
其中
Figure GDA0003888669340000181
上式是考虑了避障的冗余度机器人运动学方程，它具有明显的物理意义。左边一项表示保证终端速度
Figure GDA0003888669340000182
的伪逆解，即最小范数解，也就是说主运动；右边一项表示了满足避障要求而不引起终端变化的自由度，即附加运动。其中
Figure GDA0003888669340000183
是避障速度，一般根据实际环境信息确定。其中，关节空间的速度
Figure GDA0003888669340000184
Figure GDA0003888669340000185
表示附加运动，J+是雅可比矩阵的广义逆，dm为安全区半径，这里的
Figure GDA0003888669340000186
不仅取决于机械臂连杆上与障碍物最短距离点的位置，还取决于这个点与障碍物的距离向量
Figure GDA0003888669340000187
表示为
Figure GDA0003888669340000188
式中
Figure GDA0003888669340000189
表示沿
Figure GDA00038886693400001810
方向的单位向量，J0是避障点的雅可比矩阵。
Figure GDA00038886693400001811
是设定的避障点的速度，一般根据实际环境信息确定。di表示机械臂第i个连杆与障碍物的距离。
所述障碍物避障，包含如下步骤:
应该优先考虑距离障碍物最近的连杆，因为最近的连杆是最危险的，也最有可能导致机械臂的速度不连续甚至产生碰撞。为了同时兼顾其它在障碍物安全区之内的连杆，改善多个连杆靠近障碍物对机械臂关节角速度上引起的冲击震荡，采用带权重因子加权求和的方法。
其公式表述如下
Figure GDA00038886693400001812
这里n0是进入障碍物安全区连杆的个数，wi，λ(d0)hi，
Figure GDA00038886693400001813
分别是权重因子，速益和第i个进入障碍物安全区的连杆对应的齐次解。
权重因子wi可以表示如下。
Figure GDA00038886693400001814
这样就把每一个进入障碍物安全区的连杆都考虑进去了，使得每个进去安全区的连杆都有远离障碍物的趋势，并且越靠近障碍物的连杆远离速度越快。
上式只适用于针对单一障碍物，对于同时存在多个障碍物时，应该同时考虑多个障碍物并优先考虑距离连杆最近的障碍物，因为最近的障碍物是最危险的，也有可能导致机械臂速度不连续甚至产生碰撞。同时兼顾其他在阈值之内的其他障碍物。为了改善多个障碍物对机械臂速度上引起的冲击震荡，采用带权重因子求和的方法。其公式表述如下：
Figure GDA0003888669340000191
这里m是处于危险状态的障碍物的个数，尽管这时避障点的实际速度不是理想速度，但是当距离机械臂最近的障碍物远远比其他障碍物近时，这时的权重因子接近1，也就是说非常接近理想背离速度。
如图2所示，为新节点扩展过程示意图，在路径规划算法中，采用双向搜索策略搜出一条可行路径。此搜索方法实时性强、轨迹光滑的优点,提高了收敛速度，解决了传统RRT算法收敛速度慢、生成的航径距离过长等问题。
如图3为本发明路径规划算法示意图,图4为基于三角不等式的路径优化原理图，在图4中X1,X2,X3表示相邻的三个节点。本发明结合双树搜索的快速性和RRT*算法的渐进最优特性，再加上基于三角不等式优化处理路径方法提出一种改进的基于双向智能搜索的RRT*算法，根据与两颗树的距离大小智能选择最优树进行优化处理，从而达到均匀优化的目的，加快了路径收敛速度。它的优势共有四处：(1)、先用双向搜索策略搜出一条可行路径；(2)、直接以随机点为新节点进行局部优化处理；(3)、根据距离两颗树距离选择最优树；(4)、基于三角不等式的后处理优化。
如图5所示，为贝塞尔曲线路径处理原理图，X、Y表示路径点在整体坐标系中的横纵坐标，P0,P1,P2,P3分别为路径中的路径点，曲线为拟合后的贝塞尔曲线。贝塞尔曲线可以重建局部路线而不更改整条路径的形状，所以本发明应用贝塞尔曲线来完成改进的IB-RRT算法执行后的后续工作，创建一条更加圆滑的路径。
如图6、7、8a～8c分别为避障算法中的机械臂与障碍物简化关系示意图、安全区示意图、矢量算法求点到线段的最短距离原理图，组成本发明中避障算法总碰撞检测步骤，为轨迹规划算法奠定了障碍物建模基础。图8a～8c分别对应计算机械臂连杆与障碍物最短距离时的P点投影在A点左侧，P点投影在AB之间，P点投影在C点右侧三种情况。
如图9所示使用附加背离速度法避障算法进行计算，将避障算法中梯度投影法求最优解算法与基于附加背离速度法避障算法相结合进行避障计算，从而进行障碍物避障。
如图10、11所示，为七自由度机械臂在单障碍物和多障碍物情况下使用本发明中介绍的运动规划算法进行运动规划的示意图，图中Oi表示经过处理后的障碍物在空间中的中心点，基于附加背离速度法避障算法相结合进行避障计算，从而进行障碍物避障。

Claims (9)

1.一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：
判断环境中是否有障碍物；
如果无障碍物，采用基于IB-RRT算法的路径规划方法进行路径规划；
所述采用基于IB-RRT算法的路径规划方法进行路径规划，包含如下步骤：
1)用双向搜索策略搜出一条路径；
2)根据路径直接以随机点为新节点进行局部优化处理；
3)对于优化后的路径，根据两颗树距离选择最优树；
4)对最优树上的节点基于三角不等式进行优化，包括以下步骤：
步骤7-1：对于最优树上的相邻的三个节点X1,X2,X3，||X2-X1||+||X3-X2||＞||X3-X1||；当X1到X3的路径可行即机械臂与障碍物无碰撞，则直接连接X1到X3的路径，跳过X2，这样X1到X3的路径代价小于之前的路径代价；
步骤7-2：对最优树上的每相邻三个节点进行一次步骤7-1的优化，直到优化前后的路径节点个数没有变化；
5)基于贝塞尔曲线扩展得到最终的路径；
如果有障碍物，采用冗余机械臂运动学避障方法进行路径规划；
所述采用冗余机械臂运动学避障方法进行路径规划，包含如下步骤：
(1)对机械臂进行碰撞检测；
(2)对机械臂梯度投影法求解附加运动的最优解；
(3)使用附加背离速度法避障算法得到含有机械臂主运动和附加运动的最终解；
(4)进行障碍物避障。
2.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述用双向搜索策略搜出一条路径，包括以下步骤：
首先用初始构形x_start初始化搜索树T，从起始构形x_start目标构形x_goal构造两棵树；然后执行Extend_Tree(T)扩展函数，并判断扩展函数是否返回状态Reached；如果返回Reached状态，扩展过程中有一节点落在了目标区域，则停止循环并返回搜索树T；如果返回Trapped状态，则继续循环，直到达到循环次数的上限。
3.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述根据路径直接以随机点为新节点进行局部优化处理，包括以下步骤：
Cost(x)表示在已经扩展的树中，从起始点到x点的路径代价；Line(X1,X2)表示通过直线连接X1,X2的路径代价；X1,X2分别表示路径中直接随机生成的新节点；
5-1：碰撞检测：给定点x,x'∈X，{Y:y∈Y,y＝x+t·(x'-x),t∈[0,1]}，其中，点x,x'为产生的新节点中的随机点，X为产生的新节点的集合，Y为随机点x,x'所连线段上的任意点的集合，t为Y中的点在x,x'所连线段上分割出的两条线段的比例；如果
Figure FDA0003888669330000021
Xfree为在障碍物外的点的集合，也成为自由空间，则函数CollisionFree(x,x')返回值True，即无碰撞；否则，返回值False，即有碰撞；
5-2：从新节点x_new的相邻节点x_near中，寻找使Cost(x_new)最小的节点作为x_new的父节点；
5-3：如果经过新节点x_new到相邻节点x_near的路径代价小于节点x_near之前的路径代价，则将x_near的父节点改为节点x_new。
4.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述对于优化后的路径，根据两颗树距离选择最优树，包含如下步骤：
6-1：NearVertices(x,Ta,Tb,r)表示找到距离x点为r的所有Ta,Tb上的节点，其中，Ta,Tb分别表示首先用起始构形x_start和目标构形x_goal构造两棵树；NearestVertices(x,Ta,Tb)表示寻找x到所有Ta,Tb上距离最近的节点xnearest；GetSortedList(x,xnearest)表示xnearest内所有节点到x点路径消耗从小到大顺序排列；GetBestTreeParent(L1,L2,Connection)表示从由GetSortedList(x,Y)生成的最小消耗路径L1,L2中选择路径最小消耗点和由路径最小消耗点组成的最优树，并得到消耗最小的点为xmin，并返回判断此最优树是否与障碍物碰撞；InsertVertex(x,xmin,T)将新节点x插入最优树上；RewireVertices(x,L,T)表示修整最优树上的相邻节点，使相邻节点在加入新节点后保证每个节点上路径消耗最小；其中L为从由GetSortedList(x,Y)生成的最小消耗路径L1,L2；
6-2：最优父节点：首先把
Figure FDA0003888669330000022
和
Figure FDA0003888669330000023
中各自最小消耗点找出来，其中
Figure FDA0003888669330000024
和
Figure FDA0003888669330000025
分别表示从随机节点和附近节点中选出的路径消耗最小的路径，也是6-1中的L1,L2，再比较由L1,L2生成的两个树上各自路径消耗最小的那个节点
Figure FDA0003888669330000026
如果
Figure FDA0003888669330000027
则赋值
Figure FDA0003888669330000028
给Xmin，如果
Figure FDA0003888669330000029
则赋值
Figure FDA00038886693300000210
给Xmin，Xmin表示由L1,L2生成的两个树上路径消耗最小的节点中较小值，用于在InsertVertex(x,Y,T)步骤中，将随机节点插入在最优树后形成新的树，然后判断是否两棵树相遇，即判断两棵树是否有交点，并返回状态σf，σf为最优父节点；
6-3：判断连接：比较节点(σa,σb)的路径消耗之和与原来σf的路径消耗，若前者大于后者，则选择σf的路径，反之选择节点(σa,σb)的路径，最终，返回状态σf；然后判断是否两棵树相遇，即判断两棵树是否有交点，若不相遇，则重新规划节点(σa,σb)并进行判断，若相遇，则结束规划。
5.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述基于贝塞尔曲线扩展得到最终的路径，包含如下步骤：
8-1：n阶贝塞尔曲线为：
Figure FDA0003888669330000031
式中：Pi称为控制点，为第i个顶点的坐标值，
Figure FDA0003888669330000032
为伯恩斯坦Bernstein多项式；其定义如下：
Figure FDA0003888669330000033
ti表示第i个顶点的参数值；
n＝3时，得到三次贝塞尔曲线的参数方程：
Bp(t)＝P0(1-t)3+3P1t(1-t)2+3P2t2(1-t)+P3t3
8-2：应用三次贝塞尔曲线的属性，每个路径段Gp被构建成为P0＝qinit与P3＝qgoal，即优化后的最优树在扩展的时候每一个扩展的端点qinit可以定位P0，而每一个扩展的目标点qgoal可以定位为P3；进而得到最终的路径。
6.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述碰撞检测，包含如下步骤：
9-1：障碍物处理：采用球体包络的方法表示障碍物，用圆柱体包络法表示机械臂；把机械臂简化成线段连杆，把机械臂的实际连杆厚度叠加到障碍物上；
9-2：A表示
Figure FDA0003888669330000034
路径上的左端点，B表示
Figure FDA0003888669330000035
路径上的右端点，P表示
Figure FDA0003888669330000036
路径外一点，即机械臂连杆上与障碍物最短距离点；向量
Figure FDA0003888669330000037
为
Figure FDA0003888669330000038
在
Figure FDA0003888669330000039
方向上的投影向量，C为投影点；
定义
Figure FDA00038886693300000310
根据r值的不同，机械臂连杆与障碍物的最短距离d为：
Figure FDA0003888669330000041
9-3：当机械臂连杆与障碍物的最短距离d小于阈值，则表示机械臂连杆进入障碍物的安全区内，碰撞几率大，反之，机械臂连杆未进入障碍物的安全区内，无碰撞。
7.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述对机械臂梯度投影法求解附加运动的最优解，具体如下:
Figure FDA0003888669330000042
其中N＝(I-J+J)，J+是雅可比矩阵的广义逆，J+＝JT(JJT)-1，I为单位矩阵；机器人任务空间的操作速度
Figure FDA0003888669330000043
m表示空间自由度个数，关节空间的速度
Figure FDA0003888669330000044
n表示机械臂关节个数，
Figure FDA0003888669330000045
表示附加运动；v是关节速度矢量。
8.根据权利要求1所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述使用附加背离速度法避障算法得到含有机械臂主运动和附加运动的最终解，具体如下：
Figure FDA0003888669330000046
其中
Figure FDA0003888669330000047
右边第一项表示保证终端速度
Figure FDA0003888669330000048
的伪逆解，即最小范数解，也就是说主运动；左边一项表示了满足避障要求而不引起终端变化的自由度，即附加运动；其中，关节空间的速度
Figure FDA0003888669330000049
J+是雅可比矩阵的广义逆，dm为安全区半径，
Figure FDA00038886693300000410
式中
Figure FDA00038886693300000411
表示沿
Figure FDA00038886693300000412
方向的单位向量，J0是避障点的雅可比矩阵，
Figure FDA00038886693300000413
是设定的避障点的速度，根据实际环境信息确定；di表示第i个机械臂连杆与障碍物的最短距离，N＝(I-J+J),λ是齐次解增益,是关于障碍物距离d0的函数，称为避障增益。
9.根据权利要求8所述的一种手套箱封闭空间内机械臂的运动规划方法，其特征在于：所述进行障碍物避障，包含如下步骤:
存在一个障碍物时，采用带权重因子加权求和的方法：
Figure FDA0003888669330000051
J+是雅可比矩阵的广义逆，
Figure FDA0003888669330000052
表示终端速度，也就是主运动；n0是进入障碍物安全区连杆的个数，wi是权重因子，λ(d0)h,i是速益，
Figure FDA0003888669330000053
是第i个进入障碍物安全区的连杆对应的齐次解；权重因子wi如下：
Figure FDA0003888669330000054
dm为安全区半径，d0,i为第i个连杆距离障碍物的最短距离；
Figure FDA0003888669330000055
表示沿
Figure FDA0003888669330000056
方向的单位向量，J0是避障点的雅可比矩阵，
同时存在多个障碍物时，采用带权重因子求和的方法：
Figure FDA0003888669330000057
p是处于危险状态的障碍物的个数，即连杆与障碍物的最短距离小于安全区；dj,i表示为第i个连杆距离第j个障碍物的最短距离；λ(d0)j,i表示第i个连杆与第j个障碍物最短距离的避障增益；
Figure FDA0003888669330000058
表示第i个连杆与第j个障碍物的角速度。
CN201911363088.2A 2019-12-26 2019-12-26 一种手套箱封闭空间内机械臂的运动规划方法 Active CN110919661B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911363088.2A CN110919661B (zh) 2019-12-26 2019-12-26 一种手套箱封闭空间内机械臂的运动规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911363088.2A CN110919661B (zh) 2019-12-26 2019-12-26 一种手套箱封闭空间内机械臂的运动规划方法

Publications (2)

Publication Number Publication Date
CN110919661A CN110919661A (zh) 2020-03-27
CN110919661B true CN110919661B (zh) 2022-11-22

Family

ID=69862127

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911363088.2A Active CN110919661B (zh) 2019-12-26 2019-12-26 一种手套箱封闭空间内机械臂的运动规划方法

Country Status (1)

Country Link
CN (1) CN110919661B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112008729A (zh) * 2020-09-01 2020-12-01 云南电网有限责任公司电力科学研究院 一种用于架空线路检修机械臂的碰撞检测方法
CN112549016A (zh) * 2020-10-21 2021-03-26 西安工程大学 一种机械臂运动规划方法
CN114643576B (zh) * 2020-12-17 2023-06-20 中国科学院沈阳自动化研究所 一种基于虚拟力引导的人机协同目标抓取方法
CN113103236B (zh) * 2021-04-22 2022-06-10 山东大学 一种快速渐进最优的机械臂避障路径规划方法
CN113352319B (zh) * 2021-05-08 2022-10-21 上海工程技术大学 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法
CN113442140B (zh) * 2021-06-30 2022-05-24 同济人工智能研究院(苏州)有限公司 一种基于Bezier寻优的笛卡尔空间避障规划方法
CN113771035B (zh) * 2021-09-24 2022-10-18 河北工业大学 基于rrt*算法的冗余多自由度机械臂避障路径优化方法
CN114083519B (zh) * 2021-12-31 2023-05-09 北京敏锐达致机器人科技有限责任公司 机器人零空间拖动示教方法及示教装置
CN114406559A (zh) * 2022-01-26 2022-04-29 中国铁建电气化局集团有限公司 一种用于自动焊接设备的机械臂回零装置以及方法
CN114593743A (zh) * 2022-03-02 2022-06-07 杭州华鲲云起信息技术有限公司 一种基于改进双向rrt算法的路径规划方法及装置
US12122052B2 (en) * 2022-03-15 2024-10-22 Fanuc Corporation Swept volume deformation
CN117340890A (zh) * 2023-11-22 2024-01-05 北京交通大学 一种机器人运动轨迹控制方法
CN117944058B (zh) * 2024-03-27 2024-05-28 韦氏（苏州）医疗科技有限公司 一种自行功能机械臂的调度方法、系统及机械臂

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102902269B (zh) * 2012-09-21 2015-07-01 北京邮电大学 预选择最小距离指标冗余机器人动态避障方法
CN104392081A (zh) * 2014-06-30 2015-03-04 管小清 一种冗余机械臂运动障碍物避障算法
CN106166750B (zh) * 2016-09-27 2019-06-04 北京邮电大学 一种改进型d*机械臂动态避障路径规划方法
CN108705532B (zh) * 2018-04-25 2020-10-30 中国地质大学（武汉） 一种机械臂避障路径规划方法、设备及存储设备
CN110421556B (zh) * 2019-06-14 2022-07-19 河北工业大学 一种冗余双臂服务机器人实时避碰的轨迹规划方法及平稳运行方法
CN110228069B (zh) * 2019-07-17 2022-04-01 东北大学 一种机械臂在线避障运动规划方法
CN110497403A (zh) * 2019-08-05 2019-11-26 上海大学 一种改进双向rrt算法的机械臂运动规划方法

Also Published As

Publication number Publication date
CN110919661A (zh) 2020-03-27

Similar Documents

Publication Publication Date Title
CN110919661B (zh) 一种手套箱封闭空间内机械臂的运动规划方法
Sariff et al. An overview of autonomous mobile robot path planning algorithms
US9411335B2 (en) Method and apparatus to plan motion path of robot
Gochev et al. Path planning with adaptive dimensionality
CN106774347A (zh) 室内动态环境下的机器人路径规划方法、装置和机器人
Abu-Dakka et al. A direct approach to solving trajectory planning problems using genetic algorithms with dynamics considerations in complex environments
Ding et al. Trajectory replanning for quadrotors using kinodynamic search and elastic optimization
Lee et al. Online multiobjective evolutionary approach for navigation of humanoid robots
Dai et al. Novel potential guided bidirectional RRT* with direct connection strategy for path planning of redundant robot manipulators in joint space
Ferrer et al. Multi-objective cost-to-go functions on robot navigation in dynamic environments
Zghair et al. Intelligent Hybrid Path Planning Algorithms for Autonomous Mobile Robots.
Seder et al. Hierarchical path planning of mobile robots in complex indoor environments
Tang et al. Obstacle avoidance path planning of 6-DOF robotic arm based on improved A* algorithm and artificial potential field method
Yang et al. RMRL: Robot Navigation in Crowd Environments with Risk Map-based Deep Reinforcement Learning
Lu et al. An optimal frontier enhanced “next best view” planner for autonomous exploration
He et al. Robot path planning using improved rapidly-exploring random tree algorithm
Kumagai et al. Efficient locomotion planning for a humanoid robot with whole-body collision avoidance guided by footsteps and centroidal sway motion
Kim et al. Simple global path planning algorithm using a ray-casting and tracking method
Ata et al. COLLISION-FREE TRAJECTORY PLANNING FOR MANIPULATORS USING GENERALIZED PATTERN SEARCH.
Karagoz et al. MPC-graph: Feedback motion planning using sparse sampling based neighborhood graph
An et al. Application of an improved particle swarm optimization algorithm in inverse kinematics solutions of manipulators
Wang et al. An Improved A* Path Planning Algorithm of Autonomous Underwater Vehicles
He et al. Efficient multi-agent global navigation using interpolating bridges
Šišlák et al. Flight trajectory path planning
MARK A cell decomposition-based collision avoidance algorithm for robot manipulators

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant