CN108445778B - 一种针对空间非合作目标非完全约束组合体的动力学建模方法 - Google Patents

Abstract

本发明涉及一种空间非合作目标非完全约束组合体的动力学建模方法，属于航空航天技术领域。该方法包括四个步骤：首先，对机械臂末端和被抓捕目标分别建模；然后，定义机械臂和目标之间的接触为一个多模式的单边约束问题；接着，将机械臂和被抓捕目标的动力学方程与二者之间的连接模式模型进行集成和整合，得到一种基于多模式切换的组合体紧凑动力学模型；最后，进行动力学积分，求出系统的运动轨迹。本发明弥补了现有建模方法无法准确反映系统整体运动特性的不足，为相关研究提供了理想的动力学模型，该模型能够准确识别机械臂末端与被抓捕目标之间的连接运动模式且相关参数能够直接通过传感器测得，具有广泛的应用前景。

Description

一种针对空间非合作目标非完全约束组合体的动力学建模 方法

技术领域

本发明涉及航空航天技术领域，尤其涉及一种空间非合作目标非完全约束的动力学建模方法。

背景技术

据发明人了解，空间非合作目标抓捕技术是空间组装、空间碎片清除、在轨维修服务、空间攻防等多种任务需求的基础，是航天大国争相发展且必须突破的关键技术。美国、欧洲、俄罗斯、日本等国家和地区均对这一技术进行了大量理论研究和型号任务实验，如ESA的eDeorbit卫星，美国的OctArm和日本的TAKO等，此外，还包括欧洲的EPOS和美国的DEOS等进行的地面机械臂验证系统研究。

实际抓取空间非合作目标难度大、风险高，半物理实验和虚拟抓捕是验证抓捕策略和控制器的必要手段。由于非合作目标的形状和几何特征难以确定，抓取过程中的接触碰撞过程复杂，使得空间机械臂动力学与控制的研究进展较为缓慢。在机械臂与目标之间的接触动力学建模方面，空间机械臂还处于初步摸索阶段，研究重点主要聚焦于抓捕过程中的碰撞阶段。如何提高机械臂与非合作目标的接触动力学模型的描述精度、完备性是亟待解决的技术难题。

在先技术[1]采用显性表达式的方法，直接将碰撞过程中的接触力关于时间的表达式给出，再根据牛顿第三定律，分别对机械臂和目标的运动做研究。在先技术[2]中，接触力被视为隐含的数学联系，直接将机械臂和目标的动力学方程联立，消去公式中的接触力，从而对整个组合体进行研究。这两种思路都无法准确地描述机械臂与目标之间可能存在的相对滑动和扭动，更无法根据计算摩擦力和相对的运动趋势，无法判断任意时刻系统的运动模式，因此无法准确描述系统的整体运动特性。

在先技术[3]中，机械臂与目标之间的滑动被引入到空间自由漂浮环境之下。由于没有固定的基座，重力支撑等，数学模型的建立变得更加复杂，滑动摩擦力对机械臂和目标的运动影响也更加明显。在先技术[4]中采用的是补偿函数的方式来描述机械臂与目标之间的固连-滑动模型的互相切换，这一技术的缺陷主要在于工程实现困难：补偿函数是个抽象的概念，没有几何或物理含义，也无法通过传感器测量，对控制器设计提供的指导较为有限。

参考文献

[1]Long Zhang,Qingxuan Jia,Gang Chen,et al.Impact analysis of spacemanipulator collision with soft environment,Industrial Electronics andApplications(ICIEA),2014IEEE 9th Conference,9-11June 2014,HangZhou,China

[2]Shuanfeng Xu,Hanlei Wang,Duzhou Zhang,et al.Adaptive ReactionlessMotion Control for Free-Floating Space Manipulators with Uncertain Kinematicsand Dynamics,3rd IFAC International Conference on Intelligent Control andAutomation Science.September 2-4,2013.Chengdu,China

[3]Hu,T.,Wang,T.,Li,J.,et al.Gradient Projection of Weihted JacobianMatrix Method for Inverse Kinematics of a Space Robot with a Controlled-Floation Base.Journal of Dynamic Systems,Measurement and Control.Vol.139,No.5,2017,pp.124-140

