[发明专利]一种基于动力学模型的双臂抓捕下空间目标参数辨识方法

权利要求书

1.一种基于动力学模型的双臂抓捕下空间目标参数辨识方法，其特征在于：具体的操作步骤如下：

步骤1：基于如下条件：

条件1：空间碎片主动清除飞行器、两部机械臂以及空间非合作目标都为刚性，且机械臂末端作用器与非合作目标固连；

条件2：空间碎片主动清除飞行器处于自由飞行状态，其所受到的外力以及外力矩都很微小，从而可以忽略；

条件3：进行参数辨识时，所需的输入信息均在高精度下测量得到，包含了中心平台角速度、线速度、机械臂各关节转角和角速度信息；

条件4：捕获目标载荷后，机械臂的末端作用器即与目标固连为一体，两者之间不存在相对运动；

步骤2：结合中心平台和机械臂的运动学关系，建立双臂捕获后的闭链系统的动力学方程：具体包括如下步骤：

步骤2.1：定义坐标系及相关变量

针对对象为在运行轨道上自由漂浮的空间碎片主动清除飞行器系统，系统包括中心平台、以及两部机械臂系统，每部机械臂系统带有7节机械臂连杆，节数编号规则为，靠近中心平台的机械臂连杆为小编号，依次向外编号递增，每节机械臂相对其内接体均只具有1个转动自由度，第7节机械臂连杆为末端作用器，用于抓捕和固定目标；

a.地心赤道惯性坐标系F(OXYZ)：原点位于地球中心O，Z轴沿地球极轴指向北极，X轴指向春分点，Y轴与X轴、Z轴构成右手坐标系，春分点指太阳从南向北在黄赤道上的交点；

b.轨道坐标系F_I(O_IX_IY_IZ_I)：原点位于系统质心，Z_I轴指向地心，Y_I轴指向轨道负法线方向，X_I轴与其它两轴构成右手坐标系；

c.中心平台体坐标系F_b(O_bX_bY_bZ_b)：原点航天器本体质心O_b，X_b轴、Y_b轴与Z_b轴为航天器本体惯性主轴，且构成右手坐标系；

d.机械臂坐标系原点位于第i个机械臂系统中的第j节机械臂与其内接体的铰链处，坐标系各轴与第j节机械臂固连；其中，i＝1,2；j＝1,2,…,7；

以下各部分内容中，变量的右上标“i”均表示有关第i个机械臂系统的相关变量，变量的右下标“j”均表示有关第j节机械臂连杆的相关变量；

定义相关变量如下：

r_b为中心平台质量参考点在中心平台体坐标系中的位置投影矩阵；

为第i部机械臂系统中的第j节机械臂的质量参考点在其体坐标系中的位置投影矩阵；

为第i部机械臂系统中的第j节机械臂体坐标系原点在中心平台体坐标系中的位置投影矩阵；

为第i部机械臂系统中的第k节机械臂体坐标系原点到第j节机械臂体坐标系原点在第k节机械臂体坐标系中的投影列阵；

为第i部机械臂系统末端作用器体坐标系原点到第j个机械臂系统末端作用器捕获点在中心平台体坐标系的投影列阵；

为第i部机械臂系统末端作用器体坐标系原点到其末端作用器捕获点在中心平台体坐标系的投影列阵；

为第i部机械臂系统中的第j节机械臂体坐标系到中心平台体坐标系的坐标转换矩阵；

为中心平台体坐标系到第i部机械臂系统中的第j节机械臂体坐标系的坐标转换矩阵；

为第i部机械臂系统中的第j节机械臂体坐标系到第k节机械臂体坐标系的坐标转换矩阵；

为第i部机械臂系统中的第j节机械臂相对其体坐标系的角加速度矩阵；

为第i部机械臂系统中的第j节机械臂相对轨道系的角速度矩阵；

F为作用于中心平台上的力，以列矩阵形式表达；

T为作用于中心平台上的力矩，以列矩阵形式表达；

为作用于第i部机械臂系统的第j节机械臂关节处的控制力矩，以列矩阵形式表达，由于机械臂关节仅有一个自由度，因此列向量中三轴分量仅在对应转动自由度方向具有数值；

列阵的叉乘反对称斜方阵以字符上方的波浪线“～”表示；

右上标“T”表示矩阵的转置；

步骤2.2：说明中心平台和机械臂的运动学关系

中心平台的运动学关系说明如下：

