CN111174795A - 一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法 - Google Patents

Abstract

本发明涉及一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，包括以下步骤：步骤一、建立自由漂浮目标动力学模型；步骤二、设计混合卡尔曼滤波器，利用混合卡尔曼滤波对运动状态及动力学模型参数进行估计；步骤三、利用所估计的运动参数对漂浮目标未来一段时间轨迹进行预测。目标的姿态参数用轴角来表示，记为，位置分别为三个轴独立的运动，利用所估计运动信息对未来一段时间内的位置和姿态进行预测，得到未来某一时刻的位姿。本发明滚动实时对自由漂浮目标轨迹进行长周期预测，准确性好，从而为后续机械臂的抓捕奠定基础。

Description

一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法

技术领域

本发明属于自由漂浮目标位姿预测领域，具体涉及一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法。

背景技术

在未来的空间活动中空间机器人将扮演越来越重要的角色，其在清理空间碎片、在轨维修、空间建设和稳定卫星轨道和其他大型空间结构问题上具有广阔的应用前景。空间机器人的重要任务包括抓捕轨道上的卫星或其他类似物体，以便回收并修复失效的卫星，营救失去控制的载人航天器，或者移除较大的空间碎片。其中大部分无法测量自身的运动与姿态信息，变成了非合作目标，可能会以较大的角速度做翻滚运动。目前的方法大多是利用相机测量非合作目标的位姿，对合作目标动力学动力学参数模型进行辨识，利用辨识的动力学模型进行位姿的预测。

文献^[1]基于欧拉情形解析解提取了描述自由漂浮刚体(目标)姿态运动的一组动力学参数和描述姿态运动的动量矩坐标系的参数，将这些参数作为预测目标刚体姿态运动的参数，然后利用目标刚体姿态运动的观测结果按一系列步骤依次求出各个参数，达到预测姿态运动的目的。文献^[2]利用优化的方法求解最佳的动力学参数(刚体的惯量矩阵和初始条件)，使得动力学参数对应的姿态运动与观测结果吻合最好，然后利用动力学参数预测刚体的姿态运动。

上述方法存在一些问题，第一个是直接利用观测数据求解各个参数，不能充分利用历史观测数据，导致结果误差较大，第二个是利用优化方法求解需要直接求解高维的优化问题，计算量较大。

针对以上的缺点，需要设计一种新的非合作目标位姿预测方法，这种方法不需要复杂的优化求解，直接根据历史数据估计出目标的运动状态，惯性参数，根据运动估计信息进行预测。

发明内容

本发明的目的在于在现有方法基础上，在目标惯性参数未知的情况下，提供一种新的同时估计目标运动状态和惯性参数的方法，适用范围广，精度高，通过混合卡尔曼滤波进行运动状态及动力学模型参数估计，利用所估计参数进行长期预测。

为了实现上述目的，本发明采用的技术方案包括以下步骤：

步骤1、建立自由漂浮目标动力学模型；

步骤2、设计混合卡尔曼滤波器，利用混合卡尔曼滤波对运动状态及动力学模型参数进行估计；

步骤3、利用所估计的运动参数对漂浮目标未来一段时间轨迹进行预测，其中，定义位置为三个方向独立的运动，用p＝[x,y,z]表示，目标的姿态参数用轴角来表示，记为φ＝(e,θ)，对未来一段时间内的位置和姿态进行预测，得到未来某一时刻的位姿。

在上述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，所述动力学模型按照以下步骤建立：

航天器相对于本体系得惯量矩阵为：

刚体的角动量为：

根据动量矩定理，外力矩可表示为：

目标姿态动力学的一般方程为：

由于自由漂浮目标绕惯量主轴旋转，I_xy＝I_xz＝I_yz＝0，τ＝[0,0,0]^T动力学模型简化为：

在上述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，基于混合卡尔曼滤波估计所述步骤1中所建模型的参数，估计方法按照以下步骤建立：

步骤1.1、选取状态观测量为

X＝[q^T ω^T I^T]^T

其中q为姿态四元数，ω为角速度，I为惯性参数

步骤1.2、令

其中Ω(ω)为欧拉角与四元数的转换矩阵，且

步骤1.3、系统状态方程表示为：

此时，状态方程的雅克比矩阵可表示为：

其中，

步骤1.4、状态转移矩阵为：h(X)＝[q₁,q₂,q₃,q₄,ω_x,ω_y,ω_z]^T

状态方程为：

式中W_k代表系统过程噪声，它是均值为0，方差为Q_k的白噪声

把姿态四元数和瞬时角速度座位观测值，即有：

那么，测量方程可以表示为：

其中V代表系统测量噪声，它是均值为0，方差为的白噪声；

利用卡尔曼滤波估计出姿态，角速度，惯性参数，最终出自由漂浮目标的线速度与抓取点与转动R_k中心的距离。

在上述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，所述卡尔曼滤波为混合卡尔曼滤波，令阈值σ∈[0，1]，当k＝1，P₁ ^*＝δ·trace(P₀)；当k≥2,同时

认为当前估计的状态可信度不高，这时考虑采用UKF完成下一阶段参数估计的迭代任务，否则选用EKF完成下一阶段任务，如此，每迭代一个周期进行一次协方差判断；为了防止初始迭代误差过大，假设第一步估计采用UKF完成，后面任务通过当前时刻协方差与初始协方差的判断条件来交叉切换EKF与UKF。

在上述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，通过步骤3所估计的动力学参数模型，预测未来一段时间内的轨迹；计算公式如下：