[4]Hu,T.,Wang,T.,Li,J.,Modeling and Simulation of Space Robot withUnilateral Contact Based on Complementary Problem,Acta Scientiarum NaturaliumUniversitatis Pekinensis,Vol.52,No.4,2016,pp.627-633

发明内容

本发明的目的在于克服上述在先技术的不足，提出一种能够准确识别机械臂末端与目标之间的连接模式、准确描述系统在多种运动模式下整体的运动特性且动力学模型相关参数能够直接通过传感器测得的针对空间非合作目标非完全约束组合体的动力学建模方法。

本发明的技术方案为：

一种针对空间非合作目标非完全约束组合体的动力学建模方法，所述空间非合作目标非完全约束组合体包括如下结构：由机械臂末端抓取机构和机械臂关节组成的机械臂以及被抓捕目标；

所述机械臂安装在机械臂基座卫星上；

该建模方法包括如下步骤：

步骤一，针对所述机械臂和被抓捕目标，分别建立对应的动力学模型并初始化建模参数；

步骤二，定义拓展自由度，整合所述机械臂的动力学模型和被抓捕目标的动力学模型；

步骤三，根据输出的控制力和控制力矩，判断所述机械臂与被抓捕目标之间的相对运动趋势并定义所述机械臂和被抓捕目标的广义加速度；

步骤四，对得到的所述机械臂的广义加速度进行动力学积分，求出所述组合体的运动曲线。

本发明的有益效果在于：

本发明的建模方法基于拓展自由度，所建的数学模型较为简单。在正常模式下，该模型与传统刚性组合体模型有着一致的动力学响应，而在控制力或控制力矩较大，静摩擦力无法维持固连时，该模型还能够准确计算出机械臂与目标之间的相对滑动和扭动，因而对于系统整体运动特性的模拟更加准确。此外，该模型的所有参数可通过传感器直接测得，数据准确且具有实际意义，从而为控制器的设计指导提供了便利。

附图说明

图1是本发明的拓展自由度的几何定义和仿真测试场景图；

图2是本发明算法流程图；

图3是本发明模型的刚性模式测试结果；

图4是本发明脉冲响应测试结果；

图5是本发明姿态控制器相应测试结果；

图1中：1-机械臂卫星太阳帆板；2-机械臂基座卫星；3-机械臂末端抓取机构；4-被抓捕目标；5-机械臂关节；l-第一滑动自由度；θ-转动自由度；f-第二滑动自由度；L-定值，目标质心与被抓捕目标质心位置至抓捕点位置切线方向延长线的垂线同延长线交点的距离。

具体实施方式

结合附图对本发明的实施方式作详细说明。

本发明的针对空间非合作目标非完全约束组合体的动力学建模方法的步骤为：

第一步：建立如图1所示的仿真场景，图中，左侧为仿真场景俯视图，右侧为仿真场景主视图，右下角为机械臂末端3与被抓捕目标4之间的连接位置放大图；

所述空间非合作目标非完全约束组合体包括如下结构：由机械臂末端抓取机构3和机械臂关节5组成的机械臂以及被抓捕目标4；

机械臂卫星太阳帆板1安装在机械臂基座卫星2上，为所述机械臂提供动力源；所述机械臂也安装在机械臂基座卫星2上，机械臂基座卫星2作为所述机械臂的基座，为所述机械臂提供支撑；机械臂末端链接机构3由机械臂关节5控制抓取被抓捕目标4；

利用拉格朗日动力学，定义机械臂广义自由度向量q＝[X Ψ]^T，其中，X表示机械臂基座卫星2的位置姿态，Ψ表示机械臂关节5的角度；

利用此向量对所述机械臂进行建模，模型公式为：

式中，M，D，C均为正定矩阵，其中，M为机械臂广义质量矩阵，D包含广义阻尼参数，C包含科里奥利力参数；F为所述机械臂输出的控制力和控制力矩；d为外界扰动力和扰动力矩；

对被抓捕目标4进行动力学建模，模型公式为：

式中，E为3x3单位矩阵，m为被抓捕目标4的质量，J为被抓捕目标4的转动惯量，a代表加速度，β代表角加速度，C_t矩阵包含目标非线性科里奥利力信息，T为连接处力矩，r为抓捕点在被抓捕目标4本体坐标系下的坐标位置；

