一种单关节锁定失效下的空间机械臂逆运动学方法与流程

1.本发明属于空间机械臂逆运动学领域，涉及一种单关节锁定失效下的空间机械臂逆运动学方法，具体内容为针对空间机械臂单关节锁定失效后机械臂构型无法求得解析解时，提出了一种基于粒子群算法的逆运动学求解方法。


背景技术：

2.随着空间应用技术的发展，空间机械臂在太空中扮演着十分重要的角色，包括在轨服务与装配、空间碎片清理等。空间机械臂大多都是冗余机械臂，与6自由度机械臂相比，冗余机械臂在避障、避奇异方面有很大的优势。空间机械臂的成本较高，通常被设计成执行尽可能多的任务，在复杂多变的空间环境中，机械臂的关节执行器和传感器很有可能会发生故障，及时并有效的维修对于空间机械臂来说是十分困难的，甚至是不可能的。因此空间机械臂需具有一定的容错能力。
3.以已应用于国际空间站的肩部、肘部、腕部均有偏置的7自由度ssrms型机械臂为例，已有文献对ssrms型空间机械臂单关节锁定失效下的逆运动学求解方法进行了系统的论述，机械臂某个关节锁定便会退化成6自由度机械臂，当锁定1、2、6、7关节时，机械臂满足pieper准则，可求出逆运动学的解析解，当3、4、5关节锁定时，机械臂不满足pieper准则，对于这种情况论文中提出了一种利用实际机械臂和改进机械臂之间的关系进行数值迭代的方法，该方法在每次迭代时，由于逆解的多解性使得迭代增量的计算不唯一，没有提及该选取那个增量或者是如何选取增量来计算下一次的迭代值，因此该种方法在执行过程中存在一些不足。


技术实现要素：

