CN113459087B - 一种基于最小势能算法的可限制偏角的路径规划方法 - Google Patents
一种基于最小势能算法的可限制偏角的路径规划方法 Download PDFInfo
- Publication number
- CN113459087B CN113459087B CN202110593312.8A CN202110593312A CN113459087B CN 113459087 B CN113459087 B CN 113459087B CN 202110593312 A CN202110593312 A CN 202110593312A CN 113459087 B CN113459087 B CN 113459087B
- Authority
- CN
- China
- Prior art keywords
- path
- point
- length
- discrete
- potential energy
- 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
- B25J9/1666—Avoiding collision or forbidden zones
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Abstract
一种基于最小势能算法的可限制偏角的路径规划方法,其特征是首先建立机械臂运动学模型与障碍物模型,推导出各参数之间的转换关系;其次通过空间点与障碍物、目标点的距离求出该空间点的总体势能,并通过路径总长、关节最大偏角及与障碍物的最近距离,制定评价函数;在指定关节最大偏转角度的前提下,推导出等步长与变步长中小臂杆的最大偏角与小臂杆长度之间的关系,从而得到下个离散点的选取范围;根据是否等步长、路径方向、初始点方向是否确定这三个因素,得到了六种子算法,采用这些算法进行路径规划得到离散点,利用直线将离散点转换为连续路径,利用评价函数选出六种算法下的最优路径,完成可限制偏角的路径规划。
Description
技术领域
本发明涉及机械臂的路径规划领域,特别涉及一种基于最小势能算法的可限制偏角的路径规划方法。
背景技术
具有多个自由度、多种运动模式的机械臂可在狭小、复杂以及非结构化的环境中执行任务,具有极好的环境适应性和极高的避障能力,广泛应用于各领域。为了更好的利用机械臂完成任务,进行路径规划尤为重要。
然而,由于机械臂关节结构的影响,关节存在最大偏角,而机械臂沿着传统算法规划出的路径运动时,关节偏角会超过最大偏角,即机械臂沿着传统算法规划出的路径无法实现运动。故需提出一种考虑机械臂关节最大偏角的算法,在存在障碍物的环境中,为机械臂规划出评价指标高的路径。
发明内容
本发明的目的在于:克服现有技术的不足,提出一种基于最小势能算法的可限制偏角的路径规划方法。通过路径规划指定机械臂的运动,目前的算法都是针对路径总长、与障碍物的距离等因素进行规划,未考虑关节最大偏角进行路径规划,因此为了规划出满足机械臂关节角度限制的路径,需要制定有效的算法来确定路径中各个离散点的选取范围。
本发明采用的的技术方案是:
一种基于最小势能算法的可限制偏角的路径规划方法,步骤如下:
步骤一,建立机械臂运动学模型:建立相对于地面固定不变的世界坐标系、基座的本体坐标系以及八个臂杆的本体坐标系,通过转换矩阵进行各坐标系之间的变换,在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系,建立起机械臂运动学模型;
步骤二,障碍物建模:将不规则形状障碍物转换为常见形状障碍物;
步骤三,计算空间点与障碍物之间的距离dobs以及空间点与目标点之间的距离daim;把空间点(x,y,z)与每个障碍物之间的距离dobsi中最小值作为空间点(x,y,z)与障碍物之间的距离dobs,即dobs=min(dobsi);
步骤四,将计算得出的dobs、daim分别转换为与障碍物相关的斥力势能Eobs、与目标点相关的引力势能Eaim,进而求出该空间点(x,y,z)的总体势能E=Eaim-Eobs;
步骤五,考虑路径总长、关节最大偏角及与障碍物的最近距离,制定评价函数;
步骤六,在指定关节最大偏转角度的前提下,通过将一个臂杆等效为n个虚拟小臂杆,推导出等步长与变步长中虚拟小臂杆的最大偏角γn与虚拟小臂杆长度ln之间的关系,从而得到下个离散点的选取范围;虚拟小臂杆的末端点即为下个离散点;
步骤七,根据是否等步长、路径方向、初始点方向是否确定三个因素,确定六种算法,并分别采用六种算法进行路径规划,得到每种算法中规划出路径对应的离散点;所述六种算法分别为第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法和第三变步长算法;
步骤八,根据路径规划得到的离散点,利用直线将离散点转换为连续路径,求出每条路径各自对应的路径总长,根据逆运动学求解方法求出机械臂沿着该路径运动时的关节最大偏角及与障碍物的最近距离,利用评价函数选出所有路径中的最优路径,即可规划出可限制关节偏角且最优的路径。
进一步的,所述机械臂包括机械臂本体结构、驱动机构以及滑台;机械臂本体结构由八个刚性臂杆串联而成,臂杆之间通过十字轴万向节连接,驱动机构包括伺服电机、联轴器、丝杠和滑块,联轴器安装于丝杠两端,通过伺服电机带动丝杠转动来控制滑块前后移动,从而拉动驱动绳产生位移,通过控制驱动绳产生的位移及拉力调整各个臂杆的两个自由度;机械臂本体结构安装在滑台上,在滑台上移动。
进一步的,所述步骤四将计算得出的dobs、daim分别转换为与障碍物相关的斥力势能Eobs、与目标点相关的引力势能Eaim,进而求出该空间点(x,y,z)的总体势能E=Eaim-Eobs,具体为:
其中e为自然常数,dobs为空间点与障碍物之间的最近距离,daim为空间点与目标点之间的距离。
进一步的,所述步骤五根据路径总长、关节最大偏角及与障碍物的最近距离,制定评价函数,具体为:
其中J为评价函数的数值,Plong为路径总长,max(ψi)为机械臂沿着该路径运动过程中关节最大偏转角度,min(dobs)为机械臂沿着该路径运动过程中与障碍物的最近距离。
进一步的,所述步骤六中,当相邻离散点之间的等步长ln恒定且满足下式
其中n为大于等于1的任意自然数,ψmax为路径中的两条直线之间的最大夹角,L为臂杆长度;
若为第一个离散点,则不考虑前面小臂杆的偏转,则
γn=2ψmax/(n+1)
γn为两个相邻小臂杆之间的最大偏角;
若为第二个及后面的离散点,则
βn=ψmax(n-1)/(2n)
γn=ψmax/n
βn为一个臂杆与其等效的小臂杆之间的最大偏角;
在等步长的情况下,通过上述推导结果可得虚拟小臂杆的末端点即为下个离散点的选取范围。
进一步的,所述步骤六中,当相邻离散点之间的变步长不恒定时,若第w个小臂杆长度为是指nw个相同臂长的虚拟小臂杆可等效为一个臂杆,第w+1个小臂杆长度为是指nw+1个相同臂长的虚拟小臂杆可等效为一个臂杆,第w+1个小臂杆相对于第w个小臂杆的最大偏角加上两个小臂杆相对于实际臂杆的最大偏角与不能超过ψmax,即
整理求得
进一步的,采用第一等步长算法进行路径规划,具体包括如下步骤:
(7.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从起始点开始规划路径;n为正整数;
(7.2)根据确定的下个离散点的选取范围,在该选取范围内根据网格划分法得到多个离散点;
(7.3)计算出这些离散点的总体势能;
(7.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(7.5)判断通过(7.4)得到的离散点与目标点的距离是否小于等于步长;如果小于等于步长,则终止路径规划;如果大于步长,则返回(7.2),继续循环,从而求得多个离散点;
(7.6)遍历n的取值,循环执行7.2~7.5,得到n种不同路径对应的离散点。
进一步的,采用第二等步长算法进行路径规划,具体包括如下步骤:
(8.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从目标点朝着起始点逆向规划路径,且第一个离散点的选取方向设定为一个固定方向;
(8.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(8.3)计算出这些离散点的总体势能;
(8.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(8.5)判断通过(8.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(8.2),继续循环,从而求得多个离散点;
(8.6)遍历n的取值,循环执行8.2~8.5,得到n种不同路径对应的离散点。
进一步的,采用第三等步长算法进行路径规划,具体包括如下步骤:
(9.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从目标点朝着起始点逆向规划路径,且第一个离散点的选取方向没有限制;
(9.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(9.3)计算出这些离散点的总体势能;
(9.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(9.5)判断通过(9.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(9.2),继续循环,从而求得多个离散点;
(9.6)遍历n的取值,循环执行9.2~9.5,得到n种不同路径对应的离散点。
进一步的,采用第一变步长算法进行路径规划,具体包括如下步骤:
(10.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(10.3)计算出这些离散点的总体势能;
(10.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(10.5)判断通过(10.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(10.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
进一步的,采用第二变步长算法进行路径规划,具体包括如下步骤:
(11.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(11.3)计算出这些离散点的总体势能;
(11.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点;
(11.5)判断通过(11.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(11.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
进一步的,采用第三变步长算法进行路径规划,具体包括如下步骤:
(12.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(12.3)计算出这些离散点的总体势能;
(12.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点;
(12.5)判断通过(12.4)得到的离散点与目标点的距离是否小于等于步长,
如果小于等于步长,则终止路径规划;如果大于步长,则返回(12.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
本发明与现有技术相比的优点在于:
(1)本发明提出一种基于最小势能算法的可限制偏角的路径规划方法,该算法中将一个臂杆等效为多个虚拟小臂杆,将臂杆的长度及最大偏转角度转换为小臂杆的长度及最大偏转角度,在小臂杆的偏转范围内获取最低势能点,作为下个离散点,通过直线将最优离散点连接,得到多组可行重复路径,通过分析路径总长、臂杆之间的相对偏角、机械臂与障碍物的距离等因素,得到一组最优路径。
(2)本发明推导出了算法中的路径离散点长度与离散点选取范围的关系,从而保证机械臂沿着利用该算法规划出的路径运动时,关节偏角保持在限制范围内。
(3)本发明根据是否等步长、路径规划的方向及第一个离散点的选取方向有无限制,可将MPE-MDA算法分为六种:第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法以及第三变步长算法。通过这六种算法均可得出避障、关节偏角保持在限制范围内,并可通过改变这些算法中的斥力势能、引力势能的权值来解决路径规划陷入死区的问题。
附图说明
图1 MPE-MDA算法下的路径规划流程图;
图2机械臂结构图;
图3 D-H坐标系图;
图4臂杆等效为小臂杆示意图;
图5 MPE-MDA-ES算法下的第一个离散点选取表面范围;
图6 MPE-MDA-ES算法下的第w(w≥2)个离散点选取表面范围;
图7 MPE-MDA-VS算法下的第一个离散点选取空间范围;
图8 MPE-MDA-VS算法下的第w(w≥2)个离散点选取空间范围;
图9机械臂运动过程图;
图10驱动绳的分布图;
图11障碍物线框模型图。
具体实施方式
下面结合附图对本发明进行详细说明。
本发明采用一种基于最小势能算法(MPE-MDA)的可限制偏角的路径规划方法,算法流程图如图1所示,首先建立了冗余机械臂的模型,其次建立障碍物模型、计算空间点到障碍物的距离、通过指定的转换函数将距离转换为势能,然后通过一种子算法得出一系列离散点,通过直线连接离散点得出尽可能的远离障碍物、接近目标物的路径,且机械臂沿着该路径运动时,臂杆之间的偏转角度不超过极限角度,利用评价函数选出六种算法下的最优路径,完成可限制偏角的路径规划。具体实施步骤如下:
步骤一,建立机械臂模型。建立相对于地面固定不变的世界坐标系、基座的本体坐标系以及八个臂杆的本体坐标系,通过转换矩阵进行各坐标系之间的变换,在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系,建立起机械臂运动学模型。
冗余机械臂结构如图2所示,所述机械臂包括机械臂本体结构、驱动机构以及滑台。机械臂本体结构由八个刚性臂杆串联而成,臂杆之间通过十字轴万向节连接,驱动机构由伺服电机、联轴器、丝杠和滑块组成。联轴器安装于丝杠两端,通过伺服电机带动丝杠转动来控制滑块前后移动,从而拉动驱动绳产生位移,通过控制驱动绳产生的位移及拉力可以调整各个臂杆的两个自由度。机械臂本体结构安装在滑台上,在滑台上移动。需要保证相邻臂杆之间的偏转角度不超过极限角度,否则会发生碰撞。
建立如图3所示的相对于地面固定不变的世界坐标系{OXYZ}、基座的本体坐标系{O0}以及八个臂杆的本体坐标系{Oi},通过转换矩阵进行各坐标系之间的变换,在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系,建立起机械臂运动学模型。
接下来分析关节角度与臂杆末端点之间的转换关系,根据臂杆末端点O9的坐标及路径可通过逆运动学方法求出所有关键点Oi的坐标(xi,yi,zi)(i=1,2……8)。
进而可以通过(x1,y1,z1)与(x2,y2,z2)求得α1和θ1,求解公式如下:
可以在自然坐标系中求解α1和θ1,但α2和θ2是第2个臂杆相对于第1个臂杆的偏转角度,而不是第2个臂杆在自然坐标系中的偏转角度,所以在求解第i个臂杆相对于第i-1个臂杆的偏转角度时,需要将坐标点从自然坐标系转换为第i-1个臂杆的坐标系{Oi-1}中。
式中的(xi+1_i-1,yi+1_i-1,zi+1_i-1)为第i个臂杆末端执行器Oi+1在坐标系{Oi-1}中的坐标,通过可以将坐标系{Oi-2}中的坐标转换为坐标系{Oi-1}中的坐标。矩阵中包括αi-1、θi-1、Li-1这三个参数,该矩阵是矩阵的逆矩阵。
通过(xi+1_i-1,yi+1_i-1,zi+1_i-1)可求得第i个臂杆相对于第i-1个臂杆的偏转角度αi和θi,具体公式如下:
通过上述公式可以依次求解出16个偏转角度。
接下来分析驱动绳长与关节角度之间的转换关系,
通过各个关节位置坐标可求得各个关节的偏转角度,进而可以通过每个关节的偏转角度求得每个驱动绳在该关节处的长度,进而累加可求得每根驱动绳的总长。
本发明中的机械臂的每个关节由3根并联驱动绳驱动,驱动绳的一端固定在关节端面上,另一端固定在基座的驱动机构中,在万向节的限制下可以由驱动绳拉动臂杆从而使关节转动。第i个臂杆中穿过27-3i(i=1,2,...,7,8)个驱动绳,每个臂杆中只有3根驱动绳用于驱动本臂杆,其余的驱动绳均穿过本臂杆驱动后面的臂杆。
以第一个关节为例,分析24根驱动绳在该关节处的长度与α1、θ1、驱动绳的位置角度之间的关系。图10为24个绳孔的位置排布示意图,相邻两个绳孔之间的夹角为15。孔ija、孔ijb、孔ijc是指这三个孔在第i(i=1,2,...,8)个臂杆上,并成120均匀分布;穿过孔ija、孔ijb、孔ijc的三根驱动绳驱动第j(j=1,2,...,8且j≥i)个臂杆。令图中各个孔的位置角度取各个孔的的圆心与截面中心连成的直线与Zi轴之间的夹角为该点的位置角度则
式中的d是关节中心点到臂杆端面的距离,r是驱动绳穿过的孔的圆心与臂杆中心之间的距离。
进而可得点P11b_0与点P11b_1之间的距离:
整理可得:
式中的θ=θ1、α=α1、公式(19)为通用公式,输入两个偏转角度θ、α和驱动绳的位置角度即可求得该驱动绳在关节处的实时长度。求解出各个关节处的驱动绳的长度,利用下列公式将驱动绳每一段的长度累加,即可求得每根驱动绳的总长。
式中lja、ljb、ljc为驱动第j个臂杆的三根驱动绳在第一个关节及第一个关节之后的总长。
总之,通过变维缩放法可推导出各个关节中心点的位置,进而可以通过位置求解出16个关节角度,进而可以计算出24根驱动绳的长度。
步骤二,障碍物建模,将不规则形状障碍物转换为常见形状障碍物。
障碍物形状多种多样,有规则形状的障碍物,也有表面凹凸不平的不规则形状障碍物,为了精确计算空间点到复杂障碍物表面的距离,可将不规则形状障碍物转换为常规障碍物。进而分析障碍物的位置及尺寸数据,建立障碍物模型。一般来讲,球、长方体、圆柱、圆锥为规则形状。比如:
如图11所示,空间中具有球、长方体、圆柱、圆锥、不完整圆柱这五个障碍物,及y=-4、y=6、z=-3、z=4这四面墙,图中的点为障碍物的旋转中心或机械臂运动轨迹起始点(0,0,0)、目标点(20,0,0)。球、长方体的旋转中心均为质心,圆柱、不完整圆柱的旋转中心在底面圆心上,圆锥的旋转中心为顶点。
以第i个障碍物的旋转中心为原点,建立相对于第i个障碍物固定不变的本体坐标系{Oobsi}。世界坐标系{Oxyz}与障碍物本体坐标系{Oobsi}可通过转换矩阵进行坐标转化。最初障碍物旋转中心与原点重合,绕X轴、Y轴、Z轴旋转后,再沿X轴、Y轴、Z轴平移过去,形成最终的障碍物,故障碍物的位置数据有dxi、dyi、dzi,具体数值如表1所示。转换矩阵可用以上位置数据表示,即
球的形状可用半径r1表示,长方体的形状可用长a2、宽b2、高c2表示,圆柱的形状可用半径r3、高h3表示,圆锥的形状可用半径r4、高h4表示,不完整圆柱的形状可用切面与轴线的距离a5、半径r5、高h5表示,具体数值如表1所示。
表1障碍物的位置及尺寸数据
步骤三,分析空间点与障碍物、目标点之间的距离。
计算空间点与障碍物之间的距离dobs以及空间点与目标点之间的距离daim;把空间点(x,y,z)与每个障碍物之间的距离dobsi中最小值作为空间点(x,y,z)与障碍物之间的距离dobs,即dobs=min(dobsi)。
步骤四,分析空间点的势能。
将计算得出的dobs、daim分别转换为与障碍物相关的斥力势能Eobs、与目标点相关的引力势能Eaim,进而求出该空间点(x,y,z)的总体势能E=Eaim-Eobs,具体为:
其中e为自然常数,dobs为空间点与障碍物之间的最近距离,daim为空间点与目标点之间的距离。
步骤五,制定评价函数。
考虑路径总长、关节最大偏角及与障碍物的最近距离,制定评价函数,具体为:
其中J为评价函数的数值,Plong为路径总长,max(ψi)为机械臂沿着该路径运动过程中关节最大偏转角度,min(dobs)为机械臂沿着该路径运动过程中与障碍物的最近距离。
步骤六,在指定关节最大偏转角度的前提下,通过将一个臂杆等效为多个虚拟小臂杆(虚拟小臂杆的末端点即为下个离散点)的方法,推导出等步长与变步长中虚拟小臂杆的最大偏角γn与虚拟小臂杆长度ln之间的关系,从而得到下个离散点的选取范围,虚拟小臂杆的末端点即为下个离散点。机械臂沿着这些算法规划出的路径运动时,每个臂杆的偏转角度不超过极限角度,且可避障、不会陷入局部极小值。
(6.1)当相邻离散点之间的步长ln恒定时。
如图4所示的将一个臂杆等效为n个相同臂长的虚拟小臂杆,其中一个臂杆与其等效的小臂杆之间的最大偏角为βn,两个相邻小臂杆之间的最大偏角为γn,小臂杆的长度为ln。从图中可以看出第w+1个小臂杆相对于第w个小臂杆的极限位置及运动范围与γn相关。
当偏转角度最大时,n个ln臂长的小臂杆各偏转βn,其末端点的坐标为(ln[cos(γn+βn)+…+cos(nγn+βn)],0,ln[sin(γn+βn)+…+sin(nγn+βn)])。
一个臂杆与n个其等效的小臂杆组成n+1多边形,可得出内角和公式:
2βn+(n-1)(180°-γn)=180°(n-1)
可得βn=(n-1)γn/2。
从图中还可看出:
2βn+γn=ψmax
公式中的等号左边及等号右边均可表示两臂杆之间的最大偏角,可得γn=ψmax/n。将小臂杆的长度投影到等效的一个臂杆上,当n为奇数时,满足:
当n为偶数时,满足:
将当n为奇数及偶数时的公式整理,可得出ln与L关系:
上述推导过程中n是取得整数,最终得到上式,若取n为小数,带入上式也可求得对应的ln,故n不仅可以取整数,令n等于任意正实数时,仍然可求出对应的ln与L的关系。
若为第一个离散点,则不用考虑前面小臂杆的偏转,则βn=0、γn=2ψmax/(n+1),第一个离散点的可选取表面如图5所示。
若为第二个及后面的离散点,则βn=ψmax(n-1)/(2n)、γn=ψmax/n,离散点的可选取表面如图6所示。
若第w个小臂杆长度为是指nw个相同臂长的虚拟小臂杆可等效为一个臂杆,若第w+1个小臂杆长度为是指nw+1个相同臂长的虚拟小臂杆可等效为一个臂杆,第w+1个小臂杆相对于第w个小臂杆的最大偏角加上两个小臂杆相对于实际臂杆的最大偏角与不能超过ψmax,可得:
整理可求得:
MPE-MDA-VS算法下的第一个离散点的可选取空间如图7所示,第二个及后面的离散点的可选取空间如图8所示。
步骤七,根据是否等步长、路径方向、初始点方向是否确定这三个因素,得到了六种子算法:第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法以及第三变步长算法。
其中第一等步长算法与第一变步长算法中路径规划方向为起始点到目标点,第二等步长算法与第二变步长算法中路径规划方向为目标点到起始点,第一个离散点的选取方向设定为(0,0,1),第三等步长算法与第三变步长算法中路径规划方向为目标点到起始点,第一个离散点的选取方向无限制。
在最小势能算法的基础上,分别采用第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法以及第三变步长算法进行路径规划,在步骤六推导出的范围内选择一个最小势能点作为下个离散点,类比得出一系列离散点,当离散点与目标点的距离小于等于步长时,终止寻找离散点。
采用第一等步长算法进行路径规划,具体包括如下步骤:
(7.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从起始点开始规划路径;n为正整数;
(7.2)根据确定的下个离散点的选取范围,在该选取范围内根据网格划分法得到多个离散点;
(7.3)计算出这些离散点的总体势能;
(7.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(7.5)判断通过(7.4)得到的离散点与目标点的距离是否小于等于步长;如果小于等于步长,则终止路径规划;如果大于步长,则返回(7.2),继续循环,从而求得多个离散点;
(7.6)遍历n的取值,循环执行7.2~7.5,得到n种不同路径对应的离散点。
采用第二等步长算法进行路径规划,具体包括如下步骤:
(8.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从目标点朝着起始点逆向规划路径,且第一个离散点的选取方向设定为一个固定方向;
(8.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(8.3)计算出这些离散点的总体势能;
(8.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(8.5)判断通过(8.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(8.2),继续循环,从而求得多个离散点;
(8.6)遍历n的取值,循环执行8.2~8.5,得到n种不同路径对应的离散点。
采用第三等步长算法进行路径规划,具体包括如下步骤:
(9.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从目标点朝着起始点逆向规划路径,且第一个离散点的选取方向没有限制;
(9.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(9.3)计算出这些离散点的总体势能;
(9.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(9.5)判断通过(9.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(9.2),继续循环,从而求得多个离散点;
(9.6)遍历n的取值,循环执行9.2~9.5,得到n种不同路径对应的离散点。
采用第一变步长算法进行路径规划,具体包括如下步骤:
(10.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(10.3)计算出这些离散点的总体势能;
(10.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(10.5)判断通过(10.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(10.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
采用第二变步长算法进行路径规划,具体包括如下步骤:
(11.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(11.3)计算出这些离散点的总体势能;
(11.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点;
(11.5)判断通过(11.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(11.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
采用第三变步长算法进行路径规划,具体包括如下步骤:
(12.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(12.3)计算出这些离散点的总体势能;
(12.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点;
(12.5)判断通过(12.4)得到的离散点与目标点的距离是否小于等于步长,
如果小于等于步长,则终止路径规划;如果大于步长,则返回(12.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
步骤八,根据上述六种不同的算法进行路径规划得到离散点,利用直线将离散点转换为连续路径,求出每条路径各自对应的路径总长,根据基本的逆运动学求出机械臂沿着该路径运动时的关节最大偏角及与障碍物的最近距离,利用评价函数选出六种算法下的最优路径,完成可限制偏角的路径规划。
实施例:
在一种障碍物环境中,通过仿真求得了一条最优路径作为机械臂的最终运动路径,该路径对应的评价值J=74.84。图9为对应的机械臂沿着该路径运动的过程图。从图中可以看出在机械臂沿着指定路径运动的过程中始终与障碍物保持一定的距离,且臂杆之间的相对偏角也始终保持在限制范围内。通过仿真验证了本发明的正确性。
针对传统路径规划算法在空间避障中无法同时优化路径总长、与障碍物间的最近距离,更无法限制关节偏转角度的问题,本发明提出了一种用于路径规划的限制关节最大偏角的最小势能(MPE-MDA)算法。建立了机械臂与障碍物模型,设立了空间点势能与距离的转换关系及路径评价函数,在指定关节最大偏转角度的前提下,推导出等步长与变步长中小臂杆的最大偏角与小臂杆长度之间的关系,从而得到下个离散点的选取范围。仿真分析步长、方向等因素对MPE-MDA算法路径规划的影响,得出的每条路径的关节最大偏转角度均在限制范围内,与障碍物间的最近距离都大于0.6个单位,路径总长都在20-23个单位之间,均优于APF算法结果。得到的路径评价值最高为74.84,且该算法路径规划结果整体优于其他五种算法。总之,采用MPE-MDA算法实现了考虑关节偏转角度受限等问题的超冗余机械臂空间避障的重复路径规划。
以上所揭露的仅为本发明一种实施例而已,当然不能以此来限定本发明之权利范围,本领域普通技术人员可以理解实现上述实施例的全部或部分流程,并依本发明权利要求所作的等同变化,仍属于发明所涵盖的范围。
本发明未详细说明部分属本领域技术人员公知常识。
Claims (12)
1.一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于步骤如下:
步骤一,建立机械臂运动学模型:建立相对于地面固定不变的世界坐标系、基座的本体坐标系以及八个臂杆的本体坐标系,通过转换矩阵进行各坐标系之间的变换,在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系,建立起机械臂运动学模型;
步骤二,障碍物建模:将不规则形状障碍物转换为常见形状障碍物;
步骤三,计算空间点与障碍物之间的距离dobs以及空间点与目标点之间的距离daim;把空间点(x,y,z)与每个障碍物之间的距离dobsi中最小值作为空间点(x,y,z)与障碍物之间的距离dobs,即dobs=min(dobsi);
步骤四,将计算得出的dobs、daim分别转换为与障碍物相关的斥力势能Eobs、与目标点相关的引力势能Eaim,进而求出该空间点(x,y,z)的总体势能E=Eaim-Eobs;
步骤五,考虑路径总长、关节最大偏角及与障碍物的最近距离,制定评价函数;
步骤六,在指定关节最大偏转角度的前提下,通过将一个臂杆等效为n个虚拟小臂杆,推导出等步长与变步长中虚拟小臂杆的最大偏角γn与虚拟小臂杆长度ln之间的关系,从而得到下个离散点的选取范围;虚拟小臂杆的末端点即为下个离散点;
步骤七,根据是否等步长、路径方向、初始点方向是否确定三个因素,确定六种算法,并分别采用六种算法进行路径规划,得到每种算法中规划出路径对应的离散点;所述六种算法分别为第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法和第三变步长算法;
步骤八,根据路径规划得到的离散点,利用直线将离散点转换为连续路径,求出每条路径各自对应的路径总长,根据逆运动学求解方法求出机械臂沿着该路径运动时的关节最大偏角及与障碍物的最近距离,利用评价函数选出所有路径中的最优路径,即可规划出可限制关节偏角且最优的路径。
2.根据权利要求1所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:所述机械臂包括机械臂本体结构、驱动机构以及滑台;机械臂本体结构由八个刚性臂杆串联而成,臂杆之间通过十字轴万向节连接,驱动机构包括伺服电机、联轴器、丝杠和滑块,联轴器安装于丝杠两端,通过伺服电机带动丝杠转动来控制滑块前后移动,从而拉动驱动绳产生位移,通过控制驱动绳产生的位移及拉力调整各个臂杆的两个自由度;机械臂本体结构安装在滑台上,在滑台上移动。
7.根据权利要求6所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:采用第一等步长算法进行路径规划,具体包括如下步骤:
(7.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从起始点开始规划路径;n为正整数;
(7.2)根据确定的下个离散点的选取范围,在该选取范围内根据网格划分法得到多个离散点;
(7.3)计算出这些离散点的总体势能;
(7.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(7.5)判断通过(7.4)得到的离散点与目标点的距离是否小于等于步长;如果小于等于步长,则终止路径规划;如果大于步长,则返回(7.2),继续循环,从而求得多个离散点;
(7.6)遍历n的取值,循环执行7.2~7.5,得到n种不同路径对应的离散点。
8.根据权利要求6所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:采用第二等步长算法进行路径规划,具体包括如下步骤:
(8.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从目标点朝着起始点逆向规划路径,且第一个离散点的选取方向设定为一个固定方向;
(8.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(8.3)计算出这些离散点的总体势能;
(8.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(8.5)判断通过(8.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(8.2),继续循环,从而求得多个离散点;
(8.6)遍历n的取值,循环执行8.2~8.5,得到n种不同路径对应的离散点。
9.据权利要求6所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:采用第三等步长算法进行路径规划,具体包括如下步骤:
(9.1)将一个臂杆等效为n个相同的臂长为ln的虚拟小臂杆,指定路径步长恒定且为ln,从目标点朝着起始点逆向规划路径,且第一个离散点的选取方向没有限制;
(9.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(9.3)计算出这些离散点的总体势能;
(9.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(9.5)判断通过(9.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(9.2),继续循环,从而求得多个离散点;
(9.6)遍历n的取值,循环执行9.2~9.5,得到n种不同路径对应的离散点。
10.据权利要求6所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:采用第一变步长算法进行路径规划,具体包括如下步骤:
(10.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(10.3)计算出这些离散点的总体势能;
(10.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点;
(10.5)判断通过(10.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(10.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
11.据权利要求6所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:采用第二变步长算法进行路径规划,具体包括如下步骤:
(11.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(11.3)计算出这些离散点的总体势能;
(11.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点;
(11.5)判断通过(11.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(11.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
12.据权利要求6所述的一种基于最小势能算法的可限制偏角的路径规划方法,其特征在于:采用第三变步长算法进行路径规划,具体包括如下步骤:
(12.2)根据下个离散点的选取范围,在该选取范围内根据网格划分法得到许多离散点;
(12.3)计算出这些离散点的总体势能;
(12.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点;
(12.5)判断通过(12.4)得到的离散点与目标点的距离是否小于等于步长,如果小于等于步长,则终止路径规划;如果大于步长,则返回(12.2),继续循环,从而求得多个离散点;该多个离散点仅对应一条路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110593312.8A CN113459087B (zh) | 2021-05-28 | 2021-05-28 | 一种基于最小势能算法的可限制偏角的路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110593312.8A CN113459087B (zh) | 2021-05-28 | 2021-05-28 | 一种基于最小势能算法的可限制偏角的路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113459087A CN113459087A (zh) | 2021-10-01 |
CN113459087B true CN113459087B (zh) | 2022-07-05 |
Family
ID=77871626
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110593312.8A Active CN113459087B (zh) | 2021-05-28 | 2021-05-28 | 一种基于最小势能算法的可限制偏角的路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113459087B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102008024950A1 (de) * | 2008-05-23 | 2009-11-26 | Kuka Roboter Gmbh | Verfahren und Vorrichtung zur Steuerung eines Manipulators |
CN106424877A (zh) * | 2016-10-15 | 2017-02-22 | 中国计量大学 | 一种新型机器人铣削加工刀具轨迹生成方法 |
CN109877836A (zh) * | 2019-03-13 | 2019-06-14 | 浙江大华技术股份有限公司 | 路径规划方法、装置、机械臂控制器和可读存储介质 |
CN110142760A (zh) * | 2019-04-23 | 2019-08-20 | 南京邮电大学 | 一种软体攀爬机器人建模方法 |
CN110640736A (zh) * | 2019-10-23 | 2020-01-03 | 南京工业大学 | 用于智能制造的机械臂运动规划方法 |
DE102019126465A1 (de) * | 2018-10-01 | 2020-04-02 | KBee AG | Verfahren und Vorrichtung zur Trajektorienbestimmung für serielle Manipulatoren |
CN112276953A (zh) * | 2020-10-27 | 2021-01-29 | 清华大学深圳国际研究生院 | 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110111880B (zh) * | 2019-04-22 | 2021-09-28 | 北京航空航天大学 | 柔性针的基于障碍分级的人工势场路径规划方法及装置 |
-
2021
- 2021-05-28 CN CN202110593312.8A patent/CN113459087B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102008024950A1 (de) * | 2008-05-23 | 2009-11-26 | Kuka Roboter Gmbh | Verfahren und Vorrichtung zur Steuerung eines Manipulators |
CN106424877A (zh) * | 2016-10-15 | 2017-02-22 | 中国计量大学 | 一种新型机器人铣削加工刀具轨迹生成方法 |
DE102019126465A1 (de) * | 2018-10-01 | 2020-04-02 | KBee AG | Verfahren und Vorrichtung zur Trajektorienbestimmung für serielle Manipulatoren |
CN109877836A (zh) * | 2019-03-13 | 2019-06-14 | 浙江大华技术股份有限公司 | 路径规划方法、装置、机械臂控制器和可读存储介质 |
CN110142760A (zh) * | 2019-04-23 | 2019-08-20 | 南京邮电大学 | 一种软体攀爬机器人建模方法 |
CN110640736A (zh) * | 2019-10-23 | 2020-01-03 | 南京工业大学 | 用于智能制造的机械臂运动规划方法 |
CN112276953A (zh) * | 2020-10-27 | 2021-01-29 | 清华大学深圳国际研究生院 | 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质 |
Non-Patent Citations (1)
Title |
---|
郭亚星等.基于绳索欠驱动的连续型机械臂设计及仿真.《导弹与航天运载技术》.2021,第92-96页. * |
Also Published As
Publication number | Publication date |
---|---|
CN113459087A (zh) | 2021-10-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109895101B (zh) | 一种关节型机械臂逆运动学数值唯一解求取方法 | |
Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
WO2021184655A1 (zh) | 一种超冗余机械臂末端轨迹运动规划方法 | |
CN109676610B (zh) | 一种断路器装配机器人及其实现工作轨迹优化的方法 | |
CN107791248B (zh) | 基于不满足Pieper准则的六自由度串联机器人的控制方法 | |
CN106844951B (zh) | 基于分段几何法求解超冗余机器人逆运动学的方法及系统 | |
CN106041932B (zh) | 一种ur机器人的运动控制方法 | |
CN113043286B (zh) | 一种多自由度机械臂实时避障路径规划系统及方法 | |
CN113146600A (zh) | 基于运动学迭代学习控制的柔性机器人轨迹规划方法及装置 | |
Abbasnejad et al. | Architecture optimization of 4PUS+ 1PS parallel manipulator | |
CN109366486B (zh) | 柔性机器人逆运动学求解方法、系统、设备、存储介质 | |
CN111660296A (zh) | 一种基于全域空间条件数的优化机器人灵活度的方法 | |
Xu et al. | Design, analysis and optimization of Hex4, a new 2R1T overconstrained parallel manipulator with actuation redundancy | |
CN113459087B (zh) | 一种基于最小势能算法的可限制偏角的路径规划方法 | |
Zhou et al. | Design and kinematic of a dexterous bioinspired elephant trunk robot with variable diameter | |
Cao et al. | Orientation workspace analysis of a special class of the Stewart–Gough parallel manipulators | |
CN116673963A (zh) | 面向无序断路器零件的双机械臂协作柔性装配系统及方法 | |
Liu et al. | Optimal design of a coupling-input cable-driven parallel robot with passive limbs based on force space analysis | |
Huang et al. | A novel path planning algorithm considering the maximum deflection angle of joint | |
Liu et al. | Trajectory planning of robot manipulators based on unit quaternion | |
Pajak et al. | Planning of a point to point collision-free trajectory for mobile manipulators | |
Ge et al. | Kinematics modeling and analysis of manipulator using the dual quaternion | |
JP2021146409A (ja) | 逆運動学演算装置及び逆運動学演算方法 | |
CN113547519B (zh) | 一种基于变维缩放的机械臂逆运动学求解方法 | |
Zhang et al. | Workspace analysis and size optimization of planar 3-DOF redundantly actuated parallel mechanism |
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 |