自动化学报
ACTA AUTOMATICA SINICA
1998年 第24卷 第1期  Vol.24  No.1 1998



自由飞行空间机器人系统的协调运动控制
马保离　霍　伟
摘　要　考虑由载体和机械臂组成的空间机器人系统的协调控制问题，提出了一种新的协调控制策略.该策略首先利用简单的变结构控制器粗略控制载体的运动，进而设计机械臂控制器以保证手端精确跟踪其期望的运动轨迹.应用该策略分别对手端自由运动和受限运动设计了相应的控制器，并对两杆平面空间机器人系统进行了仿真，证实了控制策略的有效性.
关键词　空间机器人，协调运动控制，受限运动控制，变结构控制.
COORDINATED MOTION CONTROL OF FREE-
FLYING SPACE ROBOT SYSTEM
MA BAOLI　HUO WEI
(The Seventh Research Division, Beijing University of Aeronautics and Astronautics, Beijing　100083)
Abstract　The coordinated motion control of manipulator/spacecraft system is investigated in this paper. A new coordinated control scheme is proposed, in which the motion of the spacecraft is controlled using a simple variable structure control scheme and the manipulator is controlled to track the desired end-effector trajectory. The proposed scheme is applied to the controller design of free-motion and constrained motion of the manipulator′s end-effector. A two-link planar space robot system is simulated to verify the effectiveness of the control scheme.
Key words　Space robot, coordinated motion control, constrained motion control, variable structure control.
1　引言
　　对于由载体和机械臂组成的自由飞行空间机器人系统(free-flying space robot system),由于载体本身带有位置和姿态控制系统，因此仅仅通过控制载体的运动就可使机械臂手端向任意方向以任意姿态运动.然而由于载体携带的控制燃料有限，补充起来代价极高，不少人对载体不加控制的自由浮动空间机器人系统(free-floating space robot system)进行了大量的研究［1－2］.虽然这种自由浮动空间机器人系统可不用载体的控制燃料，但是其工作空间却极为有限，一般难以满足工作任务的需要.另外，当机械臂手端与环境接触时，如不对载体施加控制，系统将会在环境外力的作用下，很快达到其奇异点并与环境脱离接触，无法完成给定的任务.因此有必要对载体和机械臂的控制进行协调，以在满足手端工作任务的同时，尽量节省载体控制燃料.
　　Spofford等人提出了一种运动学切换协调控制方案［4］，其基本思想是当与机械臂构型有关的性能指标函数(如机械臂的灵活度指标等)大于某一阀值时，对载体和机械臂均施加控制；而当性能指标小于该阀值时，不对载体施加控制，以节省载体控制燃料.该方案的缺点是没有考虑系统的动力学，另外系统性能指标的选择也存在一定的任意性.
　　Egeland和Sagli等人提出了一种动力学协调控制策略［3］，该策略首先根据期望的手端任务轨迹规划载体的期望运动轨迹，然后把手端任务向量和载体的位置和姿态向量作为扩展的任务向量，并将系统的动力学方程以扩展的任务向量表示，进而应用计算力矩法将此动力学方程解耦为独立的线性系统，然后对载体和手端的运动配置不同位置的极点，以使手端的运动能快速跟踪期望的手端运动轨迹，而载体的运动仅缓慢粗略地跟踪期望的载体运动轨迹.这样既可使机械臂保持在最佳构型附近，又可使载体的控制燃料消耗较少.然而上述控制策略有以下缺点：(1)关于扩展任务向量的系统动力学方程的维数较高(它是载体的位姿和手端任务向量的维数之和)，控制器计算量大. (2)载体控制燃料消耗仍较多，因为即使载体的跟踪误差已为零，但是由于要对系统进行精确线性化，仍需消耗一定的载体控制燃料.
　　为克服以上缺点，我们提出了一种新的协调控制策略，其基本思想可概括如下：
　　(1) 根据期望的手端任务轨迹规划期望的载体运动轨迹，使机械臂总保持最佳构形.
　　(2) 将载体运动近似看作线性二阶系统，机械臂对载体的作用力和力矩作为扰动.利用简单的分散变结构控制策略控制载体的运动，以粗略跟踪期望的载体运动轨迹.为了节省控制能量，当跟踪误差在边界层内或正在向边界层运动时，不对载体施加控制.
　　显然，按以上思想所设计的控制器较好的克服了文［3］中控制策略的缺点.下面我们将对手端自由运动和受限运动两种情况分别设计位置和位置/力混合控制器，并给出对两杆平面空间机器人系统的仿真结果.
2　自由运动控制
　　本节考虑自由飞行空间机器人系统不与环境接触时的自由运动控制问题.首先讨论系统的运动学和动力学，然后分别设计载体和机械臂的控制器.
　　1) 运动学和动力学
　　记q=［qT0, qTr］T, 其中q0=［q01,q02,…,qom］T∈Rm为载体的位置和姿态向量，qr=［qr1,qr2,…,qrn］T∈Rn为机械臂的关节角向量. x=［x1,x2,…,xn］T∈Rn为手端任务向量，τ=［τT0,τTr］T, τ0=［τ01, τ02，…，τ0m］T∈Rm为作用在载体上的控制力和力矩， τr=［τr1, τr2,…,τrn］T∈Rn为机械臂的关节控制力矩.假定空间机器人系统所受的重力可以忽略不计，则系统的动力学方程可写为
　　　　　　　　　　　　　　　(2.1)
式中 M(q)∈R(n+m)×(n+m)为系统的惯性矩阵，为离心力和哥氏力项.
　　手端任务向量x∈Rn为q的非线性函数，其几何关系记为
x=f(q).　　　　　　　　　　　　　　　　　　(2.2)
　　对上式微分有
　　　　　　　　　　　　　　　　(2.3)
　　　　　　　　　　　　　　(2.4)
式中J为系统的雅可比阵.
　　2) 载体控制器设计
　　假定手端的期望运动轨迹为xd(t)，机械臂的最佳运动构型为qmg.为使机械臂保持最佳构型，载体的期望运动轨迹qd0=［qd01,qd02,…,qd0m］T可由f(qd0, qmg)=xd解出.
　　若将机械臂对载体的作用力和力矩看作扰动，则可设计变结构控制器
　　　　　　　　　(2.5)
式中
　　可以看出上面的控制器极其简单且可有效地节省载体的控制燃料.
　　3) 机械臂控制器的设计
　　现在讨论如何确定机械臂关节控制力矩τr使手端位姿x(t)跟踪期望的运动轨迹xd(t).
　　由(2.1)式可解出为
　　　　　　　　　　　　　　　(2.6)
　　(2.6)代入(2.4)式可得
　　　　　　　　　　　　　　　(2.7)
其中Ir, I0和Ir分别为m×m和n×n单位矩阵.
　　若Nr可逆，则可由(2.7)式得到机械臂的控制律为
　　　　　　　　　　　　　　　(2.8)
式中均为对称正定矩阵.
　　(2.8)式代入(2.7)式可得
　　　　　　　　　　　　　　　(2.9)
适当选择Kdx、Kpx可以使得当t→∞时有
3　位置/力混合控制
　　1) 运动方程
　　在下面的讨论中，假定约束面为刚性光滑曲面且其方程已知，另外还假定机械臂手端与约束曲面的接触为点接触.由以上假设可知，机械臂手端所受的约束力沿约束曲面的法线方向.
　　运动学关系仍为(2.2)～(2.4)式.手端的运动约束为
c1(x)=0,　　　　　　　　　　　　　　　　　　(3.1)
　　　　　　　　　　　　　　　　　(3.2)
　　　　　　　　　　　　　　　(3.3)
式中c1(x)=0为约束曲面的方程，
　　受限空间机器人系统的动力学方程可写为
　　　　　　　　　　　　(3.4)
式中fc为在适从坐标系(将在下面给出定义)中机械臂手端所受约束力.
　　由(3.4)式可解出为
　　　　　　　　　　　　　　(3.5)
　　(3.5)代入(2.4)得到
　　　　　　　　　　　　(3.6)
　　定义适从空间速度向量
　　　　　　　　　　　　　　　(3.7)
式中 Jc=［AT(x), BT(x), ］T, B(x)正交于A(x) . 从(3.7)可知，Jc定义了从手端任务坐标系至适从坐标系的坐标变换. 其中cf=A为沿约束曲面法线方向的速度向量，而为沿约束曲面切线方向的速度向量.
　　微分(3.7)两边有
　　　　　　　　　　　　　　　(3.8)
(3.6)代入(3.8)，并利用(3.7)可得到
　　　　　　　　(3.9)
由约束(3.2)及定义(3.7)知， 因此(3.9)式可重写为
　　　　　　　　　　　　(3.10)
式中
　　2) 位置/力混合控制律
　　载体的控制律τv仍用(2.5)式，若Nr非奇异，则由(3.10)可设计机械臂关节控制律τr为
　　　　　　　　　　(3.11)
其中ucp、ucf为新的控制变量.
　　控制律(3.11)代入(3.10)并利用Jc=［AT, BT］T可得到
0=AJM-1JTAT(-ucf+fc),　　　　　　　　　　　　(3.12)
　　　　　　　　　　(3.13)
　　若AJ为行满秩阵，则AJM-1JTAT为对称正定阵，所以由(3.12)可得到
fc=ucf,　　　　　　　　　　　　　　　　　　(3.14)
　　(3.14)代入(3.13)可知
　　　　　　　　　　　　　　　　　　(3.15)
　　这样我们就可以设计各种伺服控制器ucf和ucp使得fc和xcp跟踪期望的轨迹fdc和xdcp，例如ucf和ucp可设计为
　　　　　　　　　　　　(3.16)
ucf=frc,　　　　　　　　　　　　　　　　(3.17)
其中frc由下述二阶系统决定.
　　　　　　　　　　　　(3.18)
式中 ecp=xdcp-xcp; Kcpd, Kcpp, Kfd, Kfp为对称正定阵.显然适当选择Kcpd, Kcpp, Kfp, Kfd可以保证当t→∞时有 
4　仿真研究
　　针对由载体和两个杆组成的平面空间机器人系统，我们分别对手端自由运动和受限运动进行了数字仿真，因篇幅所限，我们在此只给出自由运动情况下的仿真结果.
　　所用机器人的几何参数及惯性参数如下：
　　b0=3m, b1=3m, b2=3m, a1=3m, a2=3m, 
a3=3m, m0=30kg, m1=10kg, m2=10kg, m3=10kg.
　　期望的手端轨迹为 xd1(t)=3.0+0.5t+sin(t), (m), xd2(t)=2.0+0.5t+cos(t) (m).
　　机械臂的最佳构型为 
　　期望的载体运动轨迹为
qdv1(t)=0.5t+sin(t)(m), qdv2(t)=0.5t-1+cos(t)(m), qdv3(t)=0(rad).
　　载体立姿及关节角的初始值为