中心平台为空间碎片主动清除飞行器，其姿态指向可用欧拉角描述；记中心平台体坐标系相对轨道坐标系的滚动、俯仰和偏航角分别为φ、θ和ψ，则其姿态运动学关系写为

其中，ω为中心平台角速度，ω_x、ω_y、ω_z表示其三轴分量，“·”上标表示其导数；

记欧拉角列阵则式(1)写为

根据定义的姿态角和旋转顺序，轨道坐标系到中心体坐标系的坐标转换矩阵为

式中“s”表示“sin”，“c”表示“cos”；中心体体坐标系到轨道坐标系的坐标转换矩阵为

机械臂的运动学关系说明如下：

机械臂j与其内接体的旋转轴方向通常为其体坐标系的某个坐标轴方向；记第i个机械臂系统中的第j节机械臂相对其内接体的转角为其时间导数即为第i个机械臂系统中的第j节机械臂相对其内接体的转动角速度；

第i个机械臂系统中的第j节机械臂相对其体坐标系的转动角速度的矩阵形式为若体坐标系的x轴为旋转轴方向，则有

若体坐标系的y轴为旋转轴方向，则有

若体坐标系的z轴为旋转轴方向，则有

各臂的运动学关系为式(4)～(6)中的某一个方程，取决于体坐标系的选取；上述运动学方程统一写为

其中

具体取值由体坐标系的选取确定，表示机械臂关节的转动自由度方向转换矩阵；

步骤2.3：建立双臂捕获后的闭链系统动力学方程

步骤2.3.1：建立各体的动力学方程，写为系统动力学模型的紧缩形式

按照Kane方程建模的过程，得到系统各体的动力学方程如下：

a.中心体的平动方程，对应广义速度表示中心平台速度：

式中，

——中心平台的加速度；

为系统总质量；其中，为第i部机械臂系统中的第j节机械臂的质量；m_b为中心平台的质量；

为系统对中心平台体坐标系原点的静矩；其中，

S_b＝∫_br_bdm为中心平台相对其体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对中心平台体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对其体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对第1节机械臂体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对第2节机械臂体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对第3节机械臂体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对第4节机械臂体坐标系原点的静矩；

为第i部机械臂系统中的第j节机械臂对第5节机械臂体坐标系原点的静矩；

为第i部机械臂系统中的第7节机械臂对第6节机械臂体坐标系原点的静矩；

为第i部机械臂系统中的第1节机械臂及其上外接体对其体坐标系原点的静矩；

为第i部机械臂系统中的第2节机械臂及其上外接体对其体坐标系原点的静矩；

为第i部机械臂系统中的第3节机械臂及其上外接体对其体坐标系原点的静矩；

为第i部机械臂系统中的第4节机械臂及其上外接体对其体坐标系原点的静矩；

为第i部机械臂系统中的第5节机械臂及其上外接体对其体坐标系原点的静矩；

为第i部机械臂系统中的第6节机械臂及其上外接体对其体坐标系原点的静矩；

为第i部机械臂系统中的第7节机械臂及其上外接体对其体坐标系原点的静矩；

为中心平台增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第1节机械臂增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第2节机械臂增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第3节机械臂增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第4节机械臂增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第5节机械臂增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第6节机械臂增广体对其体坐标系原点的静矩；

为第i部机械臂系统中的第7节机械臂增广体对其体坐标系原点的静矩；

b.中心体的转动方程；

式中，

为系统对中心平台体坐标系的惯量矩阵；其中，

为中心平台相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对中心平台体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对第1节机械臂体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对第2节机械臂体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对第3节机械臂体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对第4节机械臂体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂相对第5节机械臂体坐标系的惯量矩阵；

为第i部机械臂系统中的第7节机械臂相对第6节机械臂体坐标系的惯量矩阵；

为第i部机械臂系统中的第1节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第2节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第3节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第4节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第5节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第6节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第7节机械臂及其外接体相对其体坐标系的惯量矩阵；

为第i部机械臂系统中的第j节机械臂对中心平台体坐标系的惯量耦合矩阵；

为系统对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第1节机械臂对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第2节机械臂对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第3节机械臂对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第4节机械臂对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第5节机械臂对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第6节机械臂对中心平台体坐标系的拟惯量矩阵；

为第i部机械臂系统中的第7节机械臂对中心平台体坐标系的拟惯量矩阵；

c.第i部机械臂系统中的第1节机械臂的转动方程，对应于广义速度的动力学方程；

