CN110515299B - 一种星-臂耦合系统的主星姿态解耦预报方法及系统 - Google Patents

Abstract

本发明实施例公开了一种星‑臂耦合系统主星姿态解耦预报方法，先依据所述星‑臂耦合系统获取空间机器人的主星‑机械臂耦合系统运动学方程；然后获得主星姿态角速度与机械臂关节角速度的等效线性关系式，即为目标仿真模型；根据获取所有历史时刻的主星姿态角速度的仿真响应和实测响应，对所有历史数据的实测/仿真响应比加权求和获取不断更新的当前时刻的实测/仿真响应比，即为主星姿态补偿模型；最后不断滚动修正所述等效线性关系式的等效线性参数，进而预报未来时刻的预测响应。本发明基于滚动修正的思路，不断修正目标仿真模型的等效线性参数，使得目标仿真模型误差的影响不会随着预报时间的延长而不断积累。

Description

一种星-臂耦合系统的主星姿态解耦预报方法及系统

技术领域

本发明实施例涉及机器人技术领域，具体涉及一种星-臂耦合系统的主星姿态解耦预报方法及系统。

背景技术

空间机器人已经成为在轨操作和在轨服务的高技术装备，星-臂耦合系统主星姿态对在任意机械臂构型下的位姿有重要影响，因此准确的预测基座的姿态，对于执行空间机器人的在轨任务非常重要。

由于空间机器人在轨飞行时，燃料消耗不可避免，随着燃料的消耗，星-臂耦合系统的动力学参数(如质量、转动惯量、质心等)都会发生改变，而针对于动力学参数发生改变的星-臂耦合系统，在相同的机械臂运动路径下，如果对动力学参数没有补偿控制(比如喷气)的手段，自由基座产生的姿态变化会与预计的不同，这将严重影响到空间人物的执行安全。

现有一些辨识方法，通过较准确的辨识基座参数，从而改进动力学预报的精度，然而当机械臂运动模型与预计的模型相差较大、或者基座参数变化巨大(比如远比30％大，达到数倍的误差)、又或者机械臂末端抓取了一个未知的对象时，或者这三者同时存在并耦合时，单一的方式已经无法解决该问题。

发明内容

为此，本发明实施例提供一种星-臂耦合系统的主星姿态解耦预报方法及系统，以解决现有技术中的问题。

为了实现上述目的，本发明的实施方式提供如下技术方案：

在本发明实施例的第一个方面，提供了一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，包括如下步骤：

步骤100、依据所述星-臂耦合系统获取空间机器人主星-机械臂耦合系统的运动学方程；

步骤200、如若基座的惯性参数与真实值误差很大，则在已有的机械臂实测运动状态和基座运动状态的数据库条件下，对基座的惯性参数进行辨识，并将所述基座的惯性参数赋予在所述运动学方程中，否则跳过该步骤；

步骤300、如若机械臂存在抓取未知目标的情况，则对所述未知目标的惯性参数进行辨识，并将辨识后的结果，作为机械臂的下一个不能运动的节点加入到所述运动学方程中，否则跳过该步骤；

步骤400如若机械臂运动模型与预计模型相差较大，不能直接获取未来机械臂运动指令，则对多关节机械臂进行修正型预报，在获得未来一段时间的指令的同时，获得带滚动修正的机械臂运动预报状态，并将该机械臂预报状态认为是机械臂实际运动状，进入对基座姿态的预测步骤，否则跳过该步骤；

步骤500、建立用于获取主星姿态角速度的仿真响应的目标仿真模型，以及对所述仿真响应进行修正补偿的主星姿态补偿模型，进而不断滚动修正目标仿真模型来预报未来时刻的预测响应。

优选地，所述主星-机械臂耦合系统运动学方程为：

其中：

为主星的广义雅克比矩阵，满足：

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

其中

为主星姿态角速度，

为机械臂关节角速度，L₀为系统初始角动量。

优选地，所述目标仿真模型的建立方法包括：

获得主星姿态角速度与机械臂关节角速度的等效线性关系式，以机械臂关节角速度为系统激励，获取主星姿态角速度的仿真响应，即为目标仿真模型，所述主星姿态角速度与机械臂关节角速度的等效线性关系的获取方法为：

选择惯性系，并令系统的初始角动量为0，得到：

由此可以得到：

定义

得到在某个确定的机械臂构型下主星姿态角速度与机械臂关节角速度等效线性关系式：

优选地，所述主星姿态补偿模型的建立方法包括：

获取所有历史时刻的主星姿态角速度的仿真响应和实测响应，并通过对所有历史数据的实测/仿真响应比加权求和获取不断更新的当前时刻的实测/仿真响应比；

设星-臂耦合系统从t₀时刻开始运转，当前时刻为t_K，获取过去t₀～t_K时间段内每一时刻主星姿态角速度的仿真响应