根据所述机械臂与被抓捕目标4的连接位置的材料及几何特性，定义抓捕点所能够提供的最大静摩擦力和最大静扭矩并写成向量形式，定义为最大广义力向量δ_max。

第二步：通过机械臂末端抓取机构3和抓捕位置的几何特征，定义如图1中所示的三个拓展自由度：第一滑动自由度l，表示机械臂末端抓取机构3到其与被抓捕目标4的连接点的距离；转动自由度θ，表示机械臂末端抓取机构3指向方向与被抓捕目标4连接点位置切线方向的夹角；第二滑动自由度f，表示机械臂末端抓取机构3与被抓捕目标4的连接点到该连接点的切线与被抓捕目标4质心垂线交点的距离；

计算固定连接时所述机械臂和被抓捕目标4的连接点为保持固定连接状态所需的静摩擦力和静扭矩，并将所述静摩擦力和静扭矩写成向量形式，定义为需求广义力向量δ_needed；

利用所述拓展自由度给出被抓捕目标4质心相对于所述机械臂的末端的位置，公式为：

其中,f(q)为所述机械臂的末端位置矩阵,Q(q)为所述机械臂的末端的姿态矩阵可以直接通过机械臂广义自由度向量q求出。l，θ，f先前所定义的三个拓展自由度，L为定值，具体定义：从抓捕点位置切线方向做延长线,并从被抓捕目标4质心位置向此延长线做垂线，去垂线与延长线的交点，则L为目标质心到此交点的距离；

对这一位置公式求导，定义与所述需求广义力向量δ_needed对应的拓展自由度向量q_new＝[l f θ]^T，将该拓展自由度向量集成进入所述机械臂的动力学模型公式，将所述机械臂广义自由度向量q与所述拓展自由度分别列出，得到所述组合体整体的拉格朗日动力学模型公式：

其中，

分别代表拓展自由度向量q_new＝[l f θ]^T的一阶、二阶导数；

分别为机械臂广义自由度向量q的一阶、二阶导数；n_ext为拓展后的外界扰动向量；M′，M_ex分别为机械臂广义自由度和拓展自由度所对应的广义质量矩阵；D′，D_ex分别为机械臂广义自由度和拓展自由度所对应的广义阻尼矩阵；C′，C_ex分别为机械臂广义自由度与拓展自由度相对应的广义科里奥利力矩阵；M_c,D_c为机械臂广义自由度和拓展自由度之间的耦合矩阵；F为空间非合作目标非完全约束组合体的主动控制输入。

第三步：假设被抓捕目标4不具备控制能力，由所述机械臂输出控制力和控制力矩；

考虑拓展自由度的运动类别，并以该类别为依据对存在耦合作用的拓展自由度进行分组处理，分别定义Mode，S和T来描述所述机械臂和被抓捕目标4的相对运动模式，其中Mode表示机械臂和被抓捕目标4是否固连，S表示是否处于滑动状态，对应[l f]两个拓展自由度，因此这两个拓展自由度类别一致，存在耦合关系，需要统一处理，若l开始滑动，则f也开始滑动；T表示是否处于转动状态，对应拓展自由度θ；也就是说，在本实施例的抓捕模型中，若Mode＝0，S和T一定都为0，记为(0,0,0)，表示固连，既不滑动也不扭转；若Mode＝1，则(M,S,T)存在(1,1,0)，(1,0,1)，(1,1,1)三种情况，即总共需要考虑四种运动模式，以及滑动和扭转两种相对运动的类别；

在固定连接模式下，拓展自由度的速度和加速度均为0；将定义的需求广义力向量δ_needed与定义的最大广义力向量δ_max作比对，若所述需求广义力向量δ_needed的某一元素i大于所述最大广义力向量δ_max的对应元素，记为δ_needed(i)>δ_max(i)；则该元素对应的拓展自由度q_new(i)开始滑动，所述机械臂和被抓捕目标4不再保持固连，系统开始滑动；在滑动模式下计算第i个拓展自由度q_new(i)的加速度并重新判断剩余拓展自由度是否能保持固连；重复此过程直至所有拓展自由度的连接模式都确定为止；

