CN104570736B - 一种星‑臂耦合系统的动力学参数在轨辨识方法和装置 - Google Patents

Abstract

本发明公开一种星‑臂耦合系统的动力学参数在轨辨识方法，包括：获取机械臂运动关节的运动状态数据，以及对应的实测的主星数据；所述主星数据为主星姿态角和姿态角速度数据；假定一组主星动力学参数，将机械臂运动关节的运动状态数据和假定的主星动力学参数输入动力学模型进行模拟，比较动力学模型输出的主星数据和实测的主星数据的差别，然后对假定的主星动力学参数采用改进全局粒子群方法进行优化；所述主星动力学参数包括主星质量、主星质心位置和主星转动惯量。本发明能够实现星‑臂耦合系统的动力学参数在轨辨识，且具有较强的泛用性。

Description

一种星-臂耦合系统的动力学参数在轨辨识方法和装置

技术领域

本发明涉及机器人领域，特别涉及一种星-臂耦合系统的动力学参数在轨辨识方法和装置。

背景技术

星-臂耦合系统的动力学参数在轨辨识具有明确的现实意义。对于进行空间遥操作任务的航天器，在轨飞行时的燃料消耗不可避免。随着燃料的不断消耗，星-臂耦合系统的动力学参数，例如质量、转动惯量等，都会发生改变。因此，对于在轨操作的任务规划以及机械臂在线路径规划中，星-臂耦合系统的动力学参数的变化量的获取，是非常重要的输入条件。

但是，现有技术的动力学参数在轨辨识方法，主要有三种：

(1)、借助推器施加外力，通过测量机器人各关节的加速度信号，求解牛顿-欧拉运动方程来辨识动力学参数。该方法的缺点在于：需要消耗燃料，而且不易获得精确的关节加速度及力矩信息。

(2)、不施加外力，仅驱动机械臂关节转动，通过测量转动前后主星姿态和位置变化，基于动量和角动量守恒定律来辨识动力学参数。该方法的缺点在于：为求解所有的动力学参数，需要获得足够数量的方程，此时，必须要驱动多个关节运动；且该方法除了需要获取角动量之外，还要同时获取线动量，但是在现有技术水平下，卫星线动量的实时测量精度远低于角动量，达不到参数辨识所需精度。

(3)、通过运动学仿真建立样本库，对多层前向神经网络进行训练，采用训练好的神经网络进行动力学参数的在轨辨识。该方法的缺点在于：适应性较差，只能在预先选定的样本空间参数范围内运作，缺乏对不同关节工作空间的外推能力，难以用于机械臂末端载荷未知的情况。另外，当机器人关节自由度以及待辨识参数较多时，神经网络很难设计、训练。

发明内容

本发明提供一种星-臂耦合系统的动力学参数在轨辨识方法和装置，能够克服现有技术的缺陷，以实现主星动力学参数在轨辨识。

为了解决上述问题，本发明提供一种星-臂耦合系统的动力学参数在轨辨识方法和装置。

所述方法包括：

步骤1：建立与所述星-臂耦合系统在标定燃料下的动力学预测模型；

步骤2：根据所述动力学预测模型，获取所述星-臂耦合系统的动力学参数的在轨辨识模型；

步骤3：获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，以及主星运动状态实测数据；

步骤4：设定一组动力学参数的测试值，利用所述测试值和所述机械臂运动关节的运动状态数据，结合所述在轨辨识模型，得到主星运动状态模拟数据；

步骤5：将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较，根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值。

进一步地，所述动力学预测模型为：

其中，所述星-臂耦合系统的主星的广义雅克比矩阵为：

所述星-臂耦合系统的机械臂的广义雅克比矩阵为：

为主星姿态角速度，为机械臂关节角速度，L₀为惯性坐标系下的初始角动量。

进一步地，所述在轨辨识模型为：

进一步地，步骤5中将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较为：

D＝(w_x-w′_x)²+(w_y-w′_y)²+(w_z-w′_z)²

其中，D为所述比较结果；w′_x、w′_y、w′_z为所述主星运动状态模拟数据的三个姿态角速度分量；w_x、w_y、w_z为所述主星运动状态实测数据的三个姿态角速度分量；

