CN106945020B

CN106945020B - 一种空间双机械臂系统运动协调控制方法

Info

Publication number: CN106945020B
Application number: CN201710352736.9A
Authority: CN
Inventors: 李传江; 陈亮名; 邱爽; 张米令; 杜晔; 孙延超
Original assignee: Harbin Institute of Technology
Current assignee: Heilongjiang Industrial Technology Research Institute Asset Management Co ltd
Priority date: 2017-05-18
Filing date: 2017-05-18
Publication date: 2019-10-22
Anticipated expiration: 2037-05-18
Also published as: CN106945020A

Abstract

本发明涉及一种空间双机械臂系统运动协调控制方法，属于空间机械臂技术领域，本发明为了解决目前还没有针对于双机械臂的空间机械臂系统，以及现有的空间机械臂系统未考虑到机械臂与卫星本体间的协调关系，跟踪误差较高的缺点，而提出一种空间双机械臂系统运动协调控制方法。一种空间双机械臂系统运动协调控制方法，包括：构建空间双机械臂系统的运动学方程以及动力学方程；根据机械臂的初始位姿以及末端位姿，对空间双机械臂系统进行轨迹规划；通过PD控制器对空间双机械臂系统轨迹进行跟踪控制。本发明适用于空间机械臂控制系统。

Description

一种空间双机械臂系统运动协调控制方法

技术领域

本发明涉及一种空间双机械臂系统运动协调控制方法，属于空间机械臂技术领域。

背景技术

航天科技的发展，极大地改变了人类生活，空间技术的应用领域在近一、二十年中得到了飞速的拓展，除了在空间科学应用方面得到继续深化外，在空间对抗、空间服务等领域也对空间技术提出了新的需求和任务，其中对空间目标的在轨操作、交会对接技术由于在军事和民用领域上的潜在价值，正被各国广泛重视^[1]。通过使用空间机械臂系统对空间合作/非合作目标进行操作，包括对发生故障失效的卫星、燃料耗尽寿命终结的卫星等合作/非合作目标实施在轨捕获、元器件更换、燃料加注、助推离轨等操作可极大地延长卫星的使用寿命，大幅节省卫星的研制成本，因而具有极大的应用价值。

近年来，各航天大国及科研机构，如欧洲ESA、美国NASA、日本JAXA等通过使用空间机械臂技术对空间合作/非合作目标开展了大量的在轨验证项目。美国开展了“凤凰”计划，其主要目的是通过寻求对现有卫星发射方式、卫星部署方式的突破实现对空间资源的有效的再利用，凤凰计划的服务航天器系统可以利用系统上安装的空间机械臂装置，实现对弃用卫星的可以使用零部件(如天线)重新拆卸组装，达到从新构造卫星的目标，同时在上诉目标完成后具有在轨转移能力^[2]。航天大国德国在空间机械臂控制技术进行了多年的核心技术攻关，到目前为止，德国主要的研究项目包括：机器人技术实验、试验服务卫星、空间系统演示验证技术卫星和德国在轨服务计划。欧空局在空间机械臂应用领域的研究成果代表性项目为Jerico机械臂项目与欧洲机械臂项目。但是，应该看到我国对空间机械臂系统在轨操作任务发展上仍然落后于世界其他航天大国。

按照在空间项目中的应用，部署在空间的机械臂系统可分为空间单机械臂系统、空间双机械臂系统及多机械臂系统。单臂系统目前研究的内容较多，成果比较深入。与单臂系统相比，双臂空间机器人的操作臂可以同时、独立完成类似单臂空间人的各种操作，协调地执行某一项共同的任务,具有操作精度高、反映速度快、对复杂空间环境的适应性强，能够执行的任务更复杂等优点，因而更受各国的青睐^[3]。

由于机械臂和平台载体之间存在着运动学和动力学耦合，会给系统本体姿态控制与机械臂系统的运动控制产生干扰，导致对于地面固定基座的机械臂控制技术不能直接应用到空间机械臂系统上，因此必须建立空间机械臂运动学和动力学模型^[4]。对于双臂空间机器人系统的动力学建模问题、轨迹规划问题和运动控制问题，如何建立准确、简洁的空间机械臂运动学和动力学模型并实现空间机械臂的高精度运动控制是目前理论和工程问题研究的基础和难点。

一、空间机械臂系统运动建模

由于难以在地面重现空间应用环境，因此精确地建立空间机械臂系统运动学与动力学模型对系统设计、轨迹规划和控制算法等方面的研究尤为重要，并且有利于对系统运动过程进行更准确的仿真。空间机械臂属于多体系统，由运动基座和臂杆两部分共同组成，因此，适用于其他多体系统的建模方法可同样应用于空间机械臂系统^[5]。

针对地面机械臂比较成熟的建模方法有牛顿-欧拉方程和拉格朗日方程等，均属于多体系统的建模范畴。而空间机械臂系统强耦合、非线性、时变等特点使得建模问题较地面系统相对复杂，为推动空间机械臂技术的发展，各国学者纷纷针对空间机械臂的运动学与动力学特性，提出了一些极具学习价值的建模方法。

Hooker、Margulies、Roberson和Wittenburg等人所提出的牛顿-欧拉法为多体系统的动力学求解问题打下了坚实的基础^[4]。他们的共同特点是利用树形拓扑来描述多体系统的开链结构，用系统质心(Center of Mass,CM)表示平移自由度，引入增广体(AugmentedBodies)和增广体质心(Barycenters)概念来简化系统。在Ho、Frisch和Hooker提出的直接路径法中，选取系统中的一个子系统作为基座，用该子系统上的一个点作为坐标系的原点以表达系统的平移自由度，这种方法简单明了，但是存在一定的耦合^[4]。

美国学者Z.Vafa和S.Dubowsky在假设空间机械臂系统不受外力或外力矩作用的情况下，提出了虚拟臂VM(VirtualManipulator)的建模方法^[6]，即当系统质心的位置不变时，描述空间机械臂系统的几何结构，VM方法的理论基础为动量守恒定理。VM是理想状况下虚拟的一条运动链，将真实空间机械臂系统质心选为该运动链的虚拟基座，由于在空间微重力环境下，质心的位置保持不变，因此适用于地面机械臂的控制方法同样也可以应用到VM上。然而，该方法只能描述空间机械臂系统的运动学模型。