式中，

为第i部机械臂系统中的第j节机械臂对第1节机械臂体坐标系的惯量耦合矩阵；

为修正后的第i部机械臂系统中的第1节机械臂对其体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对第1节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对第1节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对第1节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对第1节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对第1节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对第1节机械臂体坐标系的拟惯量矩阵；

d.第i部机械臂系统中的第2节机械臂的转动方程；

式中，

为第i部机械臂系统中的第j节机械臂对第2节机械臂体坐标系的惯量耦合矩阵；

为修正后的第i部机械臂系统中的第1节机械臂对第2节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对其体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对第2节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对第2节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对第2节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对第2节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对第2节机械臂体坐标系的拟惯量矩阵；

e.第i部机械臂系统中的第3节机械臂的转动方程；

式中，

为第i部机械臂系统中的第j节机械臂对第3节机械臂体坐标系的惯量耦合矩阵；

为修正后的第i部机械臂系统中的第1节机械臂对第3节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对第3节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对其体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对第3节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对第3节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对第3节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对第3节机械臂体坐标系的拟惯量矩阵；

f.第i部机械臂系统中的第4节机械臂的转动方程；

式中，

为第i部机械臂系统中的第j节机械臂对第4节机械臂体坐标系的惯量耦合矩阵；

为修正后的第i部机械臂系统中的第1节机械臂对第4节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对第4节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对第4节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对其体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对第4节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对第4节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对第4节机械臂体坐标系的拟惯量矩阵；

g.第i部机械臂系统中的第5节机械臂的转动方程；

式中，

为第i部机械臂系统中的第j节机械臂对第5节机械臂体坐标系的惯量耦合矩阵；

为修正后的第i部机械臂系统中的第1节机械臂对第5节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对第5节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对第5节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对第5节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对其体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对第5节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对第5节机械臂体坐标系的拟惯量矩阵；

h.第i部机械臂系统中的第6节机械臂的转动方程；

式中，

为第i部机械臂系统中的第7节机械臂对第6节机械臂体坐标系的惯量耦合矩阵；

为修正后的第i部机械臂系统中的第1节机械臂对第6节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对第6节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对第6节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对第6节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对第6节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对其体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对第6节机械臂体坐标系的拟惯量矩阵；

i.第i部机械臂系统中的第7节机械臂的转动方程；

式中，

为修正后的第i部机械臂系统中的第1节机械臂对第7节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第2节机械臂对第7节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第3节机械臂对第7节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第4节机械臂对第7节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第5节机械臂对第7节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第6节机械臂对第7节机械臂体坐标系的拟惯量矩阵；

为修正后的第i部机械臂系统中的第7节机械臂对其体坐标系的拟惯量矩阵；

若记系统的广义速度列阵为

则系统的动力学方程式(9)～(15)写为如下的紧缩形式

式中为系统的质量阵，表达式为

式中，E₃为3阶单位阵；为正定对称阵，因此表达式中仅给出其右上半角元素；为方程中的非线性耦合项，写为

其中，表示式(18)中系统的各阶广义速度对应的非线性耦合项，具体构成形式如下

为方程中的广义外力，表达式为

步骤2.3.2：建立结合运动学关系后的动力学方程

将机械臂的运动学方程代入到系统动力学方程，并考虑到由于每节机械臂仅具有一个自由度，因此在动力学方程中仅考虑绕机械臂旋转轴方向的转动运动，即在三轴转动动力学方程中仅取绕旋转轴方向的运动，则得到考虑机械臂运动学方程后的系统动力学模型为

其中，

为系统的广义速度量；M为系统的质量阵，仍为对称阵，其表达式为

Q为广义力列阵，表达式为

其中为加在第i部机械臂系统中的第j节机械臂旋转轴上的驱动力矩大小，为标量数值；

F_#为非线性耦合力，表达式为

步骤2.3.3：设定闭链约束条件

多个机械臂同时捕获一个目标载荷时，系统成为闭链系统，捕获目标的各臂之间产生了运动约束，即各臂之间的运动必须满足闭链条件；系统动力学中的各变量不再独立；

确定一个系统中的约束参考点，不失一般性，取第2部机械臂系统在目标上的捕获点为约束参考点；考虑运动学关系后，速度约束方程写为

式中，表示第i部机械臂第p节连杆的速度约束系数矩阵；同理，表示第j部机械臂第q节连杆的速度约束系数矩阵，其具体构成形式如下

