M otom an- UP20机器人运动学分析及求解

M otom an- UP20机器人运动学分析及求解

胡中华, 陈焕明, 熊震宇, 江淑园

( 南昌航空工业学院, 江西 南昌 330034)

摘 要: 采用 Denavit- H artenberg坐标变换法建立 M otom an- UP20 六自由度机 器人运动学模 型, 并将机器人 分解为位 置结构和 姿态 结构, 利用臂腕分离法对手臂进行逆运动学分析, 并在此基础上推导出末端执行器的逆运动学算法, 该算法计算量 最小, 误差 也较小, 且可利用机器人的示教盒进行快速检验。利用仿真软件 R otsy建立机器人及 其工作环境, 仿 真证明了该方法 的有效 性。该算法用于对开发机器人离线编程系统具有重要的作用。

关键词: 弧焊机器人; 运动学分析; 坐标变换; 仿真

中图分类号: TH113. 2⁺2 文献标识码: A 文章编号: 1007 - 4414( 2006) 05 - 0024- 03

A na lysing and resolving ofM otom an- UP20 robot k inem atics H u Zhong- hua, Chen H uan- m ing, X iong Zhen- yu, Jiang Shu- yuan

(N anchang institu te of aeronautical techno logy , N anchang J iangx i 330034, China )

Abstrac t: A space coordina te fram e based on the D - H coo rd inate transform ation m ethod is built, and the kinem atics formu las o fM otoman- U P20 6- ax is- robots are established wh ich is decomposed into position structure and stance structure, and arm - w r ist separateness me thod is used to ana ly ze the inverse kinem a tics o f the robot, and inve rse k inem atics arithm etic about the robo t is bu ilt according to the theory. T he amount o f calcu lation o f the ar ithm etic is the m inim um, so is the error. And the va- lidity o f the m e thod is proved by bu ild ing a simu la tion system in R otsy. Th is research be app licable to contriv ing o ffline - pro- g ramm ing system fo r a rc w elding robo t and is also he lpful to researching and using other robots.

K ey word s: arc w elding robot; kinem atics ana lysis; D - H coo rdina te transform a tion; sim ulation M o tom an- UP 20机器人是首钢莫托曼 公司生产的 一种新

型工业机器人, 负载 20kg, 重复定位 精度 ? 0. 06 mm。笔 者给 出了 U P20机器人运 动学正解方程及求逆解的方法: 若已知机 器人终端 操作装置 的位姿, 考虑 M o tom an- UP20机器 人位置 结构和姿态结构的特点, 采用臂腕分离法, 利用末端执 行器的 位置矢量和姿态转换 矩阵分 别求解 第 1~ 3 个关节 关于 位置 的变量和第 4~ 6个关节关于姿态的变量, 从而求出对 应关节 变量值。

图 1 M o tom an- U P20机 图 2 M otom an- U P20机

器人结构简图 器人 D - H 坐标系

1 坐标系的建立

正向运动学求解 简单且 解唯一, 是 在机 器人运 动学 方程 建立的过程中, 给定各个关节变量及杆件结构参数, 求出机器

人末端执行器 的位姿。 M o tom an- U P20 属于 关节 式机 器人, 机器人结构如图 1所示, 该机 器人的 6 个关节都是 转动关节, 前 3个关节确定手腕参考 点的 位置, 后 3个关 节确 定手腕 方 位^{[ 1]}。与大多数工业机器人一样, 后 3个关节轴线 交于一点。

关节 1的轴线为铅直方向, 关 节 2和 3 的轴线水平 且平行, 距 离为 a₂, 如图 2所示。关节 1和 2的轴线垂直相交, 关 节 3和

4的轴线垂直相交^{[ 2]}。表 1为机器人 连杆参数表。

表 1 M otoman- UP20机器人 连杆参数表

关节 坐标 n

坐标 变换

连杆参数

H_n( 初值 ) A_n a_n /mm

d_n /mm

sinA_n cosA_n