日本学者Y.Umetani和K.Yoshida根据动量守恒定理，推导出能够表示空间机械臂系统微分运动学关系的广义雅克比矩阵(GeneralizedJacobiMatrix,GJM)^[7]。与地面机械臂系统不同的是，利用GJM建模是不仅需要系统的几何参数，还需要各子系统的惯量参数，如基座和各连杆质量、转动惯量等。而在实际空间应用中，基座燃料、末端载荷等会发生不确定变化，若要实现精确的运动控制，必须实时在线地对相关参数进行自适应辨识，无论从实时性还是快速性方面来讲，GJM方法都具有一定的局限性。

中国学者梁斌等人在Z.Vafa的理论基础上将空间机械臂等价作一个地面机械臂，提出了动力学等价臂DEM的概念^[8]，并解释了它与空间机械臂在运动学和动力学上的等价性。与VM不同的是，DEM是真实的物理概念，可在实际中制造出来。DEM具有VM的优良性质，可以较为准确的描述系统的动力学特性，适用于地面机械臂的控制方法都可通过DEM方法应用于空间机械臂，有助于进一步对空间机械臂进行设计、规划和控制分析。但该方法在建模前期需要大量的计算，并且系统模型不直观，因此同样具有一定的局限性。

二、空间机械臂系统轨迹规划

空间机械臂在执行任务的过程中需要遵守一定的运动规律，如果没有事先对机械臂的运行轨迹进行规划，一来机械臂无法按照任务需求到达指定位置和指定的指向，另一方面还容易引起机械臂与航天器的碰撞，对航天器造成损坏。此外，运行轨迹的合理性是保证机械臂能够正常运行的基础，机械臂各关节存在构型的约束，如转角大小，转速大小等，因此在设计控制系统之前必须综合考虑以上问题，这就需要对机械臂的运行路径进行规划。

机械臂路径规划的一个主要研究内容是规划在给定的起始和目标的位姿，选择一条从起始点到达目标点的路径，使运动物体(机器人)能准确到达目标位姿。对于点到点的路径规划，S.Pandey以及R.Lampariello等通过对关节函数进行参数化，再采用正运动学的方法规划空间机器人笛卡尔空间点到点路径。他们认为多项式函数没法约束关节变量的范围，故使用正弦函数对关节变量进行参数化，然后通过牛顿迭代法求解待定参数。由于迭代法需要对待定参数赋初值，对于空间机器人这样的非完整系统，不同的初值导致不同的收敛结果。为此，R.Lampariello等提出了一种赋初值的准则，然而该准则依赖于一定的条件，如规划的总时间t_f，当条件改变了，该准则需要重新建立。另外，在S.Pandey和R.Lampariello的方法中，没有提出当关节角加速度受限时的一般规划方法。徐文福对R.Lampariello等的方法进行了改进。首先，多项式函数可用于当关节角、角速度、角加速度受限时的路径规划；其次，姿态误差的表示采用四元数方法，使得在迭代过程中不会出现姿态奇异的问题；再次，采用归一化处理，可预先确定待定参数的范围；最后，提出了一种更通用的赋初值的准则，t_f的改变不影响该准则。

在执行空间在轨服务任务中，根据空间机器人系统2种飞行状态，将轨迹规划分为自由飞行状态轨迹规划与自由飘浮状态轨迹规划。自由飞行状态双臂空间机器人系统基座的姿态保持不变。双臂空间机器人系统不对载体的位置加以控制，只对载体的姿态进行稳定控制，属于姿态受控的自由飞行机器人，因此在系统模型的变量中可以加以任意控制的只有机械臂的各个关节角。因此轨迹规划的基本要求即是以期望的末端作用器或参考点和参考矢量的位置和姿态指向为目标，来通过一定的算法确定出各个关节角的角位置、角速度和角加速度轨迹。经过自由飞行状态空间机械臂到达目标抓取位姿初始点，为避免在抓取过程中的碰撞导致机械臂系统损坏故采用自由飘浮状态进行抓取。由于末端操作器已经接近目标，此时对于更进一步的接近接触目标只需要各个关节角很小的调整，从而确定出各个关节角的角位置、角速度和角加速度轨迹。

三、空间机械臂系统跟踪控制

由于空间机械臂是一种典型的不确定、非线性系统，同时系统又具有非完整特性，使得大部分在地面机械臂上取得良好应用效果的控制方法并不适用于空间机械臂，控制问题相对复杂。以下通过介绍几种常用的控制算法及其在机械臂跟踪控制中的应用。

(1)PD控制

PD控制不依赖于空间机械臂的动力学特性，根据系统位置跟踪误差乘以相应的静态增益即可确定控制量，从而使系统渐进收敛，控制器结构简单。

Parlaktuna和Ozkan利用DEM方法将空间机械臂的控制问题由惯性空间转化到关节空间，在得到参数线性化的动力学方程后，将适用于地面固定基座机械臂的控制方法应用于空间机械臂对关节期望轨迹的跟踪^[9]。

但是PD控制未考虑到实际系统中常存在的非线性因素和外界干扰，属于一种线性控制器，且PD控制依赖于系统的雅克比矩阵，因此当系统存在不确定因素时无法保证良好的动态和静态特性。而且，针对空间机械臂高精度的轨迹跟踪控制问题，PD控制会消耗较多的控制能量，对于空间应用来讲成本较高。

(2)自适应控制

自适应控制的显著特点是可以实时调整参数，当控制系统存在参数不确定因素时，可以用系统数学模型中的未知参数来描述不确定性，通过调整控制器参数来适应参数变化，在线学习不确定参数，并根据学习值实时修正控制策略，从而达到期望的控制效果。由于空间机械臂存在参数不确定性问题，给系统带来了动力学耦合不确定的问题，导致难以精确地获得系统的雅克比矩阵，并且无法将系统动力学模型参数线性化，因此自适应控制得以广泛应用于空间机械臂系统。

陈力针对2连杆空间机械臂系统的轨迹跟踪控制问题，提出了一种增广自适应的控制方法^[10]。Gu等在了解基座基本信息的情况下提出自适应控制方法可线性化系统动力学模型中的惯性参数。Taira、Parlaktuna等提出了基于笛卡尔空间的自适应控制方法，在线进行参数自适应估计^[11]。

陈力、刘延柱在此基础上提出了一种基于参数不确定性的增广自适应控制方法。徐栓锋等设计了一种自适应扩展雅克比零反作用控制方法，无需考虑末端执行器的速度^[12]。