和实测响应

并求得任意一历史时刻对应的实测/仿真响应比

对t_K时刻之前[t₀,t_K]区间内所有历史数据进行加权求和得到当前时刻实测/仿真响应比

即令当前时刻实测/仿真响应比：

其中，w(t)为加权权值，令

在离散情况下，

化为如下形式：

其中，t_K＝t₀+KΔt，而Δt＝t_k+1-t_k,k＝0,1,2...为离散时间步长，同样，令权值w(t_k)满足

获取预测响应

其中

为用于修正未来某一时刻t∈(t_K,t_K+T]仿真响应的预测/仿真响应比，直接取作当前时刻实测/仿真响应比

优选地，所述主星姿态补偿模型中当前时刻的实测/仿真响应比

基于递推最小二乘法实时滚动修正，具体方法包括：

取预测响应

为参数估计模型中的输出变量y(k)，取历史时刻的仿真响应

为参数估计模型中的输入变量

取响应比Θ_ji为待估计参数θ，在t_K时刻的迭代结果θ(t_K)即为所要估计的当前时刻实测/仿真响应比

代入最小二乘法的递推公式，得到下述参数估计模型:

其中

的初值

设为1，R为参数估计模型的迭代中间参数，不具备实质定义，P的初值为一个足够大的正实数，如10⁷，获取后续每个时刻的仿真响应和实测响应，并依据参数估计模型实时更新对应的K、P 和θ。

优选地，所述参数估计模型的实现方法包括：

步骤401、设定迭代参数初值

和P(t₁)；

步骤402、依据不断获取实时更新后的当前时刻t_K的主星姿态角速度实测值

以及姿态角实测值

以得到实时更新后所有历史数据；

步骤403、根据所述的参数估计模型，不断实时更新当前时刻实测/ 仿真响应比

步骤404、将

和

作为运动学仿真的初始状态，获取未来机械臂运动指令

并认为机械臂在良好受控条件下，其运动状态

与运动指令

相同，计算未来t∈(t_K,t_K+T]时间内的仿真响应

在仿真过程中，将求得的每个时刻的仿真响应

立刻修正为预测响应

并用于后续的积分计算；

步骤405、当前时刻递增为t_K+1，转步骤402步，循环更新。

优选地，滚动修正目标仿真模型的加速计算方法：

设定预报时刻记为t_K+T；则前向预报时间段记为[t_K,t_K+T-Δt]；其中t_K+T-Δt表示前向预报时间段的某一具体时刻，其中Δt为一个变量；

以当前时刻的实测/仿真响应比直接修正前向预报时间段主星姿态角速度的仿真响应，并得到所述前向预报时间段内主星姿态角速度全部的预测响应；

以当前时刻主星姿态角速度作为初始状态，叠加所述前向预报时间段内每一时刻的预测响应，从而得到所述前向预报时间段全部的主星姿态角预测响应；

在机械臂在良好受控条件下，设定其运动状态和运动指令相同，累加所述前向预报时间段全部的主星姿态角预测响应，并获取未来机械臂运动指令，基于所述目标仿真模型计算所述前向预报时间段主星角速度的仿真响应，从而获得预报时刻的仿真响应；

通过预报时刻主星角速度的仿真响应，将其通过点乘修正为该时刻的预测响应。

还包括步骤600基座姿态高精度预报评价：

记基于主星姿态补偿模型获取主星姿态角的预测值为原始预测值；基于主星姿态补偿模型不断滚动修正所述目标仿真模型的等效线性参数，进而预报未来时刻的预测响应为修正预测值；

比较并评价所述修正预测值相较原始预测值的预测精度；

定义主星姿态角预测值在某一时刻t的相对误差为：该时刻t主星姿态角预测值的绝对误差，与历史T时间段内主星姿态角实测值的积分之比，其中T为预报时长，绝对误差为某一时刻t预测值与实测值的差值；

定义所述相对误差为从滚动修正开始稳定生效时起至测试结束，计算每时刻相对误差的误差均值；根据误差均值评价所述修正预测值的预测精度，进而更准确的预报未来时刻的主星姿态。

优选地，还包括：

令所述相对误差：

其中

为某一时刻t的原始预测值；

为某一时刻t的实测值；

修正预测值；

上式在离散情况下化为如下形式：

其中t_K为所预测的时刻，Δt为数据采样周期，也即系统获取实测值的周期；

进一步获得所述误差均值：

其中

为当次实验的终止时刻，定义从系统起始时刻，2～3倍的预报时长T之后稳定生效。

在本发明实施例的第二个方面，提供了一种星-臂耦合系统的主星姿态的解耦预报系统，包括：

建模单元：用于获取空间机器人的主星-机械臂耦合系统运动学方程；

