1、陈志煌等 No.: 0610 漂浮基双臂空间机器人闭环运动的建模及控制 陈志煌 1, 陈 力 2 1 福州大学机械工程学院，福州 350002 2 福州大学机械工程学院，福州 350002 摘要 讨论了载体位置、姿态均不受控制的双臂空间机器人抓物系统的动力学建模和控制问题。利用拉格朗日方法和牛顿 欧拉法分别建立了双臂空间机器人及负载的非线性动力学模型，结合空间机器人固有的特性及闭合链约束关系，得到抓 持系统合成动力学方程。以此为基础，考虑到空间机器人系统结构的复杂性及其某些参 数 的 变 动性， 根据具 有 较强鲁棒 性的 变 结构控制 理 论， 设计 了 该 抓持系统 惯 性空间 轨迹跟踪
2、的 滑 模 变 结构控制方 案 及 相应 的 内 力控制方 案 ， 从而达 到位 置、力的 混 合控制。系统 数值仿真 ， 证明 了 上述 控制方 案 的有 效 性。 关键词 漂浮 基双臂空间机器人， 滑 模 变 结构控制， 内 力 Dynamics and Control for Dual-arm Space Robot with Closed Loop Chen Zhi-huang 1 , Chen Li 21 The College of Mechanical Engineering, Fuzhou University, Fuzhou 350002, China 2 The Colle
3、ge of Mechanical Engineering, Fuzhou University, Fuzhou 350002, China Abstract In this paper, a kind of nonlinear dynamics model of free-floating dual-arm space robot systems is presented based on Lagrange method, and the dynamics model of object is presented based on Newton-Euler method. Based on t
4、he results, the dynamics model of synthetical system is obtained, and the control problems for object to track the desired trajectory in workspace and inner force are discussed. Because of the high structure complexity and the parameter uncertainty of space manipulator systems, the scheme of variabl
5、e structure sliding-mode control with better robustness to uncertainty and disturbance is proposed to track the desired trajectory of object. Then, the corresponded scheme of inner force control is proposed. The effect of the controllers is testified by computer simulation. Key words Free-floating d
6、ual-arm space robot, Variable structure sliding-mode control, Inner force 1 引言 随着太 空 科技 的 发展 ，人 类需要在太 空 中从事 的 科 学 研究越来越多 。 但是太 空 环境恶劣 ， 送 人到 太 空 的成 本昂贵 ， 风险太 大， 因 此空间机器人 应运而生 。 从二十世纪九十年代开始 ，机械臂 在航天 器 中 的 应 用 已进入实际试验使 用 阶段 ， 因 此关 于它 的 运 动学、动 力学 与 控制问题的 研究 受到人 们 的 广 泛重视 1-6 。 然 而 值 得 注意 的 是 ， 目前 研究 工
7、作 大 部 分 是 针对单 臂空间 机器人系统的。 出 于 增 大负载 能 力、 提高 载 荷定 位 精 度 及 多 任务操作 的 需 求 ，双臂 甚至 多 臂空间机器人的 使 用 是 必 不 可免 。 目前 相 关的 研究 工 作 7-10 ， 多 见 于运 动学 规划级 ，有关控制问题的 研究开展 得 并 不 多 。 由 于在 双臂空间机器人控制系统的 实际设计中 ， 主 要 问 题 是 确定 机械臂关 节 力 矩 的控制 输 入 规律 ，以 使 机械 臂 完 成关 节或 惯 性工 作 空间的的 期望 轨迹 ， 因 此空间 基金项目：国家自然科学基金项目（ 10672040） E-mail
8、: 机器人 轨迹跟踪 控制系统的 设计 问题 是 非 常重 要 的。 本 文 讨论了载体位置、姿态均不受控制的双臂空 间机器人抓物系统的动力学建模和控制问题。利用拉 格朗日方法和牛顿欧拉法分别建立了双臂空间机器人 及负载的非线性动力学模型，结合空间机器人固有的 特性及闭合链约束关系，得到抓持系统合成动力学方 程。以此为基础，考虑到空间机器人系统结构的复杂 性及其某些参 数 的 变 动性， 根据具 有 较强鲁棒 性的 变 结构控制 理 论， 设计 了 该 抓持系统 惯 性空间 轨迹跟踪 的 滑 模 变 结构控制方 案 及 相应 的 内 力控制方 案 ， 从而 达 到位置、力的 混 合控制。系统
9、数值仿真 ， 证明 了 上 述 控制方 案 的有 效 性。 2 动力学方程 如图 1所示 ， 设 平面 双臂空间机器人抓持系统 由 自由 浮 动的载体 0 B 和机械臂 1 B、 2 B 、 3 B 、 4 B 、 5 B 、 6 B 及负载 c B 组 成。建立 平 动的 惯 性 坐标 系 ( Oxy )， 并 以 各 分体 质心 i O为基 点 建立 连 体基 坐标 系 ) ( i i i y x O ， 第 916 页陈志煌等 No.: 0610 i x沿各 分体 主轴 方 向 ，其 单 位 矢量 为 ix e ， i y的 单 位 矢 量 为 iy e ；各 分体 质心 i O相 对
10、于惯 性 坐标 系 原点 O的 矢 径 为 i r 。 各 分体的 质量 和 转 动 惯 量 分别为 i m和 i J c) , 6 , , 1 , 0 ( L = i ； 载体 质心 0 O 到 左右两端第一个铰点 的 距离 在 0 x 轴 的 投影长度 均为 0 d ， 在 0 y 轴 的 投影长 度 均为 h； 机械臂 i B在 i x轴 的 长度 为 i l ) 6 , , 2 , 1 ( L = i ； 负载 质心 c O到 两爪手 的 距离 均为 c d 。 图 1 双臂空间机器人抓持系统 由 系统 各 分体的位置 几何 关系， 可 得 ： ) 5 . 0 5 . 0 ( 5 .
11、0 5 . 0 5 . 0 5 . 0 5 . 0 5 . 0 5 . 0 5 . 0 5 . 0 5 . 0 5 . 0 6 6 6 3 3 3 c 6 6 5 5 5 6 5 5 4 4 4 5 4 4 0 0 0 0 4 3 3 2 2 2 3 2 2 1 1 1 2 1 1 0 0 0 0 1 x x x x x x x y x x x x x x y x l l l l l l l h d l l l l l h d e r e r r e e r r e e r r e e e r r e e r r e e r r e e e r r + + + = + + = + + = +
12、+ - = + + = + + = + + + =(1) 由 拉格朗日方程 可 得，双臂空间机器人系统 如下 形式 的 欠驱 动动力学方程 ： ( ) F J 0 q q C q q D T T T T ) , ( ) ( - = + & & & (2) 其 中 ， ( ) T T r T b q q q = ， T 0 0 0 b ) ( q y x = q ， T 6 5 4 3 2 1 r ) ( q q q q q q = q 为 9 阶 的系统 广 义 坐 标 列阵 ； 9 9 ) ( R q D ，为 对 称 、 正 定 的系统 质量矩 阵 ； 1 9 ) , ( R q q q
13、C & & ，为 包含哥氏 力和 离心 力的 广 义 力 列 向 量； T 6 5 4 3 2 1 ) ( t t t t t t = 为机械臂 六 个 关 节 铰 的控制力 矩组 成的 6阶 列阵 ； T J 为 9 6的力 雅 可 比 矩 阵 ； T T L T R ) ( F F F = ， T R R R R ) ( M F F y x = F ， T L L L L ) ( M F F y x = F ，为 两 末 端爪手作 用 在 负载 上 的力 矩 阵 ， x F R 、 y F R 及 R M 表 示右端爪手 施加 在 负载 上 的力及力 矩 ， x F L 、 y F L 及
14、 L M 表 示左端爪手 施加 在 负载 上 的力及力 矩 。 对式 (2)进 行 分 解 可 得 ： F J 0 I q q C 0 I q D D T 1 1 12 11 ) ( ) , ( ) ( ) ( - = + & & & (3) F J I 0 q q C I 0 q D D T 2 2 22 21 ) ( ) , ( ) ( ) ( - = + & & & (4) 其 中 ， 11 D 和 22 D 分别为 3 3和 6 6的 对 称子 矩 阵 ； 12 D 和 21 D 分别为 3 6和 6 3子 矩 阵 ， 且 有 T 21 12 D D = ； 1 I 和 2 I 分别为
15、 3 3和 6 6的 单 位 阵 。 由式 (3)可 得 ： ) ( ) , ( ) ( T 1 1 r 12 1 11 b F J 0 I q q C 0 I q D D q - - - = - & & & & & (5) 将 式 (5)代入 式 (4)， 可 得机器人系统 完 全 驱 动的动 力学方程 ： F J q q N q q M T e r ) , ( ) ( - = + & & & (6) 其 中 ， 22 12 1 11 21 D D D D M + - = - ，为 对 称阵 ； C 0 I D D I 0 N ) ( ) ( 1 1 11 21 2 - - = ； T 1
16、1 11 21 2 T e ) ( ) ( J 0 I D D I 0 J - - = 。 由 牛顿欧拉法 可 得负载 如下形式 的动力学方程 ： F J x M T ce c c = & & (7) 其 中 ， ) ( diag c c c c J m m = M ， 为 正 定 、 对 称 质量矩 阵 ； T c c c c ) ( q y x = x ， 为负载的 广 义 坐标 列阵 ； T ce J 为 3 6的抓持 矩 阵 ， 即 两爪手 施加 在 负载 上 的 广 义 力 等 效 到负载 质心 的 转 换 矩 阵 。 由式 (7)可 得 ： I c c T ce ) ( F x M
17、 J F + = + & & (8) 其 中 ， 1 ce T ce ce T ce ) ( ) ( - + = J J J J ，为 T ce J 的 伪逆阵 ； c c T ce ) ( x M J & & + 为为 操作 力 项 ； I F为 内 力 项 11 ， 定 义 在 T ce J 的 零 空间 内 ， 即 0 F J = I T ce 。 假 定 末 端爪手 与 负载 刚 性 接触且无 相 对 运 动， 则 有 ： c ce x J q J & & = (9) 将 式 (9)两 边 对 时 间 求 导 并 整 理 ，得 ： ) ( c ce c ce q J x J x J J
18、 q & & & & & & & & - + = +(10) 其 中 ， 1 T T ) ( - + = JJ J J ，为 J的 伪逆阵 。 由式 (10)可 得 ： ) ( ) ( c ce c ce 2 r q J x J x J J I 0 q & & & & & & & & - + = +(11) 将 式 (8)、 (11)代入 式 (6)并 简化 可 得抓持系统合成 动力学方程 ： I T e c c c F J B x A - = + & & (12) 其 中 ， c T ce T e ce 2 c ) ( ) )( ( M J J J J I 0 q M A + + + = ，
19、 ) , ( ) ( ) )( ( c ce 2 c q q N q J x J J I 0 q M B & & & & & + - = + 。 实际上 ，关 节铰驱 动力 矩 由 负载位姿控制力 矩 P 和 内 力控制力 矩 I 两部 分 组 成 12 ， 即 I P + = (13) 第 917 页陈志煌等 No.: 0610 其中， c c c p B x A + = & & ， I T e I F J = 。 3 控制方案设计 设 T d c d c d c d c ) ( q y x = x 为负载 在惯 性空间的 期望 运 动 轨迹 ， d c c x x e - = 为位姿 跟踪
20、 误差 ， d c c x x e & & & - = 为 速 度 跟踪 误差 ， 并定 义 如下形式 的 切换函 数 ： e e s + = & (14) 其 中 ， ) diag( 3 2 1 l l l = ， ) 3 , 2 , 1 ( 0 = i i l 。 以 P 为控制 输 入 ， 由 到 达 条件 可 得 ： e x B A e x x e e s & & & & & & & & & & & & + - - = + - = + = + d c c P c d c c ) (15) 其 中 ， 1 T c c T c c ) ( - + = A A A A 为 c A 的 伪逆阵
21、 。 取 如下形式 的 趋近 律： ) sgn(s s - = & (16) 由式 (15)、 (16)可 得 如下 的负载位置、姿态的 滑 模 变 结构控制方 案 c d c c P ) sgn( B s e x A + - - = & & & (17) 显 然式 (17)满足 ) 3 , 2 , 1 ( 0 = i s s i i & ， 根据滑 模 变 结构控制 理 论 13 ， 该 控制方 案 将 使 负载的位置、姿态 稳 定 地趋 近 于 期望 的 运 动 规律 。 设计 如下 的 内 力控制 规律： d I T e I F J = (18) 其中， d I F 为内力的期望值，定义
22、在 T ce J 的 零 空间 内 ， 即 0 F J = d I T ce 。 下面 讨论 内 力 I F的 收敛 性， 把 式 (17)、 (18)、 (13) 代入 式 (12)整 理 得 ： ) ( ) sgn( d I I T e d c c F F J s e x A - = - - & & & (19) 假设 e J 可逆，式 (19)两边同时左乘 T -1 e T ce ) (J J ，并整理 得： ) ( ) sgn( ) ( d I I T ce d c c T -1 e T ce F F J s e x A J J - = - - & & & (20) 若 c T -1
23、e T ce ) ( A J J 满秩，由式 (20)可得： 0 s e x = - - ) sgn( d c & & & (21) 把式 (21)代入式 (19)整理得： 0 F F = - d I I(22) 式 (22)表 明 ： 当时 间 t 时 ， d I I F F ， 即 I F收敛 于 d I F 。 4 数值仿真 以 作平面 运 动的 自由 漂浮 双臂空间机器人抓持系 统为 例 ，系统结构 见图 1。 设 载体的 质量 0 m =200 kg， 惯 量矩 0 J =200 2 m kg ， 0 d =1.5 m， h =0.1 m； 机械臂 各 杆 的 杆 长 6 5 4 3
24、 2 1 l l l l l l = = = = = =1 m， 各 杆 的 质量 4 1 m m = =15 kg， 5 2 m m = =10 kg， 6 3 m m = =5 kg， 各 杆 的 惯 量矩 4 1 J J = =0.3 2 m kg ， 5 2 J J = =0.2 2 m kg ， 6 3 J J = =0.1 2 m kg ； 负载的 质量 kg 10 c = m ， 惯 量矩 2 c m kg 15 = J ， m 5 . 0 c = d 。 仿真 时 ，负载姿态的 期望 运 动 规律 为 0 d c = q (单 位 ： rad) 即 负载姿态 保 持为 0不 变
25、 ； 同时 负载位置 在惯 性空间 的 期望 轨迹 为 ) / 2 cos( 2 . 0 3 . 1 d c T t x + = (单 位 ： m)， ) / 2 sin( 2 . 0 8 . 0 d c T t y + = (单 位 ： m) 内 力的 期望 值 为 T d I ) m 0N 0N N 10 m 0N 0N 10N - ( = F 图 2 负载姿态 变 化曲 线 其 中 ， s 4 = T 为 整 个 追 踪 过 程 所 用 时 间 ；图 2 为 按 式 (17) 控制 规律 得到的负载姿态 实际运 动 与 期望 运 动的 对 比 情况 ；图 3为 按 式 (17)控制 规律
26、 得到的负载位置 在惯 性空间的 实际轨迹与 期望 轨迹 的 对 比情况 ；图 4、 5 为 按 式 (18)控制 规律 得到的合成系统 内 力 实际值与 期望 值 的 对 比情况 。 仿真中 ，负载姿态 角 的 初 始值 为 0.1rad， 负载 在惯 性空间的 初 始 位置为 (1.4002m,0.9950m)。 5 结论 漂浮 基空间机器人抓持系统 高度 非线性、 强 耦 合 等 特 点 ，其动力学模型 远比地 面 机器人抓持系统复杂 ； 对 其动力学建模 与 控制的 研究具 有 重 大的 理 论和 实 践 意 义 。 本 文 首先 对 双臂空间机器人抓持系统的合成动 第 918 页陈志
27、煌等 No.: 0610 力学模型 进 行 了 推导 ， 随 后 设计 了负载位置、姿态的 滑 模 变 结构控制方 案 及 相应 的 内 力控制 规律 ， 达 到位 置、力 混 合控制的 效 果 ； 仿真 结 果 证实 了 所提 控制方 案 的有 效 性 与 可 行 性， 同时表 明 了 需要 采取 相应 的 措 施 来进 一 步 提高所 设计 控制器的 精度 。 图 3 负载位置 运 动 轨迹 图 图 4 右端爪手 内 力 变 化曲 线 图 5 左端爪手 内 力 变 化曲 线 参考文献 1. Lindsay Evans. Canadian space robotics on board th
28、e international space station. In: 2005 CCToMM Symposium on Mechanisms, Machines, and Mechatronics. Canadian Space Agency, Montreal, Canada, 2005,26-27. 2. Papadopoulos E G, Dubowsky S. On the nature of control algorithms for free-floating manipulators. IEEE Trans. Robot. Autom., 1991, 7(6), 750-758
29、. 3. Agrawal O P, Xu Y S. On the global optimum path planning for redundant space manipulators. IEEE Transaction on Systems, Man and Cybemetics, 1994, 24 (9), 1306 1316. 4. 陈力 ,刘延柱 .漂浮 基空间机器人 协调 运 动的 自 适 应 控制 与鲁棒 控制 . 机械工程学 报 ， 2001, 37(8), 18-22. 5. 陈力 . 自由 浮 动空间机器人系统 联 体 坐标 系 内轨迹 的 自 适 应跟踪 控制 . 机械
30、 科 学 与技 术 ， 2003, 22(1), 32-34. 6. 陈力 . 带 滑 移 铰 空间机械臂 协调 运 动的 自 适 应与鲁棒混 合控制 . 空间 科 学学 报 ， 2002, 22(1), 82-90. 7. Gary E Yale and Brij N Agrawal. Lyapunov controller for cooperative space manipulators. Journal of Guidance, Control and Dynamics, 1998, 21(3), 477-484. 8. Yasuyoshi Yokokohji, Takeshi To
31、yoshima and Tsuneo Yoshikawa. Efficient computational algorithms for trajectory control of free-flying space robots with multiple arms. IEEE Transactions on Robotics and Automation, 1993,9(5), 571-580. 9. 郭 益深 ，陈力 . 双臂空间机器人系统 运 动 规划 的双 向 逼 近 方法 . 空间 科 学学 报 ， 2005, 25(6), 564-568. 10. 唐晓腾 ，陈力 . 双臂空间机
32、器人姿态 调整 运 动的 最优 控制 规划 . 空间 科 学学 报 ， 2006, 26(5), 403-408. 11. Jian M. Tao and J.Y.S. Luh. Robust position and force control for a system of multiple redundant-robots. Proceedings of the 1992 IEEE Inernational Conference on Robotics and Automation, Nice, France-May 1992, 2211-2216. 12. Yan-Ru Hu, George Vukovich. Dynamic control of free-flying coordinated space robots. Proceedings of the 34 thConference on Decision & Control, New Orleans, LA-December 1995, 3898-3906. 13. 高为炳 . 变结构控制理论基础 . 北京：中国科学技术出版 社 , 1990. 第 919 页