上述自适应控制方法均能克服参数不确定因素对系统的影响，通过对不确定参数的在线系统辨识，实时地调整控制策略以适应模型的参数不确定性。

(3)神经网络控制

神经网络控制不需要过多的控制对象模型参数信息，高度并行的结构特点可以使得其具有很强的容错能力和自学能力，可以对任意的非线性函数进行逼近，且不需要大量的计算，这些年在相关领域得以广泛应用。

Sanner R M等引入神经网络控制，克制不确定因素对系统运动控制的影响^[13]。Feng B M等利用径向基神经网络逼近空间机械臂的动力学模型，自适应调整神经网络参数，抑制了外部干扰与参数摄动对系统的影响^[14]。郭益深和陈力设计了一种径向基神经网络自适应控制方法，能较好地实现空间机械臂系统对期望关节轨迹的跟踪。谢箭在此基础上设计的神经网络自适应控制方法可以有效解决空间机械臂存在不确定因素的问题，不需要预先估计不确定性上界。

发明内容

本发明的目的是为了解决目前还没有针对于双机械臂的空间机械臂系统，以及现有的空间机械臂系统未考虑到机械臂与卫星本体间的协调关系，跟踪误差较高的缺点，而提出一种空间双机械臂系统运动协调控制方法。

一种空间双机械臂系统运动协调控制方法，包括：

步骤一、构建空间双机械臂系统的运动学方程以及动力学方程；

步骤二、根据机械臂的初始位姿以及末端位姿，对空间双机械臂系统进行轨迹规划；

步骤三、通过PD控制器对空间双机械臂系统轨迹进行跟踪控制。

本发明的有益效果为：

①考虑了机械臂系统与卫星本体之间的协调关系，通过实时的估计相互间的耦合干扰作用，实现了机械臂系统的高精度快速跟踪；②所设计的控制器结构简单，计算量小，且跟踪速度快；③所设计的控制律与其他方法相比具有更高的控制精度和稳定性(误差可控制在10^-5数量级)，并且控制力矩仍保持在原控制幅值内。更加精确地实现机械臂末端轨迹跟踪；④对比了自由漂浮和自由飞行两种飞行状态，考虑更加全面，具有实际的应用价值；⑤与单机械臂相比，双机械臂系统具有操作精度高，适应性强，能够执行更复杂任务等优点，更符合空间项目中的任务要求。⑥本发明的PD反馈控制器跟踪误差在0.01rad以内，星臂协调控制算法的跟踪误差可以达到0.00001rad以内。

附图说明

图1为本发明的双臂空间机械臂结构模型；

图2为本发明的空间双机械臂系统协调控制结构框图；

图3为本发明的自由飞行机械臂轨迹规划的流程图；

图4(a)为公式(100)中f(t′)的曲线图；

图4(b)为公式(100)中f(t′)的一阶导数曲线图；

图4(c)为公式(100)中f(t′)的二阶导数曲线图；

图5为现有技术的机械臂轨迹规划曲线与跟踪曲线对比图；其中呈上升趋势的为基本重合的关节1轨迹曲线以及关节1规划曲线；呈下降趋势的为基本重合的关节2轨迹曲线以及关节2规划曲线；

图6为仿真实验中自由漂浮飞行模式下的机械臂跟踪误差曲线图；其中呈先升后降趋势的为关节1的曲线；

图7为仿真实验中自由漂浮飞行模式下的机械臂控制力矩曲线图；其中幅值变化较大的，且呈先升后降趋势的是关节1的曲线；

图8为仿真实验中自由漂浮飞行模式下的卫星本体位姿变化曲线图；由上至下依次为x变化曲线、q0变化曲线以及y变化曲线；

图9为仿真实验中卫星姿态受控而位置不受控时的机械臂1的跟踪轨迹曲线图；其中呈上升趋势的为基本重合的关节1轨迹曲线以及关节1规划曲线；呈下降趋势的为关节2轨迹曲线以及关节2规划曲线；持平的为卫星本体的曲线；

图10为仿真实验中卫星姿态受控而位置不受控时对卫星本体施加的控制力矩的曲线图；

图11为仿真实验中卫星姿态受控而位置不受控时的控制力矩的曲线图；其中幅值较大的为关节1的曲线，幅值小的为关节2的曲线；

图12为仿真实验中卫星姿态受控而位置不受控时的轨迹跟踪误差的曲线图；其中呈先升后降趋势的为关节1的曲线；

图13为仿真实验中卫星姿态受控而位置不受控时的轨迹跟踪误差曲线图；

图14为仿真实验中本发明实施例的机械臂1的跟踪轨迹曲线图；其中呈上升趋势的为基本重合的关节1轨迹曲线以及关节1规划曲线；呈下降趋势的为基本重合的关节2轨迹曲线以及关节2规划曲线；持平的为基本重合的卫星本体规划曲线以及卫星本体轨迹曲线；

图15为仿真实验中本发明实施例中对卫星本体施加的控制力矩的曲线图；

图16为仿真实验中本发明实施例中对臂1是施加的控制力矩的曲线图；其中幅值较大的为关节1的曲线；

图17为仿真实验中本发明实施例的机械臂1的轨迹跟踪误差曲线图；其中先上升后持平的曲线为关节1的曲线；

图18为仿真实验中本发明实施例的本体基座轨迹跟踪误差曲线图；

图19为仿真实验中本发明实施例的臂1的轨迹跟踪误差的局部放大图；其中幅值较大的为关节1的曲线。

具体实施方式

为了便于理解，在描述实施方式主要内容之前，先介绍一下本发明中需要使用到的关键技术：

1.1旋转矩阵表示法

描述两坐标系之间的相对姿态可以采用旋转矩阵方法。假设坐标系Σ_j的三个轴向对应的单位矢量在Σ_i中的表示分别为n、o、a∈R^3×3，则从Σ_i到Σ_j的旋转矩阵为：

A＝[n 0 a]∈R^3×3

\*MERGEFORMAT(1)

旋转矩阵对时间的导数：

其中ω为Σ_j的绝对角速度在Σ_i中的表示。当Σ_i绕其X轴旋转α角后各轴与Σ_j平行，则其旋转矩阵纪为R_x(α)，同理绕Y,Z轴分别旋转β、γ角的旋转矩阵分别记为R_y(β)、R_z(γ)，则根据上述定义，可得:

其中：

c_α＝cos(α),c_β＝cos(β),c_γ＝cos(γ)

s_α＝sin(α),s_β＝sin(β),s_γ＝sin(γ)