步骤5中根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值包括：采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，使D达到最小。

进一步地，所述采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，包括：

步骤51：在所述动力学参数的在轨辨识模型中的待辨识量(M_fuel,)所构成的四维搜索空间中，使用改进的PSO的速度更新式，进行各粒子的速度更新；

所述改进的PSO的速度更新式为：

v_n(g+1)＝ωv_n(g)+c₁ξ[ⁱx_n(g)-x_n(g)]

+c₂η[^gx_n(g)-x_n(g)]

x_n(g+1)＝x_n(g)+v_n(g+1)

其中，学习因子c1＝c2＝2；惯性权重ω＝0.9～0.2；粒子邻域采用空间邻域，即到某粒子的欧式距离最小的N_nb个粒子组成该粒子的邻域；

其中，x代表粒子位置，为矢量，每个粒子的位置初值即为所述N组动力学参数的测试值(M_fuel,)；v代表粒子速度；下标n∈[1,N]为各粒子的标号；N为粒子总数，N与所述选取的动力学参数的测试值的组数相等；参数g为迭代次数；ⁱx代表粒子个体历史最优位置，在每一代中，各粒子最优位置即为在当前代之前，min(D)所对应的动力学参数的测试值(M_fuel,)；^gx代表种群历史最优位置，即所有N个粒子在当前代之前min(D)所对应的动力学参数的测试值^g(M_fuel,)；

N_nb满足：

其中，G为总迭代次数，g为当前迭代次数，K_nb为最终邻域粒子比例；

步骤52：每隔一段时间，对所有粒子进行一次筛选，确定陷入局部最优值的粒子群，并将所述陷入局部最优值的粒子群强制重启到好的区间附近，以加速收敛；

其中，当粒子群满足下式

gmodG_re＝0且n＞K_reN时，

则确定该粒子群为所述陷入局部最优值的粒子群；

所述强制重启为：

x_n(g)＝^socx_k(g)+r

其中，G_re为筛选间隔代数，K_re为每次重启粒子比例，e[^cogx_n(g)]为递增排序的粒子n的个体历史最优误差，n＝1,2,...,N；

x_n(g)为所述陷入局部最优值的粒子群强制重启后的位置；^socx_k(g)为表现最好的粒子k的邻域历史最优位置，k∈(1,2,...,N)；r为一个小区间内的四维随机矢量，其取值区间根据周围粒子的密度自适应地计算；

步骤53：随机获取多个时刻分别对应的主星运动状态模拟数据和主星运动状态实测数据，构成一组数据序列，利用所述数据序列中不同时刻分别对应的主星运动状态模拟数据和主星运动状态实测数据作为参数，计算不同粒子的误差；

其中，采用下式计算不同粒子的误差：

其中，g＝1,2,...,G。

所述装置包括：

第一建模单元，用于建立与所述星-臂耦合系统在标定燃料下的动力学预测模型；

第二建模单元，用于根据所述动力学预测模型，获取所述星-臂耦合系统的动力学参数的在轨辨识模型；

第一数据获取单元，用于获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，以及主星运动状态实测数据；

第二数据获取单元，用于设定一组动力学参数的测试值，利用所述测试值和所述机械臂运动关节的运动状态数据，结合所述在轨辨识模型，得到主星运动状态模拟数据；

优化单元，用于将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较，根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值。

进一步地，所述动力学预测模型为：

其中，所述星-臂耦合系统的主星的广义雅克比矩阵为：

所述星-臂耦合系统的机械臂的广义雅克比矩阵为：

为主星姿态角速度，为机械臂关节角速度，L₀为惯性坐标系下的初始角动量。

进一步地，所述在轨辨识模型为：

进一步地，所述优化单元包括：比较子单元和优化子单元；

所述比较子单元，用于将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较：

D＝(w_x-w′_x)²+(w_y-w′_y)²+(w_z-w′_z)²

其中，D为所述比较结果；w′_x、w′_y、w′_z为所述主星运动状态模拟数据的三个姿态角速度分量；w_x、w_y、w_z为所述主星运动状态实测数据的三个姿态角速度分量；

