一种带肘部偏置的七自由度机械臂的几何逆解方法

技术特征：
1.一种带肘部偏置的七自由度机械臂的几何逆解方法，其特征在于：包括以下步骤：s1、建立七自由度机械臂的dh坐标系，标定其dh参数，dh参数包括：α
i-1
、a
i-1
、θ
i
、d
i
，α
i-1
分别为0、-π/2、π/2、-π/2、π/2、-π/2、π/2，a
i-1
均为0，θ
i
分别为θ2、θ2、θ3、θ4、θ5、θ6、θ7，d
i
分别为d1、0、d3、d4、d5、0、d7；其中，i表示第i个坐标系，i为1～7，α
i-1
表示z
i-1
轴与z
i
轴间夹角，a
i-1
表示z
i-1
轴与z
i
轴间距离，θ
i
表示x
i-1
轴与x
i
轴间夹角，d
i
表示x
i-1
轴与x
i
轴间距离；s2、给定七自由度机械臂的期望末端位姿将基座位置记为b，肩部关节位置记为s，腕部关节位置记为w，肘部关节位置记为e，其偏置点位置记为e
′
，肘部关节没有偏置时w所处在的点记为w
′
；将平面sew
′
与bs共面时的平面sew定义为基准臂角面，sew平面绕sw轴任意旋转，定义sew平面与基准臂角面的夹角为ψ，ψ的夹角范围为[-π，π]；s3、定义b点坐标pb＝[0，0，0]
t
，则向量s点坐标由七自由度机械臂的期望末端位姿取第四列前三行元素得到末端位置取前三行前三列元素得到末端姿态由向量的条件，得到w点坐标且向量向量进而得到向量构造坐标系来描述任意臂角平面sew，构造步骤如下：s311、构造sew
′
平面，求取取单位向量z0＝[0，0，1]
t
，在空间中构造sew
′
平面：取直线sw
′
上一点，其位置记为d，且de
⊥
sw
′
，此时z
∑
即为向量的方向向量；若z0与sw
′
共线，则直接取y
∑
＝[0，1，0]
t
；由已知条件求解的长度：由上式解得进而得到s312、构造sew平面取方向上的单位向量sew平面构造公式表述为：
将结果排列得到∑＝[x
∑
，y
∑
，z
∑
]；求取七自由度机械臂的逆解，包括如下步骤：s321：计算θ4由余弦定理，得到：引入一个控制参数控制参数e用于反映肘关节相对于sw
′
的位置关系，进而得到：θ4＝e(π-acos(cos∠sew
′
))；s322：描述臂角基准面当θ
1-3
＝0时，臂角平面处于臂角基准面上，计算得出w的位置：向量将向量代入sew平面构造公式，得到臂角基准面的描述为s323：求解θ
1-3
将代人sew平面构造公式，得到θ
1-3
分别取θ1，θ2，θ3时的臂角平面，记作此时的臂角平面旋转描述为与旋转矩阵rot(x，ψ)的运算其中rot(x，ψ)表示沿着x轴逆时针旋转角度ψ；记s处的旋转变换为r
s
，则得到r
s
·
∑0＝∑
d
，且∑0，∑
d
是正交矩阵，则有s处的旋转变换为：r
s
＝rot(z，θ1)rot(y，θ2)rot(z，θ3)，即：其中，s
i
＝sinθ
i
，c
i
＝cosθ
i
(i＝1，...，7)，*表示可忽略的项；由上式得到：cosθ2＝r
s33
；
引入一个控制参数控制参数s用于反映大臂相对于bs的位置关系，进而得到：θ2＝s
·
acos(r
s33
)；θ1＝atan2(s
·
r
s23
，s
·
r
s1a
)；θ3＝atan2(s
·
r
s32
，-s
·
r
s31
)；s324：求解θ
5-7
记w处的旋转变换为r
w
，则有以下等式成立：其中，r
s
·
rot(y，θ4)为正交矩阵，则有：w处的旋转变换描述为：r
w
＝rot(z，θ5)rot(y，θ6)rot(z，θ7)，即：其中，s
i
＝sinθ
i
，c
i
＝cosθ
i
(i＝1，...，7)，*表示可忽略的项。由上式得到：cosθ6＝r
w33
；；引入一个控制参数控制参数w用于反映手腕的翻转情况，进而得到：θ6＝w
·
acos(r
w33
)；θ5＝atan2(w
·
r
w23
，w
·
r
w13
)；θ7＝atan2(w
·
r
w32
，-w
·
r
w31
)。

技术总结
本发明公开了一种带肘部偏置的七自由度机械臂的几何逆解方法，包括以下步骤：先建立七自由度机械臂的DH坐标系，标定其DH参数；再建立几何坐标系及定义臂角；然后确定空间中各点及向量坐标；最后开展七自由度机械臂的逆解计算。该七自由度机械臂的几何逆解方法利用臂角以及空间中的基本几何结构关系，在现有的代数解法基础上，扩展出一套针对带肘部偏置的七自由度机械臂这种特殊结构的几何逆解方法，相较于现有的基于代数法的解析解法，这套方法具有计算量小，效率高，计算精度可靠，且实现方式更容易等优点，可以作为带肘部偏置的七自由度机械臂的逆解方法上的一种补充。机械臂的逆解方法上的一种补充。机械臂的逆解方法上的一种补充。


技术研发人员：简海涛 郑世祺 陈响 宋涛 宛敏红 梁定坤
受保护的技术使用者：中国地质大学（武汉）
技术研发日：2022.07.12
技术公布日：2022/10/24