CN101733749B - 空间机器人多领域统一建模与仿真*** - Google Patents

Abstract

空间机器人多领域统一建模与仿真***，由空间机器人路径规划器(1)、关节轴模块(2)、空间机器人手眼相机测量模块(3)、空间机器人机构模块(4)、世界坐标系及中心体重力场(5)、轨道动力学及空间环境模块(6)、空间机器人基座敏感器模块(7)、推进模块(8)、反作用飞轮组件(10)及空间机器人基座姿轨控模块(9)组成。各模型库采用多领域物理***建模语言Modelica开发，彻底实现了机械、电气、软件、控制等不同领域模型之间的无缝集成和数据交换，实现多学科优化设计的目标。基于该建模与仿真***，可方便地实现自由飞行、自由漂浮模式下，单臂、多臂空间机器人的建模与仿真。

Description

空间机器人多领域统一建模与仿真***

技术领域

本发明涉及一种空间机器人多领域统一建模与仿真***，可用于空间机器人***的机械、电气、控制、软件等多领域一体化的建模，并进行闭环控制仿真。 

背景技术

空间机器人***涉及的学科领域很多，包括机械、电气、自动控制、计算机、航天器轨道及姿态动力学，等等。整个***的动力学特性为多个领域交互作用的结果。在以往的工程实践中，不同阶段——部件级、分***级、***级等建模和仿真的侧重点不同。在部件开发阶段，设计师往往强调的是部件自身的细节，而该部件与其他部件的交互作用却往往被忽略或进行粗略的近似。相反地，在分***/***级的开发阶段，部件间的耦合作用却是主要考虑的因素，部件自身的很多细节又被大大地简化了(Agrawal，S.K.，Chen，M.Y.and Annapragada，M.，et al.Modelling and Simulation of Assembly in a Free-floating WorkEnvironment by a Free-floating Robot.Transactions of ASME Journal of Mechanical Design，1996，118(1)：pp.115-120)。所有的简化或近似都是基于一定的假设条件的，当条件满足时，不会对分析结果产生影响；但如果实际***在工作中出现了超出假设条件的情况，则其模型不能准确反映分***/***的行为，基于此模型设计的控制算法将会失效。举例来说，当设计一套机构的控制***时，一般认为机械的频响与电气相比慢很多，由此按经典控制理论设计PID控制器，但当机构高速运动或机构本身的质量很轻时，控制器的带宽就会与对象的一阶振动频率耦合，导致控制对象发生共振，造成灾难性的后果。因此，设计性能良好的控制器，必须将机械、电气及控制***纳入统一框架内(Samin J C，Brüls O，Collard J F，et al.Multiphysicsmodeling and optimization of mechatronic multibody systems.Multibody System Dynamic.2007(18)：345-373)，开展多领域统一建模与仿真研究，以实现多学科优化设计(Multidisciplinary Design Optimization，MDO)的目标(Sobieszczanski-Sobieski J，Haftka T.Multidisciplinary aerospace design optimization：Survey of recentdevelopments.1 996，AIAA 9620711)。 

多领域建模与仿真方法主要有三种：基于接口的方法，基于高层体系结构(HighLevel Architecture，HLA)的方法，以及基于统一建模语言的方法。基于统一建模语言的方法对来自不同领域的***构件采用统一方式进行描述，彻底实现了不同领域模型之间的无缝集成和数据交换。Modelica语言是目前盛行的一种多领域物理***建模语言，它具有模型重用性高、建模简单方便、无须符号处理等许多优点。M.Lovera等利用Modelica语言进行了卫星姿态和轨道控制的仿真，但对于执行机构——飞轮、磁力矩器等的建模仍然采用简化的数学描述(Lovera M.Control-oriented modelling and simulation of spacecraft attitude and orbitdynamics.Mathematical and Computer Modelling of Dynamical Systems.2006，12(1)：73-88)。目前文献中尚未见到对于空间机器人***的多领域建模与仿真方面的研究。因此，开发一套空间机器人多领域统一建模与仿真***是非常必要和迫切的。

发明内容

本发明的目的在于克服现有技术的不足，提供了一种空间机器人多领域统一建模与仿真***。利用该***，可建立包括机械、电气、控制、软件等多个领域的统一多领域模型，基于该模型开展的闭环控制仿真，充分地体现了***内容的耦合关系，实现多学科设计的优化。 

空间机器人多领域统一建模与仿真***，由空间机器人路径规划器(1)、关节轴模块(2)、空间机器人手眼相机测量模块(3)、空间机器人机构模块(4)、世界坐标系及中心体重力场(5)、轨道动力学及空间环境模块(6)、空间机器人基座敏感器模块(7)、推进模块(8)、反作用飞轮组件(10)及空间机器人基座姿轨控模块(9)组成。其中： 

空间机器人路径规划器(1)，接收来自于手眼视觉测量模块(3)的相对位置、姿态测量结果，自主规划机械臂和基座的运动轨迹——期望关节角、角速度、角加速度、基座姿态角、角速度，作为关节轴模块(2)和空间机器人基座姿轨控模块(9)的输入。空间机器人路径规划器(1)还能实现各种笛卡尔空间、关节空间的规划算法，包括梯形插值、三次样条、多项式插值等常规路径规划，以及机械臂与基座的协调规划等，根据不同的任务要求，可选择合适的路径规划算法； 

关节轴模块(2)由机械臂所有关节轴组成，每个关节轴包括关节控制器、关节控制器、电机及其驱动器、谐波减速器、关节敏感器。关节控制器接收任务规划器(1)输出的期望关节角、角速度、角加速度，以及关节敏感器的当前关节角、角速度、电流，实现位置环、速度环、电流环的控制算法，产生关节控制力矩，通过谐波减速器环节后作用在空间机器人机构模型(4)上； 