所述优化子单元，用于采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，使D达到最小。

本发明具有如下有益效果：

第一：该方法只需要获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，具体的，通过机械臂转动影响主星的姿态，从而测量主星姿态的角速度来获取所述运动状态数据。由此可见，本方法无需借助外力即可实现，因此不需要消耗外界材质和能量。

第二：该方法仅需要测量角动量的信息而无需测量线动量信息。由于在空间测量中，动态的角动量信息的测量精度可以很高，但是动态线动量的测量精度难以保证，一般需要长时间的累积测量滤波才能收敛。由此可见，本发明所述方法与现有技术相比，更易于实现。

第三：该方法不需要限定任务空间，不像神经网络辨识必须先在设定好的任务空间内进行训练，辨识也必须在训练空间的包络范围内。因此本发明具有较强的泛用性。

附图说明

图1为本发明实施例一的星-臂耦合系统的动力学参数在轨辨识方法流程图；

图2为本发明实施例的星-臂耦合动力学建模对象示意图；

图3为本发明实施例的基于改进PSO方法的100次主星参数(质量M)的辨识结果；

图4为本发明实施例的基于改进PSO方法的100次主星参数(质量位置X)的辨识结果；

图5为本发明实施例的基于改进PSO方法的100次主星参数(质量位置Y)的辨识结果；

图6为本发明实施例的基于改进PSO方法的100次主星参数(质量位置Z)的辨识结果；

图7为本发明实施例的基于改进PSO方法的100次主星参数(转动惯量Ixx)的辨识结果；

图8为本发明实施例的基于改进PSO方法的100次主星参数(转动惯量Iyy)的辨识结果；

图9为本发明实施例的基于改进PSO方法的100次主星参数(转动惯量Izz)的辨识结果。

图10为本发明实施例所述的星-臂耦合系统的动力学参数在轨辨识装置的结构图。

具体实施方式

下文中将结合附图对本发明的实施例进行详细说明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互任意组合。

本发明讨论的星-臂耦合系统的动力学参数在轨辨识具体为：通过在特定的安全路径下驱动机械臂，并测量机械臂运动和主星运动的状态数据，通过分析测量数据，辨识主星的动力学参数。该动力学参数具体可以包括：主星质量M_base，主星质心位置主星转动惯量

参照图1，为本发明实施例一的星-臂耦合系统的动力学参数在轨辨识方法流程图。如图1所示，所述方法包括以下步骤：

步骤S10：建立与待辨识对象标定燃料下的动力学预测模型；其中，所述待辨识对象是指：需要进行动力学参数在轨辨识的星-臂耦合系统。

具体地，所述动力学预测模型的建立过程可以如下所述：

如图2所示，该星-臂耦合系统具有n个连杆。该n个连杆通过n个旋转关节(或者铰链)依次连接，接续到主星上。其中，每个旋转关节只有一个自由度。该系统处于无轨控推力和姿控力矩作用下的在轨飞行状态，即主星位置和姿态均不受助推器或其他类似外力控制。

需要说明的是，本发明实施例中，在所述预测模型的建模过程中，地球扁率、大气阻尼、太阳光压、地球磁场等影响皆忽略不计，整个系统动量与角动量视为守恒。

首先，对本发明实施例中各公式需要涉及的各参数的定义进行说明。除特别声明之外，本发明实施例中各公式出现的参数均适合该定义，以后不再赘述。

n，机械臂包括的连杆数；

r_i，第i个连杆质心的位置矢量；

r_G，系统质心的位置矢量；

p_n，连杆末端的位置矢量；

l_i，第i个旋转关节指向第(i+1)个旋转关节的矢量；

a_i，第i个旋转关节指向质心i的矢量；

b_i，第i个连杆的质心指向第(i+1)个旋转关节的矢量；

m_i，第i个连杆的质量；

w，系统总质量；

I_i，第i个连杆相对于自身质心的惯量矩阵；

ω_i，第i个连杆的角速度；

α,β,γ，主星姿态三轴欧拉角；