前置模块：在进行主星姿态预报前，判断基座的惯性参数、未知目标的惯性参数、机械臂运动模型是否需要进行修正，并根据判定结果进行处理；

建模预报单元：建立用于获取主星姿态角速度的仿真响应的目标仿真模型，以及对所述仿真响应进行修正补偿的主星姿态补偿模型，进而不断滚动修正目标仿真模型来预报未来时刻的预测响应。

本发明的实施方式具有如下优点：

本发明通过考虑机械臂运动模型与预计的模型相差较大、或者基座参数变化巨大、又或者机械臂末端抓取了一个未知的对象时等三种情况下对主星姿态预报的影响，采用对多关节机械臂进行修正型预报以及惯性参数进行辨识的方法进行解决。

附图说明

为了更清楚地说明本发明的实施方式或现有技术中的技术方案，下面将对实施方式或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是示例性的，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图引伸获得其它的实施附图。

本说明书所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。

图1为本发明实施方式中方法流程图；

图2为本发明实施方式中星-臂耦合运动学建模对象示意图；

图3为本发明实施例2中不同的物理量之间的转换或计算过程示意图。

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供的主星姿态解耦预报方法及系统是基于现有的星-臂耦合系统来实现的，以下先定义后续建模和推导中涉及到的物理量及其符号。

如图2所示，具体为星-臂耦合运动学建模对象示意图。

其中，符号A代表特定坐标系或者坐标变换，具体包括：

坐标系A_I定义为惯性系，一般将其定义在星-臂耦合系统(统一简称为系统)的初始位置，并以与系统质心相同的初速度做惯性运动，出于保持符号统一与简便考虑，在一些推导过程中，同时令A_-1＝A_I，也表示惯性系。

A₀定义为主星随体系，其原点在主星质心上，坐标系三轴方向与主星自身设计坐标系一致即可，主星本体坐标系与惯性坐标系之间的变换依欧拉角定义。

A_i(i＝1,...,n)定义为多刚体系统中连杆i的随体系，其原点固定在关节i上前后两连杆铰接点处，坐标系z轴与关节i的旋转轴保持一致，x轴与y 轴参考机械臂D-H参数定义，

分别表示A_i的三轴单位矢量。

A_i-1,i(i＝1,...,n)定义为从坐标系A_i到坐标系A_i-1的坐标变换，其满足如下关系式：

其中ψ_i为由D-H参数规定的关节i的x轴扭转角，

同样为D-H参数规定的z轴初始旋转角，φ_i为z轴旋转角。

而主星与惯性系之间的坐标转换矩阵A_I0则满足：

其中α,β,γ分别为主星姿态三轴Euler角。

对于i＜j，坐标转换矩阵显然满足A_ij＝A_i,i+1A_i+1,i+2...A_j-1,j；对于i＝-1,...,n则有A_ii＝1；

系统中的结构参数定义如下：

l_i＝a_i+b_i(i＝1,...,n)定义为由A_i原点指向A_i+1原点的矢量；其中a_i为随体系A_i原点指向连杆i质心的矢量，b_i为由连杆i质心指向随体系A_i+1原点的矢量。这三者都定义在坐标系A_i中。

由于整个空间机器人的系统结构参数是在发射之前就设计好的，因此在一次任务中，l_i,a_i,b_i都是固定不变的常量。

出于符号统一性考虑，定义a₀＝0为由主星随体系A₀原点指向主星质心的矢量，l₀＝b₀为由主星质心(也即A₁原点)指向A₁原点的矢量。

定义在具体的坐标系中的各物理参数符号如下：

m_i(i＝0,...,n)：连杆i(或主星)的质量。

w：系统总质量。

r_i(i＝0,...,n)：连杆i(或主星)的质心位置，定义在惯性系A_-1中。

r_G，系统质心位置矢量。

I_i(i＝0,...,n)：连杆i(或主星)关于自身质心的转动惯量，定义在自身随体系A_i中。

R_i(i＝0,...,n)：坐标系A_i原点的位置，定义在A_-1中。也即，A_i原点的矢径在惯性系中的分量组成的向量。R_i满足：

其中R₀即为主星体坐标系原点在惯性系中的位置。

v_i(i＝0,...,n)：连杆i(或主星)质心速度，定义在惯性系A_I中。

V_i(i＝0,...,n)：坐标系A_i原点的线速度，定义在惯性系A_I中。也即，A_i原点的绝对速度矢量在惯性系中的分量组成的向量。V_i满足：

φ_i，关节i的旋转角。

关节i的初始D-H角参数。

连杆i(或主星)相对于连杆i-1(或主星/惯性系)的角速度，定义在A_i中。也即，A_i相对于A_i-1的角速度矢量在A_i体坐标系下的分量组成的向量。