1.2拉格朗日动力学建模方法

拉格朗日方程是分析力学的一个重要方程，因物理学家约瑟夫·拉格朗日而命名，常用来描述各类物理机械系统的动力学。拉格朗日方程可由如下所示的达朗贝尔原理(d’Alembert principle)导出

其中q∈R^p为系统的广义坐标，为系统的广义速度，δq为广义坐标q的虚位移，L∈R是系统的拉格朗日量，定义如下；

其中V(q)∈R是系统的势能，只与系统的广义坐标有关，

是系统的势能，有二次型的形式，M(q)∈R^p×p是系统对称正定的惯量矩阵。由式(6)能推出如下的Lagrange方程

将式(7)、(8)代入式(9)可得

其中g_i(q_i)代表系统的广义有势力，F∈R^p代表作用在系统上的外力，是系统的Coriolis力和偏心力，具体形式如下

1.3点到点轨迹规划

点到点的路径规划是针对室内二维工作环境，其目标是为机械臂寻找一条从起点到目标点的能够避开障碍物的尽可能短的路径。点到点的路径规划是一种从室内移动机械臂的路径规划技术。起始点到终点的最优路径策略，它要求寻找一条从起始点到终点的代价最小、路径最短、时间最短并且合理的路径，使自主机械臂能够在工作空间内顺利地行走而不碰到任何障碍物。

目前，点到点的路径规划方法大致有以下几种：

1.可视图法

该方法视自主机械臂为一个质点，将所有障碍物的顶点和机械臂起始点及目标点用直线组合连接，要求机械臂和障碍物各顶点之间、目标点和障碍物各顶点之间以及各障碍物顶点与顶点之间的连线均不能穿越障碍物，即直线是“可视的”，然后搜索从起始点到目标点的最短距离的可视直线，寻求最优的路径。

2.多项式法

该方法的基本思路是把机械臂的运动假设为一条光滑的多项式曲线，通过机械臂在初始位置、末端位置的约束条件对多项式的系数进行求解，该方法的优点是计算量小，规划曲线光滑。

3.自由空间法

该方法是把自主机械臂的工作空间分成两部分，即自由空间和障碍物空间。用某种搜索策略在自由空间中找到一条路径，自主机械臂沿着障碍物之间的通道中心运动。

4.遗传算法

以自然遗传机制和自然选择等生物进化理论为基础，构造了一类随机化搜索算法。它利用选择、交叉和变异等遗传操作来优化控制算法，不要求适应度函数是可导或连续的，只要求适应度函数为正，同时作为并行算法适用于全局搜索。

5.其他方法

近年来，人工智能方法正在成为研究的热点，神经网络在机械臂路径规划中的应用得到了广泛的重视。一些神经网络模型通过学习可生成实时的路径[12]。

1.4PD轨迹控制方法

具有比例/微分控制规律的控制器，称为PD控制器，其输出m(t)与输入e(t)的关系如下式所示

式中，K_p为比例系数；τ为微分时间常数。K_p与τ都是可调的参数。

偏差的比例(P)、积分(I)和微分(D)进行控制的PID控制器(亦称PID调节器)是应用最为广泛的一种自动控制器。它具有原理简单，易于实现，适用面广，控制参数相互独立，参数的选定比较简单等优点。而在实际应用中，PD调节器是一种理想调节器,是工业中最常用的一种调节方法。在90％以上的工业控制系统中采用了传统的PD控制策略,这是因为PD控制策略有以下优点：(1)技术成熟；(2)结构简单，在线控制实时性好；(3)不依赖精确的数学模型；(4)软件系统灵活易修改完善，控制效果令人满意。

2.1本发明所涉及的空间机器人主要由卫星基座和若干个机械臂组成，本文以双臂空间机械臂作为研究对象，其一般空间结构如图1所示，机械臂包括自由度为n₁和n₂的串联机械臂arm-1和arm-2。自由度的个数意味着每个机械臂上关节的个数。

为统一标准和方便讨论，对坐标系、基本变量及符号进行如下定义：

k＝1或2，当k＝1时，i＝1,...,n₁；当k＝2时，i＝1,...,n₂(若无特别指出，各矢量均在惯性系中表示)：

Σ_I、Σ_E1、Σ_E2:分别表示惯性系、机械臂arm-1和arm-2末端坐标系；

Σ_B：基座的几何坐标系；

Σ_H1、Σ_H2：为位于目标航天器上供arm-1和arm-2捕获的目标坐标系；

Σ_i、B_i：固连坐标系，Z_i正向为J_i的旋转方向；

B₀:刚体0，即航天器平台，或称卫星本体；

B_i ^k(i＝1,......,n)：arm-k的第i个连杆；

J_i ^k:arm-k连接B_i-1和B_i的关节；

C_i ^k:B_i ^k的质心；

O_I:惯性系原点；

O_g：整个系统的质心；

ⁱA_j∈R^3×3:Σ_j相对于Σ_i的姿态变换矩阵，当i缺省时，表示Σ_j相对于惯性系的姿态变换矩阵，ⁱA_j＝[ⁱn_j,ⁱo_j,ⁱa_j]；

ⁱT_j∈R^4×4：Σ_j相对于Σ_i的齐次变换矩阵，当i缺省时，表示Σ_j相对于惯性系的齐次变换矩阵，ⁱT_j＝[ⁱn_j,ⁱo_j,ⁱa_j,ⁱd_j]；

ⁱn_j,ⁱo_j,ⁱa_j∈R³：分别为Σ_j各轴的单位矢量，在Σ_i中的表示；

ⁱd_j∈R³：为Σ_j原点在Σ_i的位置矢量；

E_n：n维单位矩阵；

O_n×m：n×m维零矩阵；

τ∈Rⁿ：关节驱动力矩；

m_i ^k、M：为B_i ^k的质量，

I₀,I_i ^k∈R^3×3(k＝a,b):分别表示B₀和B_i ^k绕各自质心的转动惯量矩阵；

k_i ^k∈R³：表示J_i ^k的旋转方向的单位矢量；

r_i ^k∈R³：表示连杆质心C_i ^k的位置矢量；

r_g∈R³:系统质心的位置矢量；

r_o∈R³：表示系统基座质心的位置矢量；

p_i ^k∈R³:J_i ^k的位置矢量；

p_e ^k∈R³：机械臂k的末端的位置矢量；

a_i ^k、b_i ^k∈R³：分别为从J_i ^k指向C_i ^k，C_i ^k指向J_i+1 ^k的位置矢量；