以上各系数矩阵中，表示第j部机械臂系统末端作用器捕获点相对第i部机械臂系统第p节连杆体坐标系原点的位置矢量的矩阵形式；同理，表示第j部机械臂系统末端作用器捕获点相对第j部机械臂系统第q节连杆体坐标系原点的位置矢量的矩阵形式，其具体构成形式如下

式中，表示第i部机械臂系统第7节连杆体坐标系原点到第i部机械臂系统末端作用器抓捕点的矢量的矩阵形式，表示第j部机械臂系统第7节连杆体坐标系原点到第j部机械臂系统末端作用器抓捕点的矢量的矩阵形式；

考虑到系统的运动学关系，角速度约束方程写为

式(21)和(22)共同构成了闭链系统中的广义速度约束条件；

步骤2.3.4：建立双臂捕获后的闭链系统动力学方程

记构成闭链系统的机械臂系统i和j的广义速度向量为

记u_I为主动约束量，u_D为被动约束量，其具体构成形式如下

则速度和角速度约束方程统一写为

G₁u_D＝G₂u_I (24)

式中，G₁为被动约束系数矩阵，G₂为主动约束系数矩阵，其具体构成形式如下

于是有

求时间导数可得

记A为主、被动约束限制矩阵，

则有

上式还写为

式中，A₁为主、被动约束限制的扩展矩阵

A₁＝[-A E]

记闭链约束限制的系数矩阵A₂

A₂＝[E A^T]

以及机械臂系统的无约束方程形式为

式中，为无约束机械臂系统方程的质量阵，为无约束机械臂系统方程的广义外力项，无约束机械臂系统方程的非线性耦合项，其具体构成形式如下

则加入约束后方程形式为

构成闭链后，系统动力学模型在数学形式上写为

式中，M_c为闭链约束下的机械臂系统方程的质量阵，Q_c为闭链约束下的机械臂系统方程的广义外力项，F_c为闭链约束下的机械臂系统方程的非线性耦合项，其具体构成形式如下

其中，“·(1:6,:)”表示由对应矩阵“·”的第1至6行、所有列构成的新矩阵；同理，“·(7:20,:)”表示由对应矩阵“·”的第7至20行、所有列构成的新矩阵；

步骤3：建立基于系统动力学模型的辨识模型：具体步骤如下：

选取中心平台的转动方程建立辨识模型，对中心平台转动方程的项进行整理，

将上式中含有非合作目标质量特性参数的项展开，进一步拆分为常数部分、未知质量部分、未知静矩部分、未知转动惯量部分，每一项分别展开为

得到分离未知项的中心体转动方程为

式中，“(·)”表示(·)列矩阵的叉乘反对称斜方阵；

接下来对含有的项进行改写，将其分为三类；

设

其中，“·_mn”表示对应方阵中第m行第n列的数值，m,n＝1,2,3；后续同理，若矩阵为列阵，则“n”省略；

则

第一类：

设

则有

其中，(m,n)_case1表示第一类对应矩阵中第m行第n列的计算表达式通式，

(1,1)_case1＝(a₁₁r₁₁d₁+a₁₁r₁₂d₂+a₁₁r₁₃d₃)I₁₁+(a₁₂r₁₁d₁+a₁₁r₂₁d₁+a₁₂r₁₂d₂+a₁₁r₂₂d₂+a₁₂r₁₃d₃+a₁₁r₂₃d₃)I₁₂+(a₁₃r₁₁d₁+a₁₁r₃₁d₁+a₁₃r₁₂d₂+a₁₁r₃₂d₂+a₁₃r₁₃d₃+a₁₁r₃₃d₃)I₁₃+(a₁₂r₂₁d₁+a₁₂r₂₂d₂+a₁₂r₂₃d₃)I₂₂+(a₁₃r₂₁d₁+a₁₂r₃₁d₁+a₁₃r₂₂d₂+a₁₂r₃₂d₂+a₁₃r₂₃d₃+a₁₂r₃₃d₃)I₂₃+(a₁₃r₃₁d₁+a₁₃r₃₂d₂+a₁₃r₃₃d₃)I₃₃