Ω_i(i＝0,...,n)：连杆i(或主星)的角速度，其定义在A_i中,也即，坐标系A_i的绝对角速度矢量在其自身体坐标系下的分量组成的向量。显然有：

以及：

ω_i(i＝0,...,n)：连杆i(或主星)的角速度，定义在惯性系A_I中。

在惯性系中，不受外力作用条件下，包括主星、机械臂以及可能的被抓取物体，整个系统线动量守恒，如式(7)所示：

同样满足角动量守恒，如式(8)所示：

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

r_i-r_i-1＝A_Iia_i+A_I,i-1b_i-1 (9)

机械臂连杆末端手抓满足的几何方程如式(10)所示：

将式(9)展开至r₀，得到(11)式：

而整个系统质心位置如式(12)所示：

将(11)代入(12)，容易得到：

其中

为整个系统的总质量。

将(14)代回式(11)，得：

将式(15)简化，为两个不同的区间定义了形式上统一的K_il，令其满足：

需要注意的是，在一次任务过程中，卫星的设计尺寸、质量特性，一般都不会发生变化，即是说，a_i,b_i,K_il都是与时间无关的常量。因此，将 (15)式对时间求导，可以得到：

其中坐标转换矩阵A_ji满足如下关系式：

其中E为单轴旋转矩阵，满足：

将(18) 式中的坐标转换矩阵对时间求导，有：

其中

为机械臂各连杆间转换矩阵对时间的一阶导形式，其满足如下形式：

其中D_z定义为针对z轴的微分矩阵。

而与机械臂各连杆的转换矩阵稍有不同的是，从惯性系到主星基座的转换矩阵的导数

为如下形式：

将以上(18)至(22)诸式代入(17)式中，得到：

其中：

为惯性系中系统质心线速度，在线动量守恒的条件下，容易通过选取合理的初始坐标系，令

μ_ij为机械臂各关节末端的广义速度，当计算指定的机械臂连杆末端线速度时，它代表当某个关节以单位角速度旋转时所给出的线速度贡献。其具体表达式如(24)式所示：

式(23)便是系统中各个连杆刚体质心的线速度在惯性系中的表达式。

另外，考虑式(8)中的角速度表达式，其中每个刚体的角速度ω_i满足下式：

其中

表示j坐标系中的z轴单位矢量，ω_G表示惯性系中系统的初始角速度，在本推导中一般可以认为ω_G＝0。

式(25)便是机器人系统中各个连杆刚体的角速度在惯性系中的表达。

将角动量守恒式(8)中的左边第一项展开成为惯性坐标系中的表达，并将(23)中的线速度和(25)中的角速度代入，得到如下形式：

同理，式(8)的第二项展开得到：

将(26)和(27)代入(8)式，将其中涉及的主星姿态角速度

和机械臂关节角速度

提取出来，写成矩阵形式，即可以得到如下的空间机器人主星-机械臂耦合系统运动学方程。

实施例1：

本发明基于上述星-臂耦合系统的参数定义和公式推导，如图1所示，具体提供一种星-臂耦合系统的主星姿态解耦预报方法，其主要在主星姿态(实际中可相当于主星的自由基座的姿态)进行预报前加入若干前置步骤，在机械臂运动模型与预计模型相差较大的情况对多关节机械臂进行修正型预报，从而使得主星姿态的预报更为精准，其具体步骤如下：

步骤100、依据所述星-臂耦合系统获取空间机器人主星-机械臂耦合系统的运动学方程；

根据参数定义和公式推导得到的主星-机械臂耦合系统运动学方程为：

其中：

为主星的广义雅克比矩阵，包含了主星姿态角以及主星姿态角速度两个信息，满足：

为多关节机械臂的广义雅克比矩阵，包含了机械臂关节角以及机械臂关节角速度两个信息，满足：

其中

为主星姿态角速度，

为机械臂关节角速度，L₀为系统初始角动量。

步骤200、如若基座的惯性参数与真实值误差很大，则在已有的机械臂实测运动状态和基座运动状态的数据库条件下，对基座的惯性参数进行辨识，并将所述基座的惯性参数赋予在所述运动学方程中，否则跳过该步骤；

步骤300、如若机械臂存在抓取未知目标的情况，则对所述未知目标的惯性参数进行辨识，并将辨识后的结果，作为机械臂的下一个不能运动的节点加入到所述运动学方程中，否则跳过该步骤，

该步骤中的辨识方法具体为现有中一种星-臂耦合系统的动力学参数在轨辨识方法。

步骤400如若机械臂运动模型与预计模型相差较大，不能直接获取未来机械臂运动指令，则对多关节机械臂进行修正型预报，在获得未来一段时间的指令的同时，获得带滚动修正的机械臂运动预报状态，并将该机械臂预报状态认为是机械臂实际运动状，进入对基座姿态的预测步骤，否则跳过该步骤。