l_i ^k∈R³：表示由J_i ^k到J_i ^k+1的位置矢量，l_i＝a_i+b_i；

^kr_ij：表示从Σ_i原点指向Σ_j原点的矢量，在Σ_k中的表示，如果Σ_i或者Σ_k为惯性系，则可以省去相应的符号i或者k；

^jv_i,^jω_i：分别表示Σ_i相对于Σ_j的线速度和角速度，如果Σ_i或者Σ_j为惯可以省去相应的符号i或者j；

机械臂arm-k关节角向量，即Θ^k＝[θ₁ ^k,......,θ_n ^k]^T；

期望的机械臂arm-k关节角向量，即Θ_d ^k＝[θ_d1 ^k,.....θ_dn ^k]^T；

q_b∈R³：基座的姿态向量，即q_b＝[q_b1q_b2q_b3]^T；

q∈Rⁿ⁺³:空间机械臂系统关节角向量，由机械臂关节角向量和基座姿态向量组成，即q＝[q_b ^T,Θ^T]^T＝[q_b1,q_b2,q_b3,θ₁,......,θ_n]^T；

v₀、ω₀∈R³：基座的线速度和角速度；

v_i、ω_i∈R³:第i个刚体B_i的线速度和角速度；

v_e ^k、ω_e ^k∈R³：机械臂末端的线速度和角速度；

v_ed、ω_ed∈R³:机械臂末端的线速度和角速度的期望值；

Ψ_b∈R³：基座姿态角，按z-y-x欧拉角表示；

Ψ_e ^k∈R³：机械臂arm-k末端姿态角，按z-y-x欧拉角表示；

Q₀∈R⁴:基座姿态四元数；

Q_e ^k∈R⁴:为机械臂arm-k末端姿态的四元数表示。

对于矢量的表示，做如下约定：ⁱv为矢量在Σ_i中的表示，而不加左上标的v为矢量在惯性系中的表示。并定义如下操作数(叉乘操作数；)若ν＝[x,y,z]^T，则

具体实施方式一：在空间失重环境下，空间机械臂的运动学和动力学与地面机械臂存在较大的差别，因此有必要从力学原理重新建立空间机械臂的运动学和动力学方程。本专利分别利用空间双机械臂系统的动量守恒、拉格朗日方程推导空间双机械臂系统在自由飞行状态下的运动学方程和动力学方程。其次，以空间双机械臂系统抓取相对静止物体为目标，研究空间机械臂系统的轨迹规划问题。通过机械臂末端运动的点到点轨迹规划，利用五次多项式对机械臂的运动轨迹进行逼近，通过约束条件对五次多项式的系数进行化简，利用牛顿迭代法对系数进行迭代求解。最后研究空间双机械臂系统的轨迹跟踪控制问题，通过设计星臂协调PD反馈控制器实现空间机械臂系统对期望运动轨迹的高精度快速跟踪。图2为本发明的空间双机械臂系统协调控制结构框图。

本发明的空间双机械臂系统运动协调控制方法，包括如下步骤：

步骤一、构建空间双机械臂系统的运动学方程以及动力学方程。

在自由飞行模式时，基座的姿态受到控制，且保持恒定，但位置是自由的。假设施加于系统的外力为0，则系统的线动量守恒，如果初始时刻线动量为零，则可有如下的完整性约束方程：

P＝0

\*MERGEFORMAT(14)

由式(19)可知：

由于基座姿态受控，基座的角速度很小，近似为零，即：

ω≈0

\*MERGEFORMAT(16)

将式(21)代入式(20)，系统的线动量守恒方程可化简为：

根据式(22)，可解出基座的线速度为：

利用拉格朗日方法可以建立空间双机械臂系统在自由飞行状态下的动力学方程如下：

其中M(q)为机械臂的广义惯性张量，为机械臂与基座相关的非线性力矩，τ为控制力矩。

步骤二、根据机械臂的初始位姿以及末端位姿，对空间双机械臂系统进行轨迹规划。具体为：

(1)点到点路径规划

本部分需要的解决的问题是：给出机械臂的初始位姿，末端的位姿，规划一条运动路径使之从初始位姿运动到末端位姿，即为点到点路径规划问题，此处采用五次多项式对运动路径进行逼近。以下给出点到点路径规划的数学描述，五次多项式逼近的基本形式，最后将问题转化为求解非线性方程组的问题，并给出利用牛顿迭代法的求解步骤。

空间机械臂末端的初始状态、最终状态以及期望状态分别记为和则点到点的路径规划可以描述为：

规划关节运动：

使得：

其中，分别为第k条机械臂的关节角、角速度和角加速度；和分别为的下限值和上限值，和分别为的下限值和上限值、和分别为的下限值和上限值；t₀为轨迹规划的初始时刻，t_f为轨迹规划的末端时刻。

使用五次多项式对关节函数进行参数化：

令则路径规划问题转化成了求解下述非线性方程组的问题：

F(c)＝0

\*MERGEFORMAT(25)

(2)空间机械臂系统规划求解

采用迭代法对自由飞行状态参数辨识问题求解，算法步骤如下：

1)初始化各变量：

k＝1，c_k＝c₀，ε＝10^-6，X_e0，X_ed，t_f

2)求取X_ef，若满足||F(c_k)||≤ε跳转第4步，否则

Δc_k＝-(F′(c_k))^-1F(c_k)

\*MERGEFORMAT(27)

c_k+1＝Δc_k+c_k

\*MERGEFORMAT(28)

3)循环加一，跳转第二步

4)输出参数

完整流程图如图3所示。

考虑到自由飞行情况下，对本体和机械臂执行完全一致的PD控制算法具有控制精度低、收敛速度慢等缺陷，这里设计一种星臂协调的控制算法。其基本思想为：对于卫星本体，估计机械臂运动对本体的影响，并实时前馈补偿至自身的PD控制器中，对于机械臂系统，估计卫星本体运动对机械臂的影响，并将其前馈补偿至PD控制器中。

基于以上思想，首先将自由飞行星臂系统动力学方程拆分成如下形式

其中

将M矩阵分解为4个分块子矩阵M₁₁、M₁₂、M₂₁、M₂₂，将矩阵也分解为4个分块子矩阵B₁₁、B₁₂、B₂₁、B₂₂，则有

其中

针对以上模型，设计空间本体和机械臂运动控制规律为：