(2,1)_case1＝(a₂₁r₁₁d₁+a₂₁r₁₂d₂+a₂₁r₁₃d₃)I₁₁+(a₂₂r₁₁d₁+a₂₁r₂₁d₁+a₂₂r₁₂d₂+a₂₁r₂₂d₂+a₂₂r₁₃d₃+a₂₁r₂₃d₃)I₁₂+(a₂₃r₁₁d₁+a₂₁r₃₁d₁+a₂₃r₁₂d₂+a₂₁r₃₂d₂+a₂₃r₁₃d₃+a₂₁r₃₃d₃)I₁₃+(a₂₂r₂₁d₁+a₂₂r₂₂d₂+a₂₂r₂₃d₃)I₂₂+(a₂₃r₂₁d₁+a₂₂r₃₁d₁+a₂₃r₂₂d₂+a₂₂r₃₂d₂+a₂₃r₂₃d₃+a₂₂r₃₃d₃)I₂₃+(a₂₃r₃₁d₁+a₂₃r₃₂d₂+a₂₃r₃₃d₃)I₃₃

(3,1)_case1＝(a₃₁r₁₁d₁+a₃₁r₁₂d₂+a₃₁r₁₃d₃)I₁₁+(a₃₂r₁₁d₁+a₃₁r₂₁d₁+a₃₂r₁₂d₂+a₃₁r₂₂d₂+a₃₂r₁₃d₃+a₃₁r₂₃d₃)I₁₂+(a₃₃r₁₁d₁+a₃₁r₃₁d₁+a₃₃r₁₂d₂+a₃₁r₃₂d₂+a₃₃r₁₃d₃+a₃₁r₃₃d₃)I₁₃+(a₃₂r₂₁d₁+a₃₂r₂₂d₂+a₃₂r₂₃d₃)I₂₂+(a₃₃r₂₁d₁+a₃₂r₃₁d₁+a₃₃r₂₂d₂+a₃₂r₃₂d₂+a₃₃r₂₃d₃+a₃₂r₃₃d₃)I₂₃+(a₃₃r₃₁d₁+a₃₃r₃₂d₂+a₃₃r₃₃d₃)I₃₃

第二类：设则有

其中，(m,n)_case2表示第一类对应矩阵中第m行第n列的计算表达式通式，

(1,1)_case2＝(a₁₂w₁w₃-a₁₃w₁w₂)I₁₁+(-a₁₁w₁w₃+a₁₃w₁w₁+a₁₂w₂w₃-a₁₃w₂w₂)I₁₂+(a₁₁w₁w₂-a₁₂w₁w₁+a₁₂w₃w₃-a₁₃w₂w₃)I₁₃+(-a₁₁w₂w₃+a₁₃w₁w₂)I₂₂+(a₁₁w₂w₂-a₁₂w₁w₂-a₁₁w₃w₃+a₁₃w₁w₃)I₂₃+(a₁₁w₂w₃-a₁₂w₁w₃)I₃₃

(2,1)_case2＝(a₂₂w₁w₃-a₂₃w₁w₂)I₁₁+(-a₂₁w₁w₃+a₂₃w₁w₁+a₂₂w₂w₃-a₂₃w₂w₂)I₁₂+(a₂₁w₁w₂-a₂₂w₁w₁+a₂₂w₃w₃-a₂₃w₂w₃)I₁₃+(-a₂₁w₂w₃+a₂₃w₁w₂)I₂₂+(a₂₁w₂w₂-a₂₂w₁w₂-a₂₁w₃w₃+a₂₃w₁w₃)I₂₃+(a₂₁w₂w₃-a₂₂w₁w₃)I₃₃

(3,1)_case2＝(a₃₂w₁w₃-a₃₃w₁w₂)I₁₁+(-a₃₁w₁w₃+a₃₃w₁w₁+a₃₂w₂w₃-a₃₃w₂w₂)I₁₂+(a₃₁w₁w₂-a₃₂w₁w₁+a₃₂w₃w₃-a₃₃w₂w₃)I₁₃+(-a₃₁w₂w₃+a₃₃w₁w₂)I₂₂+(a₃₁w₂w₂-a₃₂w₁w₂-a₃₁w₃w₃+a₃₃w₁w₃)I₂₃+(a₃₁w₂w₃-a₃₂w₁w₃)I₃₃

第三类：形式与第一类相似；

于是令则在第一类的推导结果上进一步展开，其中，

详细推导过程如下：

式中，(m,n)_case3表示第三类对应矩阵中第m行第n列的计算表达式通式，

