基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法

1.本发明属于拟人机械臂逆运动学算法相关技术领域，更具体地，涉及一种基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法。


背景技术：

2.随着辅助机器人拟人化技术的飞速发展，人机协作与人机共融成为机器人技术未来的发展方向。在机器人提供辅助作业，以及人机交互的环境中，机器人的拟人运动能给人带来安全感与舒适感，并提高人机交互的效率。日常生活中，对于给定的抓取末端姿态，人类可以自然地在无穷的手臂姿态可能性中选择最优的姿态。因此，对于拟人机械臂，一个重要的工作就是实现在日常生活中，已知手部末端姿态的前提下保证手臂姿态的拟人性。目前，大量的研究集中在拟人机械臂的手臂逆运动学冗余姿态求解问题上，其中主要分为基于机器人模型的方法与基于数据驱动的方法。这两种方法都可以生成与实验数据保持一致的拟人手臂姿态，然而由于求解过程没有结合人体的肌骨模型，它们在整个手臂运动空间的求解能力均具有局限性。
3.目前，建立人体手臂肌骨模型与拟人机械臂运动学模型的映射，以及对肌肉损失函数进行优化求解是两大难点，由于肌骨模型与拟人机械臂运动学结构的差异性，以及优化目标的非凸性，需要对求解过程进行合理的假设，建立有效准确的映射，并且提出新的优化求解算法。


技术实现要素：