空间机器人机构模块(4)包括空间机器人***多刚体机构模型、目标卫星单刚体模型。该模块接收关节轴模块(2)输出的关节驱动力拒、反作用飞轮组件(10)输出的基座姿态控制力矩、以及轨道动力学及空间环境模块(6)输出的干扰力矩，计算作用后的机械臂各关节角、角速度，基座姿态、角速度，以及目标卫星姿态、位置，输出作为手眼视觉测量模块(3)、轨道动力学及空间环境模块(6)、空间机器人基座敏感器模块(7)以及关节轴模块(2)中的关节敏感器的输入； 

手眼视觉测量模块(3)，接收空间机器人机构模块(4)输出的机械臂末端位置、姿态，以及目标卫星的位置、姿态，计算目标卫星相对于机械臂末端坐标系的位置、姿态，该位置、姿态数据叠加上相机测量噪声数据后成为手眼视觉测量数据，作为该模块的输出、空间机器人路径规划器(1)的输入； 

世界坐标系及中心体重力场(5)，建立世界坐标系与***本体系的关系——指向、原点相对位置，以及中心体的重力场，可实现不同中心体下的空间机器人***多领域统一动力学的建模与仿真研究。该模块与空间机器人机构模块(4)相连。 

轨道动力学及空间环境模块(6)，接收空间机器人机构模块(4)输出的机器人基座本体系相对于惯性系的姿态，以及推进模块(8)输出的推力脉冲，计算空间机器人***质心的位置、基座本体系相对于轨道坐标系的姿态、角速度、所处轨道位置的磁场强度以及环境干扰力矩，作为敏感器模块(7)、空间机器人基座姿轨控模块(9)，以及空间机器人机构模块(4)的输入； 

空间机器人基座敏感器模块(7)，接收空间机器人机构模块(4)输出的基座本体系相对于惯性坐标系的姿态、角速度，以及轨道动力学及空间环境模块(6)输出的基座本体系相对于轨道坐标系的姿态、角速度，叠加测量噪声后作为敏感器的输出，该输出是空间机器入基座姿轨控模块(9)的输入； 

空间机器人基座姿轨控模块(9)，接收基座姿态敏感器(7)输出的当前姿态角、角速度，以及空间机器人路径规划器(1)输出的期望姿态角、角速度，执行航天器的各种导航、制导与控制算法，如对目标卫星进行跟踪、接近、绕飞、轨道保持等的GNC算法，以及常规模式下自身姿态、轨道的控制算法等，生成反作用飞轮组件(10)和推进***(8)的控制指令，其中反作用飞轮组件的控制指令为控制电压、推进***的控制指令为推力脉冲； 

推进***模块(8)，接收空间机器人基座姿轨控模块(9)输出的推力脉冲，产生各推力器的作用力，作用于轨道动力学及空间环境模块(6)； 

反作用飞轮组件(10)，接收空间机器人基座姿轨控模块(9)输出的各推力器控制电压，产生各个飞轮的作用力矩，作用于空间机器人机构模块(4)。 

所述的空间机器人路径规划器(1)采用多领域统一建模语言Modelica实现了一种空间机器人目标捕获的自主路径规划方法，该方法利用手眼相机的相对位姿测量值，实时规划空间机器人的运动，以最终捕获目标。主要包括如下步骤：位姿偏差计算、目标运动的预测、空间机器人末端运动速度规划、空间机器人避奇异的路径规划、基座运动的预测等。首先，根据手眼测量数据判断相对位姿偏差e_p和e_o是否小于设定的阈值ε_p和ε_o，若小于，则闭合手爪、捕获目标；反之，则根据相对位姿偏差，实时估计目标的运动状态，并将估计的结果反应到机械臂末端速度的规划中，以保证机械臂末端时刻朝最近的方向趋近目标，机械臂末端能自主跟踪目标的运动，直到最后捕获目标。规划出末端运动速度后，即调用自主奇异回避算法，以解算关节的期望角速度，并据此预测机械臂运动对基座的扰动，当扰动超出容许的范围时，则自动调整机械臂的关节运动速度，以保证期望的偏转在许可的范围内。整个过程一直持续到机械臂捕获到目标为止。 

所述的关节轴模块(2)采用多领域统一建模语言Modelica建立了机械臂各关节的机械、电气、控制等多学科领域一体的模型，每个关节轴模型由关节控制器、电机及其驱动器、关节传动机构、关节敏感器等组成。关节控制器实现了位置环和速度环的控制，其中，位置环采用PD控制，而速度环采用PI控制；电机模型中包含了电枢电阻Ra、电枢电感La、反电动势emf、电机轴Jmotor等环节；驱动器部分由电阻R、 电容C、运算放大器Op、电压源Vs及接地g等组成；关节传动机构包括谐波减速器、齿轮减速箱等，是连接电机轴和关节轴的中间部分，该部分的建模由库伦摩擦bearingFrition、弹性阻尼器springDamper，以及理想减速模型idearGear三部分组成； 

所述的空间机器人手眼相机测量模块(3)采用多领域统一建模语言Modelica的多体敏感器库MultiBody.Sensors中的相对运动敏感器RelativeSensor，叠加上相机测量噪声，作为手眼相机的测量数据； 