其中q_d,θ_d分别表示卫星本体、机械臂的待跟踪信号，即轨迹规划中所得到的信号，e_m＝θ_d-θ为机械臂的跟踪误差，e_b＝q_pd-q_b为卫星平台的跟踪误差。所得到的控制律可以使得PD控制器对空间双机械臂系统轨迹进行跟踪控制。

下面针对本实施方式的三个步骤具体说明其原理与推导过程：

空间双机械臂系统运动建模过程：

为了使研究更加具有通用性，在研究时将对象A扩展为N臂空间机械臂系统，设定N臂机械臂系统的第k条臂(k＝1,….,N)的关节自由度为n_k，本项目的研究对象是双臂空间机器系统，因此有k＝1，2。

由可知，多臂空间机械臂系统中第i个手臂的各刚体质心及机械臂末端的位置矢量为：

各杆件及机械臂末端的固连坐标系，相对于惯性系的姿态可表示为：

A_i ^k＝A₀ ⁰A₁ ^k1A₂ ^k...^i-1A_i ^k

\*MERGEFORMAT(36)

在空间机械臂系统中，整个系统的质心位置矢量与各连杆的质心位置矢量之间存在如下关系：

其中，系统质心与惯性系间的相对关系固定。根据式(32)和(36)，可得出基座质心位置为：

由对位置矢量关系(32)和式(33)进行微分可得各刚体质心以及末端的线速度为：

另一方面，各刚体质心及末端的角速度可表示为：

将式(39)和(41)写成矩阵的形式，有：

式中J_b ^k——机械臂末端速度与基座速度相关雅克比矩阵；

J_m ^k——机械臂末端速度与机械臂关节速度相关的的雅克比矩阵。

J_b ^k和J_m ^k可按照下列式子进行计算:

p_0e ^k——基座质心到机械臂末端的位置矢量，即：

式(42)即建立了多臂空间机械臂系统中arm-k，下同末端的一般运动学方程，即对于arm-1和arm-2分别有：

将(46)和式(47)组合在一起可得:

令：

式(48)可表示为：

式(46)和式(47)或式(53)即建立了双臂空间机械臂系统末端的一般运动学方程。为了更好得到特定情况：自由飞行模式下的运动学方程，我们需要分析系统的线动量和角动量。

(1)线动量方程：

空间机械臂系统总的线动量为各连杆的线动量之和，即：

将式(328)代入(54)，有：

其中：

r_0g＝r₀-r_g

\*MERGEFORMAT(58)

(2)角动量方程

多臂空间机械臂系统总的角动量为各连杆的角动量之和，即：

将式(38)和式(40)代入式(59)，有：

其中：

J_Ri ^k＝(k₁ ^k,k₂ ^k,...,k_i ^k,0,...,0)

\*MERGEFORMAT(65)

r_0i ^k＝r_i ^k-r₀

\*MERGEFORMAT(66)

另一方面，将位置矢量关系(64)代入式(59)，进一步整理，有：

令：

式(65)可表示为：

L＝L₀+r₀×P

\*MERGEFORMAT(69)

式中L₀——空间机械臂系统相对于基座质心的动量矩；

r₀——基座质心的位置矢量；

P——系统的线动量；

将和(38)式(40)代入式(66)，可计算L₀得：

其中：

(3)空间机械臂系统线动量和角动量公式组合

由上述分析可知，式(55)和式(60)分别为空间机械臂系统的总线动量和总角动量的计算公式。将线动量和角动量计算公式(55)、(60)组合，可得空间机械臂系统的动量方程为：

式(71)可写成如下矩阵形式：

其中：

空间双机械臂系统轨迹规划原理分析：

为实现机械臂从初始位姿运动到待抓捕物体位置，需要对机械臂的运动进行轨迹规划。为此，实现需要对机械臂末端进行点到点的路径规划。

规划关节运动：

使得：

根据式和可知：

因为姿态受控，姿态角变化很小，所以近似认为

末端位置可表示为：

t_f时刻的末端姿态与期望姿态之差为：

t_f时刻的末端位置与期望位置之差为：

使用五次多项式对关节函数进行参数化：

由(77)可知：

及

经整理可得：

由此可知，在给定t_f的情况下，参数化后每个关节函数中仅含有一个参数定义待定参数

令则路径规划问题转化成了求解下述非线性方程组的问题：

F(c)＝0

\*MERGEFORMAT(92)

下面给出关节函数的参数归一化处理过程。

令t′＝t/t_f，t∈(0，t_f)，t′∈(0，1)，则有：

其中

于是有：

由此可得关于参数c′^k的非线性方程组:

因而，点到点路径规划的问题转化为求解上述非线性方程组的问题。经过归一化处理后，根据关节角的限制范围，可以预先确定参数c′^k的范围。

定义函数：

则f(t′)的曲线以及该函数的一阶导数和二阶导数如图4所示。

从上图中可知，f(τ)在(0,1)上单调递增，且其函数值、一阶和二阶微分的范围为：

根据式(100)、以及可得满足关节角限制的参数c^k的范围为:

不等式(102)可写成矢量形式：

c^k∈(c^k _min，c^k _max)

\*MERGEFORMAT(105)

待定参数c^k的初值和收敛值必须位于此范围内。若迭代算法收敛是的c^k超出此范围，则需要重新赋初值并重解非线性方程组。上述过程一直重复，直到c^k收敛到上述范围内或者方程组求解次数超过一定值(此时认为不存在符合要求的参数)。当c^k收敛到符合要求的范围时，根据式(100)、(101)，可按下列不等式确定满足关节角速度、角加速度范围的规划时间t_f：

空间双机械臂系统轨迹跟踪控制稳定性证明：

我们通过Lyapunov理论证明以上所提出的星臂协调控制算法的稳定性，为此，我们首先需要用到机械臂方程中M矩阵的如下性质：

性质2：具有有界性，即

首先，我们构造如下形式的Lyapunov函数

其中

将控制器、综合，可以得到

其中

将控制器带入动力学方程可得

则对Lyapunov函数(108)求导可得

将误差动力学方程(108)带入上式可得

利用性质2可得

因此，通过选取参数使得：即可得到

同理，利用Laselle定理可以得到系统的全局渐近稳定性。

具体实施方式二：本实施方式与具体实施方式一不同的是：

步骤一具体包括：

步骤一一、根据如下公式建立空间双机械臂系统的运动学方程：

其中，k表示第k个机械臂，且k为1或2；

