一种任务空间下的机械臂运动规划方法

1.本发明属于自动化技术领域，涉及一种任务空间下的机械臂运动规划方法。


背景技术：

2.机械臂是服务机器人对目标执行操作的核心部件，也是自动化生产线上的关键设备。在完成复杂任务时，机械臂的运动规划需要面临一些末端位置和姿态的约束。末端执行器在精准的抓取、切割、焊接等任务中均需满足末端位置和姿态约束。目前基于关节空间的运动规划方法，然而，在关节空间描述的任务约束下，难以进行复杂的运动规划。因此，在任务空间下构建机械臂的运动规划方法对于机器人执行末端约束下的任务具有重要意义。


技术实现要素：

3.本发明的目标是构建任务空间下的机械臂启发式搜索运动规划方法,并对其优化。首先，在任务空间直接进行末端位姿的启发式搜索，然后，对球形包络后的障碍物与机械臂进行碰撞检测。最后将发生碰撞的位姿映射到末端工作平面中进行局部最小判断。该方法能有效避免机械臂在任务空间下采用启发式搜索规划时陷入局部最小状态的缺陷。在机械臂的任务空间中采用启发式搜索算法进行规划，具有规划速度快，能满足任务约束等优点。但是该方法容易陷入局部极小陷阱。本发明所提方法可以弥补该缺陷。
4.本发明方法的步骤包括：
5.步骤(1)、采用d
‑
h法(denavit
‑
hartenberg)建立机械臂的正运动学模型，并构建障碍物模型；将障碍物的位置数据保存至不可行集合closed表，其中障碍物的位置数据包括障碍物的中心、半径信息；同时根据机械臂的正运动学模型计算出初始状态的末端执行器位姿p
f
，其中位姿包括姿态信息和坐标信息。
6.通过机械臂的每个关节j
i
的构型与运动信息获得各关节的运动变换矩阵a
i
，通过公式(1)得到变换矩阵
[0007][0008][0009]
其中1≤i≤j，j表示机械臂关节自由度；表示p
f
的三维姿态信息；(p
x
，p
y
，p
z
)表示p
f
的三维坐标信息；
[0010]
步骤(2)、将p
f
的三维姿态信息采用欧拉角表示。初始化时将机械臂当前末端执行器的位姿p
c
设置为p
f
的位姿信息如下：
[0011]
p
f
＝(x
f
，y
f
，z
f
，α
f
，β
f
，γ
f
)
ꢀꢀꢀꢀ
式(3)
[0012]
其中x
f
，y
f
，z
f
表示p
f
的三维坐标信息；α
f
，β
f
，γ
f
表示p
f
的欧拉角；
[0013]
步骤(3)、使用任务空间下的启发式搜索算法ara
*
(anytime repairinga*)对机械臂末端执行器位姿进行规划：
[0014]3‑
1初始化：将待扩展节点列表p
next
设置为空列表，p
next
节点数目u＝0，循环参数v＝1。
[0015]3‑
2将机械臂当前末端执行器位姿的6个自由度各自加减一个单位获得12个周边节点信息；将上述12个周边节点中不在不可行集合closed表中的节点加入到待扩展节点列表p
next
；
[0016]
p
next
＝{p1，p
′1；p2，p
′2；...；p
v
，p
′
v
；...；p
u
，p
′
u
}，u≤12
ꢀꢀꢀꢀ
式(4)
[0017]
p
v
表示周边节点，p
′
v
表示p
v
的父节点。
[0018]
同时将待扩展节点列表p
next
中不在可行集合open表的节点进行初始化操作：将父节点设为空，原有代价函数f(p
v
)
old
设为无穷大；然后计算待扩展节点列表p
next
所有节点的新代价函数f(p
v
)
new
；
[0019]
f(p
v
)
new
＝g(p
v
)+h(p
v
)
ꢀꢀꢀ
式(5)
[0020]
f(p
v
)
old
＝g(p
v
)
old
+h(p
v
)
ꢀꢀꢀꢀ
式(6)
[0021]
其中，g(p
v
)表示从初始节点到p
c
的代价值加一，h(p
v
)表示p
v
到目标节点的欧式距离，初始节点为初始状态的末端执行器位姿p
f
，目标节点为机械臂末端执行器最终要到达的位姿；g(p
v
)
old
表示初始节点到p
′
v
的代价值加一。
[0022]3‑
3比较待扩展节点列表p
next
中所有节点的f(n)
old
和f(n)
new
，如果f(n)
old
＞f(n)
new
，将父节点p
′
v
改为当前节点p
c
，同时将p
v
的f(n)
old
赋值为f(n)
new
，反之不做变化。
[0023]
步骤(4)、判断是否遍历列表p
next
中所有节点
[0024]
比较v和u的大小，若v≤u则对列表p
next
的第v个节点设为子节点p
next
，执行步骤(5)
‑
(6)，若v＞u则将可行集合open表中代价函数f(n)最小的节点更新为机械臂当前末端执行器位姿p
c
，返回步骤(3)。
[0025]
步骤(5)、对子节点p
next
进行碰撞检测：
[0026]5‑
1对节点p
next
采用梯度投影法进行逆运动学计算，得到一组最优的逆运动学解，即为关节角解；将这组关节角解通过d
‑
h正运动学计算得到机械臂所有关节的三维坐标。
[0027]5‑
2根据5
‑
1得到的机械臂所有关节的三维坐标构建各关节的机械臂构型；然后对各关节的机械臂构型进行碰撞检测，判断节点p
next
是否会与障碍物发生碰撞，具体步骤如下：
[0028]5‑2‑
1选取相邻两个关节j
a
,j
b
之间的机械臂构型l,设安全距离r为障碍物半径加上ε倍的机械臂构型半径：
[0029]
r＝r
o
+ε
·
r
l
ꢀꢀꢀꢀꢀꢀ
式(7)
[0030]
其中r
p
表示障碍物半径，r
l
表示机械臂构型半径，ε为人为设定，ε＞1。
[0031]5‑2‑
2计算构型l到障碍物中心的距离s；如果s＞r，表示未发生碰撞，则将节点p
next
加入到可行集合open表中，执行步骤(7)；若s≤r,则首先计算以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠mon，并计算构型l距离障碍物中心obs为s时的最大安全角δ，若夹角∠mon小于最大安全角δ则认为未发生碰撞，则将节点p
next
加入到可行集合open表中，执行步骤(7)；若夹角∠mon大于等于最大安全角δ则发生碰撞，进行步骤
(6)。
[0032][0033]
其中m为关节j
a
的中心点，n是关节j
b
的中心点，o是障碍物的中心，a是障碍物中心到关节j
a
的距离；b是障碍物中心到关节j
b
的距离；l为机械臂构型l的长度。
[0034][0035]
其中s为障碍物中心到构型l的距离，r表示距离障碍物中心的安全距离，l表示构型l的长度。
[0036]
步骤(6)、发生碰撞后需要判断是否会出现机械臂构型无法绕开障碍物的情况，进行局部最小检测，具体步骤如下：
[0037]6‑
1建立以当前末端执行器位姿p
c
的三维坐标信息(x
c
，y
c
，z
c
),目标位置p
t
的三维坐标信息(x
t
，y
t
，z
t
)，以机械臂的基座中心为原点p
o
(0，0，0)构成末端工作平面oct。
[0038]
ax+by+cz+d＝0
ꢀꢀꢀꢀꢀꢀ
式(10)
[0039]6‑
2将障碍物及机械臂构型投影到该平面oct中，球形障碍物在平面中的投影是以障碍中心的投影点p
o
′
为圆心，以障碍物半径r
o
为半径的圆形障碍物obs’。机械臂构型在平面中的投影是各关节角在平面的投影点所组成的新的构型l
‘
。
[0040]6‑
3观察投影后的机械臂构型l
‘
是否与投影后的障碍物obs’发生碰撞：对l
‘
和obs’使用步骤(5)的方法进行碰撞检测。若构型l
‘
与obs’发生碰撞，则表示机械臂构型无法绕开障碍物。将待扩展节点加入到不可行集合closed表中，更新v＝v+1,返回步骤(4)。若未发生碰撞，判断机械臂构型l
‘
是否将障碍物obs’包围,步骤如下：
[0041]
a)根据目标位置p
t
和障碍物中心p
o
′
得向量计算在垂直方向上的单位向量
[0042][0043]
b)根据障碍物中心p
o
′
和原点p
o
得向量根据障碍物中心p
o
′
和当前节点坐标p
c
得向量计算和的数量积与和的数量积的乘积f：
[0044][0045]
若f<0，则认为机械臂构型l
‘
包围了障碍物obs’，这种情况表示机械臂构型无法绕开障碍物。因此将这些会造成局部最小问题的待扩展节点p
next
加入到不可行集合closed表中，更新v＝v+1,返回步骤(4)。若f≥0，则认为机械臂构型l
‘
未包围障碍物obs’，机械臂构型可以绕开障碍物。将节点p
next
舍去，当作未查询过的节点，更新v＝v+1,返回步骤(4)。
[0046]
步骤(7)、，判断可行集合open表中含有目标节点，若是则在可行集合open表中采用回溯父节点的方式从目标节点回溯到初始节点得到一条安全的机械臂运动路径，反之则更新v＝v+1,返回步骤(4)。
[0047]
本发明的另一个目的是提供一种计算机可读存储介质，其上存储有计算机程序，
当所述计算机程序在计算机中执行时，令计算机执行上述的方法。
[0048]
本发明的又一个目的是提供一种计算设备，包括存储器和处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现上述的方法。
[0049]
本发明提出的机械臂任务空间运动规划优化方法弥补了现阶段机械臂关节空间规划方法很难描述任务约束的缺点,并有效地解决了任务空间启发式搜索规划算法中存在的末端执行器陷入局部最优陷阱的问题。
[0050]
本发明提出的机械臂任务空间运动规划方法，能够有效地控制机械臂，完成具有末端位姿约束特点的任务。
附图说明
[0051]
图1为本发明的流程图；
[0052]
图2为夹角∠mon示意图；
[0053]
图3为投影至oct平面示意图。
具体实施方式
[0054]
下面结合具体实施例对本发明做进一步的分析。
[0055]
以一具体机械臂为例，如图1任务空间启发式搜索优化方法的实施步骤如下：
[0056]
步骤(1)、初始化机械臂系统及障碍物参数，具体操作如下：
[0057]1‑
1获得机械臂结构参数。即机械臂所对应的d
‑
h参数表。
[0058]
表1：机械臂的d
‑
h参数表
[0059][0060]
其中，0代表机械臂基座，1
…
6数字代表第1
…
6个关节。θ表示旋转关节的关节变量，d表示滑动关节的关节变量。q表示关节之间的距离。α表示关节之间的姿态变化量。
[0061]1‑
2对机械臂进行建模,将机械臂抽象为关节与关节间的圆柱形连杆组成的构型l。关节坐标由d
‑
h正运动学计算得出,构型连杆的半径为r
l
。
[0062]1‑
3对障碍物进行球形包络,得到障碍物球心坐标obs,障碍物半径为r
o
。
[0063]
步骤(2)、采用d
‑
h法(denavit
‑
hartenberg)建立机械臂的正运动学模型，并构建障碍物模型；将障碍物的位置数据保存至不可行集合closed表，其中障碍物的位置数据包括障碍物的中心、半径信息；同时根据机械臂的正运动学模型计算出初始状态的末端执行器位姿p
f
，其中位姿包括姿态信息和坐标信息。
[0064]
通过机械臂的每个关节j
i
的构型与运动信息获得各关节的运动变换矩阵a
i
，通过
公式(1)得到变换矩阵
[0065][0066][0067]
其中1≤i≤j，j表示机械臂关节自由度；表示p
f
的三维姿态信息；(p
x
，p
y
，p
z
)表示p
f
的三维坐标信息；
[0068]
步骤(3)、将p
f
的三维姿态信息采用欧拉角表示。初始化时将机械臂当前末端执行器的位姿p
c
设置为p
f
的位姿信息如下：
[0069]
p
f
＝(x
f
，y
f
，z
f
，α
f
，β
f
，γ
f
)
ꢀꢀꢀꢀꢀꢀ
式(3)
[0070]
其中x
f
，y
f
，z
f
表示三维坐标信息，α
f
，β
f
，γ
f
表示p
f
的欧拉角；
[0071]
步骤(4)、使用任务空间下的启发式搜索算法ara
*
(anytime repairinga*)对机械臂末端执行器位姿进行规划：
[0072]4‑
1初始化：将待扩展节点列表p
next
设置为空列表，p
next
节点数目u＝0，循环参数v＝1。
[0073]4‑
2将机械臂当前末端执行器位姿的6个自由度各自加减一个单位获得12个周边节点信息；
[0074]
p1＝(x
c
+1，y
c
，z
c
，α
c
，β
c
，γ
c
)
[0075]
p2＝(x
c
‑
1，y
c
，z
c
，α
c
，β
c
，γ
c
)
[0076]
p3＝(x
c
，y
c
+1，z
c
，α
c
，β
c
，γ
c
)
[0077]
p4＝(x
c
，y
c
‑
1，z
c
，α
c
，β
c
，γ
c
)
[0078]
p5＝(x
c
，y
c
，z
c
+1，α
c
，β
c
，γ
c
)
[0079]
p6＝(x
c
，y
c
，z
c
‑
1，α
c
，β
c
，γ
c
)
[0080]
p7＝(x
c
，y
c
，z
c
，α
c
+1，β
c
，γ
c
)
[0081]
p8＝(x
c
，y
c
，z
c
，α
c
‑
1，β
c
，γ
c
)
[0082]
p9＝(x
c
，y
c
，z
c
，α
c
，β
c
+1，γ
c
)
[0083]
p
10
＝(x
c
，y
c
，z
c
，α
c
，β
c
‑
1，γ
c
)
[0084]
p
11
＝(x
c
，y
c
，z
c
，α
c
，β
c
，γ
c
+1)
[0085]
p
12
＝(x
c
，y
c
，z
c
，α
c
，β
c
，γ
c
‑
1)
[0086]
将上述12个周边节点中不在不可行集合closed表中的节点加入到待扩展节点列表p
next
；
[0087]
p
next
＝{p1，p
′1；p2，p
′2；...；p
v
，p
′
v
；
…
；p
u
，p
′
u
}，u≤12
ꢀꢀꢀꢀ
式(4)
[0088]
p
v
表示周边节点，p
′
v
表示p
v
的父节点。
[0089]
同时将待扩展节点列表p
next
中不在可行集合open表的节点进行初始化操作：将父节点设为空，原有代价函数f(p
v
)
old
设为无穷大；然后计算待扩展节点列表p
next
所有节点的新代价函数f(p
v
)
new
；
[0090]
f(p
v
)
new
＝g(p
v
)+h(p
v
)
ꢀꢀꢀ
式(5)
[0091]
f(p
v
)
old
＝g(p
v
)
old
+h(p
v
)
ꢀꢀꢀꢀꢀ
式(6)
[0092]
其中g(p
v
)表示从初始节点到p
c
的代价值加一，h(p
v
)表示p
v
到目标节点的欧式距离，初始节点为初始状态的末端执行器位姿p
f
，目标节点为机械臂末端执行器最终要到达的位姿；g(p
v
)
old
表示初始节点到p
′
v
的代价值加一。
[0093]4‑
3比较待扩展节点列表p
next
中所有节点的f(n)
old
和f(n)
new
，如果f(n)
old
＞f(n)
new
，将父节点p
′
v
改为当前节点p
c
，同时将p
v
的f(n)
old
赋值为f(n)
new
，反之不做变化。
[0094]
步骤(5)、判断是否遍历列表p
next
中所有节点
[0095]
比较v和u的大小，若v≤u则对列表p
next
的第v个节点设为子节点p
next
，执行步骤(6)
‑
(7)，若v＞u则将可行集合open表中代价函数f(n)最小的节点更新为机械臂当前末端执行器位姿p
c
，返回步骤(4)。
[0096]
步骤(6)、对子节点p
next
进行碰撞检测：
[0097]6‑
1对节点p
next
采用梯度投影法进行逆运动学计算，得到一组最优的逆运动学解，即为关节角解；将这组关节角解通过d
‑
h正运动学计算得到机械臂所有关节的三维坐标。
[0098]6‑
2根据6
‑
1得到的机械臂所有关节的三维坐标构建各关节的机械臂构型；然后对各关节的机械臂构型进行碰撞检测，判断节点p
next
是否会与障碍物发生碰撞，具体步骤如下：
[0099]6‑2‑
1选取相邻两个关节j
a
,j
b
之间的机械臂构型l,设安全距离r为障碍物半径加上ε倍的机械臂构型半径：
[0100]
r＝r
o
+ε
·
r
l
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(7)
[0101]
其中r
o
表示障碍物半径，r
l
表示机械臂构型半径，ε为人为设定，ε＞1。
[0102]6‑2‑
2计算构型l到障碍物中心的距离s；如果s＞r，表示未发生碰撞，则将节点p
next
加入到可行集合open表中，执行步骤(8)；若s≤r,则首先计算以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠mon(如图2)，并计算构型l距离障碍物中心obs为s时的最大安全角δ，若夹角∠mon小于最大安全角δ则认为未发生碰撞，则将节点p
next
加入到可行集合open表中，执行步骤(8)；若夹角∠mon大于等于最大安全角δ则发生碰撞，进行步骤(7)。
[0103][0104]
其中m为关节j
a
的中心点，n是关节j
b
的中心点，o是障碍物的中心，a是障碍物中心到关节j
a
的距离；b是障碍物中心到关节j
b
的距离；l为机械臂构型l的长度。
[0105][0106]
其中s为障碍物中心到构型l的距离，r表示距离障碍物中心的安全距离，l表示构型l的长度。
[0107]
步骤(7)、发生碰撞后需要判断是否会出现机械臂构型无法绕开障碍物的情况，进行局部最小检测，具体步骤如下：
[0108]7‑
1建立以当前末端执行器位姿p
c
的三维坐标信息(x
c
，y
c
，z
c
),目标位置p
t
的三维坐标信息(x
t
，y
t
，z
t
)，以机械臂的基座中心为原点p
o
(0，0，0)构成末端工作平面oct。
[0109]
ax+by+cz+d＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(10)
[0110]
满足如下约束：
[0111]
a
×
0+b
×
0+c
×
0+d＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(11)
[0112]
ax
t
+by
t
+cz
t
+d＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(12)
[0113]
ax
c
+by
c
+cz
c
+d＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
式(13)令a＝1，得d＝0，
[0114]7‑
2如图3将障碍物及机械臂构型投影到该平面oct中，球形障碍物在平面中的投影是以障碍中心的投影点p
o
′
为圆心，以障碍物半径r
o
为半径的圆形障碍物obs’。机械臂构型在平面中的投影是各关节角在平面的投影点所组成的新的构型l
‘
。
[0115]7‑
3观察投影后的机械臂构型l
‘
是否与投影后的障碍物obs’发生碰撞：对l
‘
和obs’使用步骤(6)的方法进行碰撞检测。若构型l
‘
与obs’发生碰撞，表示机械臂构型无法绕开障碍物。将待扩展节点p
next
加入到不可行集合closed表中，更新v＝v+1,返回步骤(4)。若未发生碰撞，判断机械臂构型l
‘
是否将障碍物obs’包围,步骤如下：
[0116]
a)根据目标位置p
t
和障碍物中心p
o
′
得向量计算在垂直方向上的单位向量
[0117][0118][0119]
b)根据障碍物中心p
o
′
和原点p
o
得向量根据障碍物中心p
o
′
和当前节点坐标
[0120]
p
c
得向量计算和的数量积与和的数量积的乘积f：
[0121][0122]
若f<0，则认为机械臂构型l
‘
包围了障碍物obs’，这种情况表示机械臂构型无法绕开障碍物。因此将这些会造成局部最小问题的待扩展节点p
next
加入到不可行集合closed表中，更新v＝v+1,返回步骤(5)。若f≥0，则认为机械臂构型l
‘
未包围障碍物obs’，表示机械臂构型可以绕开障碍物。将节点p
next
舍去，当作未查询过的节点，更新v＝v+1,返回步骤(5)。
[0123]
步骤(8)、判断可行集合open表中含有目标节点，若是则在可行集合open表中采用回溯父节点的方式从目标节点回溯到初始节点得到一条安全的机械臂运动路径，反之则更新v＝v+1,返回步骤(5)。