(1,1)_case3＝(a₁₁r₁₁d₁+a₁₁r₁₂d₂+a₁₁r₁₃d₃)I₁₁+(a₁₂r₁₁d₁+a₁₁r₂₁d₁+a₁₂r₁₂d₂+a₁₁r₂₂d₂+a₁₂r₁₃d₃+a₁₁r₂₃d₃)I₁₂+(a₁₃r₁₁d₁+a₁₁r₃₁d₁+a₁₃r₁₂d₂+a₁₁r₃₂d₂+a₁₃r₁₃d₃+a₁₁r₃₃d₃)I₁₃+(a₁₂r₂₁d₁+a₁₂r₂₂d₂+a₁₂r₂₃d₃)I₂₂+(a₁₃r₂₁d₁+a₁₂r₃₁d₁+a₁₃r₂₂d₂+a₁₂r₃₂d₂+a₁₃r₂₃d₃+a₁₂r₃₃d₃)I₂₃+(a₁₃r₃₁d₁+a₁₃r₃₂d₂+a₁₃r₃₃d₃)I₃₃

(2,1)_case3＝(a₂₁r₁₁d₁+a₂₁r₁₂d₂+a₂₁r₁₃d₃)I₁₁+(a₂₂r₁₁d₁+a₂₁r₂₁d₁+a₂₂r₁₂d₂+a₂₁r₂₂d₂+a₂₂r₁₃d₃+a₂₁r₂₃d₃)I₁₂+(a₂₃r₁₁d₁+a₂₁r₃₁d₁+a₂₃r₁₂d₂+a₂₁r₃₂d₂+a₂₃r₁₃d₃+a₂₁r₃₃d₃)I₁₃+(a₂₂r₂₁d₁+a₂₂r₂₂d₂+a₂₂r₂₃d₃)I₂₂+(a₂₃r₂₁d₁+a₂₂r₃₁d₁+a₂₃r₂₂d₂+a₂₂r₃₂d₂+a₂₃r₂₃d₃+a₂₂r₃₃d₃)I₂₃+(a₂₃r₃₁d₁+a₂₃r₃₂d₂+a₂₃r₃₃d₃)I₃₃

(3,1)_case3＝(a₃₁r₁₁d₁+a₃₁r₁₂d₂+a₃₁r₁₃d₃)I₁₁+(a₃₂r₁₁d₁+a₃₁r₂₁d₁+a₃₂r₁₂d₂+a₃₁r₂₂d₂+a₃₂r₁₃d₃+a₃₁r₂₃d₃)I₁₂+(a₃₃r₁₁d₁+a₃₁r₃₁d₁+a₃₃r₁₂d₂+a₃₁r₃₂d₂+a₃₃r₁₃d₃+a₃₁r₃₃d₃)I₁₃+(a₃₂r₂₁d₁+a₃₂r₂₂d₂+a₃₂r₂₃d₃)I₂₂+(a₃₃r₂₁d₁+a₃₂r₃₁d₁+a₃₃r₂₂d₂+a₃₂r₃₂d₂+a₃₃r₂₃d₃+a₃₂r₃₃d₃)I₂₃+(a₃₃r₃₁d₁+a₃₃r₃₂d₂+a₃₃r₃₃d₃)I₃₃

其中，

综上，由式(32)～(35)三类化简通式表达式，对式(31)中含有的所有项进行化简，得到矩阵形式的线性方程如下

其中，T_c为控制力矩，为待辨识惯量列阵，A_m为待辨识质量系数矩阵，A_S为待辨识静矩系数矩阵，A_I为待辨识惯量系数矩阵，

A_I同理，整理得到，由于包含的项过多，表示十分复杂，不再次推导；

A_m∈R_3×1,A_S∈R_3×3,A_I∈R_3×6

或者写为

该式称为辨识模型；其中，Φ＝[A_m A_S A_I]_3×10为辨识模型的系数矩阵，为待辨识量，γ＝T_c-Q为辨识模型的测量矩阵；

步骤4：基于最小二乘法，进行质量特性参数辨识：

式(37)中系数矩阵的行秩为3，而待辨识量的维度是10，因此完成一次辨识至少需要选取4个时刻的状态量，构成新的辨识方程如下

式中，H为多个时刻构成的辨识方程系数矩阵，v为多个时刻构成的辨识方程测量矩阵

其中，Φ_i和γ_i的脚标“i(i＝1,2,3,4)”代表t_i时刻，由式(36)中t_i时刻状态量Φ和γ构成的矩阵；

由式(38)解得一组待辨识量结果为

即得到一组基于系统动力学模型的双臂抓捕下空间非合作目标的质量特性参数辨识结果。