Real - time and High - accurate Inverse Kinematic Control Algorithm f or Decoupled Six - DOF Manipulator
Fu Ro ng , J u Hehua
( 11 College of Elect ro nic Info rmation and Cont rol Engineering , Beijing U niversity of Technology , Beijing 100124 , China)
图1 整体结构图
・ 1638 ・
计算机测量与控制 根据式 ( 7) 和式 ( 8) 导出θ 1 的解析解 : θ co s 1 =
1
A G - FB
第 18 卷
θ θ [ - G( Cco s 3 + D sin 3 + E) +
( 9)
θ θ B ( Hco s 3 + I sin 3 + J) ] θ sin 1 =
1 逆运动学分析
解耦操作手的逆运动学包括腕心的定位逆解和定向逆解 , 即已知腕心在机械臂坐标系位置和方向单位矢量及旋转角度 , 求各关节需要旋转的角度 。 本文研究的 B2 型月球车上驱动相机盒运动的正交解耦机 械臂 , 整体结构图如图 1 所示 。机械臂腕心位置即相机盒位 置 , 关节 1～3 旋转方向两两正交 , 控制相机盒定位 , 其解耦 于控制相机盒定向的关节 4～6 , 如图 2 所示 。
ai bμ i i bλ i i
T
2 τ 1 -τ θ ≡ 2 2 ; 式中 ,τ ≡ 2 ; sin 1 +τ 1 +τ
tan (
θ ) 。 2 将其带入式 ( 11) 得到关于τ 3 的四次方程 : 4 3 2 τ τ τ τ R 3 + S 3 + T 3 + U 3 + V = 0
( 12)
表1 D - H 参数表 关节
1 2 3
2) 然后 , 将式 ( 9) 和式 ( 10) 两边平方再相加得到关于
θ θ sin co s 3 、 3 的方程 , 通过引入三角恒等式 , 推导出 θ 3 的解 析解 : 2 2 θ θ θ θ θ θ Kco s + L sin + Mco s 3 3 3 sin 3 + N co s 3 + Psin 3 + Q = 0
收稿日期 :2010201217 ; 修回日期 :2010202225 。 基金项目 : 国家 863 高科技资助项目 (2008AA0085) 。 作者简介 : 付 荣 (19862) , 女 , 硕士研究生 , 主要从事机械臂运动 学、 动力学控制 、 机械臂虚拟现实平台方向的研究 。 居鹤华 (19692) , 男 , 自动控制工程师 , 教授 , 主要从事智能控制 , 自主机器人方向的研究 。
→
Xc
Yc
Zc bi
T T
; bi = ; Qi =→; Nhomakorabeaai
→
=
θ ai co s i
θ a i sin i
式中 ,
2 2 2 2 2 ρ; R = 4 a2 +μ - 4 a2 1 ( J - H) 1 ( E - C) 1μ 1 2 S = 4 [ 4 a2 1 I ( J - H) + μ 1 D ( E - C) ] ; 2 2 2 2 2 2 2 2 ρ ]; T = 2 [ 4 a2 - H2 + 2 I 2 ) + μ 1 (J 1 ( E - C + 2 D ) - 4 a1μ 1 2 U = 4 [ 4 a2 1 I ( H + J ) +μ 1 D ( C + E) ] ; 2 2 2 2 2 ρ; V = 4 a2 +μ - 4 a2 1 ( H + J) 1 ( C + E) 1μ 1
[1 ] [2 ]
信息 , 驱动相机盒达到指定位置和朝向的 6R 机械臂 。借鉴文 献 [ 8 ] 的方法 , 利用机械臂正交的特殊构型 , 将一元四次方 程简化为一元二次方程 , 计算量更小 , 且解耦过程简单 。 机械臂的奇异位形是由于处在特殊位置 , 无法计算出关节 角度 , 导致的运动不可达 。本文借用 B2 型月球车的自由度弥 补机械臂自由度不足 , 使其避免不可达位置 , 扩展机械臂的运 动空间 。
0 引言
一般 6R 机器人的实数逆运动学解的上限为 16 , L EE 等利用矢量乘法找到 14 个线性无关的逆运动学方程 , 利用分 离变量将其化为关节变量半角正切的一元 16 次方程 。MAN2 FR ED [3 ] 利用多维几何和运动学图像理论简化了一元 16 次方 程的获取和变量的求解 。但是以上方法仍存在精度不够高和计 算复杂的缺点 。刘松国 [4 ] 在这些研究基础上提出基于矩阵分解 的一般 6R 机器人实时高精度逆运动学算法 , 满足精度要求 , 计算相对简单 。Saeed B. Niku [5 ] 、孙亮 [6 ] 利用机械臂运动学 模型的矩阵变换 , 将变量解耦 , 求得各关节角度解析解 。在满 足高精度的同时 , 计算简单 , 但解耦过程复杂 。 对于解耦 6 自由度机械臂的逆运动学 , 简化为 3 自由度位 置求解 、3 自由度方向求解 。上述 6 自由度的算法同样适用于 3 自由度位置逆运动学运算 。张敏 [7 ] 在 3 自由度机械手运动分 析中就借鉴了文献 [ 5 ] 的算法 。Jo rge Angeles [8 ] 提出利用欧 几里德范数求解解耦 6 自由度机械臂的算法 , 将位置逆运动学 解简化为关节变量半角正切的一元四次方程 。 本文主要研究位于 B2 型月球车身前端 , 为灵活获得视觉
由于机械手的正交构型 , 使式 ( 12 ) 的系数 S 、U 为零 , 2 式 ( 12) 可以看成关于τ 3 的一元二次方程 , 相比一元四次方 程 , 计算量大大减小了 : 2 2 2 τ ( 13) R (τ + T 3) 3 + V = 0 根据式 (13) 可以解出关于 θ 3 的 4 个可能的解 , 再将得 到的值代入式 (9) 和式 (10) 得到θ 1 解。 θ 3) 最后 , 将式 ( 2 ) 的前两个标量方程导出关于 co s 2 、 θ sin 2 的方程 , 得到θ 2 的解析解 : θ co s 2 =
图2 D - H 坐标系
1
A G - FB
θ θ [ F ( Cco s 3 + D sin 3 + E) ( 10)
θ θ A ( Hco s 3 + I sin 3 + J) ]
θ θ 11 1 定位逆解 ( 求解 θ 1 、 2 、 3) 运动学建模采用标准 D - H 坐标系法 , 如图 2 , 参数表如 表 1 所示 。
付 荣 , 居鹤华
( 北京工业大学 电子信息与控制工程学院 , 北京 100124)
摘要 : 根据 6 自由度机械臂正交解耦的结构特点 , 采用位姿分解方式 , 将 6 自由度逆运动学降为 3 自由度位置逆运动学 、3 自由度 方向逆运动学 ; 利用欧几里德范数导出机械臂定位 、定向的逆运动学解析解 , 使机械臂高速 、准确运动 。在定向控制方面 , 提出一种以 单位四元数为目标输入的控制形式 , 只需计算两个角度逆解 , 既简化计算 , 又利于实际操作 ; 利用逆运动学计算机械臂的工作空间和奇 异点空间 , 借助移动机器人车体自由度弥补因计算以及关节长度不够引起的奇异位形 , 极大扩展了机械手臂的有效运动区域 。 关键词 : 六自由度机械臂 ; 逆运动学 ; 解析解 ; 高精度
设计与应用
计算机测量与控制 . 2 0 1 0 . 1 8 ( 7 ) Computer Measurement & Control
文献标识码 :A
・ 1637 ・
文章编号 :167124598 (2010) 0721637204 中图分类号 : TP241
高精度解耦六自由度机械臂逆运动学解法
Abstract : This paper describes an approach of inverse kinematical cont rol for 6 DO F ort hogonal and decoupled manipulator . Based on t he st ruct ure characteristic of t his kind of manipulators , t he kinematical model is simplified to a 3 DO F orientation calculation and 3 DO F po s2 t ure calculation. This t wo part of kinematical calculation are independent . The position and attit ude determination p roblem of t his manip ula2 tor is solved based on Euclid norm. At t he same time , t his paper too k quaternion as t he cont rol inp ut to solve t he attit ude determination prob2 lem. These met hods can simplif y t he co mp utation , achieve t he real time and accuracy requirement s , and al so easy to implement . By analyzing t he singularity of t he motion region of t he manipulation , a new met hod to expand t he motion region is proposed by using extra DOF from rover. Key words : 6 - DO F manipulator ; inverse kinematic cont rol ; analytical solution ; high - accurate