φ_i，第i个旋转关节的旋转角；

第i个旋转关节的初始D-H角参数。

下面对步骤S10所述的建立与待辨识对象标定燃料下的动力学预测模型的过程进行详细介绍。

步骤S101：定义所述星-臂耦合系统的动力学预测模型的坐标系。

需要说明的是，在后续的公式中，每个物理量左上角标识的符号表示该物理量所处的坐标系。具体地，

(1)A代表惯性坐标系(简称为惯性系)，该坐标系固定在惯性空间。

(2)i代表固连在第i个连杆上的坐标系。

其中，坐标系i为机械臂第i个连杆的随体坐标，其原点固定在机械臂的第i个旋转关节上，坐标系i的z轴与第i个旋转关节的旋转轴保持一致，其x轴与y轴参考机械臂D-H参数定义。

(3)O代表固连在主星质心上的坐标系。

其中，坐标系O为主星的随体坐标系，其原点固定在主星的质心上。主星本体坐标系与惯性坐标系之间的变换按照欧拉角定义。

(4)从坐标系i到坐标系(i-1)的变换为^i-1A_i，满足如下关系：

步骤S102：建立所述星-臂耦合系统的动力学预测模型的的基本方程组，该基本方程组包括如下式(2)至式(6)所示。

系统的质心位置如式(2)：

动量守恒方程如式(3)：

角动量守恒如式(4)：

两相邻连杆质心之间的几何方程如式(5)：

r_i-r_i-1＝a_i+b_i-1 (5)

主星质心与机械臂连杆末端之间的几何方程如式(6)：

步骤S103：根据所述基本方程组推导得到所述星-臂耦合系统的动力学预测模型。

将式(5)展开至r₀，得式(7)：

代入式(2)，得：

其中再将r₀代回式(5)，得：

其中各量均为在惯性系下的表达，即得式(10)：

其中

式(10)中a_i,b_i,K_il都是与时间无关的常量。

对式(10)求导，得式(11)：

首先注意到：

其中：

则有：

其中：

代入式(11)中，可得式(17)：

其中v_ij为：

与r的几何关系类似，可有式(19)：

其中为j坐标中的z轴单位矢量，ω_G为系统初始角速度。将式(17)和式(18)代入式(4)，令初始角动量为0，在惯性坐标中，左边第一项为：

第二项为：

最终合计为耦合动力学方程，即为本发明实施例所述的动力学预测模型：

其中，主星的广义雅克比矩阵为：

多关节机械臂的广义雅克比矩阵为：

为主星姿态角速度，为机械臂关节角速度，L₀为惯性坐标系下的初始角动量。

至此，完成了所述动力学预测模型的建立过程。实际应用中，在模型建立后，还需要对建立的模型进行校验和验证，以确定建立的模型和实际的输入输出之间具有良好的模拟关系。

步骤S104：对步骤S103得到的动力学预测模型进行校验和验证。

该校验和验证手段主要为：比较在相同激励下，该模型的输出与实际输出的匹配性。例如，输入一组关节运动角度状态序列，比较模型得到的主星的角度变动和角速度变化是否与实际输出一致。

步骤S20：建立动力学参数的在轨辨识模型。

具体地，步骤S20可以包括：

步骤S201：在开始辨识之前,对所述动力学预测模型进行数学变形。

具体地，当外力矩为L₀＝0时，以主星姿态角速度为因变量，将式(22)变形得到：

可见，星-臂耦合系统的动力学参数的在轨辨识可转化为多参量的复杂映射问题。

步骤S202：对数学变形后得到的模型进行简化。

具体地，如式(25)可见，其中包括10个待辨识参数，对其进行简化。具体简化过程可以为，将燃料箱相对主星所占的体积视作小量，将原待辨识的质量、惯量矩阵简化为燃料质量M_fuel和燃料箱质心位置将10个待辨识参数简化为4个待辨识参数，其转化公式如式(26)所示：

M_base＝M⁰ _base-M_fuel

I_xy＝I⁰ _xy+M_fuel×b_{base_x}×b_{base_y}