所述的空间机器人机构模块(4)采用多领域统一建模语言Modelica编写空间机器人***及目标卫星的多个刚体属性，以及刚体间的约束实现。每个刚体的属性包括质量、惯量、质心位置、坐标系a和坐标系b，其中质量、惯量、质心位置为刚体的质量特性参数，坐标系a、坐标系b则用于定义该刚体与相应约束的连接关系；刚体间的约束用于描述相连刚体间的相对运动关系，空间机器人基座与机械臂第一个连杆，以及机械臂各连杆之间为旋转关节，而目标卫星与惯性坐标系之间为6DOF的***，通过Modelica多体库中的FreeMotion实现； 

所述的世界坐标系及中心体重力场(5)采用多领域统一建模语言Modelica编写，建立世界坐标系与***本体系的关系(指向、原点相对位置)，以及地球的微重力场； 

所述的轨道动力学及空间环境模块(6)采用多领域统一建模语言Modelica编写，实现两星的相对轨道动力学方程——Hill方程，以及轨道环境干扰力、干扰力矩，包括太阳光压力/力矩、大气拖动力/力矩、剩磁力矩等； 

所述的空间机器人基座敏感器模块(7)采用多领域统一建模语言Modelica的多体敏感器库(MultiBody.Sensors)中的相对运动敏感器RelativeSensor，通过设置输出项、再叠加上相应姿态敏感器的测量噪声，作为基座敏感器的测量数据； 

所述的推进模块(8)、反作用飞轮组件(10)采用多领域统一建模语言Modelica建立，其中反作用飞轮组件(10)包括4个反作用飞轮，采用“三轴正交安装+一等倾角斜装”，通过设置可工作于整星零动量或偏置动量模式，单个飞轮的建模由驱动电路、电机及轮体组成； 

所述的空间机器人基座姿轨控模块(9)采用多领域统一建模语言Modelica实现相应的姿态、轨道控制算法，产生执行机构的控制指令——四个飞轮的控制电压，以及推进***的推力脉冲，对基座进行6DOF的控制，控制基座姿态、轨道按期望的轨迹运动。 

本发明与现有技术相比具有如下优点：(1)建立的模型包括了机械、电气、控制、软件等多个学科领域，全面反映多个领域交互作用的整体效果；(2)建模与仿真***中的各模块重用性好，可方便地建立任意自由度的、串/并联、单臂/多臂空间机器人***的多领域模型；(3)该建模与仿真***支持全数学的、半物理的仿真实验，还可方便的实现实时***的仿真；(4)该建模与仿真***具有和Simulink的接口，其所建模型可转换为Simulink的模块，与其他Simulink模块一样可在Simulink环境中被随意使用；同时， Simulink模块亦可转换为该模型库所支持的模块，作为模型库的一员；(5)该建模与仿真***支持模型的校验和更新，即可将实际的实验数据导入到该建模与仿真平台中，与仿真数据进行校核，并根据仿真数据与实验数据之差更新模型参数，使得所建模型与真实情况更加接近。 

附图说明

图1是典型的空间机器人在轨服务***； 

图2是空间机器人***功能模块； 

图3是空间机器人多领域统一建模与仿真***组成图； 

图4是空间机器人***几何参数及坐标系； 

图5是所建立的空间机器人及目标的多体***模型； 

图6是所建立的空间机器人及目标的多体***的3D视图； 

图7是机械臂单个关节轴模型的总体组成图 

图8是电机及其驱动器模型； 

图9是关节传动机构(gear)模型； 

图10是飞轮***的多领域模型； 

图11是“3正交+1斜装”飞轮***的3D模型； 

图12是推力器***的模型； 

图13是姿态控制结构图； 

图14是手眼视觉测量的模型图； 

图15是双臂空间机器人***的多领域模型； 

图16是双臂空间机器人***的多领域模型的3D图示。 

具体实施方式

一、空间机器人***多领域功能模块划分与建模仿真***的组成 

典型的空间机器人在轨服务***由一飞行基座和空间机械手组成，如图1所示。其中，飞行基座上安装了目标测量***、对接机构、姿轨控***等，空间机械手可由6DOF机械臂、抓捕手爪及手眼视觉组成。空间目标可能是故障卫星(如太阳帆板未展开)、废弃卫星或空间碎片等。要实现完整的空间机器人在轨服务***的建模，需包含如下功能模块： 

(1)空间机器人***动力学模块，包括空间机器人***的多体动力学模型、轨道动力学模型、轨道环境模型等； 

(2)关节模型：包括关节i(i＝1，...，6)控制器、电机及其驱动器模型、关节传动机构模型等； 

(3)空间机械臂路径规划器：包括视觉伺服控制、机械臂逆运动学、关节插值等算法； 

(4)基座姿态及轨道控制器AOCS：根据敏感器测量信息，对基座的位置、姿态进行控制； 

(5)敏感器模型：包括关节传感器模型(根据不同的应用情况，提供各个关节的位置、速度、力矩等测量信息)、手眼视觉测量模型、基座姿态敏感器模型等。 

各功能模块之间的连接关系如图2所示。本发明的空间机器人动力学建模与仿真***组成如图3所示。 

二、单臂空间机器人***多领域统一模型的建立 

不失一般性，以由六自由度串联机械臂和作为其基座的卫星组成的单臂空间机器人为例，整个***由七个刚体组成，分别记为B₀～B₆，其中B₀为基座、B₆为末端执行器。B_i-1与B_i(i＝1，…，6)之间通过旋转关节J_i(i＝1，…，6)连接。所建立的空间机器人***多领域统一模型如图3所示，包括空间机器人***的多刚体动力学模型SpaceRobot、关节轴模型Axis1～Axis6、空间机器人路径规划器Planner、基座的姿态轨道控制器AOCS、敏感器Sensors、飞轮Flywheel、推力器Thruster、轨道动力学OrbitDynAndDis等模块组成。 

(1)空间机器人***的机构模型 