　　仿真结果如图1-3所示.可以看出手端的跟踪误差均趋于零，机械臂的关节角也保持在最佳构形附近.另外，为便于比较，我们应用文［3］中的控制方案进行了仿真，并对两种方案所消耗的载体燃料进行了比较，结果如图4所示.显然，利用本文所提出的控制方案所消耗的载体控制燃料较少.


图1　手端期望轨迹和实际轨迹(x1)


图2　手端期望轨迹和实际轨迹(x2)


图3　机械臂关节角变化轨迹


图4　载体控制燃料消耗比较
5　结论
　　本文分别给出了自由飞行空间机器人系统手端自由运动和受限运动时载体和机械臂的协调控制策略，该策略即可以保证机械臂手端完成给定的任务，并可使机械臂工作在最佳构型附近，同时还可以节省载体的控制资料.最后对两杆平面空间机器人系统进行了仿真研究，证实了协调控制策略的有效性.
1)　国家自然科学基金资助项目.
作者简介：马保离　1965年生，1984年毕业于西北工业大学电子工程系，1990年获西北工业大学自动控制专业硕士学位，1995年获北京航空航天大学控制理论与应用专业博士学位，现为北京航空航天大学博士后，主要研究方向为机器人动力学与控制.
　　　　　霍　伟　见本刊1994年第3期.
作者单位：北京航空航天大学七研　北京　100083
参考文献
［1］　Vafa, Dubowsky. On the nature of control algorithms for free-floating space manipulator. IEEE Trans. on Robotics and Automat, 1991, 7(6):750-758.
［2］　Yoshihiko Nakamura, Mukherjee. Bi-directional approch for nonholonoimc path planning of space robots. In: Proc. Fifth International Symposium of Robotics Research, 405-416.
［3］　Olav Egeland, Jan Richard Sigli. Kinematics and control of a space manipulator using the macro-micro manipulator concept. In:proc. IEEE Int. Conf. Robotics Automation, 1990, 3,3096-3100.
［4］　Spoford J R, Akin D L. Redundancy Control of free-flying telerobot. In:Proc. AIAA Guidance, Navigation and Control Conf., 1990, 347-357.
收稿日期　1996-01-03
