高精度解耦六自由度机械臂逆运动学解法_付荣

数 。 Xc 、 Y c 、 Zc 是腕心的期望位置 。 根据文献 [ 8] 对旋转坐 标系的定义 :a※i = Qi※bi , 把式 (1) 变换成 :
Q2(b2 +Q3 b3 + Q3 Q4 b4)= QT1 → -b1
(2)
1) 首先 , 将式 (2) 两边 分 别取 欧 几里 德 范 数 , 推 导出
根据式 (7) 和式 (8) 导出 θ1 的解析解 :
co
源自文库
sθ1
=
A
G
1 -F
B
[
-
G(Cco
sθ3
+D
si nθ3
+E)+
B(Hco sθ3 +I sinθ3 +J)]
(9)
si nθ1
=
AG
1 -F
B[
F(Cco
sθ3
+D s inθ3
+E)-
A(H co sθ3 + Isinθ3 + J)]
(10)
ai biμi biλi T ; a※i = ai co sθi a i sinθi bi T ;Qi =
cosθi -λi sinθi μi sinθi
sinθi λi cosθi -μi co sθi 是相 邻 坐 标 系 间 的 旋 转 关 系 ,
0
μi
λi
θi 、 di 、ai 、αi 分别是机 械臂的 关节角 、 偏置 、 长 度和扭 角参
(2a2 a3 -2b2 b2 μ2 μ3)co sθ3 +(2b2 μ2 a3 +2b4 a2 μ3)sinθ3 (4)
式 (2) 右边取欧几里德范数 :
展开得 :
r2
=
X
2 c
+Y
2 c
+Z
2 c
+a21
+b21
-
(2a1 Xc co sθ1 +2a1Y csinθ1 +2b1 Z c)
(6)
把式 (4) 和 (6) 合并同类项 , 得到下 式 :
Aco sθ1 +Bsinθ1 +Cco sθ3 +Dsinθ3 +E = 0
(7)
式中 , A =2a1 Xc ;B =2a1Y c ;C =2a2 a3 -2b4 b2 μ2 μ3 ;D = 2b2 μ3
+2b4 a2 μ3 ;E
=
a22
+b22
+a
2 3
+b23
+ b24
+2b2b4 λ2λ3
关键词 :六自由度机械臂 ;逆运动学 ;解析解 ;高精度
Real -time and High -accurate Inverse Kinematic Control Algorithm for Decoupled Six -DOF Manipulator
Fu Rong , Ju H ehua
(1.Colleg e of Elect ronic Informa tion and Contro l Engineering , Beijing Univ ersity of T echno lo gy , Beijing 100124 , China)
信息 , 驱动相机盒达到指 定位置和朝 向的 6R 机械臂 。 借 鉴文 献 [ 8] 的方法 , 利用机械 臂正交 的特殊 构型 , 将一 元四 次方 程简化为一元二 次方程 , 计算量更小 , 且解耦过程简单 。
机械臂的奇异位形是 由于处在特殊位置 , 无法计算出关节 角度 , 导致的运动不可达 。 本 文借用 B2 型 月球车 的自由 度弥 补机械臂自由度 不足 , 使 其避免不可达位置 , 扩展机械臂的运 动空间 。
文献标识码 :A
· 1637 ·
高精度解耦六自由度机械臂逆运动学解法
付 荣 , 居鹤华
(北京工业大学 电子信息与控制工程学院 , 北京 100124)
摘要 :根据 6 自由度机械臂正交解耦的结构特点 , 采用位姿分解方式 , 将 6 自由度逆运动 学降为 3 自由 度位置逆 运动学 、 3 自由度 方向逆运动学 ;利用欧几里德范数导出机械臂定位 、 定向的逆运动学解析解 , 使机械臂 高速 、 准确运动 。 在 定向控制方面 , 提 出一种以 单位四元数为目标输入的控制形式 , 只需计算两个角度逆解 , 既简化计算 , 又利于实际操作 ;利 用逆运动学计 算机械臂的工作 空间和奇 异点空间 , 借助移动机器人车体自由度弥补因计算以及关节长度不够引起的奇异位形 , 极大扩展了机械手臂的有效运动区域 。
=
X
2 c
+Y
2 c
。
引入三角
恒等式
:co
sθ≡
1 1
-+ττ22 ;sinθ≡
1
+2ττ2 ;式中
,
τ≡
ta n(θ2 )。
将其带入式 (11) 得到关于 τ3 的四次方 程 : Rτ43 +Sτ33 + Tτ23 +Uτ3 +V = 0
式中 , L = 2a21 I2 + μ21D2 ;M = 2(4a21 HI + μ21 CD);K =
4a21 H2 +μ21 C2 ;N =2(4a21 HJ +μ21 CE);P =2(4a21 IJ +μ21 DE);
Q
= 4a21 J 2
+μ21 E
-4a
2 1
μ21 ρ2 ;其中
,
ρ2
本文主要研究位于 B2 型 月球车 身前端 , 为 灵活获 得视觉
收稿日期 :2010-01-17 ; 修回日期 :2010-02-25 。 基金项目 :国家 863 高科技资助项目(2008A A 0085)。 作者简介 :付 荣(1986-), 女 , 硕士研究生 , 主要从事机 械臂运动 学 、动力学控制 、机械臂虚拟现实平台方向的研究 。 居鹤华(1969-), 男 , 自动控制工程师 , 教 授 , 主要从事 智能控制 , 自主机器人方向的研究 。
Abstract:T his paper des cribes an approach of inverse ki nemat ical cont rol f or 6 DO F ort hogonal and decoupled manipulat or .Based on th e st ructu re charact eris tic of t his ki nd of mani pulat ors , t he kinematical model i s sim plified t o a 3 DO F orientat ion calcu lation and 3 D O F post u re cal culati on .T his t wo part of kinematical calcu lat ion are independent .The posi ti on and at ti tude determin at ion prob lem of t his manipulat or i s solved based on Eucli d norm .A t t he same tim e, t hi s paper t ook quat erni on as t he cont rol inpu t to s olve t he att it ude det erminati on problem .T hese m et hods can simplif y t he compu tat ion , achieve t he real tim e and accu racy requirement s , and al so easy to im plement .By anal yzing the singularit y of t he motion region of the manipulati on , a new method t o expand the mot ion regi on i s proposed by using extra DO F from rover.
θ1 的解析解 。 式 (2) 左边取欧几里德范数 : l2 ≡ ‖Q2(b※2 +Q3 b※3 +b4 u3)‖ 2 = ‖ Q2 ‖2 ‖ b※2 +Q3 b※3 +b4 u3 ‖ 2
(3)
展开得 :
l2
= a22
+b22
+a
2 3
+b23
+b24
+2b24 λ2 λ3 +2b4 b3λ3 +2b2 b3λ2 +
2) 然后 , 将式 (9) 和式 (10) 两边平方再相加得 到关于
sinθ3 、 co sθ3 的方 程 , 通 过 引 入三 角 恒 等式 , 推 导出 θ3 的 解 析解 :
K cosθ3 2 + Lsinθ3 2 + Mcosθ3 sinθ3 +N co sθ3 +Psinθ3 +Q = 0 (11)
1
θ1
0
1
2
θ2 90° 0
3
θ3 90° 0
0 L1 0
0
0 L2
0
0 L3
根据 D -H 系结构图 2 ,
可以推导出腕心
(向量
※
c
)
所在
机械臂坐标系的位置 : a※1 +Q1 a※2 + Q1 Q2 a※3 +Q1 Q2 Q3 a※4 = c※
(1)
式中 , λi ≡ cosαi ;μi ≡ sinαi ;c※ = X c Y c Z c T ;b※i =
+2b4b3 λ3 +
2b2
b
3λ2
-
X
2 c
-Y
2 c
-Z
2 c
-a21
-b21
+2b1 Z c 。
由式 (2) 第 3 个标量方 程推导 出关于 θ1 、 θ3 的 方程 , 化
简得到下式 :
Fcosθ1 +Gsinθ1 + Hcosθ3 + I sinθ3 +J = 0
(8)
式中 , F = μ1Y ;G =-μ1 X c ;H =-μ2 μ3b4 ;I =μ2 a3 ;J =b2 +b3 λ2 +λ2λ3b4 -λ1 Z c +b2 λ1 。
Key words:6 -D O F manipulat or ;i nverse kinematic con t rol ;analyti cal s ol uti on ;hi gh -accurat e
0 引言
一般 6R 机器人的实数逆运动学解 的上限为 16[ 1] , L EE[ 2] 等利用矢量乘法找到 14 个线 性无关 的逆运动 学方程 , 利用分 离变量将其化为 关节变 量半 角正切 的一 元 16 次方 程 。 M A NF RED[3] 利用多维几 何和 运动 学图 像理 论简 化了 一元 16 次方 程的获取和变量的求解 。 但是以上方法仍存在精度不够高和计 算复杂的缺点 。 刘松国[ 4] 在这些研究基础上提出基于矩阵分解 的一般 6R 机器人 实时高 精度 逆运动 学 算法 , 满 足精 度要 求 , 计算相对简单 。 Saeed B.Niku[ 5] 、 孙亮[ 6] 利用 机械 臂运 动学 模型的矩阵变换 , 将变量解耦 , 求得各关节角度解析解 。 在满 足高精度的同时 , 计算简单 , 但解耦过程复杂 。
图 1 整体结构图
· 1638 ·
计算机测量与控制
第 18 卷
图 2 D -H 坐标系
1.1 定位逆解 (求解 θ1 、 θ2 、 θ3) 运动学建模采用标准 D -H 坐标系法 , 如图 2, 参 数表如
表 1 所示 。 表 1 D -H 参数表
关节 θ α cosα sinα α d
对于解耦 6 自由度机械臂的逆运动学 , 简化为 3 自由度位 置求解 、 3 自由度方向求解 。上述 6 自由度的 算法同样 适用于 3 自由度位置逆运 动学运算 。 张敏[ 7] 在 3 自 由度机械手 运动分 析中就借鉴 了文献 [ 5] 的 算法 。 Jo rge A ng eles[ 8] 提出利 用欧 几里德范数求解解耦 6 自由度机械臂的算法 , 将位置逆运动学 解简化为关节变量半角正切的一元四次方程 。
1 逆运动学分析
解耦操作手 的逆运动学包 括腕心 的定位 逆解和定 向逆 解 , 即已知腕心在机械臂坐 标系位 置和方 向单位 矢量及 旋转角 度 , 求各关节需要旋 转的角度 。
本文研究的 B2 型月球车上驱 动相机盒 运动的 正交解 耦机 械臂 , 整体结 构图 如图 1 所 示 。 机械 臂 腕心 位置 即相 机盒 位 置 , 关节 1 ～ 3 旋转方向 两两正 交 , 控制相 机盒定 位 , 其 解耦 于控制相机盒定 向的关节 4～ 6 , 如图 2 所示 。
DO I :10.16526/j .cnki .11 -4762/tp.2010.07.067
设计与应用
计 算机 测量 与 控制 .2 010 .18(7) Computer Measurement & Control
文章编号 :1671-4598(2010)07-1637-04 中图分类号 :TP241