为方便空间机器人***的建模，首先建立如图4所示的坐标系(相应于机械臂的折叠位置，此时定义为关节角的零位)，其中坐标系∑_b，即坐标系O_bX_bY_bZ_b为基座的参考坐标系，三轴对地模式下O_bX_b指向飞行方向、O_bZ_b指向地心，O_bY_b根据右手定则确定；∑₀为基座的质心坐标系，指向与∑_b一致；∑_i(i＝1，…，6)为杆件i的固连坐标系，原点位于第i个关节J_i，折叠状态下指向与∑_b一致；∑_e为机械臂末端工具坐标系，折叠状态下O_eX_e垂直于基座+Z面并指向Z轴，O_eZ_e沿手爪轴向指向外。 

按下面的步骤建立空间机器人***机构部分的模型： 

(a)建立重力场和世界坐标系 

任何机械模型的建立，都首先要建立惯性坐标系和重力场。将MultiBody库中的World图标拖到当前模型编辑窗中，双击该图标可对相关参数进行设置：i)重力类型“gravityType”选择“UniformGravity”，ii)重力加速度g赋值为0，即g＝0；iii)重力矢量方向定义为惯性系的Z轴。World模块同时建立了一个惯性系，后续各杆件的建立以此为参考。 

(b)建立基座及其与惯性系的约束 