p_t+1＝p_t+Δp

φ_t+1＝φ_t+Δφ

其中：

Δp＝vΔt+ωrcos(Δφ)

Δφ＝ωΔt。

与现有技术相比，本发明具有如下有益效果：混合卡尔曼滤波融合了EKF效率高，UKF精度与鲁棒性高的优势，针对现有非合作目标位姿预测中计算量大，无法长周期预测等缺点，本发明使用混合卡尔曼滤波器对非合作目标的运动信息与惯性参数进行估计，利用所估计的角速度，线速度进行预测，相对于目前基于动力学参数估计的方法，本发明计算速度快，精度高，能够实时进行估计，过程简单方便，能够有效进行在线实施。

附图说明

图1为EKF与UKF算法流程图。

图2为混合卡尔曼滤波流程图。

图3为非合作自由漂浮目标抓取示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明。

如图3所示抓取场景，目标处于漂浮状态不受任何外力，以一定的角速度和线速度匀速运动，抓取点的位姿能够通过相机实时反馈，通过相机反馈的信息利用混合卡尔曼滤波估计出整个漂浮目标的运动信息及惯性参数。

假设相机所获得的抓取点的位姿为：p＝[x,y,z]，φ(e,θ)

目标的欧拉动力学方程为：

选取状态观测量为

X＝[q^T ω^T I^T]^T

其中Ω(ω)为四元数的转换矩阵，且

系统状态方程可表示为：

此时，状态方程的雅克比矩阵可表示为：

其中，

状态转移矩阵为：

状态方程为：

式中W_k代表系统过程噪声，它是均值为0，方差为Q_k的白噪声

把姿态四元数和瞬时角速度座位观测值，即有：

h(X)＝[q₁,q₂,q₃,q₄,ω_x,ω_y,ω_z]^T

那么，测量方程可以表示为：

其中V代表系统测量噪声，它是均值为0，方差为R_k的白噪声。

利用卡尔曼滤波可以估计出姿态，角速度，惯性参数

根据所估计运动信息，可以估计出自由漂浮目标的线速度与抓取点与转动中心的距离。

根据线速度，角速度，抓取点与转动中心的距离即可达到实现长期预测的目的。

p_t+1＝p_t+Δp

φ_t+1＝φ_t+Δφ

其中：

Δp＝vΔt+ωrcos(Δφ)

Δφ＝ωΔt

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (5)

1.一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，其特征在于，包括以下步骤：

步骤1、建立自由漂浮目标动力学模型；

步骤2、设计混合卡尔曼滤波器，利用混合卡尔曼滤波对运动状态及动力学模型参数进行估计；

步骤3、利用所估计的运动参数对漂浮目标未来一段时间轨迹进行预测，其中，定义位置为三个方向独立的运动，用p＝[x,y,z]表示，目标的姿态参数用轴角来表示，记为φ＝(e,θ)，对未来一段时间内的位置和姿态进行预测，得到未来某一时刻的位姿。

2.根据权利要求1所述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，其特征在于，所述动力学模型按照以下步骤建立：

航天器相对于本体系得惯量矩阵为：

刚体的角动量为：

根据动量矩定理，外力矩可表示为：

目标姿态动力学的一般方程为：

由于自由漂浮目标绕惯量主轴旋转，I_xy＝I_xz＝I_yz＝0，τ＝[0,0,0]^T动力学模型简化为：

3.根据权利要求2所述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，其特征在于，基于混合卡尔曼滤波估计所述步骤1中所建模型的参数，估计方法按照以下步骤建立：

步骤1.1、选取状态观测量为

X＝[q^T ω^T I^T]^T

其中q为姿态四元数，ω为角速度，I为惯性参数

步骤1.2、令

其中Ω(ω)为欧拉角与四元数的转换矩阵，且

步骤1.3、系统状态方程表示为：

此时，状态方程的雅克比矩阵可表示为：

其中，

步骤1.4、状态转移矩阵为：h(X)＝[q₁,q₂,q₃,q₄,ω_x,ω_y,ω_z]^T

状态方程为：

式中W_k代表系统过程噪声，它是均值为0，方差为Q_k的白噪声

把姿态四元数和瞬时角速度座位观测值，即有：

那么，测量方程可以表示为：

其中V代表系统测量噪声，它是均值为0，方差为的白噪声；

利用卡尔曼滤波估计出姿态，角速度，惯性参数，最终出自由漂浮目标的线速度与抓取点与转动R_k中心的距离。

4.根据权利要求3所述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，其特征在于，所述卡尔曼滤波为混合卡尔曼滤波，令阈值σ∈[0，1]，当k＝1，P₁ ^*＝δ·trace(P₀)；当k≥2,同时

认为当前估计的状态可信度不高，这时考虑采用UKF完成下一阶段参数估计的迭代任务，否则选用EKF完成下一阶段任务，如此，每迭代一个周期进行一次协方差判断；为了防止初始迭代误差过大，假设第一步估计采用UKF完成，后面任务通过当前时刻协方差与初始协方差的判断条件来交叉切换EKF与UKF。

5.根据权利要求3所述的一种基于混合卡尔曼滤波的自由漂浮目标位姿预测方法，其特征在于，通过步骤3所估计的动力学参数模型，预测未来一段时间内的轨迹；计算公式如下：

p_t+1＝p_t+Δp

φ_t+1＝φ_t+Δφ

其中：

Δp＝vΔt+ωrcos(Δφ)

Δφ＝ωΔt。

Images