一种基于最小势能算法的可限制偏角的路径规划方法与流程

1.本发明涉及机械臂的路径规划领域，特别涉及一种基于最小势能算法的可限制偏角的路径规划方法。


背景技术：

2.具有多个自由度、多种运动模式的机械臂可在狭小、复杂以及非结构化的环境中执行任务，具有极好的环境适应性和极高的避障能力，广泛应用于各领域。为了更好的利用机械臂完成任务，进行路径规划尤为重要。
3.然而，由于机械臂关节结构的影响，关节存在最大偏角，而机械臂沿着传统算法规划出的路径运动时，关节偏角会超过最大偏角，即机械臂沿着传统算法规划出的路径无法实现运动。故需提出一种考虑机械臂关节最大偏角的算法，在存在障碍物的环境中，为机械臂规划出评价指标高的路径。


技术实现要素：

4.本发明的目的在于：克服现有技术的不足，提出一种基于最小势能算法的可限制偏角的路径规划方法。通过路径规划指定机械臂的运动，目前的算法都是针对路径总长、与障碍物的距离等因素进行规划，未考虑关节最大偏角进行路径规划，因此为了规划出满足机械臂关节角度限制的路径，需要制定有效的算法来确定路径中各个离散点的选取范围。
5.本发明采用的的技术方案是：
6.一种基于最小势能算法的可限制偏角的路径规划方法，步骤如下：
7.步骤一，建立机械臂运动学模型：建立相对于地面固定不变的世界坐标系、基座的本体坐标系以及八个臂杆的本体坐标系，通过转换矩阵进行各坐标系之间的变换，在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系，建立起机械臂运动学模型；
8.步骤二，障碍物建模：将不规则形状障碍物转换为常见形状障碍物；
9.步骤三，计算空间点与障碍物之间的距离d
obs
以及空间点与目标点之间的距离d
aim
；把空间点(x,y,z)与每个障碍物之间的距离d
obsi
中最小值作为空间点(x,y,z)与障碍物之间的距离d
obs
，即d
obs
＝min(d
obsi
)；
10.步骤四，将计算得出的d
obs
、d
aim
分别转换为与障碍物相关的斥力势能e
obs
、与目标点相关的引力势能e
aim
，进而求出该空间点(x,y,z)的总体势能e＝e
aim
‑
e
obs
；
11.步骤五，考虑路径总长、关节最大偏角及与障碍物的最近距离，制定评价函数；
12.步骤六，在指定关节最大偏转角度的前提下，通过将一个臂杆等效为n个虚拟小臂杆，推导出等步长与变步长中虚拟小臂杆的最大偏角γ
n
与虚拟小臂杆长度l
n
之间的关系，从而得到下个离散点的选取范围；虚拟小臂杆的末端点即为下个离散点；
13.步骤七，根据是否等步长、路径方向、初始点方向是否确定三个因素，确定六种算法，并分别采用六种算法进行路径规划，得到每种算法中规划出路径对应的离散点；所述六
种算法分别为第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法和第三变步长算法；
14.步骤八，根据路径规划得到的离散点，利用直线将离散点转换为连续路径，求出每条路径各自对应的路径总长，根据逆运动学求解方法求出机械臂沿着该路径运动时的关节最大偏角及与障碍物的最近距离，利用评价函数选出所有路径中的最优路径，即可规划出可限制关节偏角且最优的路径。
15.进一步的，所述机械臂包括机械臂本体结构、驱动机构以及滑台；机械臂本体结构由八个刚性臂杆串联而成，臂杆之间通过十字轴万向节连接，驱动机构包括伺服电机、联轴器、丝杠和滑块，联轴器安装于丝杠两端，通过伺服电机带动丝杠转动来控制滑块前后移动，从而拉动驱动绳产生位移，通过控制驱动绳产生的位移及拉力调整各个臂杆的两个自由度；机械臂本体结构安装在滑台上，在滑台上移动。
16.进一步的，所述步骤四将计算得出的d
obs
、d
aim
分别转换为与障碍物相关的斥力势能e
obs
、与目标点相关的引力势能e
aim
，进而求出该空间点(x,y,z)的总体势能e＝e
aim
‑
e
obs
，具体为：
[0017][0018][0019]
其中e为自然常数，d
obs
为空间点与障碍物之间的最近距离，d
aim
为空间点与目标点之间的距离。
[0020]
进一步的，所述步骤五根据路径总长、关节最大偏角及与障碍物的最近距离，制定评价函数，具体为：
[0021][0022]
其中j为评价函数的数值，p
long
为路径总长，max(ψ
i
)为机械臂沿着该路径运动过程中关节最大偏转角度，min(d
obs
)为机械臂沿着该路径运动过程中与障碍物的最近距离。
[0023]
进一步的，所述步骤六中，当相邻离散点之间的等步长l
n
恒定且满足下式
[0024][0025]
其中n为大于等于1的任意自然数，ψ
max
为路径中的两条直线之间的最大夹角，l为
臂杆长度；
[0026]
若为第一个离散点，则不考虑前面小臂杆的偏转，则
[0027]
γ
n
＝2ψ
max
/(n 1)
[0028]
γ
n
为两个相邻小臂杆之间的最大偏角；
[0029]
若为第二个及后面的离散点，则
[0030]
β
n
＝ψ
max
(n
‑
1)/(2n)
[0031]
γ
n
＝ψ
max
/n
[0032]
β
n
为一个臂杆与其等效的小臂杆之间的最大偏角；
[0033]
在等步长的情况下，通过上述推导结果可得虚拟小臂杆的末端点即为下个离散点的选取范围。
[0034]
进一步的，所述步骤六中，当相邻离散点之间的变步长不恒定时，若第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，第w 1个小臂杆长度为是指n
w 1
个相同臂长的虚拟小臂杆可等效为一个臂杆，第w 1个小臂杆相对于第w个小臂杆的最大偏角加上两个小臂杆相对于实际臂杆的最大偏角与不能超过ψ
max
，即
[0035][0036]
第w 1个小臂杆中的与又存在以下关系：
[0037][0038]
与n
w
的关系为
[0039][0040]
整理求得
[0041][0042]
推出第w个离散点后，带入不同的常数n
w 1
，求得进而确定第w 1个离散点的选取范围，从该空间范围内选取最小势能值点作为第w 1个离散点。
[0043]
进一步的，采用第一等步长算法进行路径规划，具体包括如下步骤：
[0044]
(7.1)将一个臂杆等效为n个相同的臂长为l
n
的虚拟小臂杆，指定路径步长恒定且为l
n
，从起始点开始规划路径；n为正整数；
[0045]
(7.2)根据确定的下个离散点的选取范围，在该选取范围内根据网格划分法得到多个离散点；
[0046]
(7.3)计算出这些离散点的总体势能；
[0047]
(7.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0048]
(7.5)判断通过(7.4)得到的离散点与目标点的距离是否小于等于步长；如果小于等于步长，则终止路径规划；如果大于步长，则返回(7.2)，继续循环，从而求得多个离散点；
[0049]
(7.6)遍历n的取值，循环执行7.2～7.5，得到n种不同路径对应的离散点。
[0050]
进一步的，采用第二等步长算法进行路径规划，具体包括如下步骤：
[0051]
(8.1)将一个臂杆等效为n个相同的臂长为l
n
的虚拟小臂杆，指定路径步长恒定且为l
n
，从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向设定为一个固定方向；
[0052]
(8.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0053]
(8.3)计算出这些离散点的总体势能；
[0054]
(8.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0055]
(8.5)判断通过(8.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(8.2)，继续循环，从而求得多个离散点；
[0056]
(8.6)遍历n的取值，循环执行8.2～8.5，得到n种不同路径对应的离散点。
[0057]
进一步的，采用第三等步长算法进行路径规划，具体包括如下步骤：
[0058]
(9.1)将一个臂杆等效为n个相同的臂长为l
n
的虚拟小臂杆，指定路径步长恒定且为l
n
，从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向没有限制；
[0059]
(9.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0060]
(9.3)计算出这些离散点的总体势能；
[0061]
(9.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0062]
(9.5)判断通过(9.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(9.2)，继续循环，从而求得多个离散点；
[0063]
(9.6)遍历n的取值，循环执行9.2～9.5，得到n种不同路径对应的离散点。
[0064]
进一步的，采用第一变步长算法进行路径规划，具体包括如下步骤：
[0065]
(10.1)第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，路径步长不确定为从起始点开始规划路径；
[0066]
(10.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0067]
(10.3)计算出这些离散点的总体势能；
[0068]
(10.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0069]
(10.5)判断通过(10.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(10.2)，继续循环，从而求得多个离散点；该多个离散点仅对应一条路径。
[0070]
进一步的，采用第二变步长算法进行路径规划，具体包括如下步骤：
[0071]
(11.1)第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，路径步长不确定为从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向设定为一个固定方向；
[0072]
(11.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离
散点；
[0073]
(11.3)计算出这些离散点的总体势能；
[0074]
(11.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点；
[0075]
(11.5)判断通过(11.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(11.2)，继续循环，从而求得多个离散点；该多个离散点仅对应一条路径。
[0076]
进一步的，采用第三变步长算法进行路径规划，具体包括如下步骤：
[0077]
(12.1)第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，路径步长不确定为从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向没有限制；
[0078]
(12.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0079]
(12.3)计算出这些离散点的总体势能；
[0080]
(12.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点；
[0081]
(12.5)判断通过(12.4)得到的离散点与目标点的距离是否小于等于步长，
[0082]
如果小于等于步长，则终止路径规划；如果大于步长，则返回(12.2)，继续循环，从而求得多个离散点；该多个离散点仅对应一条路径。
[0083]
本发明与现有技术相比的优点在于：
[0084]
(1)本发明提出一种基于最小势能算法的可限制偏角的路径规划方法，该算法中将一个臂杆等效为多个虚拟小臂杆，将臂杆的长度及最大偏转角度转换为小臂杆的长度及最大偏转角度，在小臂杆的偏转范围内获取最低势能点，作为下个离散点，通过直线将最优离散点连接，得到多组可行重复路径，通过分析路径总长、臂杆之间的相对偏角、机械臂与障碍物的距离等因素，得到一组最优路径。
[0085]
(2)本发明推导出了算法中的路径离散点长度与离散点选取范围的关系，从而保证机械臂沿着利用该算法规划出的路径运动时，关节偏角保持在限制范围内。
[0086]
(3)本发明根据是否等步长、路径规划的方向及第一个离散点的选取方向有无限制，可将mpe
‑
mda算法分为六种：第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法以及第三变步长算法。通过这六种算法均可得出避障、关节偏角保持在限制范围内，并可通过改变这些算法中的斥力势能、引力势能的权值来解决路径规划陷入死区的问题。
附图说明
[0087]
图1 mpe
‑
mda算法下的路径规划流程图；
[0088]
图2机械臂结构图；
[0089]
图3 d
‑
h坐标系图；
[0090]
图4臂杆等效为小臂杆示意图；
[0091]
图5 mpe
‑
mda
‑
es算法下的第一个离散点选取表面范围；
[0092]
图6 mpe
‑
mda
‑
es算法下的第w(w≥2)个离散点选取表面范围；
[0093]
图7 mpe
‑
mda
‑
vs算法下的第一个离散点选取空间范围；
[0094]
图8 mpe
‑
mda
‑
vs算法下的第w(w≥2)个离散点选取空间范围；
[0095]
图9机械臂运动过程图；
[0096]
图10驱动绳的分布图；
[0097]
图11障碍物线框模型图。
具体实施方式
[0098]
下面结合附图对本发明进行详细说明。
[0099]
本发明采用一种基于最小势能算法(mpe
‑
mda)的可限制偏角的路径规划方法，算法流程图如图1所示，首先建立了冗余机械臂的模型，其次建立障碍物模型、计算空间点到障碍物的距离、通过指定的转换函数将距离转换为势能，然后通过一种子算法得出一系列离散点，通过直线连接离散点得出尽可能的远离障碍物、接近目标物的路径，且机械臂沿着该路径运动时，臂杆之间的偏转角度不超过极限角度，利用评价函数选出六种算法下的最优路径，完成可限制偏角的路径规划。具体实施步骤如下：
[0100]
步骤一，建立机械臂模型。建立相对于地面固定不变的世界坐标系、基座的本体坐标系以及八个臂杆的本体坐标系，通过转换矩阵进行各坐标系之间的变换，在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系，建立起机械臂运动学模型。
[0101]
冗余机械臂结构如图2所示，所述机械臂包括机械臂本体结构、驱动机构以及滑台。机械臂本体结构由八个刚性臂杆串联而成，臂杆之间通过十字轴万向节连接，驱动机构由伺服电机、联轴器、丝杠和滑块组成。联轴器安装于丝杠两端，通过伺服电机带动丝杠转动来控制滑块前后移动，从而拉动驱动绳产生位移，通过控制驱动绳产生的位移及拉力可以调整各个臂杆的两个自由度。机械臂本体结构安装在滑台上，在滑台上移动。需要保证相邻臂杆之间的偏转角度不超过极限角度，否则会发生碰撞。
[0102]
建立如图3所示的相对于地面固定不变的世界坐标系{o
xyz
}、基座的本体坐标系{o0}以及八个臂杆的本体坐标系{o
i
}，通过转换矩阵进行各坐标系之间的变换，在转换矩阵的基础上推导出各驱动绳长度、关节角度、臂杆末端点之间的转换关系，建立起机械臂运动学模型。
[0103]
其中第i
‑
1个臂杆的本体坐标系{o
i
‑1}沿x
i
‑1轴平移a
i
，然后绕z
i
‑1轴旋转α
i
，再绕y
i
轴旋转θ
i
，最后沿z
i
轴平移d
i
可转换为第i个臂杆的本体坐标系{o
i
}，这两个坐标系之间的转换矩阵为
[0104][0105]
式中c代表cos，s代表sin；与分别表示坐标系{o
i
}相对于坐标系{o
i
‑1}的旋转变换矩阵与平移变换矩阵；a
i
等于l
i
‑1为坐标系的平移距离。
[0106]
接下来分析关节角度与臂杆末端点之间的转换关系，根据臂杆末端点o9的坐标及路径可通过逆运动学方法求出所有关键点o
i
的坐标(x
i
,y
i
,z
i
)(i＝1,2
……
8)。
[0107]
进而可以通过(x1,y1,z1)与(x2,y2,z2)求得α1和θ1，求解公式如下：
[0108][0109][0110]
可以在自然坐标系中求解α1和θ1，但α2和θ2是第2个臂杆相对于第1个臂杆的偏转角度，而不是第2个臂杆在自然坐标系中的偏转角度，所以在求解第i个臂杆相对于第i
‑
1个臂杆的偏转角度时，需要将坐标点从自然坐标系转换为第i
‑
1个臂杆的坐标系{o
i
‑1}中。
[0111][0112]
式中的(x
i 1_i
‑1,y
i 1_i
‑1,z
i 1_i
‑1)为第i个臂杆末端执行器o
i 1
在坐标系{o
i
‑1}中的坐标，通过可以将坐标系{o
i
‑2}中的坐标转换为坐标系{o
i
‑1}中的坐标。矩阵中包括α
i
‑1、θ
i
‑1、l
i
‑1这三个参数，该矩阵是矩阵的逆矩阵。
[0113]
通过(x
i 1_i
‑1,y
i 1_i
‑1,z
i 1_i
‑1)可求得第i个臂杆相对于第i
‑
1个臂杆的偏转角度α
i
和θ
i
，具体公式如下：
[0114]
[0115][0116]
通过上述公式可以依次求解出16个偏转角度。
[0117]
接下来分析驱动绳长与关节角度之间的转换关系，
[0118]
通过各个关节位置坐标可求得各个关节的偏转角度，进而可以通过每个关节的偏转角度求得每个驱动绳在该关节处的长度，进而累加可求得每根驱动绳的总长。
[0119]
本发明中的机械臂的每个关节由3根并联驱动绳驱动，驱动绳的一端固定在关节端面上，另一端固定在基座的驱动机构中，在万向节的限制下可以由驱动绳拉动臂杆从而使关节转动。第i个臂杆中穿过27
‑
3i(i＝1,2,...,7,8)个驱动绳，每个臂杆中只有3根驱动绳用于驱动本臂杆，其余的驱动绳均穿过本臂杆驱动后面的臂杆。
[0120]
以第一个关节为例，分析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)个臂杆。令图中各个孔的位置角度取各个孔的的圆心与截面中心连成的直线与z
i
轴之间的夹角为该点的位置角度则则
[0121]
通过每根驱动绳的位置角度可得每个点的位置，通过坐标转换可以转换到同一坐标系中，进而可求出两点之间的距离，即可得一个关节处的驱动绳长l
ija
、l
ijb
、l
ijc
与关节角度α
i
、θ
i
之间的映射关系。
[0122]
以求解从孔11b中穿过的驱动绳为例，讲述如何计算l
11b
。图中的点p
11b_0
在坐标系{o0}中的位置为(x
11b_0
,y
11b_0
,z
11b_0
)，通过孔11b的位置角度可求出：
[0123][0124]
式中的d是关节中心点到臂杆端面的距离，r是驱动绳穿过的孔的圆心与臂杆中心之间的距离。
[0125]
点p
11b_1
在坐标系{o1}中的位置为(x
11b_1
,y
11b_1
,z
11b_1
)，通过孔11b的位置角度可求出：
[0126][0127]
将点p
11b_1
在的位置坐标通过转换矩阵可转换到坐标系{o0}中，进而通过下列公式可得点p
11b_0
与点p
11b_1
在三个方向上的差值：
[0128][0129]
进而可得点p
11b_0
与点p
11b_1
之间的距离：
[0130][0131]
整理可得：
[0132][0133]
式中的θ＝θ1、α＝α1、公式(19)为通用公式，输入两个偏转角度θ、α和驱动绳的位置角度即可求得该驱动绳在关节处的实时长度。求解出各个关节处的驱动绳的长度，利用下列公式将驱动绳每一段的长度累加，即可求得每根驱动绳的总长。
[0134][0135]
式中l
ja
、l
jb
、l
jc
为驱动第j个臂杆的三根驱动绳在第一个关节及第一个关节之后的总长。
[0136]
总之，通过变维缩放法可推导出各个关节中心点的位置，进而可以通过位置求解出16个关节角度，进而可以计算出24根驱动绳的长度。
[0137]
步骤二，障碍物建模，将不规则形状障碍物转换为常见形状障碍物。
[0138]
障碍物形状多种多样，有规则形状的障碍物，也有表面凹凸不平的不规则形状障碍物，为了精确计算空间点到复杂障碍物表面的距离，可将不规则形状障碍物转换为常规障碍物。进而分析障碍物的位置及尺寸数据，建立障碍物模型。一般来讲，球、长方体、圆柱、圆锥为规则形状。比如：
[0139]
如图11所示，空间中具有球、长方体、圆柱、圆锥、不完整圆柱这五个障碍物，及y＝
‑
4、y＝6、z＝
‑
3、z＝4这四面墙，图中的点为障碍物的旋转中心或机械臂运动轨迹起始点(0,0,0)、目标点(20,0,0)。球、长方体的旋转中心均为质心，圆柱、不完整圆柱的旋转中心在底面圆心上，圆锥的旋转中心为顶点。
[0140]
以第i个障碍物的旋转中心为原点，建立相对于第i个障碍物固定不变的本体坐标系{o
obsi
}。世界坐标系{o
xyz
}与障碍物本体坐标系{o
obsi
}可通过转换矩阵进行坐标转化。最初障碍物旋转中心与原点重合，绕x轴、y轴、z轴旋转后，再沿x轴、y轴、z轴平移过去，形成最终的障碍物，故障碍物的位置数据有d
xi
、d
yi
、d
zi
，具体数值如表
1所示。转换矩阵可用以上位置数据表示，即
[0141][0142]
球的形状可用半径r1表示，长方体的形状可用长a2、宽b2、高c2表示，圆柱的形状可用半径r3、高h3表示，圆锥的形状可用半径r4、高h4表示，不完整圆柱的形状可用切面与轴线的距离a5、半径r5、高h5表示，具体数值如表1所示。
[0143]
表1障碍物的位置及尺寸数据
[0144][0145]
步骤三，分析空间点与障碍物、目标点之间的距离。
[0146]
计算空间点与障碍物之间的距离d
obs
以及空间点与目标点之间的距离d
aim
；把空间点(x,y,z)与每个障碍物之间的距离d
obsi
中最小值作为空间点(x,y,z)与障碍物之间的距离d
obs
，即d
obs
＝min(d
obsi
)。
[0147]
步骤四，分析空间点的势能。
[0148]
将计算得出的d
obs
、d
aim
分别转换为与障碍物相关的斥力势能e
obs
、与目标点相关的引力势能e
aim
，进而求出该空间点(x,y,z)的总体势能e＝e
aim
‑
e
obs
，具体为：
[0149][0150][0151]
其中e为自然常数，d
obs
为空间点与障碍物之间的最近距离，d
aim
为空间点与目标点之间的距离。
[0152]
步骤五，制定评价函数。
[0153]
考虑路径总长、关节最大偏角及与障碍物的最近距离，制定评价函数，具体为：
[0154][0155]
其中j为评价函数的数值，p
long
为路径总长，max(ψ
i
)为机械臂沿着该路径运动过程中关节最大偏转角度，min(d
obs
)为机械臂沿着该路径运动过程中与障碍物的最近距离。
[0156]
步骤六，在指定关节最大偏转角度的前提下，通过将一个臂杆等效为多个虚拟小臂杆(虚拟小臂杆的末端点即为下个离散点)的方法，推导出等步长与变步长中虚拟小臂杆的最大偏角γ
n
与虚拟小臂杆长度l
n
之间的关系，从而得到下个离散点的选取范围，虚拟小臂杆的末端点即为下个离散点。机械臂沿着这些算法规划出的路径运动时，每个臂杆的偏转角度不超过极限角度，且可避障、不会陷入局部极小值。
[0157]
(6.1)当相邻离散点之间的步长l
n
恒定时。
[0158]
如图4所示的将一个臂杆等效为n个相同臂长的虚拟小臂杆，其中一个臂杆与其等效的小臂杆之间的最大偏角为β
n
，两个相邻小臂杆之间的最大偏角为γ
n
，小臂杆的长度为l
n
。从图中可以看出第w 1个小臂杆相对于第w个小臂杆的极限位置及运动范围与γ
n
相关。
[0159]
当偏转角度最大时，n个l
n
臂长的小臂杆各偏转β
n
，其末端点的坐标为(ln[cos(γ
n
β
n
)
…
cos(nγ
n
β
n
)],0,ln[sin(γ
n
β
n
)
…
sin(nγ
n
β
n
)])。
[0160]
一个臂杆与n个其等效的小臂杆组成n 1多边形，可得出内角和公式：
[0161]
2β
n
(n
‑
1)(180
°‑
γ
n
)＝180
°
(n
‑
1)
[0162]
可得β
n
＝(n
‑
1)γ
n
/2。
[0163]
从图中还可看出：
[0164]
2β
n
γ
n
＝ψ
max
[0165]
公式中的等号左边及等号右边均可表示两臂杆之间的最大偏角，可得γ
n
＝ψ
max
/n。将小臂杆的长度投影到等效的一个臂杆上，当n为奇数时，满足：
[0166]
n是奇数时
[0167]
当n为偶数时，满足：
[0168]
n是偶数时
[0169]
将当n为奇数及偶数时的公式整理，可得出l
n
与l关系：
[0170]
n是大于等于1的任意自然数
[0171]
上述推导过程中n是取得整数，最终得到上式，若取n为小数，带入上式也可求得对应的l
n
，故n不仅可以取整数，令n等于任意正实数时，仍然可求出对应的l
n
与l的关系。
[0172]
若为第一个离散点，则不用考虑前面小臂杆的偏转，则β
n
＝0、γ
n
＝2ψ
max
/(n 1)，第一个离散点的可选取表面如图5所示。
[0173]
若为第二个及后面的离散点，则β
n
＝ψ
max
(n
‑
1)/(2n)、γ
n
＝ψ
max
/n，离散点的可选取表面如图6所示。
[0174]
(6.2)当相邻离散点之间的步长不恒定时。
[0175]
若第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，若第w 1个小臂杆长度为是指n
w 1
个相同臂长的虚拟小臂杆可等效为一个臂杆，第w 1个小臂杆相对于第w个小臂杆的最大偏角加上两个小臂杆相对于实际臂杆的最大偏角与不能超过ψ
max
，可得：
[0176][0177]
第w 1个小臂杆中的与又存在以下关系：
[0178][0179]
把上式中的n 1改为n，即为与n
w
的关系。
[0180]
整理可求得：
[0181][0182]
推出第w个离散点后，带入不同的n
w
1，可求进而可确定第w 1个离散点的选取范围，从该空间范围内选取最小势能值点作为第w 1个离散点。
[0183]
mpe
‑
mda
‑
vs算法下的第一个离散点的可选取空间如图7所示，第二个及后面的离散点的可选取空间如图8所示。
[0184]
步骤七，根据是否等步长、路径方向、初始点方向是否确定这三个因素，得到了六种子算法：第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法以及第三变步长算法。
[0185]
其中第一等步长算法与第一变步长算法中路径规划方向为起始点到目标点，第二等步长算法与第二变步长算法中路径规划方向为目标点到起始点，第一个离散点的选取方向设定为(0,0,1)，第三等步长算法与第三变步长算法中路径规划方向为目标点到起始点，第一个离散点的选取方向无限制。
[0186]
在最小势能算法的基础上，分别采用第一等步长算法、第二等步长算法、第三等步长算法、第一变步长算法、第二变步长算法以及第三变步长算法进行路径规划，在步骤六推导出的范围内选择一个最小势能点作为下个离散点，类比得出一系列离散点，当离散点与目标点的距离小于等于步长时，终止寻找离散点。
[0187]
采用第一等步长算法进行路径规划，具体包括如下步骤：
[0188]
(7.1)将一个臂杆等效为n个相同的臂长为l
n
的虚拟小臂杆，指定路径步长恒定且为l
n
，从起始点开始规划路径；n为正整数；
[0189]
(7.2)根据确定的下个离散点的选取范围，在该选取范围内根据网格划分法得到
多个离散点；
[0190]
(7.3)计算出这些离散点的总体势能；
[0191]
(7.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0192]
(7.5)判断通过(7.4)得到的离散点与目标点的距离是否小于等于步长；如果小于等于步长，则终止路径规划；如果大于步长，则返回(7.2)，继续循环，从而求得多个离散点；
[0193]
(7.6)遍历n的取值，循环执行7.2～7.5，得到n种不同路径对应的离散点。
[0194]
采用第二等步长算法进行路径规划，具体包括如下步骤：
[0195]
(8.1)将一个臂杆等效为n个相同的臂长为l
n
的虚拟小臂杆，指定路径步长恒定且为l
n
，从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向设定为一个固定方向；
[0196]
(8.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0197]
(8.3)计算出这些离散点的总体势能；
[0198]
(8.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0199]
(8.5)判断通过(8.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(8.2)，继续循环，从而求得多个离散点；
[0200]
(8.6)遍历n的取值，循环执行8.2～8.5，得到n种不同路径对应的离散点。
[0201]
采用第三等步长算法进行路径规划，具体包括如下步骤：
[0202]
(9.1)将一个臂杆等效为n个相同的臂长为l
n
的虚拟小臂杆，指定路径步长恒定且为l
n
，从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向没有限制；
[0203]
(9.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0204]
(9.3)计算出这些离散点的总体势能；
[0205]
(9.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0206]
(9.5)判断通过(9.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(9.2)，继续循环，从而求得多个离散点；
[0207]
(9.6)遍历n的取值，循环执行9.2～9.5，得到n种不同路径对应的离散点。
[0208]
采用第一变步长算法进行路径规划，具体包括如下步骤：
[0209]
(10.1)第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，路径步长不确定为从起始点开始规划路径；
[0210]
(10.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0211]
(10.3)计算出这些离散点的总体势能；
[0212]
(10.4)从这些离散点中选择总势能最小的点作为路径中的下个离散点；
[0213]
(10.5)判断通过(10.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(10.2)，继续循环，从而求得多个离散点；该多个离散点仅对应一条路径。
[0214]
采用第二变步长算法进行路径规划，具体包括如下步骤：
[0215]
(11.1)第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，路径步长不确定为从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向设定为一个固定方向；
[0216]
(11.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0217]
(11.3)计算出这些离散点的总体势能；
[0218]
(11.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点；
[0219]
(11.5)判断通过(11.4)得到的离散点与目标点的距离是否小于等于步长，如果小于等于步长，则终止路径规划；如果大于步长，则返回(11.2)，继续循环，从而求得多个离散点；该多个离散点仅对应一条路径。
[0220]
采用第三变步长算法进行路径规划，具体包括如下步骤：
[0221]
(12.1)第w个小臂杆长度为是指n
w
个相同臂长的虚拟小臂杆可等效为一个臂杆，路径步长不确定为从目标点朝着起始点逆向规划路径，且第一个离散点的选取方向没有限制；
[0222]
(12.2)根据下个离散点的选取范围，在该选取范围内根据网格划分法得到许多离散点；
[0223]
(12.3)计算出这些离散点的总体势能；
[0224]
(12.4)从这些离散点中选择总体势能最小的点作为路径中的下个离散点；
[0225]
(12.5)判断通过(12.4)得到的离散点与目标点的距离是否小于等于步长，
[0226]
如果小于等于步长，则终止路径规划；如果大于步长，则返回(12.2)，继续循环，从而求得多个离散点；该多个离散点仅对应一条路径。
[0227]
步骤八，根据上述六种不同的算法进行路径规划得到离散点，利用直线将离散点转换为连续路径，求出每条路径各自对应的路径总长，根据基本的逆运动学求出机械臂沿着该路径运动时的关节最大偏角及与障碍物的最近距离，利用评价函数选出六种算法下的最优路径，完成可限制偏角的路径规划。
[0228]
实施例：
[0229]
在一种障碍物环境中，通过仿真求得了一条最优路径作为机械臂的最终运动路径，该路径对应的评价值j＝74.84。图9为对应的机械臂沿着该路径运动的过程图。从图中可以看出在机械臂沿着指定路径运动的过程中始终与障碍物保持一定的距离，且臂杆之间的相对偏角也始终保持在限制范围内。通过仿真验证了本发明的正确性。
[0230]
针对传统路径规划算法在空间避障中无法同时优化路径总长、与障碍物间的最近距离，更无法限制关节偏转角度的问题，本发明提出了一种用于路径规划的限制关节最大偏角的最小势能(mpe
‑
mda)算法。建立了机械臂与障碍物模型，设立了空间点势能与距离的转换关系及路径评价函数，在指定关节最大偏转角度的前提下，推导出等步长与变步长中小臂杆的最大偏角与小臂杆长度之间的关系，从而得到下个离散点的选取范围。仿真分析步长、方向等因素对mpe
‑
mda算法路径规划的影响，得出的每条路径的关节最大偏转角度均在限制范围内，与障碍物间的最近距离都大于0.6个单位，路径总长都在20
‑
23个单位之间，
均优于apf算法结果。得到的路径评价值最高为74.84，且该算法路径规划结果整体优于其他五种算法。总之，采用mpe
‑
mda算法实现了考虑关节偏转角度受限等问题的超冗余机械臂空间避障的重复路径规划。
[0231]
以上所揭露的仅为本发明一种实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。
[0232]
本发明未详细说明部分属本领域技术人员公知常识。