I_yz＝I⁰ _yz+M_fuel×b_{base_y}×b_{base_z}

I_zx＝I⁰ _zx+M_fuel×b_{base_z}×b_{base_x} (26)

式(26)中，上标0代表在标定的燃料质量下，主星的动力学参数的对应值，通过式(26)，可由[M_fuel,b_fuel(x),b_fuel(y),b_fuel(z)]四个参量，表示原需要辨识的10个参量主星质量M_base、主星质心位置、和主星的转动惯量

对式(22)，令L₀＝0，可以得到：

考虑到式(27)中，主星姿态角速度与机械臂关节角速度之间为线性关系，因此在仅转动一个旋转关节(即令n维向量中的(n-1)个分量为0)的基础上，可以将不同的关节角速度归一到同一尺度上，得到式(28)。

这种归一化方法有利于处理多个不同条件下的激励-响应数据组。

最后，经过路径规划、物理化简以及数学变形三个层面的简化之后，步骤S201中得到的数学变形后的模型可以简化为式(29)所示：

则，式(29)即为步骤S20得到的动力学参数的在轨辨识模型。

步骤S30：获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，以及对应的主星运动状态实测数据。

具体地，所述机械臂运动关节的运动状态数据可以通过机械臂运动关节的角速度传感器、角度传感器、以及力矩传感器获取。

所述主星运动状态实测数据可以通过星光敏感器等姿态测量传感器件实时测量得到。

步骤S40：设定N组动力学参数的测试值(M_fuel,)，利用所述测试值和所述机械臂运动关节的运动状态数据，结合所述在轨辨识模型，得到N组主星运动状态模拟数据。

步骤S50：将步骤S40中得到的主星运动状态模拟数据和步骤S30中得到的主星运动状态实测数据进行比较，根据比较结果，对步骤S40中设定的动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值。

其中，所述将步骤S40中得到的主星运动状态模拟数据和步骤S30中得到的主星运动状态实测数据进行比较具体为：

D＝(w_x-w′_x)²+(w_y-w′_y)²+(w_z-w′_z)² (30)

其中，D为所述比较结果；w′_x、w′_y、w′_z为所述主星运动状态模拟数据的三个姿态角速度分量；w_x、w_y、w_z为所述主星运动状态实测数据的三个姿态角速度分量。

根据所述比较结果，对步骤S40中设定的动力学参数的测试值进行优化具体为：对式(30)进行使D最小的最优化处理，即为min(D)。下面，对本发明实施例中，所述最优化处理的方法进行详细介绍。本发明实施例中，采用改进的PSO(Particle Swarm Optimization粒子群优选算法)对所述设定的动力学参数的测试值进行优化。

步骤S501：针对式(29)所示的动力学参数的在轨辨识模型中的待辨识量(M_fuel,)所构成的四维搜索空间(x∈R⁴)，使用改进的PSO的速度更新式，进行各粒子的速度更新。

具体地，所述改进的PSO的速度更新式为：

v_n(g+1)＝ωv_n(g)+c₁ξ[ⁱx_n(g)-x_n(g)]

+c₂η[^gx_n(g)-x_n(g)]

(31)

x_n(g+1)＝x_n(g)+v_n(g+1)

其中，学习因子c1＝c2＝2；惯性权重ω＝0.9～0.2；粒子邻域采用空间邻域，即到某粒子的欧式距离最小的N_nb个粒子组成该粒子的邻域。

其中，x代表粒子位置，为矢量，每个粒子的位置初值即为所述N组动力学参数的测试值(M_fuel,)；v代表粒子速度；下标n∈[1,N]为各粒子的标号；N为粒子总数，N与所述选取的动力学参数的测试值的组数相等；参数g为迭代次数。ⁱx代表粒子个体历史最优位置，在每一代中，各粒子最优位置即为在当前代之前，式(30)中所述的min(D)所对应的动力学参数的测试值(M_fuel,)；^gx代表种群历史最优位置，即所有N个粒子在当前代之前所述的min(D)所对应的动力学参数的测试值^g(M_fuel,)。

N_nb满足：

其中，G为总迭代次数，g为当前迭代次数，K_nb为最终邻域粒子比例。