4.要解决的技术问题
5.为了避免现有技术的不足之处，本发明提出一种单关节锁定失效下的空间机械臂逆运动学方法，是一种基于粒子群优化算法的逆运动学计算方法，该粒子群算法采用了动态分群的思想，将粒子群分成几个普通子群和一个精英群，在普通群和精英群之间进行交换，更新适应值较差的粒子，加快粒子群的收敛速度。
6.技术方案
7.一种单关节锁定失效下的空间机械臂逆运动学方法，其特征在于步骤如下：
8.步骤1、建立ssrms机械臂的正运动学模型：
[0009][0010]
[n
r
,o
r
,a
r
,p
r
]＝f
r
(θ
r
)
ꢀꢀꢀꢀꢀꢀ
(2)
[0011]
建立将ssrms机械臂去掉一个偏置量后的改进机械臂的正运动学模型和逆运动学模型：
[0012][0013]
[n
m
,o
m
,a
m
,p
m
]＝f
m
(θ
m
)
ꢀꢀꢀꢀꢀꢀ
(4)
[0014][0015]
式中θ
r
表示实际ssrms机械臂的7个关节角，[n
r
,o
r
,a
r
,p
r
]表示实际ssrms机械臂的末端位姿，θ
m
表示改进机械臂的7个关节角，[n
m
,o
m
,a
m
,p
m
]表示改进机械臂的末端位姿；
[0016]
将待求解目标末端位姿[n
r
,o
r
,a
r
,p
r
]带入等效机械臂的逆运动学函数式5中，得到8组逆运动学结果；
[0017]
步骤2、确定逆运动学的优化目标函数：
[0018]
机械臂末端的期望位置为p
r
，机械臂的位置误差函数定义为：
[0019][0020]
机械臂的各关节角为θ＝[θ
1 θ
2 θ
3 θ
4 θ
5 θ
6 θ7]，初始关节角为将机械臂各个关节角的改变量定义为：
[0021][0022]
将逆运动学的求解转换成下面的最小约束优化问题来解决：
[0023][0024]
δp
‑
δp
max
≤0
ꢀꢀꢀꢀꢀꢀ
(9)
[0025]
式中δp
max
为最大误差；
[0026]
利用罚函数将上述约束优化问题转化成无约束优化问题进行处理，转化后得到下面的优化目标函数：
[0027][0028][0029]
式中f(θ)即为优化目标函数，为罚函数，α为惩罚系数；
[0030]
步骤3、设置相关参数：
[0031]
设粒子群优化算法的种群规模为m，7自由度的机械臂锁定失效一个关节，需求解的粒子维数即机械臂剩余关节的个数为6；将种群中8个粒子的初值分别设为步骤1改进机械臂所对应的8组逆运动学结果，其余的m
‑
8个粒子的初值按下面的式子进行混沌初始化
[0032]
θ
ij
＝l
θj
rand1(h
θj
‑
l
θj
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0033]
v
ij
＝l
vj
rand2(h
vj
‑
l
vj
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0034]
式中θ
ij
表示第i个粒子的第j维位置分量，v
ij
表示第i个粒子的第j维速度分量，h
θj
、l
θj
分别表示粒子第j维位置分量的最大值和最小值，h
vj
、l
vj
分别表示粒子第j维速度分
量的最大值和最小值，rand1、rand2分别为0～1之间的随机数；
[0035]
步骤4：采用k均值聚类的方法将种群分成n＝8个子群，将改进机械臂8组逆运动学结果作为初始聚类中心；
[0036]
步骤5：选取每个子群的前％top个评价值最小的粒子，即按四舍五入取整数，即为z
k
个，构成一个新的精英群，原来的普通子群中仍然包括那些精英粒子；
[0037]
步骤6：8个普通子群和精英群分别独立进行粒子群算法进行更新，每个子群的学习对象为该子群的最优粒子即局部最优值：
[0038]
v
ij
(t 1)＝ωv
ij
(t) c1r1(p
ij
(t)
‑
θ
ij
(t)) c2r2(g
kj
(t)
‑
θ
ij
(t))
ꢀꢀꢀꢀꢀꢀ
(14)
[0039]
θ
ij
(t 1)＝θ
ij
(t) v
ij
(t 1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0040][0041]
式中t为当前进化代数，t
max
为算法最大迭代次数，g
kj
(t)表示第k个子群的局部最优值(k＝1～n)，p
ij
(t)为粒子的历史最优值，ω为惯性权重，ω∈[ω
min
,ω
max
]，c1、c2分别为自身学习因子和社会学习因子，r1、r2、r3分别为0～1之间的随机数；
[0042]
粒子位置和速度更新完成后判断当前代数t是否已经达到最大迭代次数t
max
，若t＝t
max
，则退出循环，得到机械臂的逆运动学结果；
[0043]
否则，执行步骤7；
[0044]
步骤7：对每个子群和精英群分别独立进行差分变异进化操作
[0045]
θ
i
′
(t)＝g
k
(t) f(g
k
(t)
‑
p
i
(t))
ꢀꢀꢀꢀꢀ
(17)
[0046][0047]
式中，θ
i
′
(t)表示粒子i对应的变异向量，f为0～1之间的缩放因子，按照式18对粒子历史最优值和子群局部最优值进行更新；如果差分变异后的粒子越界，则从改进机械臂8组逆解中任选一组，将其设置成该粒子的位置；
[0048]
步骤8、判断精英群是否进入停滞状态：f
max
、f
min
分别表示精英群中粒子的最大评价值和最小评价值，det为给定的精度误差，若f
max
＞det且f
max
‑
f
min
＜det，则认为子群进入停滞状态，此时按下式改变精英群中的每个粒子：
[0049]
θ
ij
＝g
kj
‑
l
j
rand3(2l
j
)
ꢀꢀꢀꢀꢀꢀ
(19)
[0050][0051]
式中rand3为0～1之间的随机数，β为比例因子；
[0052]
否则，执行步骤9；
[0053]
步骤9：每个普通子群从精英群中选择z
k
个粒子替换掉自己子群中评价值最小的z
k
个粒子；
[0054]
步骤10、判断是否达到子群合并的条件：计算8个普通子群局部最优值的方差d
(g
kj
)，若d(g
kj
(t))≤ε、d(g
kj
(t 1))≤ε、d(g
kj
(t 2))≤ε，即连续三代的方差均小于给定误差ε，则将所有普通子群进行合并，迭代计数器t＝t 1，返回步骤5继续进行更新计算；
[0055]
若不满足子群合并的条件，则迭代计数器t＝t 1，返回步骤4，进行下一代的分群操作；
[0056]
一旦达到子群合并条件将子群合并后，后续的每一代便只剩下一个普通群和一个精英群不断进行更新进化，直到达到算法的最大迭代次数后，输出机械臂的逆运动学结果。
[0057]
所述β＝0.1。
[0058]
有益效果
[0059]
本发明提出的一种单关节锁定失效下的空间机械臂逆运动学方法，在粒子群算法的基础上，增加了分群操作，普通子群向子群的优秀个体(局部最优值)学习，精英群(普通子群的优秀个体)向全局最优值学习，模拟了生物界的社会行为(子群向子群中的优秀个体学习，每个子群中的优秀个体带领子群不断进化)，且每次迭代会在精英群和普通群之间进行交流，将适应值差的个体进行替换，在迭代后期进行子群合并操作，这样算法的收敛速度会得到提升，减少了算法的计算时间。此外，依据ssrms机械臂和其改进机械臂在之间的特征关系，用其等效机械臂的逆运动学结果来作为算法的迭代初值，会大大减少算法收敛到最优值的时间。综上该算法在完成任务要求的前提下可以减少计算时间。且该算法是一种数值求解方法，可以类比推广到其它构型的机械臂，另外关节正常情况下机械臂逆运动学的计算也可以使用该方法，具有较强的普适性。
附图说明
[0060]
图1为本发明所使用的算法流程图
[0061]
图2为ssrms型机械臂的d
‑
h坐标系示意图
[0062]
图3为ssrms型机械臂改进后的机械臂的d
‑
h坐标系示意图(锁定关节5)
具体实施方式
[0063]
现结合实施例、附图对本发明作进一步描述：
[0064]
本发明提出了一种基于粒子群优化算法的逆运动学计算方法，总体流程如图1所示。该算法在标准粒子群算法的基础上采用了k均值动态分群的思想，将粒子群分成几个普通子群，然后在普通子群中选择优秀个体组成精英群。后期子群之间的多样性会减弱，此时再将普通子群进行合并，减少计算资源的消耗。整个种群中评价值最差的粒子的收敛时间将决定最终整个粒子群算法的收敛时间，因此在普通群和精英群之间加入了交换操作，替换评价值较差的粒子，进而加快粒子群的收敛速度，降低了算法的时间消耗。
[0065]
下面结合附图对本发明作进一步详细描述，具体实施方式如下：
[0066]
步骤1:根据图2以及表1的d
‑
h参数建立ssrms型机械臂的正运动学模型。
[0067][0068]
[n
r
,o
r
,a
r
,p
r
]＝f
r
(θ
r
)
ꢀꢀꢀꢀꢀꢀꢀ
(2)
[0069]
表1实际ssrms型机械臂的d
‑
h参数
[0070][0071]
将ssrms机械臂去掉一个偏置量后的机械臂构型定义为改进机械臂，如图3所示，可以看到改进机械臂的前3个关节轴线相交于一点，满足pieper准则，因此改进机械臂的逆运动学存在解析解，建立改进机械臂的正运动学模型和逆运动学模型：
[0072][0073]
[n
m
,o
m
,a
m
,p
m
]＝f
m
(θ
m
)
ꢀꢀꢀꢀꢀꢀ
(4)
[0074][0075]
式中θ
r
表示实际ssrms型机械臂的7个关节角，[n
r
,o
r
,a
r
,p
r
]表示实际ssrms型机械臂的末端位姿，θ
m
表示改进机械臂的7个关节角，[n
m
,o
m
,a
m
,p
m
]表示改进机械臂的末端位姿。
[0076]
假设待求解目标末端位姿为[n
r
,o
r
,a
r
,p
r
]，由于改进机械臂的逆运动学是存在解析解的，将待求解目标末端位姿带入等效机械臂的逆运动学函数中，可以得到8组逆运动学结果；
[0077]
步骤2：确定逆运动学的优化目标函数。对于多自由度机械臂而言，一个末端位姿往往对应多组逆解，然而实际只需要一组逆解，因此需要添加各种约束来选取最合适的那组逆解。通常会考虑机械臂末端位置姿态的误差、运动过程中的能量消耗、柔顺性、安全性等指标，根据这些约束指标来定义逆运动学的优化目标函数，本发明的案例只考虑末端位置误差和能量消耗这两个指标。设机械臂末端的期望位置为p
r
，机械臂的位置误差函数定义为：
[0078][0079]
机械臂的各关节角为θ＝[θ
1 θ
2 θ
3 θ
4 θ
5 θ
6 θ7]，初始关节角为为使机械臂的能耗最小，将机械臂各个关节角的改变量定义为：
[0080][0081]
在满足一定的位置误差条件下，以能耗最小为优化目标，因此可以将逆运动学的求解转换成下面的最小约束优化问题来解决：
[0082]
[0083]
δp
‑
δp
max
≤0
ꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0084]
式中δp
max
为最大误差。
[0085]
利用罚函数将上述约束优化问题转化成无约束优化问题进行处理，转化后得到下面的优化目标函数：
[0086][0087][0088]
式中f(θ)为优化目标函数，为罚函数，α为惩罚系数；
[0089]
步骤3：设置相关参数，种群初始化。种群规模为m，由于7自由度的机械臂锁定一个关节，因此需求解的粒子维数为6。将种群中8个粒子的初值分别设为步骤1改进机械臂所对应的8组逆运动学结果，其余的m
‑
8个粒子的初值按下面的式子进行混沌初始化：
[0090]
θ
ij
＝l
θj
rand1(h
θj
‑
l
θj
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0091]
v
ij
＝l
vj
rand2(h
vj
‑
l
vj
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0092]
式中θ
ij
表示第i个粒子的第j维位置分量，v
ij
表示第i个粒子的第j维速度分量，h
θj
、l
θj
分别表示粒子第j维位置分量的最大值和最小值，h
vj
、l
vj
分别表示粒子第j维速度分量的最大值和最小值，rand1、rand2分别为0～1之间的随机数；
[0093]
步骤4：采用k均值聚类的方法将种群分成n个子群，此处n＝8，将改进机械臂的8组逆运动学结果作为初始聚类中心；
[0094]
步骤5：选取每个子群的前％top个评价值最小的粒子(按四舍五入取整数，即为z
k
个)构成一个新的精英群，原来的普通子群中仍然包括那些精英粒子；
[0095]
步骤6：8个普通子群和精英群分别按下式独立进行粒子群算法进行更新，需注意每个子群的学习对象为该子群的最优粒子(局部最优值)。
[0096]
v
ij
(t 1)＝ωv
ij
(t) c1r1(p
ij
(t)
‑
θ
ij
(t)) c2r2(g
kj
(t)
‑
θ
ij
(t))
ꢀꢀꢀꢀꢀꢀ
(14)
[0097]
θ
ij
(t 1)＝θ
ij
(t) v
ij
(t 1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0098][0099]
式中t为当前进化代数，t
max
为算法最大迭代次数，g
kj
(t)表示第k个子群的局部最优值(k＝1～n)，p
ij
(t)为粒子的历史最优值，ω为惯性权重，ω∈[ω
min
,ω
max
]，c1、c2分别为自身学习因子和社会学习因子，r1、r2、r3分别为0～1之间的随机数。
[0100]
粒子位置和速度更新完成后判断当前代数t是否已经达到最大迭代次数t
max
，若t＝t
max
，则退出循环，得到机械臂的逆运动学结果。
[0101]
否则，执行步骤7；
[0102]
步骤7：对每个子群和精英群分别独立进行差分变异进化操作：
[0103]
θ
i
′
(t)＝g
k
(t) f(g
k
(t)
‑
p
i
(t))
ꢀꢀꢀꢀꢀꢀ
(17)
[0104][0105]
式中，θ
i
′
(t)表示粒子i对应的变异向量，f为0～1之间的缩放因子，按照式(18)对
粒子历史最优值和子群局部最优值进行更新。如果差分变异后的粒子越界，则从改进机械臂的8组逆解中任选一组，将其设置成该越界粒子的位置；
[0106]
步骤8：判断精英群是否进入停滞状态。f
max
、f
min
分别表示精英群中粒子的最大评价值和最小评价值，det为给定的精度误差，若f
max
＞det且f
max
‑
f
min
＜det，则认为子群进入停滞状态，此时按下式改变精英群中的每个粒子：
[0107]
θ
ij
＝g
kj
‑
l
j
rand3(2l
j
)
ꢀꢀꢀꢀꢀ
(19)
[0108][0109]
式中rand3为0～1之间的随机数，β为比例因子，此处可选β＝0.1；
[0110]
步骤9：每个普通子群从精英群中选择z
k
个粒子替换掉自己子群中评价值最小的z
k
个粒子；
[0111]
步骤10：判断是否达到子群合并的条件。计算8个普通子群局部最优值的方差d(g
kj
)，若d(g
kj
(t))≤ε、d(g
kj
(t 1))≤ε、d(g
kj
(t 2))≤ε，即连续三代的方差均小于给定误差ε，则认为子群间的多样性减弱趋于一致，此时再分群进行计算对结果影响十分微小，为了节约计算资源提升计算速度，则可将所有普通子群进行合并，迭代计数器t＝t 1，返回步骤5继续进行更新计算。
[0112]
若不满足子群合并的条件，则迭代计数器t＝t 1，返回步骤4，进行下一代的分群操作。
[0113]
一旦达到子群合并条件将子群合并后，后续的每一代便只剩下一个普通群和一个精英群不断进行更新进化，直到达到算法的最大迭代次数后，输出机械臂的逆运动学结果。
[0114]
以上可以看出本发明所提出的算法是一种数值求解方法，该方法可以很好的用于逆运动学无法求得解析解的机械臂。空间机械臂大多都是具有偏置构型的，本发明是以ssrms型机械臂为例。ssrms型机械臂和其改进机械臂在同一组关节角下对应的末端位置不同，但姿态是相同的，因此将目标末端位姿用等效机械臂的逆运动学计算出的逆解结果是很接近实际ssrms机械臂的逆解结果的，利用该特征，用其等效机械臂的逆运动学结果来作为算法的迭代初值，会大大减少算法收敛到最优值的时间。该算法在满足任务要求的前提下可以减少计算时间，且该算法也可以类比推广到其它构型的机械臂，而且关节正常情况下机械臂逆运动学的计算也可以参考使用该方法。