该步骤中对多关节机械臂进行修正型预报的方法具体为现有的一种不确定时延条件下机械臂运动状态的预测方法。

步骤500、建立用于获取主星姿态角速度的仿真响应的目标仿真模型，以及对所述仿真响应进行修正补偿的主星姿态补偿模型，进而不断滚动修正目标仿真模型来预报未来时刻的预测响应。

通过考虑机械臂运动模型与预计的模型相差较大、或者基座参数变化巨大、又或者机械臂末端抓取了一个未知的对象时等三种情况下对主星姿态预报的影响，采用对多关节机械臂进行修正型预报以及惯性参数进行辨识的方法进行解决。

在本方法中，由于实际中一般最需要获取的为当前主星的相关参数 (本实施方式均以自由基座角速度为例)，因此，根据历史数据就能够不断滚动更新当前时刻的预测响应，而对于未来时刻的预测响应一般等同于当前时刻预测响应的时间函数，该时间函数可以通过历史数据获得，并且时间函数的参数也可以根据历史数据的积累不断滚动更新。

在本实施方式中，所需要预报的未来时刻定义为包括当前时刻在内的未来时刻。

进一步地，目标仿真模型的建立方法包括：

获得主星姿态角速度与机械臂关节角速度的等效线性关系式，以机械臂关节角速度为系统激励，获取主星姿态角速度的仿真响应，即为目标仿真模型，所述主星姿态角速度与机械臂关节角速度的等效线性关系的获取方法为：

选择惯性系，并令系统的初始角动量为0，得到：

由此可以得到：

其中，等式右侧的

为机械臂关节角速度，是由空间机器人控制系统给出的输入指令，也即系统的激励，在本实施方式中相当于用于获取目标参数的重要现有已知参量，

是一个与机械臂当前构型相关的状态矩阵，其本质是一个3×n的矩阵，因此，可以定义

得到主星姿态角速度与机械臂关节角速度等效线性关系式：

结合式(29)和(30)，可以得到

的表达式中只有与机械臂当前构型相关的变量，而不包含这些量对时间的导数；换而言之，在任意特定时刻，机械臂具有特定的构型，系统响应或仿真响应(主星的姿态角速度

)与系统激励(机械臂关节角速度

)之间为等效线性关系。

因此，在确定的构型和其他条件下，给定瞬时的机械臂关节角速度作为系统激励，可以得到瞬时的主星姿态角速度的系统响应或仿真响应。

在实际的星-臂耦合系统中，机械臂末端执行机构(夹具、手爪等抓取机构)已经抓牢了某一刚性目标物体的情况下，可以将该目标物体视为机械臂的一个新的连杆。不失一般性，不妨设该假想连杆的随体坐标系朝向与机械臂原本的末端连杆(抓取机构)始终保持相同。于是可以通过将被抓取的目标物体视为一个额外新增的机械臂连杆，即将系统中的连杆数n简单加一。

主星姿态补偿模型的建立方法包括：

获取所有历史时刻的主星姿态角速度的仿真响应和实测响应，并通过对所有历史数据的实测/仿真响应比加权求和获取不断更新的当前时刻的实测/仿真响应比：

设星-臂耦合系统从t₀时刻开始运转，当前时刻为t_K，获取过去t₀～t_K时间段内每一时刻主星姿态角速度的仿真响应

和实测响应

并求得任意一历史时刻对应的实测/仿真响应比

对t_K时刻之前[t₀,t_K]区间内所有历史数据进行加权求和得到当前时刻实测/仿真响应比

即令当前时刻实测/仿真响应比：

其中，w(t)为加权权值，令

在离散情况下，

化为如下形式：

其中，t_K＝t₀+KΔt，而Δt＝t_k+1-t_k,k＝0,1,2...为离散时间步长，同样，令权值w(t_k)满足

获取预测响应

其中

为用于修正未来某一时刻t∈(t_K,t_K+T]仿真响应的预测/仿真响应比，直接取作当前时刻实测/仿真响应比

在未来某一时刻的响应比Θ_ji；既是所预测的时刻t的函数(响应比是时间的缓变函数)，又是当前时刻t_K的函数；显然，预测是在当前时刻t_K做出的，对某一特定时刻t做出的预测将随着当前时刻向前流逝而不断地滚动更新。同理，随着历史实测数据的不断获取，仿真值和预测值也在滚动更新。因此实际上，所有未来值都是当前时刻t_K的函数，可以分别写作

和

但在当前时刻t_K，将其略去。

在本实施方式中，主星姿态补偿模型中当前时刻的实测/仿真响应比

可以基于递推最小二乘法实时滚动修正，具体方法包括：

取预测响应

为参数估计模型中的输出变量y(k)，取历史时刻的仿真响应

