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

权利要求书

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。