1 0 A₁ H₁( 0b ) - 90b 150 0 - 1 0 2 1 A₂ H₂( - 90b ) 180^o 730 0 0 - 1 3 2 A₃ H₃( 0b ) - 90b 140 0 - 1 0 4 3 A₄ H₄( 0b ) 90b 0 - 765 1 0 5 4 A₅ H₅( 90b ) 90b 0 0 1 0 6 5 A₆ H₆( 180b ) 180b 0 - 457. 5 0 - 1

2 运动学正解

根据 D - H 坐 标 变 换 方法, 可 得各 连 杆 坐 标的 变 换 矩 阵^{[ 3]}:

#

# 24

V o l 19 N o 5

2006-10 机械研究与应用

M ECHAN ICA L RESEARCH & A PPL ICAT ION 第 19卷 第 5期 2006年 10月

* 项 目: 航空科学基金资助项目 (编号: 02H 56007) 收稿日期: 2006- 06- 19

作者简介: 胡中华 ( 1981- ) , 男, 江西进贤人, 硕士, 研究方向: 机器人离线编程及远程控制系统。

A₁ =

c₁ 0 - s₁ a₁c₁ s₁ 0 c₁ a₁s₁ 0 - 1 0 d₁ 0 0 0 1

, A₂ =

c₂ s₂ 0 a₂c₂ s₂ - c₂ 0 a₂s₂ 0 0 - 1 0 0 0 0 1

, A₃ =

c₃ 0 - s₃ a₃c₃ s₃ 0 c₃ a₃s₃ 0 - 1 0 0 0 0 0 1

A₄ =

c₄ 0 s₄ 0 s₄ 0 - c₄ 0 0 1 0 d₄ 0 0 0 1

, A₅ =

c₅ 0 s₅ 0 s₅ 0 - c₅ 0 0 1 0 d₅ 0 0 0 1

, A₆ =

c₆ s₆ 0 0 s₆ - c₆ 0 0 0 0 - 1 d₆ 0 0 0 1 末端执行器相对 于机器人坐标系的总变换矩阵:⁰T₆= A₁A₂A₃A₄A₅A₆

末端执行器位姿 位为: T =

nx ox ax px

ny oy ay py

n_z o_z a_z p_z 0 0 0 1

, 又因为 ⁰T₆= T, 故有以下方程成立。

末端执行器位置 方程:

p_x = C₁C₂₃C₄S₅d₆- S₁S₄S₅S₆+ C₁S₂₃[ - C₅d₆+ d₄] + a₃C₁C₂₃+ a₂C₁C₂+ a₁C₁ ( 1) py = S₁C₂₃C₄S₅d₆- C₁S₄S₅S₆+ S₁S₂₃[ - C₅d₆+ d₄] + a₃S₁C₂₃+ a₂S₁C₂+ a₁S₁ ( 2) pz = - S₂₃C₄S₅d₆+ C₂₃[ - C₅d₆+ d₄] - a₃S₂₃- a₂S₂₃ ( 3) 末端执行器姿态 方程为:

n_x = C₁C₂₃(C₄C₅C₆+ S₄S₆) - S₁( S₄C₅C₆- C₄S₆) + C₁S₂₃S₅C₆ ( 4) n_y = S₁C₂₃( C₄C₅C₆+ S₄S₆) + C₁( S₄C₅C₆- C₄S₆) + S₁S₂₃S₅C₆ ( 5) n_z = - SC₂₃(C₄C₅C₆+ S₄S₆) + C₂₃S₅C₆ ( 6) o_x = C₁C₂₃(C₄C₅C₆- S₄C₆) - S₁( S₄C₅S₆+ C₄C₆) + C₁S₂₃S₅S₆ ( 7) oy = S₁C₂₃(C₄C₅S₆- S₄C₆) + C₁( S₄C₅S₆+ C₄C₆) + S₁S₂₃S₅S₆ ( 8) oz = - S₂₃(C₄C₅S₆- S₄C₆) + C₂₃S₅S₆ ( 9) ax = - C₁C₂₃C₄S₅+ S₁S₄S₅+ C₁S₂₃C₅ ( 10) ay = - S₁C₂₃C₄S₅- C₁S₄S₅+ S₁S₂₃C₅ ( 11)