为参数估计模型中的输入变量

取响应比Θ_ji为待估计参数θ，在t_K时刻的迭代结果θ(t_K)即为所要估计的当前时刻实测/仿真响应比

代入最小二乘法的递推式，得到下述参数估计模型:

其中

的初值

设为1，R为参数估计模型的迭代中间参数，不具备实质定义，P的初值为一个足够大的正实数，如10⁷，获取后续每个时刻的仿真响应和实测响应，并依据参数估计模型实时更新对应的K、P 和θ。

参数估计模型的实现方法包括：

步骤401、设定迭代参数初值

和

步骤402、依据不断获取实时更新后的当前时刻t_K的主星姿态角速度实测值

以及姿态角实测值

以得到实时更新后所有历史数据；

步骤403、根据所述的参数估计模型，不断实时更新当前时刻实测/ 仿真响应比

步骤404、将

和

作为运动学仿真的初始状态，获取未来机械臂运动指令

并认为机械臂在良好受控条件下，其运动状态

与运动指令

相同，计算未来t∈(t_K,t_K+T]时间内的仿真响应

在仿真过程中，将求得的每个时刻的仿真响应

立刻修正为预测响应

并用于后续的积分计算；

步骤405、当前时刻递增为t_K+1，转步骤402步，循环更新。

实施例2：

在实施例1的基础上，还包括滚动修正目标仿真模型的加速计算方法：

设定预报时刻记为t_K+T；则前向预报时间段记为[t_K,t_K+T-Δt]；其中t_K+T-Δt表示前向预报时间段的某一具体时刻，其中Δt为一个变量；

基于主星姿态补偿模型不断更新获取当前时刻的实测/仿真响应比

以当前时刻的实测/仿真响应比

直接修正前向预报时间段[t_K, t_K+T-Δt]内主星姿态角速度的仿真响应

并得到所述前向预报时间段内主星姿态角速度全部的预测响应

其中t_K+T-Δt表示前向预报时间段的某一具体时刻，其中Δt为一个变量；

以当前时刻主星姿态角速度

作为初始状态，叠加[t_K,t_K+T-Δt] 时间段内每一时刻的预测响应，从而得到所述前向预报时间段全部的主星姿态角预测响应

累加所述前向预报时间段全部的主星姿态角预测响应求得

并获取未来机械臂运动指令

并认为机械臂在良好受控条件下，其运动状态

与运动指令

相同，更新的主星姿态角速度和姿态角并没有改变后续时刻的姿态角速度响应；基于所述目标仿真模型计算所述前向预报时间段主星角速度的仿真响应

从而获得预报时刻t_K+T的仿真响应

通过预报时刻t_K+T主星角速度的仿真响应

进行点乘将其修正为该时刻的预测响应

随当前时刻的向前推移，循环前序所有步骤。

该方法只要把原来计算好的姿态角速度叠加到新的初值中去，就能得到随后各个时刻的新的预测值；因此，只需要类比仿真流程的数值积分进度，在每个时刻积分出下一时刻的姿态角速度仿真值即可，而不必每次都对过去时段内的姿态角速度响应重新积分。

如图3所示，表示不同的物理量之间的转换或计算过程，物理量包括机械臂关节角速度、关节角、主星姿态角、响应比参数以及主星姿态角速度的实测、仿真和预测值等；从上到下依次经历了t_K-1，t_K，t_K～t_K+T - Δ t 以及t_K+T等不同时段。

在运动学累加模块中，将过去T时间内的姿态角速度预测值直接累加到新的姿态角实测值上，从而得到一组新的姿态角预测值。而在此后的运动学数值积分中，则只用到最新的t_K+T时刻的状态和激励数据来进行数值积分。可极大提高了整个修正模块的计算速度，使得其中运动学模块数值积分的计算量与正常的仿真速度相当，而将其余计算量都转化为了简单的代数运算。

显而易见，这一对系统进行前向预报的方法，也可以很自然的同样用于具有通讯时延情况下的系统响应估计。只要将T重新解释为系统时延，t_K+T为系统运行的当前时刻，t_K则为当前时刻所获取到的下行实测数据所生成的那一时刻，则所有的迭代方法和流程都能完美地平移到时延问题中来。

实施例3：

在实施例1的基础上，还包括基座姿态高精度预报评价方法：具体步骤如下：

记基于主星姿态补偿模型获取主星姿态角的预测值为原始预测值；基于主星姿态补偿模型不断滚动修正所述目标仿真模型的等效线性参数，进而预报未来时刻的预测响应为修正预测值；

比较并评价所述修正预测值相较原始预测值的预测精度；

定义主星姿态角预测值在某一时刻t的相对误差为：该时刻t主星姿态角预测值的绝对误差，与历史T时间段内主星姿态角实测值的积分之比，其中T为预报时长，绝对误差为某一时刻t预测值与实测值的差值；