具体地，根据当前的相对运动状态，利用相应的公式判断所述机械臂和被抓捕目标4的相对运动模式是否发生了变化，变化包括由静到动和由动到静两个方面，根据三个拓展自由度和四种运动模式可知，需要对系统进行两个层次的判断，如图2所示：

A.由静到动：

a.Mode取值的判断，若Mode＝0，则直接进入固连模式进行判断，判断方法为：计算维持运动状态的需求广义力向量δ_needed：

为预测出的机械臂广义加速度，

将需求广义力向量δ_needed与最大广义力向量δ_max各自对应的元素进行对比，并以此为依据切换模式，对应的元素用序号1、2和3进行表示：

若Mode＝1，则需要进行第二层判断

b.第二层判断分为三个模型，判断过程与第一种情况类似：

i.(1,1,0)to(1,1,1)：预测需求广义力向量δ_needed：

其中，

为预测的广义自由度θ的加速度；判断是否切换：

ii.(1,0,1)to(1,1,1)：预测需求广义力向量δ_needed：

其中，

为预测的广义自由度l和f的加速度。

判断是否切换：

iii.(1,1,1)：不需要判断由静到动的情况。

在最终确定了此时刻所述机械臂和被抓捕目标4所处的运动模式后，定义所述机械臂和被抓捕目标4在此时刻的广义加速度：

B.由动到静：

判断经过动力学外推之后所述机械臂和被抓捕目标4的相对运动速度，此速度是包含了相对滑动的线速度和相对扭转的角速度的广义速度，分别对应三个新的拓展自由度，定义为广义速度向量。若该广义速度向量的元素速度小于5.10^-4m/s，则认为所述机械臂和被抓捕目标4的对应拓展自由度上的相对运动已经停止并返回模式A，具体判断公式为：

其中，υ_min＝3·10^-4rad/s，V_min＝5·10^-4m/s。

最后，为了提高系统判断运动模式切换时的准确性，对滑动摩擦力表达式进行了修改。在所述机械臂和被抓捕目标(4)的相对滑动速度小于10^-2m/s时，利用饱和函数适当降低所述机械臂和被抓捕目标4间的滑动摩擦力，所述机械臂和被抓捕目标4之间的滑动摩擦力不再是一个仅与压力相关的值，而被定义为一个随着相对滑动速度减小而减小的值，使得相对滑动速度向0收敛而非线性下降，具体定义为：

其中，δ_max为标称最大静摩擦力值，系数a为大于0的固定常数。

图3、图4和图5分别给出了三个动力学模型的测试，分别在不同的控制输入条件下，对上述动力学模型进行了仿真测试。

图3给出了在固连模式下的运动曲线，可以看出，在固连模式下，新模型的动力学特性与单刚体模型完全一致，说明动力学模型的正确性得以保障。

图4给出了动力学系统的脉冲响应，可以看出模式切换的过程，从相对滑动加扭转(1,1,1)到相对扭转(1,0,1)到最终恢复固连(0,0,0)，响应曲线符合多刚体系统的动力学特征。

图5给出了动力学模型在传统PID姿态控制器控制下的相应结果。可以看出，由于考虑了机械臂与目标之间的非完全约束情况，两者之间的相对滑动对组合体的几何构型造成了巨大的影响，随着机械臂向逆时针转动，目标向顺时针方向给出了动力学响应，满足角动量守恒规律，进一步说明了新的动力学模型的可靠性和正确性。

Claims (5)

1.一种针对空间非合作目标非完全约束组合体的动力学建模方法，其特征在于，

所述空间非合作目标非完全约束组合体包括如下结构：由机械臂末端抓取机构(3)和机械臂关节(5)组成的机械臂以及被抓捕目标(4)；

所述机械臂安装在机械臂基座卫星(2)上；

该建模方法包括如下步骤：

步骤一，针对所述机械臂和被抓捕目标(4)，分别建立对应的动力学模型并初始化建模参数，其中，被初始化的建模参数至少包括：所述机械臂和被抓捕目标(4)的连接位置在固连模式下所能承受的最大静摩擦力和最大固连扭矩；具体为：