a_z = S₂₃C₄S₅+ C₂₃C₅ ( 12)

式中: C

= cosH

, S

= sinH

, C

= cos( H

- H

) , S

= sin( H

- H

) 。

3 运动学逆解

运动学逆解是在 机器人 运动学 方程, 由 给定工 具坐 标系 所期望的位姿, 求出在该位姿时各关节的变量 值, 它是 离线编 程的关键, 运动学逆问题是机器人控制的基础, 逆问题 的有解 及无解、解的连续性及优化, 直接关系到 离线编程的可 行性和 正确性^{[ 3]}。笔者采 用臂 腕分 离的 方法 求运 动学 逆解。 M oto- m an- UP20机器人有各种手 臂形态 , 所 以逆 运动学 的解 不是 唯一的。采用代数解法 (分离变量法 ), 即将机器人末端的位

姿分解为臂部和腕部两部分, 一般情况下, 机 器人前 3个 关节 变量仅与机器人末端点 的位置 有关, 而 后 3个 变量 与末端 点 的姿态有关。分离变量法的原则是保证左端 仅有 1个关 节变 量, 寻找一 个恰好右端某项是常数或仅含该变量的 等式, 依次 求出各关节变量^{[ 4]}。

3. 1 求解臂关节角 H₁、H₂、H₃ ( 1) 求解 H₁

将式 T = A₁A₂A₃A₄A₅A₆左乘 A^{- 1}₁ , 右乘 A₆^{- 1}, 得:

A₂A₃A₄A₅= A^{- 1}₁ T A^{- 1}₆ ( 13) 则有:

A₂A₃A₄A₅ =

C₂₃C₄C₅+ S₂₃S₅ C₂₃S₄ C₂₃C₄S₅- S₂₃C₅ d₄S₂₃+ a₃C₂₃+ a₂C₂ S₂₃C₄C₅- C₂₃S₅ S₂₃S₄ S₂₃C₄S₅+ C₂₃C₅ - d₄C₂₃ + a₃S₂₃ + a₂S₂

S₄C₅ - C₄ S₄S₅ 0

0 0 0 1

令: A^{- 1}₁ TA^{- 1}₆ =

E₁₁ E₁₂ E₁₃ E₁₄ E₂₁ E₂₂ E₂₃ E₂₄ E₃₁ E₃₂ E₃₃ E₃₄ E₄₁ E₄₂ E₄₃ E₄₄

取其中第 4列元素得: 其中:

E₁₄ E₂₄ E₃₄ E₄₄

=

d₆( C₁a_x + S₁a_y) + C₁p_x + S₁p_y - a₁- d₆a_z- p_z

d₆( - S₁a_x + C₁a_y) - S₁p_x + C₁p_y 1

由元素 ( 3, 4) 相等得: d₆( - S₁a_x+ C₁a_y) - S₁p_x+ C₁p_y= 0 tan( H₁) = d₆a_y+ py /d₆a_x+ px

讨论H₁的解:

#

# 25 第 19卷 第 5期

2006年 10月 机械研究与应用

M ECHAN ICA L RESEARCH & A PPL ICAT ION V o l 19 N o 5

2006-10

H₁= a rctan( d₆a_y+ py, d₆a_x+ px)或 H₁= H₁+ P ( 2) 求解 H₃

将式 ( 13)两边分别左乘 A₂^{- 1}得:

A₃A₄A₅ = A^{- 1}₂ A^{- 1}₁TA₆^{- 1} ( 14) A₃A₄A₅ =

C₃C₄C₅- S₃S₅ C₃S₄ C₃C₄S₅+ S₃C₅ - d₄S₃+ a₃C₃ S₃C₄C₅+ C₃S₅ S₃S₄ S₃C₄S₅- C₃C₅ d₄C₃+ a₃S₃