为第k个机械臂第i个连杆的质量；k_i ^k表示J_i ^k的旋转方向的单位矢量，J_i ^k为第k个机械臂中连接第i-1个连杆与第i个连杆的关节；p_i ^k表示关节J_i ^k的位置矢量；Θ^k为第k个机械臂的关节角向量；

步骤一二、根据如下公式建立空间双机械臂系统的动力学方程：

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：

步骤二具体包括：

步骤二一、获取空间机械臂末端的初始状态最终状态和期望状态

步骤二二、假设存在t，使得：

其中，分别为第k条机械臂的关节角、角速度和角加速度；和分别为的下限值和上限值，和分别为的下限值和上限值、和分别为的下限值和上限值；t₀为轨迹规划的初始时刻，tf为轨迹规划的末端时刻；

使用如下的五次多项式对关节函数进行参数化处理：

为待求解的参数；

步骤二三、令求方程组F(c)＝0的解；其中：

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：

步骤二三中，使用牛顿迭代法求解方程组，具体为：

步骤二三一、对如下变量进行初始化：迭代次数k、c_k、极限中的极小正数ε、以及t_f；

步骤二三二、对运动学方程进行数值积分，得到末端位姿

步骤二三三、判断是否满足||F(a_k)||≤ε；

若满足，则输出参数

若不满足则进行如下计算：

Δa_k＝-(F′(a_k))^-1F(a_k)

a_k+1＝Δa_k+a_k

并将k值加1，跳转至步骤二三二。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：

步骤三具体包括：

步骤三一、将步骤一二中的公式拆分成如下形式：

步骤三二、将步骤三一中的矩阵M(q)拆分成4个分块子矩阵M₁₁、M₁₂、M₂₁、M₂₂，将矩阵也分解为4个分块子矩阵B₁₁、B₁₂、B₂₁、B₂₂，则有

其中

步骤三三、得出空间卫星本体和机械臂的控制律为：

其中q_d,θ_d分别表示卫星本体、机械臂的待跟踪信号，即轨迹规划中所得到的信号，e_m＝θ_d-θ为机械臂的跟踪误差，e_b＝q_pd-q_b为卫星平台的跟踪误差。

其它步骤及参数与具体实施方式一至四之一相同。

<实施例>：

现有技术的方案内容及仿真效果：

机械臂的飞行模式有三种：基座固定，自由漂浮，自由飞行。此处首先通过自由漂浮下的机械臂运动过程进行建模及控制，并得到相关的仿真曲线，以便于自由飞行下的协调控制方案进行比较。由于第二类拉格朗日方程常用来描述物理机械系统的动力学，因此由第二类拉格朗日方程，可以得到载体姿态、位置均不受控时的平面双臂空间机械臂的系统动力学方程：

其中:M(q)∈R^5×5为系统质量矩阵；为包含科氏力、离心力的五阶列阵；τ＝[0 τ₁ τ₂ τ₃ τ₄]^T∈R⁵为由系统施加于关节的控制力矩组成的五阶列阵；q＝[q_b q_r ^T]^T为系统的广义坐标，且q_b＝θ₀为本体的姿态角，q_r＝[θ₁ θ₂ θ₃ θ₄]^T为关节的转动角。

基于上一部分所设计的PD控制器对平面双臂空间机械臂进行轨迹跟踪控制仿真。

由图5至图8的曲线可以得出PD控制器可以使机械臂跟踪轨迹规划得出的目标轨线，且跟踪误差在10^-3量级。值得注意的是，由于自由漂浮飞行模式下，卫星本体不受控，在机械臂的跟踪过程中，卫星本体的位姿均受到了影响。平台的运动漂移在0.1m以内，机械臂的控制力矩在0.1Nm以内。仿真结果表明，机械臂的角度跟踪误差在10^-2量级以内，机械臂从平台跟踪运动到目标的时间为30s。

本发明的仿真效果：

为了保持同数据中继卫星的通讯，并且保持太阳能电池帆板对日定向，装载机械臂的卫星自身的姿态必须受到控制，因此，自由漂浮状态只能在短时间或小范围内实现，大部分时间内，空间机器人都处于自由飞行状态下，那么，针对不同的任务要求，保持一定精度的卫星(即基座)的姿态控制是必需的。

因为我们要对平面双臂空间机械臂进行仿真分析，所以将5.1节中的运动学方程代入能量守恒中，由第二类拉格朗日方程，可以得到载体姿态受控，位置不受控时的平面双臂空间机械臂的系统动力学方程：

其中：M(q)∈R^5×5为系统质量矩阵；为包含科氏力、离心力的五阶列阵；τ＝[τ₀ τ₁ τ₂ τ₃ τ₄]^T∈R⁵为由系统施加于机械臂基座和关节的控制力矩组成的五阶列阵；q＝[q_b q_r ^T]^T为系统的广义坐标，且q_b＝θ₀为本体姿态角，q_r＝[θ₁ θ₂ θ₃ θ₄]^T为关节转动角。

我们基于具体实施方式一或四中的PD控制器对平面双臂空间机械臂进行轨迹跟踪控制仿真。

仿真结果如图9至图13所示。

对比空间机械臂自由漂浮和自由飞行的仿真结果，我们可以看出由于自由飞行状体下需要对本体基座施加控制力矩，且本体基座质量很大。所以仿真结果中的基座力矩τ₀的幅值明显大于关节力矩，而自由漂浮状态下基座不控，所以所需要的控制能量明显小于自由飞行状态。

由于自由飞行姿态可控，所以基座的运动轨迹可以逐渐收敛于期望轨迹，即误差趋近于0。而自由漂浮状态不可控，所以基座的轨迹曲线逐渐发散，误差不收敛。

从误差仿真曲线中可以看出，双臂空间机械臂的系统通过PD控制可以很好的跟踪上期望的运动轨迹，使误差可以稳定在10^-3的范围内，实现稳定控制，满足控制要求。

仿真结果表明，机械臂的角度跟踪误差在10^-3量级以内，机械臂从平台跟踪运动到目标的时间为30s。

本发明使用的仿真参数：由图1可知：

本体质心位置坐标为：

x₀＝x_c+B₀₁cosθ₀+B₀₂cos(θ₁-θ₀)+B₀₃cos(θ₁+θ₂-θ₀)-B₀₄cos(θ₀+θ₃)-