上述粒子邻域是指粒子群的拓扑结构。

需要说明的是，传统的PSO算法中，N_nb占总粒子数N_all的比从一个较小的初值线性地增至1，传统PSO算法不易平衡充分优化与种群多样性之间的矛盾。

为获取更高效、更稳定的迭代结果，本发明实施例所述改进的PSO算法中，不再将邻域的定义扩散到全局，而是定义N_nb满足式(32)，以便保持种群多样性，使每一个局部最优区间获得充分发育。

步骤S502：每隔一段时间，对所有粒子进行一次筛选，确定陷入局部最优值的粒子群，并将所述陷入局部最优值的粒子群强制重启到好的区间附近，以加速收敛。

具体地，如果粒子群满足式(33)：

gmodG_re＝0且n＞K_reN (33)

则可以认为该粒子群为所述陷入局部最优值的粒子群，需要将该粒子群强制重启到好的区间附近。具体地，该强制重启过程为：令

x_n(g)＝^socx_k(g)+r (34)

其中，G_re为筛选间隔代数，K_re为每次重启粒子比例，e[^cogx_n(g)]为递增排序的粒子n的个体历史最优误差，n＝1,2,...,N。

x_n(g)为所述陷入局部最优值的粒子群强制重启后的位置；^socx_k(g)为表现最好的粒子k的邻域历史最优位置，k∈(1,2,...,N)；r为一个小区间内的四维随机矢量，其取值区间根据周围粒子的密度自适应地计算。

需要说明的是，为了使每个区间的粒子得到足够发育，筛选每隔若干代进行一次。

步骤S503：随机获取多个时刻分别对应的主星运动状态模拟数据和主星运动状态实测数据，构成一组数据序列，利用所述数据序列中不同时刻分别对应的主星运动状态模拟数据和主星运动状态实测数据作为参数，计算不同粒子的误差。具体地，采用式(35)计算不同粒子的误差：

其中，g＝1,2,...,G。

需要说明的是，为了充分利用不同关节构型、不同激励条件下的数据，使用一组数据序列中不同时刻的主星运动状态模拟数据和主星运动状态实测数据作为参数，计算不同粒子的误差。其中，需要注意对于特定粒子，在整个迭代期间，该特定粒子对应的参数不变。这样虽然不同的粒子计算误差时使用不同的方程，但它们都收敛到相同的优化目标点，即真实的(M_fuel,)。

在实际应用中，所述改进的PSO算法的参数可以根据实际需要具体设定。

下面结合一组具体的参数对本发明实施例所述的动力学参数的辨识方法的辨识结果进行说明。需要说明的是，在本发明其他实施例中，所述改进的PSO算法的参数设定不限于该实例所述。

具体地，假设所述改进PSO方法中的参数设定如下：迭代总次数G＝50，最终邻域粒子比例K_nb为10％，筛选间隔代数G_re为10，每次重启粒子比例K_re为20％。

此时，使用本发明实施例所述的辨识方法，对100组随机生成的动力学参数进行辨识，统计得到的平均辨识相对误差如表1所示。

其中，所述平均辨识相对误差通过下式得到：

相对误差＝|(辨识结果-实际值)/实际值|) (36)

100组主星质量、质心位置x、主惯量Ixx、Iyy、Izz的辨识结果如图3～9。

表1：对动力学参数进行100次辨识得到的平均相对误差值

从表1可知，除了转动交叉惯性积，和质心位置Y以外，其他的主星参数的辨识精度都比较高，交叉惯性积辨识之所以精度差，是因为交叉惯性积的量级非常小，其变化对动力学响应改变的敏感度低，因此难以做到高精度辨识；质心位置Y的辨识精度低也是因为质心在Y轴上的基准值为0mm，量级很小。从图3～9可以看出，参数辨识的稳定性和精度均比较理想。

对应于本发明实施例提供的星-臂耦合系统的动力学参数在轨辨识方法，本发明实施例还提供一种星-臂耦合系统的动力学参数在轨辨识装置。如图10所示，为本发明实施例所述的星-臂耦合系统的动力学参数在轨辨识装置的结构图。

