多臂协调操作自由飞行空间机器人的位置和内力混合控制(1)

的, 自由飞行空间机器人 ( Free Flying Space Robot 简 称 FFSR) 的发展和应用成为必然。多臂空间机器人 具有单臂空间机器人所不具备的优点因而引起许多 国家的重视
[ 1]
。E. Papadopoulos 等对单臂 FFSR 的控
[ 2]
制问题和控制系统的稳定性问题进行了论述 。而 对安装于一个空间飞行器上的多个机械臂进行协调 控制的问题比单臂 FFSR 的控制更困难 。特别是 当 FFSR 的多个机械臂同时抓住同一个负载并使负 载按照预定的轨迹运动时, 便形成了一个或多个封 闭的运动链, 此时各个机械臂的协调操作尤为重要。 FFSR 的各个机械臂的机械手对负载的作用不仅使 负载运动, 同时在负 载内部产生应 力。在 FFSR 执 行某一个任务过程中, 如果内力不受控制, FFSR 的 关节力矩所产生的力可能损坏负载或 FFSR 系统自 身
0
引言 由于地球外层空间对宇航员来说是异 常危险
的机械手牢固地抓住负载, 与负载没有相对运动 ; 整 个 FFSR 系统不受外力作用。设每个机械臂都有 6 个自由度, 所有机械臂完全相同; 多臂 FSSR 的本体 的位置和姿态均不受控制。建立的 FFSR 系统的坐 标系系统如图 1 所示。其中为惯性坐标系。
( 5)
图2
多臂协调操作 FFSR 控制系统结构图
Fig. 2 Control system structural diagram of multi- arm coordinated FFSR
第 4期
袁景阳等 : 多臂协 调操作自由飞行空间机器人的位置和内力混合控制
1051
2
稳定性分析 当多臂 FFSR 系统的机械手抓住负载时, 便形成
i
了一个甚至是多个封闭的运动链。当多臂型 FFSR 系 统形成封闭运动链后, 要求各个机械臂协调操作 , 此 时各个机械臂之间存在比单臂 FFSR 更为严重的动力 学奇异问题。这种奇异问题将导致控制系统的不稳 定 , 因此必须研究控制系统的稳定性问题。 将式 ( 1) 代入 ( 2) , 利用( 5) , 可得到多臂 FFSR 系 统的控制系统的闭环方程 : ( M c + W J ce J W MJ e J ce W) [ & xc - & x c) + Kv ( x Ûc - x Ûc ) + Kp ( xd - xc ) ] + W
T - T i T - T +T - 1 T - T + T - 1
F b0 + ( X ÛN
+ D ÛJs ) Û H] ; F = [ [ F ] , [ F ] ] ; F ( j = 1, , K) 是第 j 机械臂的机械手施加于负载的广义力。负载 mc E 3 @3 O 的惯性矩阵为 M c = ; 负 载的离心 力 O Ic 与科氏力组成的列向 量为 Cc = = [ E 1 , E K] 。 定义 F 和 F , 分别是由第 j 个机械臂手产生的 期望内力和实际内力 , 并且可以由下面的式子表示 : i T T e F dj = ( [ O , Ej , O ] - E j A ( 3) j W ) J ceF d F = ( [ O , E j , O] ) - E j A jW ) J F 其中 E j , 是 6 @6 阶单位矩阵。 由式( 3) , ( 4) 中内力的定义 , 内力向 F 量可以 表示为 ( E - W W ) J ce F , 显然它属于 W 的零空 间。因此 F 可以分成两部 分: 即 J
第 28 卷第 4 期 2007 年 7 月
宇
航
学
报
Journal of Astronautics
Vol. 28 July
No. 4 2007
多臂协调操作自由飞行空间机器人 的位置和内力混合控制
袁景阳 , 柳长安 , 李瑰贤
1 2 1
(1. 哈尔滨工业大学机电工程学院, 哈尔滨 150001; 2. 华北电力大学计算机学院 , 北京 102206)
T T ce e e - T ce + T T T e T i i j T T ce e i dj i j T
e j
O Xc @I c @ Xc
; W
( 6)
负载的位置是通过位置和速度的误差反馈信号 控制。所受到的内力是通过内力误差信号的比例和 积分反馈方法进行调解 , 控制算法如下式所示: x & c = x & c + Kv ( Û xc - Û x c ) + Kp ( x c - x c )
i i i i T - T T - T + T - 1 d p d p d i p p i i i i i i i p p p p d d d
Q
i
i
W
+ T
WJ F
T T ce
e
是对负载运动直接做出贡献的部分, J
- T ce
( E- W
+ T
W ) J F 表示不对负载的运动做出贡献的 内力部 分。类似地 , 机械手与 负载的作用 力的期望 值 F 也可以分成对负载的运动直接做出贡献的部分 J
T T
N X Ms XN 是机 械臂惯性 矩阵; J = N M sM VQ 是
基金项目 : 国家自然科学基金资助 ( 60305009)
1050
宇航学报
第 28 卷
力雅可比矩阵; 机械臂系统的离心力和科氏力构成 的向量为 : C = N 5 Ms [ M F b - DM
e e 1 T e T K T T T - 1 V - 1 0
i i i i i d p d p d
( 7)
i
( 4)
F = F d + Kp ( F d - F ) + Ki ( Fd - F ) d t ( 8) 此处 x c 、 x Ûc 和 & x c 分别是负载的位置 , 速度和加速度 的期望值; Û x c 和 xc 分别是负载的速度和位置的实际 检测值。 Kp = kp E 、 Kv = kv E 分别是负载位置误差 和速度误差反馈增益矩阵。 Kp , Ki 分别是内力误差 反馈的比例和积分增益矩阵 , 此处 , Kp = kp E , Ki = ki E 、 kp 、 kv 、 kp 和 ki 都是正标量。将式 ( 7) , ( 8) 代入 式( 6) 可以得到 S= Sp + Si ; 此处 Sp 和 Si 分别是负 载位置和内力控制算法 , 控制算法的具体形式分别 表示如下: T - T +T - 1 Sp = ( J J ce W M c + MJ e W) [x & Ûc - x Ûc ) + Kp ( xc - c c ) ] + C + c + Kv ( x J J ce W Cc - MJ e ( Û J eÛ H- Û J ce WÛ xc ) Si = J J ce [ K i ( F d - F ) d t + Kp ( F d - F ) + F d ] 系统模型如图 2 所示。
式( 11) 要求矩阵 J 可逆 , 从矩阵 J 的定义可以 知道它是同 FFSR 的本体和所有的机械臂的动态参 数相关。在式( 9) 中要求可逆。因此本文定义具有 封闭链的多臂协调操作 FFSR 的动力学奇异状态为 矩阵 J 或矩阵 J e 为奇异矩阵 , 或者两者都为奇异矩 阵时的状态。如果假设多臂协调操作 FFSR 不处于 奇异状态 , 则负 载的位 置和 内力的 动态 误差 由式 ( 13) 和 ( 16) 决定。适当选择反馈增益可以使负载的 位置误差和内力误差逐渐收敛到零。 3 仿真实验分析 ( 11)
其中 J ce 是将负载的速度转换为机械手速度的雅可 比矩阵。 将式( 5) 代入式 ( 1) 并利用式( 2) , 结合上述内力 的定义, 可以得到 FFSR 系统的机械臂 关节的驱动 力矩计算公式: S = ( J J ce W Mc + MJ e W) & xc + J J ce W Cc + C - MJ e (Û J eÛ H ce W x c ) + J J ce F Û
T T 1 6 T T ce e [ 6]
制负载的位置和方向变量和内力 ( 包括内力和内部 弯矩 ) 。 1 控制系统建模 假设 FFSR 的所有 部件都是刚体 ; 各个机 械臂
收稿日期 : 2006 203 214; 修回日期 : 2006 212 226
( 1) ( 2)
SK]
T
是关节驱动力矩向量; M =
+ T T T e e d - T
W W J ceF d , 以及使负载产生内部应力的内力部 分: 和 J ce ( E T - T
W
+ T
W ) J ce F d 。 W = [ A 1 E1 ,
T
T
e
+
T
AKE K] , E j 是 6 @6 阶的单位阵; A j 是多臂 FFSR 第 j 机械臂的负载分配因子, 并且满足条件 :
[ 4,5] [ 3]
图 1 FFSR 的坐标系系统 图 Fig. 1 Coordinate system of multi 2 arm FFSR
。多臂型 FFSR 在对负载进 行操作要同时 控
根据多刚体动力学建立的多臂 FFSR 的机械臂 系统动力学方程和负载的动力学方程为 : T e M& H+ C = S - J F Mc & x c + Cc = W J F 其中 : S= [ S1 ,
i i i i p d p d T T T T + T - 1 d [7]
为验证多臂 型 FFSR 协调 控制系统 , 本文对平 面双臂 6 自由度 FFSR 系统进行了仿真研究 , 该 FF2 SR 的模型每个机械臂有三个连杆, 靠近 FFSR 本体 的两个机械臂的两级连杆的长度均为 m; 两个机械 臂第三级连杆的长度均为 1m; FFSR 械臂、 目标物体 和本体的质量为 : mij = 10kg( i , j = 1, 2) , mi 3 = 5kg; mc = 20kg; m0 = 100kg。 利用状空间法建立了控制算法的微分方程 , 利 用控制系统仿真软件 Simulink 在计算机上进行了仿 真分析。设定负载的期望位姿方程以及目标负载的 位姿初始条件为: [1 15+ 012cos( 2t ) 1. 0+ 0. 2cos( 2t )0. 2cos( 2t ) ] T 和[ 118 018 0] 。负载的期望内力和内力初始值分 别为: [ 10 715 5] 和[ 0 0 0]
p i T T T
[ F d + Kp ( F d - F ) + K i ( F d - F ) d t ] = 0 由于负载的期望和实际内力可以分别表示为 : ( E W
+ T
Q
i + Tຫໍສະໝຸດ Baidu
i
W ) J F 和( E + T
T
T ce
e
W
W ) J F , 又由 于
T
T ce
e d
W W = E , 所以式 ( 11) 的左侧最 后一项是零 , 因 此式 ( 11) 可以写成 : [ Mc + W J J W MJ J ce W] [& ep + K Û e + Ke ]= 0
K j= 1
( 9)
E
A j = E 6@ 6
由于多臂 FFSR 系统机械臂的机械手与负载没 有相对运动 , 驱动电机的角加速度用负载的加速度 和速度以及电机的转速表示: H = J e [ J ce W & x & c+ J Û ce W xc - Û Û J eÛ H]
- 1
Q
i
i
i
( 10)
根据式 ( 9) 和式 ( 10) 建立多臂 FFSR 系统的控制
摘
要 : 基于多臂 自由飞行空间机器人多臂协调操使 负载沿 着期望 的轨迹 运动并 且控制负 载所受 的内力 为
目的 , 在多臂自由飞行空间机器人系统协调操作的动 力学方 程基础 上 , 推 导了各 个机械 臂协调操 作负载 按期望 轨 迹运 动时各个机械臂关节的驱动力矩的计算方法 ; 给出了 作用在负载的内力的定义 , 根据关节力矩 计算方法和 PID 反馈控制原理 , 建立了多臂自由飞行空间机器人协调 操作负 载时的 位置和内 力的控 制算法 ; 讨论 了所提 出的控 制 算法 的稳定性问题 , 得到了负载的位置误差和内力误差的 约束条件。通过仿真实验证明该 控制算法 能够使负载 的 实际位姿和内力收敛到期望的轨迹和内力。 关键词 : 多臂自由飞行空间机器人 ; 协调操作 ; 位置控制 ; 内力控制 中图分类号 : TP24 文献标识码 : A 文章编号 : 1000 2 1328( 2007) 0421049 2 04