[发明专利]S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法

权利要求书

1.一种S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法，其特征在于，所述数值稳定算法包括：(1)正运动学求解、(2)参考平面求解、(3)逆运动学求解、(4)奇异臂角获取、(5)关节限位及奇异值规避；

所述(3)逆运动学求解包括：

当给定参数ψ，即可进行逆运动学求解；其中θ₄与ψ无关，仅由末端位置和姿态确定，即θ₄＝θ₄'；真实⁰R₄即为参考平面⁰R₄'沿旋转ψ角求得，

⁰R₄＝⁰R_ψ⁰R′₄

θ₄＝θ′₄

³R₄＝³R′₄

⁰R₃³R₄＝⁰R_ψ⁰R′₃³R′₄

⁰R₃＝⁰R_ψ⁰R′₃；

通过以上推导得到真实⁰R₃即为参考平面⁰R₃'沿旋转ψ角求得，其中⁰R_ψ为相对向量旋转ψ角形成的旋转矩阵，由罗德里格斯公式得到：

其中I为3×3单位矩阵，因此⁰R₃可以表示为：

⁰R₃(ψ)＝sinψA_s+cosψB_s+C_s

其中：

同时，由于1-3关节为ZYZ球关节角，通过正运动学求得的公式为：

由⁰R₃(θ₁,θ₂,θ₃)＝⁰R₃(ψ)可得：

当θ₂≠0或±π时

其中GC₂同GC₄，取值范围为{-1,1}，表明θ₂的正负，且：

当θ₂＝0或±π时

因此对应可求出θ₁～θ₃，θ₄＝θ₄'，之后便求解θ₅～θ₇；

同理，5-7关节也为ZYZ球关节角，因此同样可对应求出θ₅～θ₇，即将上式完成如下替换即可：

θ₁←θ₅,θ₂←θ₆,θ₃←θ₇,s←w,GC₂←GC₆

其中：

GC₂、GC₄和GC₆均为姿态参数，取值为{-1,1}，分别表明θ₂、θ₄和θ₆的正负，臂角ψ为自运动冗余参数，至此完成逆运动学求解过程；

所述(4)奇异臂角获取包括：

奇异臂角是机械臂局部自运动流形中的奇异位置，在球关节θ₂＝0或±π时出现；

描述球关节的奇异臂角的关键是找到使θ_i＝0或±π的ψ_s，肩部i＝2，腕部i＝6，此臂角ψ_s同时也是与其相关的两个正切关节的奇异臂角；

余弦关节表达式为：

θ_i＝GC_iarccos(a sinψ+b cosψ+c)

θ_i相对ψ求导公式为：

由表达式可知θ_i＝0或±π对应于a sin(ψ)+b cos(ψ)+c＝±1，而a sin(ψ)+b cos(ψ)+c的值均位于±1间；即使得θ_i＝0或±π的ψ_s一定为a sin(ψ)+b cos(ψ)+c的极值；因此，通过使其导数为零可得两个ψ_s值：

容易判断求得和对应于θ_i的最大值或最小值；为了检验是否存在奇异臂角ψ_s，只需将i∈{1,2}与1进行比较；采用一种数值稳定的确定ψ_s的方法：

所述(5)关节限位及奇异值规避包括：

将每个关节i的极限范围映射到冗余臂角ψ的范围内，求取各关节可行臂角Ψ_i的交集即可求得最终冗余臂角范围；用和来表示第i个关节的上下关节角极限；其中θ₄与ψ无关，其关节限位规避可直接由末端位姿求得θ₄后与关节限位对比判断是否超限；

5.1余弦关节

余弦关节包括第2和6关节，给定θ_i，通过万能公式求得两个对应的ψ，ψ的计算公式为：

和对应于θ_i的最大值或最小值，记为θ_i^max和θ_i^min；下一步将分别为关节下限和上限找到对应的可行范围Ψ_il和Ψ_iu，最终得到此关节避免关节限位后对应的可行臂角范围：Ψ_i＝Ψ_il∩Ψ_iu；其基本思想是利用上式(2)求出极限关节角对应的ψ₁和ψ₂，并通过ψ₁和ψ₂点的导数的正负判断ψ₁、ψ₂是可行区间的入口点还是离开点；若且ψ₁为关节上限通过式(2)所求得，则1为离开点；5.2正切关节

给定θ_i，通过万能公式求得两个对应的ψ，ψ的计算公式为：

其中

a_p＝(c_d-b_d)tanθ_i+b_n-c_n

b_p＝2(a_dtanθ_i-a_n)

c_p＝(b_d+c_d)tanθ_i-b_n-c_n

接下来将根据这三种情况来解决正切关节的关节极限映射问题；

(I)Δ0，为单调

Δ0时ψ-θ_i图除±π间翻转外整体保持单调；即与和均各有一个交点，这意味着根据式(3)计算的只有一个解是正确的；判断具体为哪个解只需将计算结果反代回θ关于ψ的公式验证即可；求解时当a_p＝0时，解由给出；设ψ_l和ψ_u分别为与下极限和上极限的交点；设ψ₁＝min(ψ_l,ψ_u)，ψ₂＝max(ψ_l,ψ_u)，关节极限内的可选臂角范围为Ψ_i＝[ψ₁,ψ₂]或者Ψ_i＝[-π,ψ₁]∪[ψ₂,π]；具体为哪个范围可通过计算ψ₁是进入点还是离开点判断，或者通过观察偏转是否发生在ψ₁和ψ₂之间判断；偏转是否发生可通过计算是否在极限关节角范围内进行判断；(II)Δ0

ψ-θ_i图有两个局部极值，可通过式(1)求得，并通过二阶导的正负判断出局部极小值和极大值，并分别表示为和则设定之后通过求解关节极限与ψ-θ_i图的交点，可得关节极限和对应的可选臂角区域和对于如果极限超出了由和组成的区域，则选臂角范围或否则，则用和表示与ψ-θ_i的两个交点，并设定则关节下极限对应的可选臂角范围通过判断处导数的符号，判断为进入点或离开点，确定臂角范围或关节上极限对应臂角范围使用相同的规则获得；最后，Ψ_i由还是求得取决于是否存在翻转及θ_i^min、θ_i^max的相对位置；

(III)Δ＝0

如前所述，当Δ＝0时存在奇异臂角；在求解时设定一个阈值tol，令和为了避免奇异，在臂角范围内排除范围此时考虑如果ψ_s接近±π，则或可能从[-π,π]中脱落，所以最终得到的奇异臂角范围在实际计算可行臂角范围时应避开此奇异臂角范围；

设且如果则轮廓为“N”型，否则为“H”型；之后计算思路与第二种Δ0情况相似，即求得关节极限与图的交点，并为上下极限各求得一个可行区域，最终的可行区域按照判断选择两个极限线区域的交集或并集；

5.3整体算法

至止求得了各关节的可行臂角范围Ψ_i，则最终避免关节限位及奇异值的臂角范围为对于任一末端位姿，于其姿态对应的可行臂角范围内任意选出一冗余臂角即可进行数值稳定的逆运动学求解，实现S-R-S结构七自由度机械臂逆运动学解析解的获得。