利用拉格朗日动力学，定义机械臂广义自由度向量q＝[X Ψ]^T，其中，X表示机械臂基座卫星(2)的位置姿态，Ψ表示机械臂关节(5)的角度；

利用此向量对所述机械臂进行建模，模型公式为：

式中M，D，C均为正定矩阵，其中，M为机械臂广义质量矩阵，D包含广义阻尼参数，C包含科里奥利力参数；F为所述机械臂输出的控制力和控制力矩；d为外界扰动力和扰动力矩；

对被抓捕目标(4)进行动力学建模，模型公式为：

式中，E为3x3单位矩阵，m为被抓捕目标(4)的质量，J为被抓捕目标(4)的转动惯量，a代表加速度，β代表角加速度，C_t矩阵包含目标非线性科里奥利力信息，T为连接处力矩，r为抓捕点在被抓捕目标(4)本体坐标系下的坐标位置；

根据所述机械臂与被抓捕目标(4)的连接位置的材料及几何特性，定义抓捕点所能够提供的最大静摩擦力和最大静扭矩并写成向量形式，定义为最大广义力向量δ_max；

步骤二，定义拓展自由度，整合所述机械臂的动力学模型和被抓捕目标(4)的动力学模型，其中，所述拓展自由度至少包括第一滑动自由度l，表示机械臂末端抓取机构(3)到其与被抓捕目标(4)的连接点的距离；转动自由度θ，表示机械臂末端抓取机构(3)指向方向与被抓捕目标(4)连接点位置切线方向的夹角；第二滑动自由度f，表示机械臂末端抓取机构(3)与被抓捕目标(4)的连接点到该连接点的切线与被抓捕目标(4)质心垂线交点的距离；

计算固定连接时所述机械臂和被抓捕目标(4)的连接点的静摩擦力和静扭矩，并将所述静摩擦力和静扭矩写成向量形式，定义为需求广义力向量δ_needed；

利用三个拓展自由度将所述机械臂的动力学模型和被抓捕目标(4)的动力学模型整合，得到所述组合体的动力学模型；具体为：

利用所述拓展自由度给出被抓捕目标(4)质心相对于所述机械臂的末端的位置P，公式为：

其中,f(q)为所述机械臂的末端位置矩阵，Q(q)为所述机械臂的末端的姿态矩阵可以直接通过机械臂广义自由度向量q求出；l，θ，f先前所定义的三个拓展自由度，L为常数，表示目标质心到接触点切线的距离；

对这一位置公式求导，定义与所述需求广义力向量δ_needed对应的拓展自由度向量q_new＝[l f θ]^T，将该拓展自由度向量集成进入所述机械臂的动力学模型公式，将所述机械臂广义自由度向量q与所述拓展自由度分别列出，得到所述组合体整体的拉格朗日动力学模型公式：

其中，

分别代表拓展自由度向量q_new＝[l f θ]^T的一阶、二阶导数；

分别为机械臂广义自由度向量q的一阶、二阶导数；n_ext为拓展后的外界扰动向量；M′，M_ex分别为机械臂广义自由度和拓展自由度所对应的广义质量矩阵；D′，D_ex分别为机械臂广义自由度和拓展自由度所对应的广义阻尼矩阵；C′，C_ex分别为机械臂广义自由度与拓展自由度相对应的广义科里奥利力矩阵；M_c,D_c为机械臂广义自由度和拓展自由度之间的耦合矩阵；F为空间非合作目标非完全约束组合体的主动控制输入；

步骤三，根据输出的控制力和控制力矩，判断所述机械臂与被抓捕目标(4)之间的相对运动趋势并定义所述机械臂和被抓捕目标(4)的广义加速度；

步骤四，对得到的所述机械臂的广义加速度进行动力学积分，求出所述组合体的运动曲线。

2.根据权利要求1所述的针对空间非合作目标非完全约束组合体的动力学建模方法，其特征在于，步骤三中，假设被抓捕目标(4)不具备控制能力，由所述机械臂输出控制力和控制力矩；

根据所述机械臂与被抓捕目标(4)当前的相对运动模式，计算相应的运动参数来判断此时输出的控制力和控制力矩是否对相对运动模式产生了影响，方法为：