- S₄C₅ C₄ - S₄S₅ 0

0 0 0 1

令: A^{- 1}₂ A^{- 1}₁TA^{- 1}₆ =

F₁₁ F₁₂ F₁₃ F₁₄ F₂₁ F₂₂ F₂₃ F₂₄ F₃₁ F₃₂ F₃₃ F₃₄ F₄₁ F₄₂ F₄₃ F₄₄

其中:

F₁₄ F₂₄ F₃₄ F₄₄

=

C₂E₁₄+ S₂E₂₄- a₂ S₂E₁₄- C₂E₂₄ - E₃₄ 1

由对应元素 ( 1, 4) ( 2, 4)相等可得:

C₂E₁₄+ S₂E₂₄- a₂= - d₄S₃+ a₃C₃ ( 15) S₂E₁₄- C₂E₂₄ = d₄C₃+ a₃S₃ ( 16) 式 ( 15)与式 ( 16) 平方后两边相加, 整理得:

E²₁₄+ E²₁₄ = a²₂+ a²₃+ d²₄+ 2a₂( a₃C₃- d₄S₃) 则 ( E²₁₄+ E²₁₄- a²₂+ a²₃+ d²₄) /2a₂= a₃C₃- d₄S₃ ( 17) 令 Q = ( E²₁₄+ E²₁₄- a²₂+ a²₃+ d²₄) /2a₂

即 Q = a₃C₃- d₄S₃, 令 R = a²₃+ d²₄, C= ? arctana₃/d₄ 则由式 ( 9)可推出:

H₃= ? arctan( Q / R²- Q²) - C或 H₃= H₃+ P ( 3) 求解 H₂

由式 ( 15) C₂E₁₄+ S₂E₂₄= - d₄S₃+ a₃C₃= Q

C₂E₁₄+ S₂E₂₄= Q + a₂, 令 V = Q + a₂, U = E²₁₄+ E²₂₄, R = ? arctanE₁₄/E₂₄

则由式 ( 9)可推出:

H₂= ? arctan( V / U²- V²) - R 或 H₂= H₂+ P 即求出决定机器人末端执行器位置的 3个关节变量。

3. 2 求解腕关节角 H4、H5、H6

各关节的姿态变换矩阵:

R₁ =

C₁ 0 - S₁ S₁ 0 C₁ 0 - 1 0

, R₂ =

C₂ S₂ 0 S₂ - C₂ 0 0 0 - 1

, R₃ =

C₃ 0 - S₃ S₃ 0 C₃ 0 - 1 0

R₄ =

C₄ 0 S₄ S₄ 0 - C₄ 0 1 0

, R₅ =

C₅ 0 S₅ S₅ 0 - C₅ 0 1 0

, R₆ =

C₆ S₆ 0 S₆ - C₆ 0 0 0 - 1

末端执行器姿态 矩阵为: K =

n_x o_x a_x n_y o_y a_y n_z o_z a_z

则⁰R₆ = R₁R₂R₃R₄R₅R₆;⁰R₆ = K 有等式成立:

R^{- 1}₃ R^{- 1}₂ R^{- 1}₁K = R₄R₅R₆ ( 18)

其中, R^{- 1}₃R^{- 1}₂R^{- 1}₁K =

( nxC₁+ nyS₁) C₂₃- ( oxC₁+ oyS₁) C₂₃- ( axC₁+ ayS₁) C₂₃- nzS₂₃ - nxS₁+ nyC₁ ozS₂₃- oxS₁+ oyC₁ azS₂₃- axS₁+ ayC₁ ( nxC₁+ nyS₁) S₂₃+ nzC₂₃ ( oxC₁+ oyS₁) S₂₃- ozC₂₃ ( axC₁+ ayS₁) S₂₃+ azC₂₃

R₄R₅R₆ =

C₄C₅C₆+ S₄S₆ C₄C₅S₆- S₄S₆ - C₄S₅ S₄C₅C₆- C₄S₆ S₄C₅S₆+ C₄C₆ - S₄S₅ S₅C₆ S₅S₆ C₅ ( 1) 求解 H₄^{[ 4]}