如图10所示，所述装置包括：第一建模单元U10、第二建模单元U20、第一数据获取单元U30、第二数据获取单元U40和优化单元U50。

所述第一建模单元U10，用于建立与所述星-臂耦合系统在标定燃料下的动力学预测模型。

所述第二建模单元U20，用于根据所述动力学预测模型，获取所述星-臂耦合系统的动力学参数的在轨辨识模型。

所述第一数据获取单元U30，用于获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，以及主星运动状态实测数据。

所述第二数据获取单元U40，用于设定一组动力学参数的测试值，利用所述测试值和所述机械臂运动关节的运动状态数据，结合所述在轨辨识模型，得到主星运动状态模拟数据。

所述优化单元U50，用于将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较，根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值。

优选的，所述动力学预测模型为：

其中，所述星-臂耦合系统的主星的广义雅克比矩阵为：

所述星-臂耦合系统的机械臂的广义雅克比矩阵为：

为主星姿态角速度，为机械臂关节角速度，L₀为惯性坐标系下的初始角动量。

优选的，所述在轨辨识模型为：

优选的，所述优化单元U50可以包括：比较子单元和优化子单元。

所述比较子单元，用于将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较：

D＝(w_x-w′_x)²+(w_y-w′_y)²+(w_z-w′_z)² (30)

其中，D为所述比较结果；w′_x、w′_y、w′_z为所述主星运动状态模拟数据的三个姿态角速度分量；w_x、w_y、w_z为所述主星运动状态实测数据的三个姿态角速度分量。

所述优化子单元，用于采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，使D达到最小。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种星-臂耦合系统的动力学参数在轨辨识方法，其特征在于，包括：

步骤1：建立与所述星-臂耦合系统在标定燃料下的动力学预测模型；

步骤2：根据所述动力学预测模型，获取所述星-臂耦合系统的动力学参数的在轨辨识模型；

步骤3：获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，以及主星运动状态实测数据；

步骤4：设定一组动力学参数的测试值，利用所述测试值和所述机械臂运动关节的运动状态数据，结合所述在轨辨识模型，得到主星运动状态模拟数据；

步骤5：将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较，根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值；

其中，所述动力学预测模型为：

其中，所述星-臂耦合系统的主星的广义雅克比矩阵为：

所述星-臂耦合系统的机械臂的广义雅克比矩阵为：

为主星姿态角速度，为机械臂关节角速度，L₀为惯性坐标系下的初始角动量。

2.根据权利要求1所述的方法，其特征在于，所述在轨辨识模型为：

其中为主星三个姿态角速度的合成向量，M_fuel为燃料质量，为燃料箱质心位置，为机械臂各关节角速度的合成向量，θ_arm(1,2)为机械臂各关节角度的合成向量，θ_base为主星三个姿态角度的合成向量。

3.根据权利要求2所述的方法，其特征在于，

步骤5中将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较为：

D＝(w_x-w_x′)²+(w_y-w_y′)²+(w_z-w_z′)²

其中，D为所述比较结果；w_x′、w′_y、w_z′为所述主星运动状态模拟数据的三个姿态角速度分量；w_x、w_y、w_z为所述主星运动状态实测数据的三个姿态角速度分量；

步骤5中根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值包括：采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，使D达到最小。

4.根据权利要求3所述的方法，其特征在于，所述采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，包括：

步骤51：在所述动力学参数的在轨辨识模型中的待辨识量所构成的四维搜索空间中，使用改进的PSO的速度更新式，进行各粒子的速度更新；

所述改进的PSO的速度更新式为：

v_n(g+1)＝ωv_n(g)+c₁ξ[ⁱx_n(g)-x_n(g)]

+c₂η[^gx_n(g)-x_n(g)]

x_n(g+1)＝x_n(g)+v_n(g+1)

其中，学习因子c1＝c2＝2；惯性权重ω＝0.9～0.2；粒子邻域采用空间邻域，即到某粒子的欧式距离最小的N_nb个粒子组成该粒子的邻域；