4.针对现有技术的以上缺陷或改进需求，本发明提供了一种基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法，以最小化肌肉损失作为逆运动学求解过程中的优化目标，通过实验评估实现了在给定机械臂末端抓取姿态前提下，冗余机械臂手臂拟人姿态的准确求解，以及较低的摇摆角复现误差；同时，该方法的解析结果在整个机械臂工作空间具有良好的连续性。
5.为实现上述目的，按照本发明的一个方面，提供了一种基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法，所述方法主要包括以下步骤：
6.(1)基于人体手臂肌肉损失的数学模型及人体手臂肌骨模型对应的运动学方程，以最小化肌肉损失为目标构建目标方程；
7.(2)基于所述目标方程及拟人机械臂运动学模型，采用位置域的解析ik算法计算关节角反解qs，将qs映射为基于肌骨模型的数值ik求解的初始值q
ind
，进而采用lm方法进行优化求解以得到满足运动学约束的关节角q，将q作为局部最优求解的初值；
8.(3)基于每一个求解得到的手臂关节角q初值进行局部优化搜索，对容许范围内的手臂旋转角ψh进行预定间隔范围内的重采样，获取不同手臂关节角q初值并重复进行局部搜索过程，直至满足终止准则，在当前所有局部最优值中选择最优结果作为全局最优解q
opt
；
9.(4)通过臂的摇摆角建立肌骨模型与拟人机械臂的映射，进而将得到的全局最优解映射到机械臂运动学模型中，以确定机械臂的拟人姿态，计算得到机械臂在拟人姿态下的各个关节角度，完成机械臂逆运动学冗余姿态解析。
10.进一步地，基于人体手臂的肌骨模型，对于给定的末端抓取目标位姿，对应的运动学方程为：
[0011][0012]
te＝fe(q)
[0013]
q≤qu[0014]
q≥q
l
[0015]
其中，q∈r
20*1
是肌骨模型关节的广义坐标系，a(q)∈r
20*20
是肌骨模型的质量矩阵，是向心力和科式力向量，g(q)∈r
20*1
是重力向量，τ∈r
20*1
是广义控制力矩向量，te∈r
4*4
是抓取目标姿态矩阵，qu∈r
20*1
是广义坐标系的上限，q
l
∈r
20*1
是广义坐标系的下限，fe(
·
)：r
20
→r4*4
是基于手臂肌骨模型的从关节角到手臂末端姿态的连续非线性正运动学映射。
[0016]
进一步地，人体手臂肌肉损失的数学模型为：
[0017][0018]
s.t.
[0019][0020]am
是肌肉m的激活水平，是肌肉m的最大轴向力，lm是肌肉m的长度，υm是肌肉m的收缩速度，是肌肉m的力矩-长度-速度平面，r
m，j
是肌肉m关于关节轴j的力臂，αm是肌肉m的羽状角，τj是作用在关节轴j上的广义力。
[0021]
进一步地，以最小化肌肉损失为优化目标的目标方程为：
[0022][0023][0024][0025]
τ＝g(q)
[0026]
te＝fe(q)
[0027]
q≤qu[0028]
q≥q
l
[0029]
其中，lm(q)是肌肉m与姿态相关的肌肉长度，r
m，j
(q)是肌肉m关于关节轴j的姿态相关的力臂。
[0030]
进一步地，步骤(2)中采用的映射准则为：
[0031]qind
＝kqs[0032]
其中，k一般为稀疏矩阵，稀疏矩阵k的设定取决于具体机械臂的结构特性。
[0033]
进一步地，局部优化搜索的方向为：
[0034][0035][0036][0037]
其中，q
ind
是手臂肌骨模型的独立关节坐标系，jv(q)是几何雅可比矩阵，w是将q
ind
转换为q的权重矩阵，n(
·
)是求算矩阵的零空间的运算符，步长ρ为正值标量，以在搜索过程中减少e(q)；设置ρ初始值为1e-3，当ρ小于设定的常量，或者e(q)在两个搜索方向同时递增时，则满足局部收敛准则。
[0038]
进一步地，步骤(3)中重采样的终止准则为：
[0039][0040][0041]
其中，ω为所有求解得到的不同的局部最优值的数量，n是应用于局部最优求解的采样值的数量，ε和δ是具体的终止准则。
[0042]
进一步地，利用摇摆角建立肌骨模型与机械臂运动学模型的冗余参数，进而利用冗余参数将全局最优解映射到机械臂运动学模型中，以确定机械臂的拟人姿态。
[0043]
进一步地，冗余参数包括手臂旋转角ψr及ψh，ψh为：
[0044][0045]
其中，s0,e,w与n
gravity
表示机器人基坐标系b下的三维向量。
[0046]
总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法主要具有以下有益效果：
[0047]
1.以最小化肌肉损失作为优化目标，求解给定末端抓取姿态条件下，机械臂拟人姿态的关节角度；考虑到人体手臂肌骨模型与拟人机械臂在运动学结构上的差异性，通过利用手臂的摇摆角建立了肌骨模型与拟人机械臂模型间的映射，在机械臂冗余姿态中求解最优的拟人手臂姿态，增加了机械臂辅助的安全感与舒适度，提高了人机交互的效率。
[0048]
2.针对最小肌肉损失的优化目标提出了一种两步优化算法，通过在位置域多次采样，利用数值法求解局部最优，再通过分析得到全局最优解，提高了优化算法的求解速度，保证了优化结果的稳定性与准确性。
[0049]
3.针对局部最优求解过程，提出了一种混合ik方法，为目标优化函数提供了有效的初始值，提高了优化过程的收敛速度与精度。
附图说明
[0050]
图1是本发明所提供的基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法
的流程框图；
[0051]
图2中的(a)与(b)分别是本发明的优选实施例所构建的七自由度拟人假肢臂与肌骨模型冗余参数示意图。
具体实施方式
[0052]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0053]
请参阅图1，本发明提供的基于人体肌骨模型的冗余机械臂拟人运动学逆解求解方法，所述方法采用了一种包含局部最优求解与全局最优求解的两步优化算法，为了保证局部最优求解的快速收敛，采用混合ik算法以获取满足运动学约束的计算初值；通过臂的摇摆角建立运动学冗余参数，完成肌骨模型与拟人机械臂运动学模型的映射，从而得到拟人机械臂关节角度，实现了冗余机械臂的姿态求解。
[0054]
所述方法主要包括以下步骤：
[0055]
步骤一，建立人体手臂的肌骨模型。
[0056]
该肌骨模型包括7个独立关节、13个耦合关节及关节间的50条肌肉；其中，通过固定手部保持在抓握姿态以消除手部运动自由度。
[0057]
步骤二，已知末端抓取目标姿态的条件下，基于肌骨模型获取运动学方程。
[0058]
基于人体手臂的肌骨模型，对于给定的末端抓取目标位姿，运动方程为：
[0059]
其中，q∈r
20*1
是肌骨模型关节的广义坐标系，a(q)∈r
20*20
是肌骨模型的质量矩阵，是向心力和科式力向量，g(q)∈r
20*1
是重力向量，τ∈r
20*1
是广义控制力矩向量，te∈r
4*4
是抓取目标姿态矩阵，qu∈r
20*1
是广义坐标系的上限，q
l
∈r
20*1
是广义坐标系的下限，fe(
·
)：r
20
→r4*4
是基于手臂肌骨模型的从关节角到手臂末端姿态的连续非线性正运动学映射。
[0060]
步骤三，构建人体手臂肌肉损失的数学模型。
[0061]
具体地，对于特定手臂末端姿态下求得关节力矩，可以通过以下数学模型求解得到对应的肌肉激活程度，对应的数学模型为：
[0062][0063]
s.t.
[0064]
[0065]am
是肌肉m的激活水平，是肌肉m的最大轴向力，lm是肌肉m的长度，υm是肌肉m的收缩速度，是肌肉m的力矩-长度-速度平面，r
m，j
是肌肉m关于关节轴j的力臂，αm是肌肉m的羽状角，τj是作用在关节轴j上的广义力。
[0066]
步骤四，基于人体手臂肌肉损失的数学模型及人体手臂肌骨模型对应的运动学方程，以最小化肌肉损失为目标，建立目标方程。
[0067]
其中，在手臂肌骨模型中，假设只考虑臂和手的重力产生的关节轴力矩；手臂抓取过程被视为准静态过程，忽略手臂运动产生的微小惯性力；忽略抓取的目标物体产生的重力矩；同时，在所有的反解可能性中，具有最小肌肉损失的手臂姿态作为人体抓取目标物体时手臂的最佳姿态。
[0068]
基于以上，以最小化肌肉损失为优化目标的目标方程为：
[0069][0070][0071][0072]
τ＝g(q)
[0073]
te＝fe(q)
[0074]
q≤qu[0075]
q≥q
l
[0076]
其中，lm(q)是肌肉m与姿态相关的肌肉长度，r
m，j
(q)是肌肉m关于关节轴j的姿态相关的力臂。
[0077]
步骤五，基于所述目标方程及拟人机械臂运动学模型，采用位置域的解析ik算法计算关节角反解qs，将qs映射为基于肌骨模型的数值ik求解的初始值q
ind
，进而采用lm方法进行优化求解以得到满足运动学约束的关节角q，将q作为局部最优求解的初值。
[0078]
在运动学约束下的肌肉最小损失优化函数具有非凸性，存在局部最优解并受目标抓取姿态te影响，因此采用数值方法求解，适用于非凸目标的优化。而数值解法对初始值敏感，采用基于位置域的ik算法可以获取满足机器人运动学约束的良好初始值，提高优化求解过程的收敛速度与准确性。
[0079]
由于肌骨模型的冗余与欠驱动特性，ik求解具有较大的计算复杂度，而本发明采用混合ik算法进行求解，具体为：首先，给定末端抓取目标位姿，采用位置域的解析ik算法，基于拟人机械臂模型，计算关节角反解为qs；其次，由于拟人机械臂与肌骨模型的运动学结构相似性，qs可以映射为基于肌骨模型的数值ik求解的初始值q
ind
，并采用该初始值，通过lm方法进行优化求解，准确计算满足运动学约束的q。
[0080]
映射规则可以表示为：
[0081]qind
＝kqs[0082]
其中，k一般为稀疏矩阵，将基于机械臂运动学模型的关节角映射到人体肌骨模型中，并作为基于人体肌骨模型lm优化过程的初值，稀疏矩阵k的设定取决于具体机械臂的结构特性；q
ind
作为基于手臂肌骨模型反解的初值，采用lm算法确定混合ik求解过程的最终求
解结果q，作为局部最优求解的初值。
[0083]
步骤六，基于每一个求解得到的手臂关节角q初值进行局部优化搜索，对容许范围内的手臂旋转角ψh进行预定间隔范围内的重采样，获取不同手臂关节角q初值并重复进行局部搜索过程，直至满足终止准则，在当前所有局部最优值中选择最优结果作为全局最优解q
opt
。
[0084]
具体地，对于每一个求解得到的手臂关节角参数q初值，局部优化搜索方向可以表示为：
[0085][0086][0087][0088]
其中，q
ind
是手臂肌骨模型的独立关节坐标系，jv(q)是几何雅可比矩阵，w是将q
ind
转换为q的权重矩阵，n(
·
)是求算矩阵的零空间的运算符，步长ρ为正值标量，以在搜索过程中减少e(q)。设置ρ初始值为1e-3，当ρ小于设定的常量，或者e(q)在两个搜索方向同时递增时，则满足局部收敛准则。
[0089]
对容许范围内的手臂旋转角ψh进行小间隔范围内的重采样，并重复进行局部搜索过程，直至满足终止准则，以避免局部最优解对采样值的依赖性。终止准则可以表示为下式：
[0090][0091][0092]
其中，ω为所有求解得到的不同的局部最优值的数量，n是应用于局部最优求解的采样值的数量，ε和δ是具体的终止准则。
[0093]
当满足局部最优求解过程的终止准则后，在当前所有局部最优值中选择最优结果作为全局最优解q
opt
。
[0094]
步骤七，通过臂的摇摆角建立肌骨模型与拟人机械臂的映射，进而将得到的全局最优解映射到机械臂运动学模型中，以确定机械臂的拟人姿态，计算得到机械臂在拟人姿态下的各个关节角度，完成机械臂逆运动学冗余姿态解析。
[0095]
由于人体手臂肌骨模型与拟人机械臂在运动学结构上的差异，基于肌骨模型的优化结果q
opt
不能直接应用到机械臂上。基于两者肩-肘-腕运动学结构的相似性，可以借助臂的摇摆角建立统一的运动冗余参数，利用摇摆角映射，可以得到拟人姿态下的机械臂关节角度。
[0096]
具体地，对于七自由度拟人机械臂，肩部s，肘部e以及腕部w形成手臂平面π
arm
，肩部s、肘部w以及重力方向向量n
gravity
形成参考平面π
ref
，手臂旋转角可以表示为下式：
[0097]
[0098]
其中，s，e，w与n
gravity
表示机器人基坐标系b下的三维向量。
[0099]
对于人体手臂肌骨模型，手臂旋转角ψr通过特定初始位置下的肩部坐标s0定义，设定初始位置为手臂向下时肌肉损失最小的姿态，ψh可以表示为：
[0100][0101]
其中，s0，e，w与n
gravity
表示机器人基坐标系b下的三维向量。
[0102]
借助运动学冗余参数ψh及ψr，将全局最优解q
opt
映射到机械臂运动学模型中，确定机械臂拟人姿态，计算得到机械臂在拟人姿态下的各个关节角度，完成机械臂逆运动学冗余姿态解析。
[0103]
以下一个具体实施例来对本发明进行进一步地详细说明。
[0104]
请参阅图2及表1，本实施例涉及一种应用于上肢残疾患者的七自由度假肢臂模型，该假肢臂主要由肩-肘-腕三部分组成，其中：
[0105]
肩部具有两个旋转自由度，大臂和小臂各具有一个旋转自由度，肘部具有一个旋转自由度，腕部具有两个旋转自由度。
[0106]
表1参数表
[0107][0108]
对于给定的假肢臂末端抓取位姿，基于人体手臂肌骨模型，经过上述的冗余解析算法计算之后，得到已知假肢臂末端位姿前提下，具备拟人特性的假肢臂手臂姿态，以及在该姿态下的关节角度，进而控制假肢臂完成目标物体的拟人操作。
[0109]
对于给定的假肢臂末端位姿，假肢臂拟人手臂姿态的整个解析中，针对具体的作业场景，给定假肢臂末端位姿矩阵te；为针对目标位姿，反解得到假肢臂拟人的操作姿态，基于人体手臂肌骨模型，以最小化肌肉损失作为求解关节角度的优化目标，利用臂的摇摆角，建立起肌骨模型与假肢臂模型的冗余参数，将反解结果映射到七自由度假肢臂模型上，实现逆运动学冗余姿态解析。具体包括以下步骤：
[0110]
1.构建优化目标函数。
[0111]
优化目标函数可以表示如下：
[0112][0113][0114]
[0115]
τ＝g(q)
[0116]
te＝fe(q)
[0117]
q≤qu[0118]
q≥q
l
[0119]
其中，am是肌肉m的激活水平，是肌肉m的最大轴向力，lm(q)是肌肉m与姿态相关的肌肉长度，υm是肌肉m的收缩速度，是肌肉m的力矩-长度-速度平面，r
m，j
(q)是肌肉m关于关节轴j的姿态相关的力矩臂，αm是肌肉m的羽状角，τj是作用在关节轴j上的广义力。
[0120]
在对目标优化函数求解过程中，做出以下假设：
[0121]
1)不考虑手臂运动过程中产生的惯性力，只考虑手臂产生的重力作用；
[0122]
2)不考虑假肢臂抓取的目标物体的重力，假肢臂关节力矩仅由手臂的重力产生；
[0123]
2.在运动学约束条件下，求解目标函数。按照求解顺序，主要过程如下：
[0124]
(a)首先，基于七自由度假肢臂模型，采用位置域的解析ik方法，求解假肢臂模型的ik解为qs；
[0125]
(b)基于手臂肌骨模型与假肢臂模型在运动学结构上的相似性，解析解qs可以映射为基于手臂肌骨模型的数值ik解的初值q
ind
：
[0126]qind
＝kqs[0127]
其中，k为稀疏矩阵：
[0128][0129]
采用q
ind
作为基于肌骨模型的数值ik解的初值，使得ik反解过程能够快速收敛，并采用lm数值计算方法，得到满足运动学约束的精确反解q。
[0130]
其中，稀疏矩阵的具体结构取决于假肢臂模型的结构特点，针对本实施例中七自由度假肢臂模型，稀疏矩阵k设置如上。
[0131]
步骤(a)(b)组成混合ik算法，求解得到的运动学反解q作为目标优化函数求解局部最优过程中的初值。
[0132]
3.采用迭代的方法，以求解得到的q作为初值进行局部最优搜索。设置优化过程的搜索方向为：
[0133][0134][0135][0136]
其中，q
ind
∈r
7*1
是手臂肌骨模型的独立关节坐标，jv(q)∈r
6*20
是几何雅可比矩阵，w∈r
20*7
是将q
ind
转换为q的权重矩阵，n(
·
)是求算矩阵的零空间的运算符，步长ρ为正值标量，如果e(q)在n与-n两个搜索方向非同时递增，则为达到局部最优，否则ρ成半递减。
[0137]
当ρ小于特定值或者e(q)在两个搜索方向均递增时，达到局部收敛条件。
[0138]
由于局部最优求解依赖于臂的摇摆角ψh容许范围内的采样值，设置摇摆角ψh合适的变化区间进行重采样，重复局部最优求解过程，直到满足下列终止条件：
[0139][0140][0141]
其中，ω为所有求解得到的不同的局部最优值的数量，n是应用于局部最优求解的采样值的数量，ε和δ是具体的终止准则。
[0142]
4.当达到局部最优求解的终止准则后，在所有不同的局部最优解中选择最优值作为最终的全局最优解。
[0143]
5.基于七自由度假肢臂模型与人体手臂肌骨模型在运动学结构上的相似性，利用冗余参数ψh与ψr，将基于肌骨模型的求解结果映射到七自由度假肢臂上，作为假肢臂冗余姿态解析的最优解，完成对假肢臂拟人姿态作业的控制。
[0144]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。