(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210815663.3 (22)申请日 2022.07.12 (71)申请人 中国地质大 学 （武汉） 地址 430000 湖北省武汉市洪山区鲁磨路 388号 (72)发明人 简海涛　郑世祺　陈响　宋涛　 宛敏红　梁定坤　 (74)专利代理 机构 武汉知产时代知识产权代理 有限公司 42 238 专利代理师 徐欢 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种带肘部偏置的七自由度机械臂的几何 逆解方法 (57)摘要 本发明公开了一种带肘部偏置的七自由度 机械臂的几何逆解方法， 包括以下步骤： 先建立 七自由度机械臂 的DH坐标系， 标定其DH参数； 再 建立几何坐标系及定义臂角； 然后确定空间中各 点及向量坐标； 最后开展七自由度机械臂的逆解 计算。 该七自由度机械臂的几何逆解方法利用臂 角以及空间中的基本几何结构关系， 在现有的代 数解法基础上， 扩展出一套针对 带肘部偏置的七 自由度机械臂这种特殊结构的几何逆解方法， 相 较于现有的基于代数法的解析解法， 这套方法具 有计算量小， 效率高， 计算精度可靠， 且实现方式 更容易等优点， 可以作为带肘部偏 置的七自由度 机械臂的逆解方法上的一种补充。 权利要求书3页 说明书9页 附图4页 CN 115229786 A 2022.10.25 CN 115229786 A 1.一种带肘部偏置的七自由度机 械臂的几何逆解方法， 其特 征在于： 包括以下步骤： S1、 建立七自由度机械臂的DH坐标系， 标定其DH参数， DH参数包括： αi‑1、 ai‑1、 θi、 di， αi‑1 分别为0、 ‑π/2、 π/2、 ‑π/2、 π/2、 ‑π/2、 π/2， ai‑1均为0， θi分别为θ2、 θ2、 θ3、 θ4、 θ5、 θ6、 θ7， di分别 为d1、 0、 d3、 d4、 d5、 0、 d7； 其中， i表示第i个坐标系， i为1～7， αi‑1表示zi‑1轴与zi轴间夹角， ai‑1表示zi‑1轴与zi轴 间距离， θi表示xi‑1轴与xi轴间夹角， di表示xi‑1轴与xi轴间距离； 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平面构造公式表述 为：权　利　要　求　书 1/3 页 2 CN 115229786 A 2将结果排列得到∑＝[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处的旋转变换为Rs， 则得到Rs·∑0＝∑D， 且∑0， ∑D是正交矩阵 ， 则有 S处的旋转变换为： Rs＝Rot(z， θ1)Rot(y， θ2)Rot(z， θ3)， 即： 其中， si＝sinθi， ci＝cosθi(i＝1， ...， 7)， *表示可忽略的项； 由上式得到： cosθ2＝Rs33； 权　利　要　求　书 2/3 页 3 CN 115229786 A 3