定义所述相对误差为从滚动修正开始稳定生效时起至测试结束，计算每时刻相对误差的误差均值；根据误差均值评价所述修正预测值的预测精度，进而更准确的预报未来时刻的主星姿态。

考虑刚体动力学中用于表征坐标系旋转的欧拉轴/角(或称轴角)表述：任何坐标变换矩阵可以写作Φe的形式，其中

为两个坐标系之间的相对转轴，称作欧拉轴，

和

为参考坐标系的三轴单位矢量；而Φ称作轴角。该变换表示，所有坐标之间的转换关系可以通过绕着欧拉轴e，一次性转动Φ角度而完成。

通过轴/角表述，我们可以用单一标量Φ来表征两个坐标系之间的整体偏差程度。若一个坐标系相对于另一个坐标系的三个对应坐标轴的偏角分别为α，β和γ，则轴角Φ满足：

当偏角较小时，该式化为：

该式实际上相当于将三轴偏角合成为一个，从而给出了主星姿态角的整体绝对误差水平，相当于：

在此基础上，引入姿态角相对误差的概念。即令：

相对误差：

其中

为某一时刻t的原始预测值；

为某一时刻t的实测值；

修正预测值；

上式在离散情况下化为如下形式：

其中t_K为所预测的时刻，Δt为数据采样周期，也即系统获取实测值的周期；在实际计算中，可以随意取该式中两个等号右边的两种形式；在不考虑采样误差的情况下，这两者应当是等价的进一步获得所述误差均值：

其中

为当次实验的终止时刻，定义从系统起始时刻，2～3倍的预报时长T之后稳定生效。

通过该评价方法，能够更好的比较修正预测值对主星姿态角预报的准确性。

实施例4：

本发明还提供了提供了一种星-臂耦合系统的主星姿态的解耦预报系统，包括：

建模单元：用于获取空间机器人的主星-机械臂耦合系统运动学方程；

前置模块：在进行主星姿态预报前，判断基座的惯性参数、未知目标的惯性参数、机械臂运动模型是否需要进行修正，并根据判定结果进行处理；

建模预报单元：建立用于获取主星姿态角速度的仿真响应的目标仿真模型，以及对所述仿真响应进行修正补偿的主星姿态补偿模型，进而不断滚动修正目标仿真模型来预报未来时刻的预测响应。

通过前置模块处理机械臂运动模型与预计的模型相差较大、或者基座参数变化巨大、又或者机械臂末端抓取了一个未知的对象时等三种情况下对主星姿态预报的影响。

虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (6)

1.一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，包括如下步骤：

步骤100、依据所述星-臂耦合系统获取空间机器人主星-机械臂耦合系统的运动学方程；

步骤200、如若基座的惯性参数与真实值误差很大，则在已有的机械臂实测运动状态和基座运动状态的数据库条件下，对基座的惯性参数进行辨识，并将辨识后的基座的惯性参数赋予在所述运动学方程中，否则跳过该步骤；

步骤300、如若机械臂存在抓取未知目标的情况，则对所述未知目标的惯性参数进行辨识，并将辨识后的结果，作为机械臂的下一个不能运动的节点加入到所述运动学方程中，否则跳过该步骤；

步骤400如若机械臂运动模型与预计模型相差较大，不能直接获取未来机械臂运动指令，则对多关节机械臂进行修正型预报，在获得未来一段时间的指令的同时，获得带滚动修正的机械臂运动预报状态，并将该机械臂运动预报状态认为是机械臂实际运动状态，进入对基座姿态的预测步骤，否则跳过该步骤；

步骤500、建立用于获取主星姿态角速度的仿真响应的目标仿真模型，以及对所述仿真响应进行修正补偿的主星姿态补偿模型，进而不断滚动修正目标仿真模型来预报未来时刻的预测响应；

所述目标仿真模型的建立方法包括：

获得主星姿态角速度与机械臂关节角速度的等效线性关系式，以机械臂关节角速度为系统激励，获取主星姿态角速度的仿真响应，即为目标仿真模型，所述主星姿态角速度与机械臂关节角速度的等效线性关系的获取方法为：

选择惯性系，并令系统的初始角动量为0，得到：

其中，

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

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

由此可以得到：

定义

得到在某个确定的机械臂构型下主星姿态角速度与机械臂关节角速度等效线性关系式：

所述主星姿态补偿模型的建立方法包括：

获取所有历史时刻的主星姿态角速度的仿真响应和实测响应，并通过对所有历史数据的实测/仿真响应比加权求和获取不断更新的当前时刻的实测/仿真响应比；

设星-臂耦合系统从t₀时刻开始运转，当前时刻为t_K，获取过去t₀～t_K时间段内每一时刻主星姿态角速度的仿真响应