由元素 ( 1, 3), ( 2, 3) 相等得:

S₄S₅/C₄S₅ = ( - axS₁ + ayC₁) /[ C₂₃( - axC₁ + ayS₁) - azS₂₃]

讨论: ①当 S₅= 0 时, 则关 节 4 和关 节 6 在同 一 条直 线 上, 机器人退化为 5个自由度, 这时 H₄可 任选, 通常 取它的 当 前值, 即 H₄= 0。

②当 S₅X 0, tanH₄= ( - a_xS₁+ a_yC₁) / [ C₂₃( - a_xC₁+ a_yS₁) - a_zS₂₃] 。 ( 2) 求解 H₆

由元素 ( 3, 1), ( 3, 2) 相等得: S₅S₆/S₅C₆=

[ ( o_xC₁+ o_yS₁) S₂₃+ o_zC₂₃] /[ ( n_xC₁+ n_yS₁) S₂₃+ n_zC₂₃] tanH₆= [ ( o_xC₁+ o_yS₁) S₂₃+ o_zC₂₃] / [ ( n_xC₁+ n_yS₁) S₂₃+ n_zC₂₃]

( 3) 求解 H₅

由元素 ( 3, 2)得: S₅S₆= ( o_xC₁+ o_yS₁) S₂₃+ o_zC₂₃

由元素 ( 3, 3) 得: C₅= ( a_xC₁+ a_yS₁) S₂₃+ a_zC₂₃

则: tanH₅= [ ( o_xC₁+ o_yS₁) S₂₃+ o_zC₂₃] /{ [ ( a_xC₁+ a_yS₁) S₂₃+ a_zC₂₃] S₆}

至此, 关 于 M o tom an - UP20机器 人运动 学方程 的逆解 已 全部求解出来, 经检验符合要求。

4 结 论

建立 M otom an- U P20机器人的 D - H 坐标系及其运 动学 方程, 实现运动学 的正 解和逆 解, 该算法 具有 计算 量小、速 度 快等优点, 且有很强的实用性, 通过该坐 标所建立 的运动 学方 程的正解和逆解可直接与机器人内部 默认的坐 标系的正 解和 逆解一致, 因此, 只需通过机器人示教编 程器就可 检验运 动学 正解和逆解, 提高 了它 的正确 性、准确性 及使 用价 值, 为进 行 离线编 程 和 仿 真 奠 定 了 坚 实 的 基 础。经 机 器 人 仿 真 软 件 Ro tsy上进 行仿 真验 算, 结 果 一致 , 图 3所 示 的离 线编 程 系统

(下转第 37页 )

#

# 26

V o l 19 N o 5

2006-10 机械研究与应用

M ECHAN ICA L RESEARCH & A PPL ICAT ION 第 19卷 第 5期

2006年 10月

可知 P = F# r# 2Pn /60= 2P /60# F# r# n 在卷绕过程中若 保持张力不变, 则

P_最小 P最大

=r₁# n₁

r₂# n₂ = 100 @ 60 800 @ 300= 1

40= 2. 5%

表 1 卷绕过程中各电机的电流 /A

收辊电机电流 中间辊电机电流 放辊电机电流

6 5. 5 8

7. 5 5 7. 5

8. 5 4. 5 7

10 4 7

11 4 6. 5

12 4 6

13 4 5. 5

14 4 5

15 5 4. 5

16 4 3. 5

16. 5 4 3

17. 5 4. 5 2. 5

18. 5 4. 5 2

故在卷径变化的 过程中 若保持 张力 不变, 则电 机功 率的 变化范围在 2. 5% 。分析主要是 由于晶 闸管整流 装置中 没有 串联电感, 电机电流较大时连续, 电机电 流减小时不连 续造成 转矩脉动引起的系统 不稳定。

图 3 VT₁、VT₄导通时的回路 图 4 主回路电流临界连续的情况