B₀₅cos(θ₀+θ₃+θ₄) \*MERGEFORMAT(11

y₀＝y_c+B₀₁sinθ₀-B₀₂sin(θ₁-θ₀)-B₀₃sin(θ₁+θ₂-θ₀)-B₀₄sin(θ₀+θ₃)-

B₀₅sin(θ₀+θ₃+θ₄) \*MERGEFORMAT(118

其中x_c和y_c为系统质心的位置坐标，且：

B₀₁＝l₀(m₁+m₂-m₃-m₄)/M,B₀₂＝(m₂l₁+m₁a₁)/M

B₀₃＝m₂a₂/M,B₀₄＝(m₄l₃+m₃a₃)/M,B₀₅＝(m₄a₄)/M对式，，求时间t的导数，得到本体质心速度表达式：

各分体质心的速度表达式:

式中：

B₁₁＝l₀(m₁+2m₃+2m₄)/M

B₁₂＝[(m₀+m₂+m₃+m₄)a₁-m₂l₁]/M

B₁₃＝m₂a₂/M，B₁₄＝(m₄l₃+m₃a₃)/M,B₁₅＝(m₄a₄)/M

B₂₁＝l₀(m₁+2m₃+2m₄)/M

B₂₂＝[(m₀+m₁+m₃+m₄)l₁-m₁a₁]/M

B₂₃＝(m₀+m₁+m₃+m₄)a₂/M

B₂₄＝(m₄l₃+m₃a₃)/M,B₂₅＝(m₄a₄)/M

B₃₁＝l₀(m₀+2m₁+2m₂)/M

B₃₂＝(m₂l₁+m₁a₁)/M,B₃₃＝m₂a₂/M

B₁₄＝[-m₄l₃+(m₀+m₁+m₂+m₄)a₃]/M,B₃₅＝(m₄a₄)/M

B₄₁＝l₀(m₀+2m₁+2m₂)/M

B₄₂＝(m₂l₁+m₁a₁)/M,B₄₃＝m₂a₂/M

B₄₄＝[-m₄a₃+(m₀+m₁+m₂+m₃)l₃]/M

B₄₅＝(m₀+m₁+m₂+m₃)a₄/M

表1两自由度平面空间机械臂的标称参数

仿真所使用的控制器设计参数：

期望关节角轨迹是用五次多项式插值得到的，期望关节角起始状态为终止状态实际的关节角初始位置为与初始期望位置有一定的偏差。基座航天器初始姿态角与期望值均设为0。

跟踪控制器中的K_p＝10I₅,K_d＝10I₅。

仿真分析：

由以上仿真结果可以看出，机械臂的跟踪误差无法收敛，呈震荡状态，且卫星本体的跟踪误差数值也较大，基于此，通过机械臂与卫星本体分开进行协调控制的方式设计一种前馈补偿的PD控制器来使系统的控制精度进一步提高，且控制力矩仍保持在原最大控制幅值内。

基于式(116)的机械臂系统的动力学模型，我们对其采用协调控制的方法。

将M矩阵分解为4个分块子矩阵M₁₁、M₁₂、M₂₁、M₂₂，将C矩阵也分解为4个分块子矩阵C₁₁、C₁₂、C₂₁、C₂₂。

选择空间机械臂运动控制规律为：

基于上述的控制律对平面双臂空间机械臂进行轨迹跟踪控制仿真。

仿真结果如图14至图19所示。

根据仿真结果可以看出，应用协调控制策略能够使轨迹跟踪的误差控制在10^-5以内。相比于其他控制算法，能够达到更加精确的控制，更好地克服参数不确定性和非参数不确定性对控制系统带来的影响，且控制力矩仍保持在原控制力矩范围内，更好地达到控制系统的要求，实现稳定跟踪控制。

仿真结果表明，机械臂的角度跟踪误差在10^-5量级以内，机械臂从平台跟踪运动到目标的时间为30s。因此，该技术指标达到项目合同书中的要求。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims

1.一种空间双机械臂系统运动协调控制方法，包括：

步骤三、通过PD控制器对空间双机械臂系统轨迹进行跟踪控制；

其特征在于，步骤二具体包括：

步骤二二、假设存在t，使得：

其中，分别为第k条机械臂的关节角、角速度和角加速度；和分别为的下限值和上限值，和分别为的下限值和上限值、和分别为的下限值和上限值；t₀为轨迹规划的初始时刻，t_f为轨迹规划的末端时刻；

使用如下的五次多项式对关节函数进行参数化处理：

令向量c^k中的元素为预设的已知值；为待求解的参数；

步骤二三、令求方程组F(c^k)＝0的解；其中：

其中J_ω ^k为机械臂末端角速度相关的雅克比矩阵，为机械臂末端线速度相关的雅克比矩阵；为机械臂末端运动到的期望角度，为机械臂末端运动到的期望位置。

2.根据权利要求1所述的空间双机械臂系统运动协调控制方法，其特征在于，步骤一具体包括：

其中，N表示机械臂的总数；k表示第k个机械臂，且k为1或2；n_k表示第k个机械臂的连杆个数；表示自由度为3×n_k的实数空间；

为第k个机械臂第i个连杆的质量；k_i ^k表示J_i ^k的旋转方向的单位矢量，J_i ^k为第k个机械臂中连接第i-1个连杆与第i个连杆的关节；p_i ^k表示关节J_i ^k的位置矢量；为第k个机械臂的关节角速度向量；r_i ^k为连杆质心的位置矢量；

其中M(q)为机械臂的广义惯性张量，q为空间机械臂系统关节角向量，为机械臂与基座相关的非线性力矩，为耦合系数矩阵，τ为控制力矩。

3.根据权利要求2所述的空间双机械臂系统运动协调控制方法，其特征在于，所述步骤三具体包括：

步骤三一、将步骤一二中的公式拆分成如下形式：

其中τ_m为机械臂的控制律，τ_b为卫星本体的控制律；

其中其中表示向量空间；

其中

步骤三三、得出空间卫星本体和机械臂的控制律为：

其中q_d,θ_d分别表示卫星本体、机械臂的待跟踪信号；q_b为基座的姿态向量；为机械臂的关节角速度；为期望的位置移动轨迹的二阶导数；K_md为机械臂的速度及角速度的增益系数；K_mp为机械臂的位置及角度的增益系数；K_bd为卫星本体的速度及角速度的增益系数；K_bp为卫星的位置及角度的增益系数；e_m＝θ_d-θ为机械臂的跟踪误差；e_b＝q_pd-q_b为卫星平台的跟踪误差。