和实测响应

并求得任意一历史时刻对应的实测/仿真响应比

对t_K时刻之前[t₀,t_K]区间内所有历史数据进行加权求和得到当前时刻实测/仿真响应比

即令当前时刻实测/仿真响应比：

其中，w(t)为加权权值，令

在离散情况下，

化为如下形式：

其中，t_K＝t₀+K△t，而△t＝t_k+1-t_k,k＝0,1,2...为离散时间步长，同样，令权值w(t_k)满足

获取预测响应

其中

为用于修正未来某一时刻t∈(t_K,t_K+T]仿真响应的预测/仿真响应比，直接取作当前时刻实测/仿真响应比

2.根据权利要求1所述的一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，所述主星姿态补偿模型中当前时刻的实测/仿真响应比

基于递推最小二乘法实时滚动修正，具体方法包括：

取预测响应

为参数估计模型中的输出变量y(k)，取历史时刻的仿真响应

为参数估计模型中的输入变量

取响应比Θ_ji为待估计参数θ，在t_K时刻的迭代结果θ(t_K)即为所要估计的当前时刻实测/仿真响应比

代入最小二乘法的递推公式，得到下述参数估计模型:

其中

的初值

设为1，R为参数估计模型的迭代中间参数，不具备实质定义，P的初值为一个足够大的正实数，获取后续每个时刻的仿真响应和实测响应，并依据参数估计模型实时更新对应的K、P和θ。

3.根据权利要求2所述的一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，所述参数估计模型的实现方法包括：

步骤401、设定迭代参数初值

和P(t₁)；

步骤402、依据不断获取实时更新后的当前时刻t_K的主星姿态角速度实测值

以及姿态角实测值

以得到实时更新后所有历史数据；

步骤403、根据所述的参数估计模型，不断实时更新当前时刻实测/仿真响应比

步骤404、将

和

作为运动学仿真的初始状态，获取未来机械臂运动指令

并认为机械臂在良好受控条件下，其运动状态

与运动指令

相同，计算未来t∈(t_K,t_K+T]时间内的仿真响应

在仿真过程中，将求得的每个时刻的仿真响应

立刻修正为预测响应

并用于后续的积分计算；

步骤405、当前时刻递增为t_K+1，转步骤402步，循环更新。

4.根据权利要求1或3所述的一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，滚动修正目标仿真模型的加速计算方法：

设定预报时刻记为t_K+T；则前向预报时间段记为[t_K,t_K+T-△t]；

其中，t_K+T-△t表示前向预报时间段的某一具体时刻，其中△t为一个变量；

以当前时刻的实测/仿真响应比直接修正前向预报时间段主星姿态角速度的仿真响应，并得到所述前向预报时间段内主星姿态角速度全部的预测响应；

以当前时刻主星姿态角速度作为初始状态，叠加所述前向预报时间段内每一时刻的预测响应，从而得到所述前向预报时间段全部的主星姿态角预测响应；

在机械臂在良好受控条件下，设定其运动状态和运动指令相同，累加所述前向预报时间段全部的主星姿态角预测响应，并获取未来机械臂运动指令，基于所述目标仿真模型计算所述前向预报时间段主星角速度的仿真响应，从而获得预报时刻的仿真响应；

通过预报时刻主星角速度的仿真响应，将其通过点乘修正为该时刻的预测响应。

5.根据权利要求1所述的一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，还包括步骤600基座姿态高精度预报评价：

记基于主星姿态补偿模型获取主星姿态角的预测值为原始预测值；基于主星姿态补偿模型不断滚动修正所述目标仿真模型的等效线性参数，进而预报未来时刻的预测响应为修正预测值；

比较并评价所述修正预测值相较原始预测值的预测精度；

定义主星姿态角预测值在某一时刻t的相对误差为：该时刻t主星姿态角预测值的绝对误差，与历史T时间段内主星姿态角实测值的积分之比，其中T为预报时长，绝对误差为某一时刻t预测值与实测值的差值；

定义所述相对误差为从滚动修正开始稳定生效时起至测试结束，计算每时刻相对误差的误差均值；根据误差均值评价所述修正预测值的预测精度，进而更准确的预报未来时刻的主星姿态。

6.根据权利要求5所述的一种星-臂耦合系统的主星姿态解耦预报方法，其特征在于，还包括：

令所述相对误差：

其中

为某一时刻t的原始预测值；

为某一时刻t的实测值；

修正预测值；

上式在离散情况下化为如下形式：

其中t_K为所预测的时刻，△t为数据采样周期，也即系统获取实测值的周期；

进一步获得所述误差均值：

其中

为当次实验的终止时刻，定义从系统起始时刻，2～3倍的预报时长K＝2T/D之后稳定生效。

Images