3 电机电流不连续时的特性

直流 电 机 控 制 主 回 路 如 图 2 所 示, 当 VT₁、VT₄导 通, VT₂、VT₃关断时如图 3所示^{[ 3]}。

则有 u₂= u_L+ E + i_d6 R, 6 R 对 XL 而言很小, 在电流很

小时, i_d6 R 值更小, 忽 略该项, 则 L di_d

dt+ E = 2U₂sinX t, 解 此 微分方程并将X t= 0时 i_d= 0的 条件代入解得:

i_d= - 2U₂

XL ( co sXt- cosA) - E L t+EA

XL

则平均值 I_d= 1

P

Q

解得 I_d= 1 P

2U₂

XL cos A+ H

2 Hcos H

2- 2 sin H 2 , 当电 流临界连续时H= P(如图 4所示 )代入, 则

I_{dm in}= 1 2U₂

PXL cos A+ P

2 Pcos P

2- 2sin P 2

= 2 2 P

U₂

XL sinA= 0. 9 U₂ XL sinA

将工频电路中X = 314代入时, L = 2. 87 U₂ I_{dm in}sinAmH, 当A= 90b时, 需要的 L 值最大, 即 L = 2. 87U₂/I_{dm in}mH 上式中 U₂为整流变压器次级电压值 。

I_{dm in}的值按直流电动机拖 动生产 机械空 载时的 最小工 作

电流来确定。在本系 统的 工作 过程 中, 需串 联的 最小 电感 L 为 L = L_{m in}- (L_T+ L_D), L_T为变压器 漏感, L_D为电 动机的 等效 电感。若 I_{dm in}= 1. 5A, U₂= 220V 则 L_{m in}= 2. 87 @ 220 /1. 5mH

= 420mH 根据上式及 L_T、L_D的值可计算得电流连续需串联的 最小电感 L_{m in}。经计 算串 入电 感后 再进 行跑 膜实 验, 当中 间 辊电流在 1. 5A 时, 跑膜过程基 本上能 保证张 力控制 合适、收 卷薄膜边缘齐整。

4 结束语

通过以上分析表明, 单纯的提高控制回路的精 度, 而 忽略 电流不连续对电动机 转矩的 影响无 法提高 张力 控制 的精度, 串入平波电抗器后电流连续的区间增大, 电流脉动 减小, 系统 性能有很大提高。经过半 年多 的运 行表明, 控 制系 统工作 可 靠、设备运行平稳。

参考文献:

[ 1 ] 周 志文. 卷绕 式镀 膜 机中 的张 力控 制 [ J ]. 机 械研 究 与应 用, 2003( 1 ): 48- 49.

[ 2 ] 顾绳谷. 电机及拖动基础 [ M ] . 北京: 机械工业出版社, 1997.

[ 3 ] 王兆安. 电力电子技术 [ M ] . 北京: 机械工业出版社, 2000.

( 上接第 26页 )

中的运动学逆解模块 就是按照此算法编程实现的。

图 3 M o tom an- U P系列机器人运动学逆解模块 界面

参考文献:

[ 1 ] Pau,l Sh im ano B, M ayer G. K inem at ics control equat ion s for sim p le m an ipu lators [ J] . IEEE Tran s SM C, 1981, 11( 6) : 449- 455.

[ 2 ] 张 福学. 机 器人 技术 及其 应用 [ M ] . 北 京: 电子 工业 出版 社, 2000.

[ 3 ] 王 朋, 武传宇. Staub li RX 60机器人运动 学求解 [ J] . 浙 江理工 大学学报, 2005, 22( 3) : 245- 250.

[ 4 ] 殷树言, 卢振洋. 弧焊 机器人系 统的运 动学求 解 [ J ]. 北京 工业 大学学报, 2002, 28( 3) : 381- 384.

#

# 37 第 19卷 第 5期

2006年 10月 机械研究与应用

M ECHAN ICA L RESEARCH & A PPL ICAT ION V o l 19 N o 5

2006-10