基座是整个多体***中的第一个刚体，通过定义其质量、惯量、质心位置、与惯性系的约束关系和与下一个刚体的连接关系，可完整反映其运动状态。首先创建一个刚体，命名为B₀，并给B₀的相关参数赋值。双击该图标，弹出的对话框中，“General”界面定义质量特性(矢量r相当于ⁱL_i；r_CM相当于ⁱa_i；m即为Mass；I_11为I_xx，I_22为I_yy，I_33为I_zz，I_21为I_yx，I_31为I_zx，I_32为I_zy；“Animation”界面可定义几何外形，在″shapeType″一栏选择B₀的形状，标准形状包括矩形″box″、球形″sphere″、圆柱形″cylinder″等；对于复杂 的形状，可以由用户自行定义。基座的形状由文件0.dxf确定，因此″shapeType″一栏输入″0″即可。 

由于基座在空间具有6自由度运动能力，因此，B0与惯性系之间通过约束“FreeMotion”连接，表明基座的位置、姿态均可改变，同时，定义基座控制力、力矩的输入接口Tb、fb，其中fb作用于基座质心，因此需要定义一个固定平移的坐标变换，引出基座质心坐标系。 

(c)建立机械臂各关节及连杆的模型 

基座定义好后，即可定义关节J₁，其旋转轴为Z轴；由于J₁是可驱动的旋转关节，故用″ActuatedRevolute″进行定义，并引出其驱动轴接口axis1，该接口包含了力矩、旋转角信息。然后定义B₁，其动力学参数、几何外形的定义与B₀的定义类似，机械臂各杆的形状分别由文件1.dxf，…，6.dxf等定义。 

接着定义J₂(旋转轴为-Y轴)、B₂；J₃(旋转轴为-Y轴)、B₃；J₄(旋转轴为-X轴)、B₄；J₅(旋转轴为-Y轴)、B₅；J₆(旋转轴为X轴)、B₆，以及各关节的驱动轴接口axis2～axis6。 

目标卫星按类似于空间机器人基座的方法建立。最后所建立的空间机机器人机构部分的模型如图5所示，折叠状态下机械臂的3D视图如图6所示。 

(2)空间机器人***轨道动力学及环境 

(a)轨道动力学的建模 

记质点在惯性系下的位置为r₁，在中心天体体固系下的惯性加速度a_E，惯性系到中心天体体固系的坐标旋转矩阵为C_EI，反之为C_IE，则有 

${\stackrel{\·\·}{r}}_I=C_{\mathrm{IE}}{a}_E\left(C_{\mathrm{EI}}{r}_I\right)---\left(1\right)$

上式中，求导为在惯性系下的导数，括号表示a_E为C_EIr₁的函数。将上式积分即可得到质点在惯性系下的位置速度。 

追踪星和目标星的轨道坐标系分别记为O_o1X_o1Y_o1Z_o1、O_o2X_o2Y_o2Z_o2，两航天器的相对位置(追踪星质心O_o1在目标星轨道坐标系O_o2X_o2Y_o2Z_o2中的坐标)r_c＝[x，y，z]，相对速度为 ${\stackrel{\·}{r}}_c=[\stackrel{\·}{x},\stackrel{\·}{y},\stackrel{\·}{z}].$ 在两航天器相距较近，且均运行在近圆轨道的条件下，相对运动可用Hill方程进行简化描述： 

$\left\{\begin{array}{c}\stackrel{\·\·}{x}+2\mathrm{\ω}\stackrel{\·}{y}=f_x/m_1\\ \stackrel{\·\·}{y}-2\mathrm{\ω}\stackrel{\·}{x}-3{\mathrm{\ω}}^{2}y=f_y/m_1\\ \stackrel{\·\·}{z}+{\mathrm{\ω}}^{2}z=f_z/m_1\end{array}\right.---\left(2\right)$

其中，(f_x，f_y，f_z)为施加在追踪星上的控制力(在目标星轨道坐标系下的投影)。m₁为追踪星的质量，ω为轨道角速度，对圆轨道来说，ω为常值。 

(b)空间环境的建模 

对于低轨道航天器，主要的空间环境干扰包括：气动力矩、太阳辐射力矩、剩磁力矩和重力梯度力矩。气动力、力矩表示为： 

${\stackrel{\→}{T}}_A={\stackrel{\→}{L}}_B\×{\stackrel{\→}{F}}_B---\left(4\right)$

式中： 

---质心到卫星中心体压心的矢径 

S_B---星体的迎流面积 

v， 

---气流速率大小和来流方向的单位矢量 

太阳辐射压力、力矩 

${\stackrel{\→}{T}}_s=\underset{i}{\overset{N}{\mathrm{\Σ}}}{\stackrel{\→}{L}}_i\×{\stackrel{\→}{F}}_i---\left(5\right)$

式中： 

F_e---太阳常数1358W/m²

C---真空中光速 

---受晒面外法向的单位矢量 

ε_i---入射角 

S_i---受晒面面积 

N---受晒面个数 

---整星质心到第i个受晒面压心的矢径 

剩磁力矩表示为： 

${\stackrel{\→}{T}}_M={\stackrel{\→}{M}}_r\×\stackrel{\→}{B}---\left(7\right)$

式中： 

---整星的剩磁矩 

---卫星所处位置上的地磁场强度，由星上实时轨道计算和地磁场模型计算得到。 

重力梯度力矩为： 

${\stackrel{\→}{T}}_g=3{{\mathrm{\ω}}_0}^2{\stackrel{\→}{i}}_g\×(I\·{\stackrel{\→}{i}}_g)---\left(8\right)$

式中： 

ω₀---轨道角速度 

---地垂单位矢量 

I---卫星转动惯量阵 

在对地指向三轴稳定姿态情况下，卫星重力梯度力矩可根据下式计算： 

$\left\{\begin{array}{c}{T}_{\mathrm{gx}}=3{{\mathrm{\ω}}_0}^2(I_z-I_y)c_{23}{c}_{33}\\ T_{\mathrm{gy}}=3{{\mathrm{\ω}}_0}^2(I_x-I_z)c_{13}{c}_{33}\\ T_{\mathrm{gx}}=3{{\mathrm{\ω}}_0}^2(I_z-I_y)c_{23}{c}_{33}\end{array}\right.---\left(9\right)$

式中：(c₁₃，c₂₃，c₃₃)＝(-sinθ，sinφcosθ，cosφcosθ)。 

(3)机械臂关节轴的建模 

机械臂关节轴包含了关节控制器、电机及其驱动器、谐波减速器、关节位置敏感器等，模型的总体组成如图7所示。控制器实现了位置环和速度环的控制，电流环的控制在“电机及其驱动器模型”中实现。其中，位置环采用PD控制，而速度环采用PI控制，各模块可从Modelica.Blocks.Continuous库中选取后，对相关参数进行赋值来实现。电机模型中包含了电枢电阻Ra、电枢电感La、反电动势emf、电机轴Jmotor等环节，驱动器部分由电阻R、电容C、运算放大器Op、电压源Vs及接地g等组成，除Jmotor外，其它部分可在Modelica.Electrical.Analog.Basic中选取。电机相关参数及模型如图8所示。关节传动机构部分一般为谐波减速器、齿轮减速箱等，是连接电机轴和关节轴的中间部分。为反映真实情况，该部分的建模由库伦摩擦bearingFrition、弹性阻尼器springDamper，以及理想减速模型idearGear三部分组成，如图9所示。 

(4)机械臂路径规划器 

机械臂的路径规划器用于规划期望的关节角、角速度轨迹，作为关节控制器的输入。根据不同的任务，可采用不同的路径规划方法，如关节空间点到点PTP路径规划、关节空间连续CP路径规划、笛卡尔点到点路径规划、笛卡尔空间连续路径规划，以及基于视觉的自主路径规划(视觉伺服控制)等。以关节空间点到点路径规划为例，采用五次多项式对规划关节i(i＝1，…，6)在[0，t_f]时间段内的运动，即： 

θ_i＝a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t+a_i0    (10) 

其中，θ_i为关节i的运动角度，a_i0～a_i5为五次多项数的待定参数，t为时间。相应的关节角速度和角加速度分别为： 

${\stackrel{\·}{\mathrm{\θ}}}_i=5a_{i5}{t}^4+4a_{i4}{t}^3+3a_{i3}{t}^2+2a_{i2}t+a_{i1}---\left(11\right)$

${\stackrel{\·\·}{\mathrm{\θ}}}_i=20a_{i5}{t}^3+12a_{i4}{t}^2+6a_{i3}t+2a_{i2}---\left(12\right)$

有下列约束条件： 

θ_i(0)＝θ_i0，θ_i(t_f)＝θ_if             (13) 

${\stackrel{\·}{\mathrm{\θ}}}_i\left(0\right)={\stackrel{\·\·}{\mathrm{\θ}}}_i\left(0\right)={\stackrel{\·}{\mathrm{\θ}}}_i\left(t_f\right)={\stackrel{\·\·}{\mathrm{\θ}}}_i\left(t_f\right)=0---\left(14\right)$

可解出： 

a_i0＝θ_i0，a_i1＝a_i2＝0                  (15) 

$a_{i3}=\frac{20({\mathrm{\θ}}_{\mathrm{if}}-{\mathrm{\θ}}_{i0})-(8{\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{if}}+12{\stackrel{\·}{\mathrm{\θ}}}_{i0})t_f+({\stackrel{\·\·}{\mathrm{\θ}}}_{\mathrm{if}}-3{\stackrel{\·\·}{\mathrm{\θ}}}_{i0})t_f^2}{2t_f^3}---\left(16\right)$

$a_{i4}=\frac{30(-{\mathrm{\θ}}_{\mathrm{if}}+{\mathrm{\θ}}_{i0})+(14{\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{if}}+16{\stackrel{\·}{\mathrm{\θ}}}_{i0})t_f+(-2{\stackrel{\·\·}{\mathrm{\θ}}}_{\mathrm{if}}+3{\stackrel{\·\·}{\mathrm{\θ}}}_{i0})t_f^2}{2t_f^4}---\left(17\right)$

$a_{i5}=\frac{12({\mathrm{\θ}}_{\mathrm{if}}-{\mathrm{\θ}}_{i0})-6({\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{if}}+{\stackrel{\·}{\mathrm{\θ}}}_{i0})t_f+({\stackrel{\·\·}{\mathrm{\θ}}}_{\mathrm{if}}-{\stackrel{\·\·}{\mathrm{\θ}}}_{i0})t_f^2}{2t_f^5}---\left(18\right)$

利用Modelica语言实现如上的路径规划算法。 

(5)基座姿态控制执行机构的建模 

飞轮实质上是一个带有大转动惯量的力矩马达，由驱动电路、电机及轮体组成。以“三轴正交安装+一等倾角斜装”的飞轮组件为例，其多领域模型如图11所示，由驱动电流、电机及其驱动电路(Motor1~Motor4)、轴承摩擦(bearingFriction1~bearingFriction4)、转动关节(Jx，Jy，Jz，Js)、轮体(Bx，By，Bz，Bs)，以及坐标转换关系(T1~T4)。其中，T1~T4分别建立了各飞轮安装坐标系相对于基座质心坐标系的位置和姿态(frame_a1与***质心坐标系CM直接连接)，而旋转关节定义了各轮体与基座之间的旋转关系，电机及其轴承的输出轴与关节的驱动轴相连。飞轮***的3D模型如图11所示。 

推力器用于基座姿态、轨道的6DOF控制。每一个推力器的推力矢量表示为f_i、作用点矢量表示为r_i，推力脉冲表示为τ_i，则推力器的作用等效于作用于质心的作用力及力矩。其中作用力 

$F_i\left(t\right)=\left\{\begin{array}{cc}{f}_i,& \mathrm{if}{t}_0\≤t\≤t_0+{\mathrm{\τ}}_i\\ 0,& \mathrm{else}\end{array}\right.---\left(19\right)$

相应的作用力矩 

T_i(τ_i)＝r_i×F_i                         (20) 

多个推力器的合成作用按矢量和计算： 

$F_b=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{F}_i---\left(21\right)$

$T_b=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{T}_i---\left(22\right)$

推力器组件的模型如图12所示。 

(6)基座AOCS***的建模 

姿态控制采用PID+前馈补偿的策略，控制框图如图13所示。控制律如下： 

T_c＝K_p·q_e+K_I·∫q_e+K_d·(ω_d-ω_b)+T_B    (23) 

其中，K_p、K_I、K_d分别为控制器的比例、积分、微分控制参数，q_e为姿态四元数误差，ω_d为期望的姿态角速度，ω_b为实际通过姿态敏感器测出角速度；T_B为补偿力矩；T_c为期望的作用于基座的控制力矩，该力矩通过飞轮的反作用力矩来实现。飞轮组的方向矩阵为 

$C=\left[\begin{array}{cccc}{n}_x& n_y& n_z& n_s\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& -\frac{1}{\sqrt{3}}\\ 0& 1& 0& -\frac{1}{\sqrt{3}}\\ 0& 0& 1& -\frac{1}{\sqrt{3}}\end{array}\right]---\left(24\right)$

令四个飞轮的控制电流分别为i₁～i₄，组成的矢量为U＝[i₁，i₂，i₃，i₄]^T。飞轮组中仅选择三个参与控制，如果第i个飞轮不参与控制，则由三轴姿态控制的指令力矩T_c分配各个飞轮的控制电压如下(其中负号表示作用于飞轮的力矩为作用于星体的力矩符号相反)： 

$U=-{\mathrm{KC}}_i^{-1}{T}_c---\left(25\right)$

其中，K为电机的力矩常数组成的4×4对角阵，C_i为令矩阵(24)中的第i列全为0后得到的矩阵，C_i ^-1为C_i的广义逆。例如，若第四个飞轮不用于控制，则 

$U=-{\mathrm{KC}}_4^{-1}{T}_c=-K\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]$ $T_c=-\left[\begin{array}{c}{k}_1{T}_{\mathrm{cx}}\\ k_2{T}_{\mathrm{cy}}\\ k_3{T}_{\mathrm{cz}}\\ 0\end{array}\right]---\left(26\right)$

即三个正交飞轮分别完成三个轴的姿态控制。当其中一个出现故障时，斜装飞轮将用于备份。如假设X轴飞轮出现故障，则按下式分配各飞轮的控制电流： 

$U=-{\mathrm{KC}}_1^{-1}{T}_c=-\mathrm{KC}={\left[\begin{array}{cccc}0& 0& 0& -\frac{1}{\sqrt{3}}\\ 0& 1& 0& -\frac{1}{\sqrt{3}}\\ 0& 0& 1& -\frac{1}{\sqrt{3}}\end{array}\right]}^{-1}{T}_c---\left(27\right)$

$=-\left[\begin{array}{c}0\\ k_2(-T_{\mathrm{cx}}+T_{\mathrm{cy}})\\ k_3(-T_{\mathrm{cx}}+T_{\mathrm{cz}})\\ -\sqrt{3}{k}_4{T}_{\mathrm{cx}}\end{array}\right]$

其中T_cx，T_cy，T_cz为控制力矩T_c在各轴的分量。 

(7)敏感器的建模 

敏感器用于提供控制器的测量信息。在Modelica的多体库中有一些现成的敏感器包MultiBody.Sensors，但提供的是理想的相对/绝对位置、姿态、线速度、角速度等，而实际中的敏感器是有测量误差的，因此，通过在理想敏感器的基础上叠加测量数据，实现真实敏感器的建模。以机械臂的手眼相机为例，首先用RelativeSensor(路径为MultiBody.Sensors.RelativeSensor)提供理想的位置、姿态测量，然后叠加相机的测量噪声，其中测量噪声为零均值的高斯噪声，位置、姿态测量的标准差分别为： 

σ_p＝1.2×10^-3                 (28) 

σ_o＝0.25                    (29) 

随机数采用自定义的随机函数RandomNormal(Time)实现，该函数的Modelica程序如下： 

     function randn″random″/*--y＝randn(seed，std)--*/ 

       input Real seed； 

       input Real std； 

       output Real y； 

       algorithm 

       y：＝RandomNormal(seed)*std； 

     end randn； 

     model randnBlk 

      import Modelica.Constants.pi； 

      parameter Integer num＝6； 

      parameter Real stdPose＝1.2″The standard deviation of the position″； 

      parameter Real stdAtt＝0.25″The standard deviation of the attitude″； 

      final parameter Real std[num]＝{stdPose*1e-3，stdPose*1e-3，stdPose*1e-3，stdAtt*pi/180.0， 

stdAtt*pi/180.0，stdAtt*pi/180.0}； 

      Modelica.Blocks.Interfaces.OutPort OutSig(n＝num) 

        annotation(extent＝[100，-10；120，10])； 

     equation 

       for i in 1：num loop 

            OutSig.signal[i]＝SpaceRobotLibNew.MathFcn.randn(time+(i-1)*10，std[i])； 

       end for； 

     end randnBlk； 

最后建立的手眼相机敏感器模型如图14所示。其它敏感器的建模过程与此类似。 

三、多臂空间机器人***的多领域统一模型 

上面所建立的机械模型、关节轴模型、规划器模型等具有可重用性，因此，在建立单臂空间机器人***多领域模型的基础上，可方便的建立双臂空间机器人***的多领域模型。 

假设在飞行基座上对称安装了两套完全一致的空间机械臂，除图5所示的连杆关系外，另一臂的第一个关节位于基座参考系下(1.1，0，-0.712)的位置，其安装坐标系相对于基座参考系的姿态为(0，0，180°)(本文的所有姿态角采用3-2-1欧拉角的形式)。因此，将臂1的第一个杆件B₁坐标系(∑₁)进行平移、旋转后，可得到臂2的第一个杆件B₇坐标系(∑₇)，即定义平移MultiBody.Parts.FixedTranslation、旋转MultiBody.Parts.FixedRotation可实现将∑₁到∑₇的变换：Translate(Z，-0.712×2)、Rotate(X，180°)。由此，将原臂1的B₁~B₆、J₁~J₆、Axis1~Axis6同时复制，并进行合适的连线，可完成双臂空间机器人机构部分的建模。基于各模块的可重用性，所建立的包含关节各轴模型、路径规划模块，以及基座GNC模块的整个双臂空间机器人***的多领域模型如图15所示，相应的3D图示如图16所示。 

Claims (8)

1.空间机器人多领域统一建模与仿真***，其特征在于包括：空间机器人路径规划器(1)、关节轴模块(2)、空间机器人手眼相机测量模块(3)、空间机器人机构模块(4)、世界坐标系及中心体重力场(5)、轨道动力学及空间环境模块(6)、空间机器人基座敏感器模块(7)、推进模块(8)、空间机器人基座姿轨控模块(9)、反作用飞轮组件(10)，其中：

空间机器人路径规划器(1)，接收来自于手眼相机测量模块(3)的相对位置、姿态测量结果，自主规划机械臂和基座的运动轨迹——期望关节角、关节角速度、关节角加速度、基座姿态角、基座姿态角速度，作为关节轴模块(2)和空间机器人基座姿轨控模块(9)的输入；空间机器人路径规划器(1)还能实现各种笛卡尔空间、关节空间的规划算法，包括梯形插值、三次样条、多项式插值常规路径规划，以及机械臂与基座的协调规划，根据不同的任务要求，可选择合适的路径规划算法；

关节轴模块(2)由机械臂所有关节轴组成，每个关节轴包括关节控制器、电机及其驱动器、谐波减速器、关节敏感器；关节控制器接收空间机器人路径规划器(1)输出的期望关节角、角速度、角加速度，以及关节敏感器的当前关节角、角速度、电流，实现位置环、速度环、电流环的控制算法，产生关节控制力矩，通过谐波减速器环节后作用在空间机器人机构模块(4)上；

空间机器人机构模块(4)包括空间机器人***多刚体机构模型、目标卫星单刚体模型；该模块接收关节轴模块(2)输出的关节驱动力矩、反作用飞轮组件(10)输出的基座姿态控制力矩、以及轨道动力学及空间环境模块(6)输出的干扰力矩，计算作用后的机械臂各关节角、关节角速度、基座姿态角、基座姿态角速度，以及目标卫星姿态、位置，输出作为手眼相机测量模块(3)、轨道动力学及空间环境模块(6)、空间机器人基座敏感器模块(7)以及关节轴模块(2)中的关节敏感器的输入；

手眼相机测量模块(3)，接收空间机器人机构模块(4)输出的机械臂末端位置、姿态，以及目标卫星的位置、姿态，计算目标卫星相对于机械臂末端坐标系的位置、姿态，该位置、姿态数据叠加上相机测量噪声数据后成为手眼视觉测量数据，作为该模块的输出、空间机器人路径规划器(1)的输入；

世界坐标系及中心体重力场(5)，建立世界坐标系与***本体系的关系——指向、原点相对位置，以及中心体的重力场，可实现不同中心体下的空间机器人***多领域统一动力学的建模与仿真研究；世界坐标系及中心体重力场(5)与空间机器人机构模块(4)相连；

轨道动力学及空间环境模块(6)，接收空间机器人机构模块(4)输出的机器人基座本体系相对于惯性系的姿态，以及推进模块(8)输出的推力脉冲，计算空间机器人***质心的位置、基座本体系相对于轨道坐标系的姿态、角速度、所处轨道位置的磁场强度以及环境干扰力矩，作为空间机器人基座敏感器模块(7)、空间机器人基座姿轨控模块(9)，以及空间机器人机构模块(4)的输入；

空间机器人基座敏感器模块(7)，接收空间机器人机构模块(4)输出的基座本体系相对于惯性坐标系的姿态、角速度，以及轨道动力学及空间环境模块(6)输出的基座本体系相对于轨道坐标系的姿态、角速度，叠加测量噪声后作为敏感器的输出，该输出是空间机器人基座姿轨控模块(9)的输入；

空间机器人基座姿轨控模块(9)，接收空间机器人基座敏感器模块(7)输出的当前姿态角、角速度，以及空间机器人路径规划器(1)输出的期望姿态角、角速度，执行对目标卫星进行跟踪、接近、绕飞、轨道保持的导航、制导与控制算法，以及常规模式下自身姿态、轨道的控制算法，生成反作用飞轮组件(10)和推进模块(8)的控制指令，其中反作用飞轮组件的控制指令为控制电压、推进模块的控制指令为推力脉冲；

推进模块(8)，接收空间机器人基座姿轨控模块(9)输出的推力脉冲，产生各推力器的作用力，作用于轨道动力学及空间环境模块(6)；

反作用飞轮组件(10)，接收空间机器人基座姿轨控模块(9)输出的各推力器控制电压，产生各个飞轮的作用力矩，作用于空间机器人机构模块(4)。

2.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的空间机器人路径规划器(1)采用多领域统一建模语言Modelica实现了一种空间机器人目标捕获的自主路径规划方法，该方法利用手眼相机的相对位姿测量值，实时规划空间机器人的运动，以最终捕获目标；主要包括如下步骤：位姿偏差计算、目标运动的预测、空间机器人末端运动速度规划、空间机器人避奇异的路径规划、基座运动的预测；首先，根据手眼测量数据判断相对位姿偏差e_p和e_o是否小于设定的阈值ε_p和ε_o，若小于，则闭合手爪、捕获目标；反之，则根据相对位姿偏差，实时估计目标的运动状态，并将估计的结果反应到机械臂末端速度的规划中，以保证机械臂末端时刻朝最近的方向趋近目标，机械臂末端能自主跟踪目标的运动，直到最后捕获目标；规划出末端运动速度后，即调用自主奇异回避算法，以解算关节的期望角速度，并据此预测机械臂运动对基座的扰动，当扰动超出容许的范围时，则自动调整机械臂的关节运动速度，以保证期望的偏转在许可的范围内；整个过程一直持续到机械臂捕获到目标为止。

3.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的关节轴模块(2)采用多领域统一建模语言Modelica建立了机械臂各关节的机械、电气、控制多学科领域一体的模型，每个关节轴模型由关节控制器、电机及其驱动器、关节传动机构、关节敏感器组成；关节控制器实现了位置环和速度环的控制，其中，位置环采用PD控制，而速度环采用PI控制；电机模型中包含了电枢电阻Ra、电枢电感La、反电动势emf、电机轴Jmotor环节；驱动器部分由电阻R、电容C、运算放大器Op、电压源Vs及接地g组成；关节传动机构包括谐波减速器、齿轮减速箱，是连接电机轴和关节轴的中间部分，关节传动机构的建模由库伦摩擦bearingFrition、弹性阻尼器springDamper，以及理想减速模型三部分组成。

4.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的空间机器人手眼相机测量模块(3)采用多领域统一建模语言Modelica的多体敏感器库MultiBody.Sensors中的相对运动敏感器RelativeSensor，叠加上相机测量噪声，作为手眼相机的测量数据。

5.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的空间机器人机构模块(4)采用多领域统一建模语言Modelica编写空间机器人***及目标卫星的多个刚体属性，以及刚体间的约束实现；每个刚体的属性包括质量、惯量、质心位置、坐标系a和坐标系b，其中质量、惯量、质心位置为刚体的质量特性参数，坐标系a、坐标系b则用于定义该刚体与相应约束的连接关系；刚体间的约束用于描述相连刚体间的相对运动关系，空间机器人基座与机械臂第一个连杆，以及机械臂各连杆之间为旋转关节，而目标卫星与惯性坐标系之间为6DOF的***，通过Modelica多体库中的FreeMotion实现。

6.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的世界坐标系及中心体重力场(5)采用多领域统一建模语言Modelica编写，建立世界坐标系与***本体系的关系，以及地球的微重力场。

7.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的空间机器人基座敏感器模块(7)采用多领域统一建模语言Modelica的多体敏感器库MultiBody.Sensors中的相对运动敏感器RelativeSensor，通过设置输出项、再叠加上相应姿态敏感器的测量噪声，作为基座敏感器的测量数据。

8.根据权利要求1所述的空间机器人多领域统一建模与仿真***，其特征在于：所述的空间机器人基座姿轨控模块(9)采用多领域统一建模语言Modelica实现相应的姿态、轨道控制算法，产生执行机构的控制指令——四个飞轮的控制电压，以及推进模块的推力脉冲，对基座进行6DOF的控制，控制基座姿态、轨道按期望的轨迹运动。

Images