CN113459086B - 一种超冗余机械臂路径规划方法 - Google Patents
一种超冗余机械臂路径规划方法 Download PDFInfo
- Publication number
- CN113459086B CN113459086B CN202110593223.3A CN202110593223A CN113459086B CN 113459086 B CN113459086 B CN 113459086B CN 202110593223 A CN202110593223 A CN 202110593223A CN 113459086 B CN113459086 B CN 113459086B
- Authority
- CN
- China
- Prior art keywords
- path
- new
- arm
- max
- node
- 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
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
-
- 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/1628—Programme controls characterised by the control loop
- B25J9/1643—Programme controls characterised by the control loop redundant control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
本发明涉及一种超冗余机械臂路径规划方法,步骤1:针对超冗余机械臂建立运动学模型,确定绳长、偏角、坐标之间的转换关系;步骤2:建立障碍物模型,确定与障碍物对应的不可抵达区域;步骤3:在步骤1的基础上,通过正弦定理及两个极值点,理论分析两相邻直线路径之间的最大夹角Ψmax与第i个臂杆端点沿其中一段直线路径运动过程中的臂杆偏角变化最大值ψmax、臂杆长度Li、路径步长lw之间的关系;步骤4:根据步骤2确定的不可抵达区域以及步骤3中的约束关系,提出改进RRT算法进行路径规划。本发明的可行性与优化性优于传统算法。
Description
技术领域
本发明涉及机械臂路径规划技术领域,具体涉及一种基于改进RRT算法的超冗余机械臂路径规划方法。
背景技术
在很多领域,路径规划技术都得到了广泛的应用。但是随着科技的发展及需求的提高,传统的路径规划算法难以满足复杂环境下的规划条件。基于节点采样的快速扩展随机树(Rapidly-Exploring Random Tree,RRT)算法,具有既能用于完整***的路径规划,也能添加约束条件用于非完整***的自主寻路等优点,为高维复杂环境下的路径规划开辟了一种解决方案。
RRT算法在路径规划领域取得较大的进展,但算法仍存在计算量偏大、路径不稳定、节点利用率低、不容易获得满足条件的路径等缺点。并且很少有算法在有关节偏角限制的条件下,分析如何规划路径,使得机械臂从狭窄空间中穿过。目前,可以借助一些软件、程序、工具箱(例如RVC、ROS)等工具可以规划出满足关节角度限制的路径,但在这些规划中都需要在规划过程中实时验算是否满足约束。因此,需要提出一种方法,可直接规划出符合关节角度限制的路径,可以保证机械臂在沿着规划的路径运动时,不仅可以穿过狭窄空间抵达目标点,并且还可以保证机械臂关节偏角始终在限制范围内。
发明内容
本发明解决的技术问题是:提出一种超冗余机械臂路径规划方法。该方法可与概率路线图(PRM)算法、A*算法、人工势能(APF)算法等路径规划算法相结合,可直接规划出符合关节角度限制的路径,大大提高了可行性与优化性。
本发明解决技术的方案是:一种超冗余机械臂路径规划方法,步骤如下:
步骤1:针对超冗余机械臂建立机械臂运动学模型,确定驱动绳长、臂杆偏转角度、臂杆末端点坐标之间的转换关系;
步骤2:针对环境中的障碍物建立障碍物模型,从障碍物模型中获得障碍物的位置、形状、大小信息,进而确定与障碍物对应的不可抵达区域;
步骤3:假设相邻臂杆长度相同且与路径步长lw成线性关系,根据步骤1中确定的机械臂运动学模型,理论分析两相邻直线路径之间的最大夹角Ψmax与第i个臂杆初始点沿其中一段直线路径运动过程中的臂杆偏角变化最大值ψmax、臂杆长度Li、路径步长lw之间的关系;
步骤4:根据步骤2确定的与障碍物对应的不可抵达区域以及步骤3中的关系约束进行路径规划。
优选的,所述的步骤2中,将路径与障碍物之间的最近距离设置为臂杆表面上的所有点距离该臂杆轴线的最远距离加上臂杆轴线上的点与直线路径之间的最远距离,与障碍物的距离小于等于上述最近距离的范围视为不可抵达区域。
优选的,步骤4采用MDA+RRT算法进行路径规划,具体通过下述方式实现:
步骤41:定义步长δ即lw,以路径规划的起始点xinit作为随机树T的根节点;
步骤42:随机选择一个节点xrand,选择一个离xrand最近的节点xnear,根据节点xnear与目标点xgoal之间的距离dgoal确定Ψmax的取值;然后根据步骤3中的关系与xrand、xnear、T分析xnew的选取范围,进而计算出xnew的位置;
步骤43:若xnew符合条件要求即未进入步骤2中确定的不可抵达区域,则将xnew附近的节点存放至xneighbor中,并将这些附近节点的祖先存放至xparent中,进而得到一个由xnew整理出的Xin集合;
步骤44:在Xin中找到使xnew成本最小且满足偏角要求的新父节点xpar,删除xnew与原父节点xnear的连线,连接xnew与xpar,并在T中进行修剪;
步骤45:若xneighbor内一个节点xfrom的权值大于当前xnew的权值与到该节点的距离之和,且满足偏角要求,则以当前xnew节点作为xfrom的父节点;
步骤46:重复寻找xnew及修剪xnew与附近节点的连接关系,直到到达目标点。
优选的,所述的步骤42通过下述方式实现:
S1、若xnear是起始点xinit,则在-60°到60°之间随机选取xnew的方向,执行步骤S2,否则,在-min(Ψmax)到min(Ψmax)之间随机选取xnew的方向,执行步骤S2;min(Ψmax)为节点xnear与目标点xgoal之间不同距离下确定的Ψmax的最小值;
S2、判断是否满足下列不等式Abs(angrand-angnear)≤Ψmax,若满足,则保持xnew方向;若不满足,则令Ψi为可选取范围中的边界值;
其中,xrand与xnear两点连成的线段相对于水平线的夹角为angrand,在T中读取xnear与其父节点连成的线段相对于水平线的夹角为angnear。
优选的,步骤42中通过下述方式确定Ψmax的取值:
dgoal<L,则令Ψmax取值为根据倒数第二个臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角;
dgoal≥L,则令Ψmax取值为根据最长臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角;
L为最后两个臂杆的长度和。
优选的,Ψi为可选取范围中的边界值,在二维情况下Ψi=angnear+Sign(angrand-angnear)·Ψmax,xnew为边界上的一个点;在三维情况下,保证两相邻直线路径之间的夹角为Ψmax,xnew的选取空间为一个半径为δ·sin(Ψmax)的圆,xnew为该圆上的一个点。
优选的,步骤3中关系通过下述方式确定:
假设lw=k·Li=k·Li-1,k∈(0,1],让第i-1个臂杆的起始点沿着第w段直线路径运动,其运动过程中第i个臂杆的偏角ψi变化的极大值出现的位置有两处;第一处为第i-1个臂杆末端点即第i个臂杆初始点运动到一段直线路径的起始点或终止点时,此时ψi变化的极大值记为ψi_end;第二处为第i个臂杆初始点运动到一段直线路径的中点时,该点处ψi变化的极大值记为ψi_mid;
求出这两个极大值后即求得臂杆偏角变化最大值max(ψi_end,ψi_mid)。
优选的,k=1时通过下述方式确定步骤3中的关系:
将相邻两臂杆沿轴线简化为一条直线,相邻两臂杆与路径上连续的三条直线构成两个三角形,根据三角形的正弦定理及角度变换确定关系如下:
9、根据权利要求7所述的一种超冗余机械臂路径规划方法,其特征在于:
Nend为第i个臂杆端点运动到一段直线路径的端点时,臂杆两个端点之间有几个路径转折点;
Nmid为第i个臂杆端点运动到一段直线路径的中点时,臂杆两个端点之间有几个路径转折点。
优选的,所述的步骤44中,在寻找新父节点xpar时,除了满足降低代价,且无碰撞的条件外,还需满足从xin的父节点到xin的路径与xin到xnew的路径之间的夹角不大于Ψmax;xin为Xin集合中的节点。
优选的,所述的步骤45中,若以当前xnew节点作为xfrom的父节点,则不仅需要满足降低代价、无碰撞的条件外,还需要满足从xnew的父节点到xnew的路径与xnew到xfrom的路径之间的夹角不大于Ψmax。
优选的,所述的超冗余机械臂具有2N+1个自由度,N为机械臂的数量,不少于3。
优选的,在无人汽车的自动驾驶、遥控飞机的自主规划、机器人的自主无碰行动中的应用。
本发明与现有技术相比的有益效果是:
(1)本发明方法可保证在二维、三维(2D、3D)环境中,机械臂的关节偏转角度始终在一定范围内。(2)MDA+RRT算法相对于RRT算法未增加计算量,规划出的路径的可行性与优化性也都优于对应的RRT算法。(3)本文中的方法可以与任何一种RRT算法或其他路径规划算法相结合,可为各种运动物体,规划出满足角度约束条件的路径,具有通用性。
附图说明
图1 MDA+RRT算法流程图;
图2超冗余机械臂结构图;
图3规划路径时的不可抵达区域示意图;
图4机械臂在直线路径上的示意图及等效图;
图5机械臂运动示意图及关节角度变化图;
图6 xnew选取范围的示意图;
图7六种算法在第一种障碍物中的树状分布图;
图8六种算法在第二种障碍物中的树状分布图;
图9六种算法在第三种障碍物中的树状分布图;
图10六种算法针对三种障碍物规划出的路径的性能参数图;
图11超冗余机械臂的运动过程图。
具体实施方式
下面结合实施例对本发明作进一步阐述。
本发明提供的一种基于改进RRT算法的超冗余机械臂路径规划方法,该方法除了可用于超冗余机械臂的路径规划外,还可以将该算法应用于其他运动物体(如:无人汽车的自动驾驶、遥控飞机的自主规划、机器人的自主无碰行动等)的路径规划,从而规划出指定偏角范围内的路径。算法流程图如图1所示,首先建立机械臂运动学模型和障碍物模型,然后理论分析各参数之间的关系,最后根据对应的RRT算法进行改进,得到对应的MDA+RRT算法。
在本实施例中,设三个规划地图的范围分别为2000×1665、3000×2500、1700×2000×1400。机械臂的数量N=8。
如图1所示,本发明具体实施步骤如下:
步骤1:建立机械臂运动学模型,即针对对应的超冗余机械臂建立的D-H坐标系,并推导出机械臂模型中个参数之间的转换关系。
建立的D-H坐标系如图2所示,其中包括相对于地面固定不变的世界坐标系{Oxyz}、基座的本体坐标系{O0}以及八个臂杆的本体坐标系{O1}-{O8}。各坐标系之间可以通过转换矩阵进行坐标系转换,两个相邻臂杆的本体坐标系之间的转换矩阵与第i个臂杆的长度Li、第i个臂杆的偏航角αi与第i个臂杆的俯仰角θi相关。驱动绳穿过相对于臂杆本体坐标系{Oi}固定不变的孔去驱动机械臂,所以可以根据驱动绳长度求出臂杆的偏转角度进而求出臂杆末端点坐标,也可以通过臂杆末端点坐标求出臂杆的偏转角度进而求出驱动绳长度。若已知机械臂的重复运动路径即可求出个臂杆末端点的位置,从而也可求出两相邻臂杆之间的偏转角度ψi。
步骤2:建立障碍物模型:针对环境中的障碍物建立对应的模型,可以从该模型中获得障碍物的位置、形状、大小等信息。
以第一个障碍物为例,分析为了保证机械臂不碰到障碍物,应当保证路径与障碍物之间的距离保持在多少之上。每个臂杆表面上的所有点距离该臂杆轴线的最远距离为75,考虑到臂杆沿着直线路径运动时,只是臂杆两端点沿着路径运动,并且在机械臂运动过程中还存在控制误差、定位误差等误差,所以臂杆轴线上的点与路径之间存在一定的距离(认为最大距离为Δmax),故令规划出的路径与障碍物之间的最近距离为75+Δmax。将与障碍物的距离小于等于75+Δmax的地方都视为不可抵达区域,如图3所示的黑色线框以内的区域都为不可抵达区域。因此,可以根据空间点与障碍物之间的最近距离来判断该空间点是否可抵达,在后面的路径规划中将障碍物模型变为该障碍物对应的不可抵达区域模型。
步骤3:理论分析臂杆长度Li、关节最大夹角ψmax、路径步长lw、路径中两相邻直线之间的最大夹角Ψmax这四个参数之间的关系,从而将关节偏角限制的问题转换到路径偏角限制的问题。
分析当lw=Li=Li-1(w=1,2,3…)时,各参数之间应满足的关系。如图4所示的两臂杆轴线与路径上的三条直线构成了两个三角形:△ABC与△CDE。根据三角形的正弦定理及角度转换可得出max(ψi)的数学模型如下:
进而可求出:
其次分析当lw=k·Li=k·Li-1(其中的k∈(0,1],w=1,2,3…)时,各参数之间应满足的关系。让第i-1个臂杆的起始点沿着第w段直线路径运动,其运动过程及第i个关节处的偏角变化如图5所示。
ψi的极大值出现的位置有两处。第一处为第i-1个臂杆末端点即第i个臂杆初始点运动到一段直线路径的起始点或终止点时(图3中的a=0或b=0),经推导得出该点处的ψi_end如下所示:
第二处为第i个臂杆初始点运动到一段直线路径的中点时(图3中的a=b),该点处的ψi_mid如下所示:
求出这两个极大值后即可求得关节最大偏角ψmax=max(ψi_end,ψi_mid)。
公式中的Nend为臂杆端点运动到一段直线路径的端点时,臂杆两个端点之间有几个路径转折点,例如图5中第一幅图中的Nend=1。其中的Nmid为臂杆端点运动到一段直线路径的中点时,臂杆两个端点之间有几个路径转折点,例如图5中第二幅图中的Nmid=2。令lw=300、Ψmax=22.40°、Li=Li-1=486,带入公式可求得ψi_end=39.99°、ψi_mid=35.77°,即让臂杆长度为486的机械臂沿着步长为300、两相邻直线之间的最大夹角为22.40°的路径运动时,关节偏角ψi始终不超过40°的限制范围。
根据公式可确定当lw=k·Li=k·Li-1、ψmax=40°时,Ψmax应取值多少,如表1所示。
表1不同步长下Ψmax的取值(k=0.1、0.2…1)
从表中可以看出k越小Ψmax越小,即lw越小Ψmax越小,并且lw与Ψmax之间是非线性关系。
下面结合具体的RRT算法进行改进,这里以Q-RRT*算法改进后的MDA-QRRT*算法为例,介绍如何规划出满足机械臂结构要求的运动路线。
步骤4:定义及步长δ(即路径补偿lw),以路径规划的起始点xinit作为随机树T的根节点。MDA-QRRT*算法以路径起点xinit作为随机树T的根节点,树中节点xi用集合V存储,V在n维空间中具有n列,每一行对应一个节点的位置数据,节点间的连接用边集E存储,E中的第一列表示节点xi的父节点是第几个节点,第2列为节点xi与其父节点这两点连成线段与水平线之间的角度,第3列为节点xi到其父节点再到父父节点一直到xinit处的叠加欧氏距离。始终保证V和E中的所有数据都在Xfree中。
步骤5:利用随机函数选择一个xrand,选择一个离xrand最近的节点xnear,然后在Angle-MDA函数中根据步骤3中的结论,根据xrand、xnear与T分析xnew的选取范围。在确定好Ψi即xnew的方向之后,根据下列公式可计算出xnew的位置。
xnew=xnear+δ·Ψi
其中的Angle-MDA函数根据xrand、xnear与T分析xnew的选取范围,进而确定下一个离散点xnew的具***置。
在该函数中首先求出xrand与xnear这两点连成的线段相对于水平线的夹角angrand,并在T中读取xnear与其父节点连成的线段相对于水平线的夹角angnear。本算法的目的是保证关节偏角始终在限定范围内,因为机械臂臂杆长度不恒定,所以首先需要根据节点xnear与目标点xgoal之间的距离dgoal来分析Ψmax的取值。由于本发明选择的机械臂的臂杆长度不相等,所以在前面的路径规划中把Li当成486即最长臂杆的长度,当路径上的离散点与目标物的距离小于L7+L8时,把Li当成206计倒数第二根臂杆的长度。根据步骤三中的结论,如果dgoal<L7+L8,则令Ψmax=40°,如果dgoal≥L7+L8,则令Ψmax=22.40°。
在确定好路径中两相邻直线之间的最大偏角Ψmax后,分析xnew是否超出可选取范围。如果xnear是起始点xinit,则在-60°到60°之间随机选取xnew的方向。否则,在-22.40°到22.40°之间随机选取xnew的方向;
如果两相邻直线之间的夹角Abs(angrand-angnear)≤Ψmax,则保持xnew的方向Ψi=angrand。如果两相邻直线之间的夹角Abs(angrand-angnear)>Ψmax,则令Ψi变为可选取范围中的边界值,图6列举了ang超过选取范围的情况。在二维情况下令Ψi=angnear+Sign(angrand-angnear)·Ψmax,xnew的选取点唯一,为边界上的一个点;在三维情况下,保证两相邻直线之间的夹角为Ψmax,xnew的选取空间为一个半径为δ·sin(Ψmax)的圆,xnew为该圆上的一个点。
步骤6:若xnew复合条件要求,即xnew与xnear之间无障碍物,则将xnew附近的节点存放至Xneighbor中,并将这些附近节点的祖先存放至Xparent中,进而得到一个由xnew整理出的Xin集合。
步骤7:利用ChooseParent-MDA函数在Xin中找到使xnew成本最小且满足偏角要求的新父节点xpar,删除xnew与原父节点xnear的连线,连接xnew与xpar,并在T中进行修剪。
ChooseParent-MDA函数内容如下,在初始化最小父节点xmin、最小路径σmin、最小代价cmin后,若xnew把在Xin中的节点xin当作新的父节点会降低代价,且xin与xnew形成的路径σ满足无碰撞,且满足从xin的父节点到xin的路径与xin到xnew的路径之间的夹角不大于Ψmax,则将节点xin设为xnew新的父节点。其中的Xin不仅包括xnew附近的节点还包括这些节点的祖先,从而提高了算法的优化速度。
步骤8:若Xneighbor内一个节点xfrom的权值大于当前xnew的权值与到该节点的距离之和,且满足偏角要求,则以当前xnew节点作为xfrom的父节点,具体过程如下。
在MDA-QRRT*算法中不仅需要修剪与xnew相关的路径,还会探寻Xneighbor中所有节点的更优父节点。在该函数中分析Xneighbor中所有节点xnei与xnew及xnew的父节点形成的新路径(即分析xnei与xfrom形成的新路径),是否降低代价且满足无碰撞及角度限制的条件(即满足从xnew的父节点到xnew的路径与xnew到xfrom的路径之间的夹角不大于Ψmax。)。如果满足,则将xnei的父节点更新为xfrom,修剪路径。
步骤9:重复寻找xnew及修剪xnew与附近节点的连接关系,修改随机树T中的数据,直到到达目标点。
仿真结果1:针对三种障碍物,采用每个算法运行1次,产生500个根节点,分别得到如图7、8、9所示的树状分布图。在产生500个根节点的运算过程中,MDA-RRT*算法、MDA-QRRT*算法的运行时间和RRT*算法、Q-RRT*算法的运行时间较为接近,这说明了改进算法未增加算法的计算量,只是限制了下个节点的选取范围,利用该约束来保证规划出的路径一定满足关节偏角限制的要求。从仿真结果中可以看出,在产生500个根节点后,采用三种MDA+RRT算法无论是在obs1中的简单的路径规划,还是在obs2中的复杂的路径规划,还是在obs3中的三维路径规划,都可规划出满足关节偏角限制要求的路径。由于RRT算法中存在随机性,在与RRT算法相关的路径规划中,快速得到一个高质量的初始解尤为重要。
仿真结果2:将终止条件由500个节点样本修改为规划出一条可抵达终点的路径,采用这六种算法在每个障碍物中运行50次,分析采用这六种算法规划出初始路径的所需时间与质量。仿真结果如图10所示,从图中可以看出三种MDA+RRT算法的可行性与优化性都优于对应的RRT算法。在这六种算法中,MDA-QRRT*算法的可行性与优化性均为最好。
如图11所示,展示了机械臂在运动过程中的5个位置对应的静态图以及最后机械臂抓取到目标物时的静态图。图中的红色线段为采用MDA-QRRT*算法规划出的长度为3085mm的路径,图中还包括障碍物、超冗余机械臂、当前位置对应的第i-1个臂杆与第i个臂杆之间的偏角ψi的大小及一些注释。
从图中可以看出机械臂在运动过程中始终未碰到障碍物,且可以抓取到目标物,并根据前面的仿真结果可知关节偏角始终小于40°,从而验证了采用本发明中的改进算法规划出的路径的可行性。
本发明针对超冗余机械臂结构中存在关节偏角限制的问题,在RRT算法的基础上,提出了MDA+RRT算法进行路径规划。在该发明中理论分析了各参数之间的关系,将关节偏角限制的问题转换到路径偏角限制的问题,在理论分析的基础上,对几种RRT算法进行了改进,发现改进后的MDA+RRT算法相对于改进前的RRT算法未增加计算量,只是限制了节点的选取范围,利用该约束来保证规划出的路径一定满足关节偏角限制的要求。
本发明还可以在本算法的基础上进行变步长RRT算法、双向RRT算法或与其他智能RRT算法相结合,从而规划出满足约束的路径。除了RRT还有:PRM算法、A*算法、APF算法。
本发明除了将MDA+RRT算法用于超冗余机械臂的路径规划外,还可以将该算法应用于其他运动物体(如:无人汽车的自动驾驶、遥控飞机的自主规划、巡航导弹躲避雷达搜索、机器人的自主无碰行动等)的路径规划,从而规划出指定偏角范围内的路径。
本发明未详细说明部分属于本领域技术领域的公知常识。
Claims (12)
1.一种超冗余机械臂路径规划方法,其特征在于步骤如下:
步骤1:针对超冗余机械臂建立机械臂运动学模型,确定驱动绳长、臂杆偏转角度、臂杆末端点坐标之间的转换关系;
步骤2:针对环境中的障碍物建立障碍物模型,从障碍物模型中获得障碍物的位置、形状、大小信息,进而确定与障碍物对应的不可抵达区域;
步骤3:假设相邻臂杆长度相同且与路径步长lw成线性关系,根据步骤1中确定的机械臂运动学模型,理论分析两相邻直线路径之间的最大夹角Ψmax与第i个臂杆初始点沿其中一段直线路径运动过程中的臂杆偏角变化最大值ψmax、臂杆长度Li、路径步长lw之间的关系;
步骤4:根据步骤2确定的与障碍物对应的不可抵达区域以及步骤3中的关系约束进行路径规划;
步骤4采用MDA+RRT算法进行路径规划,具体通过下述方式实现:
步骤41:定义步长δ即lw,以路径规划的起始点xinit作为随机树T的根节点;
步骤42:随机选择一个节点xrand,选择一个离xrand最近的节点xnear,根据节点xnear与目标点xgoal之间的距离dgoal确定Ψmax的取值;然后根据步骤3中的关系与xrand、xnear、T分析xnew的选取范围,进而计算出xnew的位置;
步骤43:若xnew符合条件要求即未进入步骤2中确定的不可抵达区域,则将xnew附近的节点存放至xneighbor中,并将这些附近节点的祖先存放至xparent中,进而得到一个由xnew整理出的Xin集合;
步骤44:在Xin中找到使xnew成本最小且满足偏角要求的新父节点xpar,删除xnew与原父节点xnear的连线,连接xnew与xpar,并在T中进行修剪;
步骤45:若xneighbor内一个节点xfrom的权值大于当前xnew的权值与到该节点的距离之和,且满足偏角要求,则以当前xnew节点作为xfrom的父节点;
步骤46:重复寻找xnew及修剪xnew与附近节点的连接关系,直到到达目标点。
2.根据权利要求1所述的一种超冗余机械臂路径规划方法,其特征在于,所述的步骤2中,将路径与障碍物之间的最近距离设置为臂杆表面上的所有点距离该臂杆轴线的最远距离加上臂杆轴线上的点与直线路径之间的最远距离,与障碍物的距离小于等于上述最近距离的范围视为不可抵达区域。
3.根据权利要求2所述的一种超冗余机械臂路径规划方法,其特征在于,所述的步骤42通过下述方式实现:
S1、若xnear是起始点xinit,则在-60°到60°之间随机选取xnew的方向,执行步骤S2,否则,在-min(Ψmax)到min(Ψmax)之间随机选取xnew的方向,执行步骤S2;min(Ψmax)为节点xnear与目标点xgoal之间不同距离下确定的Ψmax的最小值;
S2、判断是否满足下列不等式Abs(angrand-angnear)≤Ψmax,若满足,则保持xnew方向;若不满足,则令Ψi为可选取范围中的边界值;
其中,xrand与xnear两点连成的线段相对于水平线的夹角为angrand,在T中读取xnear与其父节点连成的线段相对于水平线的夹角为angnear。
4.根据权利要求2所述的一种超冗余机械臂路径规划方法,其特征在于步骤42中通过下述方式确定Ψmax的取值:
dgoal<L,则令Ψmax取值为根据倒数第二个臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角;
dgoal≥L,则令Ψmax取值为根据最长臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角;
L为最后两个臂杆的长度和。
6.根据权利要求1或4所述的一种超冗余机械臂路径规划方法,其特征在于步骤3中关系通过下述方式确定:
假设lw=k·Li=k·Li-1,k∈(0,1],让第i-1个臂杆的起始点沿着第w段直线路径运动,其运动过程中第i个臂杆的偏角ψi变化的极大值出现的位置有两处;第一处为第i-1个臂杆末端点即第i个臂杆初始点运动到一段直线路径的起始点或终止点时,此时ψi变化的极大值记为ψi_end;第二处为第i个臂杆初始点运动到一段直线路径的中点时,该点处ψi变化的极大值记为ψi_mid;
求出这两个极大值后即求得臂杆偏角变化最大值max(ψi_end,ψi_mid)。
9.根据权利要求1所述的一种超冗余机械臂路径规划方法,其特征在于,所述的步骤44中,在寻找新父节点xpar时,除了满足降低代价,且无碰撞的条件外,还需满足从xin的父节点到xin的路径与xin到xnew的路径之间的夹角不大于Ψmax;xin为Xin集合中的节点。
10.根据权利要求1所述的一种超冗余机械臂路径规划方法,其特征在于,所述的步骤45中,若以当前xnew节点作为xfrom的父节点,则不仅需要满足降低代价、无碰撞的条件外,还需要满足从xnew的父节点到xnew的路径与xnew到xfrom的路径之间的夹角不大于Ψmax。
11.根据权利要求1所述的一种超冗余机械臂路径规划方法,其特征在于,所述的超冗余机械臂具有2N+1个自由度,N为机械臂的数量,不少于3。
12.根据权利要求1所述的一种超冗余机械臂路径规划方法,其特征在于,在无人汽车的自动驾驶、遥控飞机的自主规划、机器人的自主无碰行动中的应用。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110593223.3A CN113459086B (zh) | 2021-05-28 | 2021-05-28 | 一种超冗余机械臂路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110593223.3A CN113459086B (zh) | 2021-05-28 | 2021-05-28 | 一种超冗余机械臂路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113459086A CN113459086A (zh) | 2021-10-01 |
CN113459086B true CN113459086B (zh) | 2022-07-29 |
Family
ID=77871623
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110593223.3A Active CN113459086B (zh) | 2021-05-28 | 2021-05-28 | 一种超冗余机械臂路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113459086B (zh) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4967126A (en) * | 1990-01-30 | 1990-10-30 | Ford Aerospace Corporation | Method of controlling a seven degree of freedom manipulator arm |
JP2000020117A (ja) * | 1998-06-29 | 2000-01-21 | Yaskawa Electric Corp | ロボットの動作経路計画方法およびその装置 |
DE102015211865B3 (de) * | 2015-06-25 | 2016-05-12 | Kuka Roboter Gmbh | Verfahren zum redundanzoptimierten Planen eines Betriebs eines mobilen Roboters |
CN106695802A (zh) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | 基于多自由度机械臂的改进式rrt*的避障运动规划方法 |
CN106737689A (zh) * | 2017-01-19 | 2017-05-31 | 哈尔滨工业大学深圳研究生院 | 基于模式函数的超冗余机械臂混合逆向求解方法及*** |
CN108908331A (zh) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学(深圳) | 超冗余柔性机器人的避障方法及***、计算机存储介质 |
CN109227549A (zh) * | 2018-11-08 | 2019-01-18 | 汕头大学 | 一种基于切线递推的机器人平滑避障运动规划方法 |
CN111070238A (zh) * | 2020-01-14 | 2020-04-28 | 北京精密机电控制设备研究所 | 一种组合式弱扭转连续型机械臂连接关节 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6576255B2 (ja) * | 2016-01-25 | 2019-09-18 | キヤノン株式会社 | ロボット軌道生成方法、ロボット軌道生成装置、および製造方法 |
US11667035B2 (en) * | 2019-07-01 | 2023-06-06 | Wisconsin Alumni Research Foundation | Path-modifying control system managing robot singularities |
-
2021
- 2021-05-28 CN CN202110593223.3A patent/CN113459086B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4967126A (en) * | 1990-01-30 | 1990-10-30 | Ford Aerospace Corporation | Method of controlling a seven degree of freedom manipulator arm |
JP2000020117A (ja) * | 1998-06-29 | 2000-01-21 | Yaskawa Electric Corp | ロボットの動作経路計画方法およびその装置 |
DE102015211865B3 (de) * | 2015-06-25 | 2016-05-12 | Kuka Roboter Gmbh | Verfahren zum redundanzoptimierten Planen eines Betriebs eines mobilen Roboters |
CN106737689A (zh) * | 2017-01-19 | 2017-05-31 | 哈尔滨工业大学深圳研究生院 | 基于模式函数的超冗余机械臂混合逆向求解方法及*** |
CN106695802A (zh) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | 基于多自由度机械臂的改进式rrt*的避障运动规划方法 |
CN108908331A (zh) * | 2018-07-13 | 2018-11-30 | 哈尔滨工业大学(深圳) | 超冗余柔性机器人的避障方法及***、计算机存储介质 |
CN109227549A (zh) * | 2018-11-08 | 2019-01-18 | 汕头大学 | 一种基于切线递推的机器人平滑避障运动规划方法 |
CN111070238A (zh) * | 2020-01-14 | 2020-04-28 | 北京精密机电控制设备研究所 | 一种组合式弱扭转连续型机械臂连接关节 |
Also Published As
Publication number | Publication date |
---|---|
CN113459086A (zh) | 2021-10-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110653805B (zh) | 笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法 | |
Cheng et al. | RRT-based trajectory design for autonomous automobiles and spacecraft | |
KR101339480B1 (ko) | Rrt 기반의 듀얼 트리 구조를 이용한 이동 로봇의 궤적 계획 방법 | |
Hota et al. | Optimal geometrical path in 3D with curvature constraint | |
CN113050640B (zh) | 一种基于生成对抗网络的工业机器人路径规划方法和*** | |
CN112356033B (zh) | 一种融合低差异序列与rrt算法的机械臂路径规划方法 | |
CN112130582B (zh) | 一种多智能体编队形成方法 | |
CN110580740B (zh) | 多智能体协同三维建模方法及装置 | |
CN111216125A (zh) | 面向狭窄通道的移动机械臂装置的避障方法及*** | |
CN112762957A (zh) | 一种基于多传感器融合的环境建模及路径规划方法 | |
CN110146087B (zh) | 一种基于动态规划思想的船舶路径规划方法 | |
CN114147708B (zh) | 基于改进的天牛须搜索算法的机械臂避障路径规划方法 | |
CN113625702B (zh) | 基于二次规划的无人车同时路径跟踪与避障方法 | |
CN112497216B (zh) | 一种基于深度学习的工业机器人位姿精度补偿方法 | |
CN113467476A (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
CN113459086B (zh) | 一种超冗余机械臂路径规划方法 | |
Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
CN113589809A (zh) | 可避障的挖掘机工作装置作业轨迹规划方法及装置 | |
CN116009558A (zh) | 一种结合运动学约束的移动机器人路径规划方法 | |
Liu et al. | Simulation of manipulator path planning based on improved rrt* algorithm | |
Vanegas et al. | Smooth Three-Dimensional Route Planning for Fixed-Wing Unmanned Aerial Vehicles With Double Continuous Curvature | |
Nazarzehi et al. | Decentralized three dimensional formation building algorithms for a team of nonholonomic mobile agents | |
Asqui et al. | Path planning based in algorithm rapidly-exploring random tree RRT | |
Chen et al. | FDSPC: Fast and Direct Smooth Path Planning via Continuous Curvature Integration |
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 |