其中，x代表粒子位置，为矢量，每个粒子的位置初值即为所述动力学参数的测试值v代表粒子速度；下标n∈[1,N]为各粒子的标号；N为粒子总数，N与选取的所述动力学参数的测试值的组数相等；参数g为迭代次数；ⁱx代表粒子个体历史最优位置，在每一代中，各粒子最优位置即为在当前代之前，min(D)所对应的动力学参数的测试值 ^gx代表种群历史最优位置，即所有N个粒子在当前代之前min(D)所对应的动力学参数的测试值

N_nb满足：

其中，G为总迭代次数，g为当前迭代次数，K_nb为最终邻域粒子比例；

步骤52：每隔一段时间，对所有粒子进行一次筛选，确定陷入局部最优值的粒子群，并将所述陷入局部最优值的粒子群强制重启到好的区间附近，以加速收敛；

其中，当粒子群满足下式

gmodG_re＝0且n＞K_reN时，

则确定该粒子群为所述陷入局部最优值的粒子群；

所述强制重启为：

x_n(g)＝^socx_k(g)+r

其中，G_re为筛选间隔代数，K_re为每次重启粒子比例，e[^cogx_n(g)]为递增排序的粒子n的个体历史最优误差，n＝1,2,...,N；

x_n(g)为所述陷入局部最优值的粒子群强制重启后的位置；^socx_k(g)为表现最好的粒子k的邻域历史最优位置，k∈(1,2,...,N)；r为一个小区间内的四维随机矢量，其取值区间根据周围粒子的密度自适应地计算；

步骤53：随机获取多个时刻分别对应的主星运动状态模拟数据和主星运动状态实测数据，构成一组数据序列，利用所述数据序列中不同时刻分别对应的主星运动状态模拟数据和主星运动状态实测数据作为参数，计算不同粒子的误差；

其中，采用下式计算不同粒子的误差：

其中，g＝1,2,...,G。

5.一种星-臂耦合系统的动力学参数在轨辨识装置，其特征在于，所述装置包括：

第一建模单元，用于建立与所述星-臂耦合系统在标定燃料下的动力学预测模型；

第二建模单元，用于根据所述动力学预测模型，获取所述星-臂耦合系统的动力学参数的在轨辨识模型；

第一数据获取单元，用于获取所述星-臂耦合系统的机械臂运动关节的运动状态数据，以及主星运动状态实测数据；

第二数据获取单元，用于设定一组动力学参数的测试值，利用所述测试值和所述机械臂运动关节的运动状态数据，结合所述在轨辨识模型，得到主星运动状态模拟数据；

优化单元，用于将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较，根据比较结果，对所述动力学参数的测试值进行迭代优化，得到所述动力学参数的辨识值；

其中，所述动力学预测模型为：

其中，所述星-臂耦合系统的主星的广义雅克比矩阵为：

所述星-臂耦合系统的机械臂的广义雅克比矩阵为：

为主星姿态角速度，为机械臂关节角速度，L₀为惯性坐标系下的初始角动量。

6.根据权利要求5所述的星-臂耦合系统的动力学参数在轨辨识装置，其特征在于，所述在轨辨识模型为：

其中为主星三个姿态角速度的合成向量，M_fuel为燃料质量，为燃料箱质心位置，为机械臂各关节角速度的合成向量，θ_arm(1,2)为机械臂各关节角度的合成向量，θ_base为主星三个姿态角度的合成向量。

7.根据权利要求5所述的星-臂耦合系统的动力学参数在轨辨识装置，其特征在于，

所述优化单元包括：比较子单元和优化子单元；

所述比较子单元，用于将所述主星运动状态模拟数据和所述主星运动状态实测数据进行比较：

D＝(w_x-w_x′)²+(w_y-w_y′)²+(w_z-w_z′)²

其中，D为所述比较结果；w_x′、w′_y、w_z′为所述主星运动状态模拟数据的三个姿态角速度分量；w_x、w_y、w_z为所述主星运动状态实测数据的三个姿态角速度分量；

所述优化子单元，用于采用改进的粒子群优选算法PSO对所述动力学参数的测试值进行优化，使D达到最小。