a、固连模式，则此时拓展自由度的速度和加速度均为0；将定义的需求广义力向量δ_needed与定义的最大广义力向量δ_max作比对，若所述需求广义力向量δ_needed的某一元素i大于所述最大广义力向量δ_max的对应元素，记为δ_needed(i)＞δ_max(i)；则该元素对应的拓展自由度q_new(i)开始滑动，所述机械臂和被抓捕目标(4)不再保持固连，系统进入滑动模式b；在b模式下计算第i个拓展自由度q_new(i)的加速度并重新判断剩余拓展自由度是否能保持固连；重复此过程直至所有拓展自由度的连接模式都确定为止；

b、滑动模式，若所述机械臂与被抓捕目标(4)相对滑动速度小于设定值，则认为滑动停止，将连接模式切换回固连模式a。

3.根据权利要求2所述的针对空间非合作目标非完全约束组合体的动力学建模方法，其特征在于，所述机械臂和被抓捕目标(4)的相对运动模式包括:

分别定义Mode，S和T来描述所述机械臂和被抓捕目标(4)的运动模式，其中Mode表示系统是否固连，S表示是否处于滑动状态，T表示处于转动状态；也就是说，若Mode＝0，S和T一定都为0，记为(0,0,0)，表示固连，既不滑动也不扭转；若Mode＝1，则(M,S,T)存在(1,1,0)，(1,0,1)，(1,1,1)三种情况，总共需要考虑四种运动模式；

根据当前的相对运动模式，利用相应的公式判断所述机械臂和被抓捕目标(4)的相对运动模式是否发生了变化，变化包括由静到动和由动到静两个方面，根据所述的三个拓展自由度和四种运动模式可知，需要进行两个层次的判断：

A.由静到动：

a.Mode取值的判断，若Mode＝0，则直接进入固连模式进行判断，判断方法为：计算维持运动状态的需求广义力向量δ_needed：

为预测出的机械臂广义加速度，

将需求广义力向量δ_needed与最大广义力向量δ_max各自对应的元素进行对比，并以此为依据切换模式：

若Mode＝1，则需要进行第二层判断b

b.第二层判断分为三个模式，判断过程与a类似：

i.(1,1,0)to(1,1,1)：预测需求广义力向量δ_needed：

其中，

为预测的广义自由度θ的加速度；判断是否切换：

ii.(1,0,1)to(1,1,1)：预测需求广义力向量δ_needed：

其中，

为预测的广义自由度l和f的加速度；

判断是否切换：

iii.(1,1,1)：不需要判断由静到动的情况；

在最终确定了此时刻所述机械臂和被抓捕目标(4)所处的运动模式后，定义所述机械臂和被抓捕目标(4)在此时刻的广义加速度：

B.由动到静：

判断经过动力学外推之后所述机械臂和被抓捕目标(4)的相对运动速度，此速度是包含了相对滑动的线速度和相对扭转的角速度的广义速度，分别对应三个新的拓展自由度，定义为广义速度向量；所述的速度设定值为5·10^-4m/s，若该广义速度向量的元素的速度均小于所述速度设定值，则认为所述机械臂和被抓捕目标(4)的对应拓展自由度上的相对运动已经停止并返回模式A，具体判断公式为：

其中，υ_min＝3·10^-4rad/s，V_min＝5·10^-4m/s。

4.根据权利要求3所述的针对空间非合作目标非完全约束组合体的动力学建模方法，其特征在于，在所述机械臂和被抓捕目标(4)的相对滑动速度小于10^-2m/s时，利用饱和函数适当降低所述机械臂和被抓捕目标(4)间的滑动摩擦力，所述机械臂和被抓捕目标(4)之间的滑动摩擦力不再是一个仅与压力相关的值，而被定义为一个随着相对滑动速度减小而减小的值，具体定义为：

其中，δ_max为标称最大静摩擦力值，系数a为大于0的固定常数，得相对滑动速度向0收敛而非线性下降。

5.根据权利要求1至4中任何一项所述的针对空间非合作目标非完全约束组合体的动力学建模方法，其特征在于，所述空间非合作目标非完全约束组合体结构还包括：为所述机械臂提供动力源的机械臂卫星太阳帆板(1)，机械臂卫星太阳帆板(1)安装在机械臂基座卫星(2)上。

Images