CN107883965A - 基于光学信息交互多模型强跟踪容积卡尔曼滤波导航方法 - Google Patents

Abstract

一种基于光学测量信息的交互多模型的强跟踪容积卡尔曼滤波导航方法，建立相机的坐标系和小行星本体坐标系，建立航天器的光学测量模型，根据航天器的动力学方程，建立导航滤波模型，得到航天器的系统状态离散方程，建立交互多状态模型，对参数空间采用多模型描述，预测航天器的状态估计，完成航天器的导航滤波过程。本发明解决了因模型不准确和强非线性造成的滤波发散问题，降低位置估计误差、速度估计误差以及姿态角估计误差。该方法过程简单，适用于航天航空领域中的导航过程。

Description

基于光学信息交互多模型强跟踪容积卡尔曼滤波导航方法

技术领域

本发明属于航天航空导航控制领域，针对导航过程中模型不准确和非线性程度高以及模型中存在的未知干扰信号的滤波过程，提出一种基于交互多模型的强跟踪卡尔曼滤波的导航方法，可用于小行星航天器等的导航过程。

背景技术

导航是指将运动物体从初始地引导至目的地的技术，导航分为自主式导航和非自主式导航。自主导航系统是一种不与外界进行信息交换与传递，并且不依赖于地面设备的自主导航系统，是一种利用光学相机采集小行星光学信息并依靠自身实现自主导航。其基本原理是利用光学相机测得的光学信息结合航天器的轨道动力学方程和姿态动力学方程，利用最优估计的方法估计航天器的位置、速度等导航信息，即航天器轨道动力学和姿态动力学的建模问题、航天器量测量的选择问题、滤波估计方法的设计。

实现航天器的导航，需要建立航天器的动力学模型，完成对航天器的位置与速度的控制。考虑小行星的形状特点和引力场环境特点建立小行星的引力场模型，根据航天器的运动特点、受力情况以及航天器轨道坐标系的几何关系运用数学知识得到航天器的轨道动力学方程，为实现航天器的导航提供相应的动力学信息。

根据得到的轨道动力学模型、姿态动力学模型以及光学相机采集到的光学信息建立航天器的导航滤波模型，为导航滤波过程提供模型信息。在导航过程中，由于外界的噪声干扰存在有用信号中，使得到的估计信息存在误差，要使得到的估计信息准确，则需要降低有用信号中干扰噪声的影响，强跟踪容积卡尔曼滤波可以降低干扰信号对有用信号的干扰，同时可以很好的逼近非线性部分，改善系统的鲁棒性。

卡尔曼滤波是一种递归估计，对于卡尔曼滤波过程，只需要获得上一时刻状态变量的估计值和前状态的观测值就可以得到当前状态的估计值，在这个过程中不需要记录观测或者估计的历史信息。容积卡尔曼滤波是一种非线性高斯滤波方法，其具有等权值的n个容积点，对随机变量非线性变换后概率分布具有良好的逼近精度。由于容积卡尔曼滤波的复杂度最低故选择容积卡尔曼滤波逼近非线性部分。同时，由于导航系统模型和外界干扰存在不确定性，降低了容积卡尔曼滤波器的鲁棒性。由于强跟踪滤波有较强的关于模型不确定性的鲁棒性和极强的关于突变状态的跟踪能力，同时计算复杂性适中，故引入强跟踪滤波算法对容积卡尔曼滤波进行改善。

强跟踪容积卡尔曼滤波器将航天器的位置、速度、姿态角信息融合起来，并且通过预测与更新信息来消除噪声干扰的影响，从而使获得的信息更准确，提高系统的导航精度。交互多模型方法，可以在滤波的开始和结束处采用多状态模型对航天器的状态进行描述，并将各个模型得到的结果按照各自的概率进行加权求和转换，从而进一步精确系统的状态模型。

发明内容

本发明的主要目的是公开一种基于光学测量信息的交互多模型的强跟踪容积卡尔曼滤波导航方法，针对航天器模型的不确定性和模型中非线性程度高的问题以及未知信号对有用信号的干扰，采用多个状态模型对滤波过程的状态进行多状态模型优化，即用多个不同的状态模型，对同一目标的位置、速度信息进行强跟踪容积卡尔曼滤波。同时，在滤波的开始和结束处，将各个模型所得到的结果按照各自的转移概率进行加权求和与转换，精确系统的状态模型。通过优化现有的滤波算法，降低干扰噪声在有用信号中的干扰和模型不确定性以及非线性部分对于导航过程的影响，从而提高导航的精度。

本发明的基于光学测量信息的交互多模型的强跟踪容积卡尔曼滤波导航方法通过如下步骤实现：

(1)建立光学测量模型：采用光学导航相机，用坐标图方式对成像平面上有关特征路标的位置进行描述，从而得到航天器的特征点和特征路标的光学信息。

(2)数据采集：根据获得的光学信息获取当前状态观测值，即三轴方向的加速度、速度与位置信息。

(3)建立姿态运动学方程和姿态动力学方程：根据探测器的空间坐标系之间的几何关系以及四元素与姿态矩阵之间的表示关系，采用欧拉四元素式描述航天器的姿态矩阵，通过公式推导得到探测器的姿态四元素运动方程。根据动量矩定理以及在参考惯性坐标系下角动量与航天器转动惯量和角速度的关系，推导公式得到航天器的姿态动力学方程。

(4)建立导航滤波模型：根据得到的航天器动力学模型和姿态动力学方程，定义航天器的状态向量，结合航天器的动力学方程，通过公式推导得到航天器的状态方程。

(5)求解滤波离散方程:求取状态方程的转移矩阵，并对状态转移矩阵做近似处理，同时，对状态方程的噪声输入矩阵做离散化处理，并求取第i个特征点的量测方程的敏感测量矩阵，利用求得的矩阵得出滤波离散方程。

(6)建立输入交互多状态模型：对于r个交互模型，计算模型j的转移概率以及前一时刻的转移概率，模型i到模型j的预测概率以及模型间的混合概率，并对模型j的混合状态进行估计。同时对于交互模型中的r个模型采用马尔科夫链来控制模型之间的转换，其中模型之间的转换概率采用马尔科夫链的转移概率矩阵表示。

(7)对模型j强跟踪容积卡尔曼滤波：对于模型M_j(k)，以及Z(k)作为输入进行强跟踪容积卡尔曼，同时更新模型j的概率，为下一时刻的滤波过程提供信息。

(8)输出总的状态估计以及总的协方差估计P_k+1/k。

(9)进入下一时刻的滤波过程。

本发明的基本原理是：利用光学导航相机获得航天器的特征点和特征路标的光学信息，得到航天器在相机坐标系下的位置坐标(u_i,v_i)，并在相机坐标系下，应用数学几何变换关系求取特征路标的视线角，如图1导航矢量构建示意图，以及相机坐标系下特征点与特征路标所在空间位置矢量，并通过特征点的视线矢量得到在焦平面上三维空间坐标点投影的测量方程。通过动力学方程、姿态运动学方程以及姿态动力学方程推导得到航天器的状态方程，推导出实际模型的系统噪声和量测噪声统计特性，对状态方程经过离散化处理计算得出滤波过程中所用到的离散形式的系统方程。

由于航天器导航模型的不确定性和非线性部分程度高以及模型存在许多未知的干扰信号，因此在滤波开始和结束处采用多个不同的状态模型对同一目标轨迹进行强跟踪容积卡尔曼滤波，将各个状态模型所得到的结果按照各自概率进行加权求和与转换，其模型间的转换概率用马尔科夫转移矩阵表示。通过信息更新，进入下一时刻的滤波过程，提高了滤波器的鲁棒性以及导航的精度。

本发明的有效效果为：提出一种基于光学测量信息的交互多模型强跟踪容积卡尔曼滤波导航方法，该方法有效改善航天器模型的不确定性和模型中非线性程度高对滤波过程的影响，降低未知信号对有用信号的干扰，提高滤波精度和导航精度。

附图说明

图1航天器坐标系示意图

图2导航矢量构建示意图

图3方法过程流程图

具体实施方式

1、建立光学测量模型

在采集的图像信息中存在预定的特征点或是特征路标，在图像处理时对这些点进行识别和跟踪，能够获得其在相机系下所处位置的坐标值(u_i,v_i)。在相机系下，应用数学几何变换关系求取特征路标的视线角。其中，式中θ_x、θ_y分别为视线矢量与相机系两平面O_cx_cz_c与O_cy_cz_c夹角,f是相机焦距。

选取任意的特征点i，假设特征点i与航天器之间的距离定义为d_i，可获得相机坐标系下将特征点或特征路标所在空间位置矢量。

定义为第i个特征点视线矢量，可得在焦平面上三维空间坐标点投影的量测方程如下：

其中，m_i为测量噪声，将其建模成独立的零均值白噪声，l是为量级为1的常数。

2、建立姿态运动学模型和姿态动力学模型

由于环境因素的影响，元件测量转动角速度ω_n与真实且无噪声环境下的转动角速度ω间存在一定的偏差，两者之间的关系如下：

其中，表示星体引力加速度。a_n为元件测量非引力加速度，a为航天器在真实且无噪声环境下的引力加速度。n_g表示加速度计存在的零均值测量高斯白噪声，b_g表示加速度计的器件漂移，为零均值驱动高斯白噪声。n_a表示测角度变化的陀螺仪存在的零均值测量高斯白噪声，b_a表示陀螺仪存在的零均值驱动高斯白噪声。

采用欧拉四元素式对航天器的姿态进行描述，其中q由三维矢量和标量q₄构成，q₁、q₂、q₃其中为欧拉轴轴向，q₄欧拉转角。

将姿态矩阵用四元素描述，其中，为的斜对称矩阵。

通过对航天器的运动特点和运动行为分析，可以得到航天器的姿态四元素运动方程。

将上式简化可得姿态四元素运动方程。

其中，表示姿态四元素。

根据动量矩定理，结合转动动力学和姿态运动学，可以得到航天器的姿态动力学方程如下：

其中，H是航天器总角动量，M是航天器所受到的合力矩。

在参考坐标系下，航天器角速度为ω，则角动量H＝Iω。其中， I＝diag[I_x I_y I_z]为航天器的转动惯量。

将上式角动量H带入姿态动力学方程可得。

3、建立导航滤波模型

在着陆点坐标系下用四元素法表示航天器的运动过程。其中，和n分别是旋转的角度与旋转时所围绕的主轴。

定义航天器的状态向量其中，r表示航天器的位置，v表示航天器的速度，q表示探测器的姿态四元素，b_g为加速度计的器件漂移，为零均值驱动高斯白噪声。b_a为测量角度变化的陀螺仪的器件漂移，为零均值的高斯白噪声。

结合航天器动力学模型，获得四元素下航天器的动力学方程.

式中，表示航天器本体固连坐标系与相机坐标系的转换矩阵，l代表航天器本体固连坐标系。四元素表达式如下：

依据航天器的动力学方程可求得航天器的状态方程如下：

其中，N(t)为该系统的噪声输入矩阵，w(t)表示系统的高斯白噪声。其矩阵表达式如下：

求取状态方程的状态转移矩阵。

对状态转移矩阵F_k做近似处理。

4、求解滤波离散方程

对噪声输入矩阵做离散化处理。

求取第i个特征点测量方程的敏感测量矩阵。

利用以上计算结果可以得出在滤波过程中需用的离散形式的系统方程。

其中，w_k,v_k为高斯白噪声项。其统计特性如下：

5、输入交互模型

假设有r个运动模型，即r个状态转移方程

其中是均值为零，协方差矩阵为Q^(j)的白噪声序列。用马尔科夫链来控制这些模型之间的转换，马尔科夫链的转移概率为

测量模型如下式：

在k-1时刻模型j的预测概率为：

在k-1时刻模型j到模型ij的混合概率为：

在k-1时刻模型j的混合估计状态为：

在k-1时刻模型j的混合协方差估计为：

其中，模型i到模型j的转移概率为p^(ij)，模型j在k-1时刻的概率为μ_j(k-1)。模型i、j在k-1时刻的混合概率为模型i在k-1时刻的状态估计为

6、强跟踪容积卡尔曼滤波过程

由于导航系统模型和外界干扰存在不确定性，降低了容积卡尔曼滤波的鲁棒性，故采用强跟踪滤波算法改善容积卡尔曼滤波的鲁棒性，提高系统的鲁棒性。对于每个状态模型进行滤波，强跟踪容积卡尔曼滤波算法的具体实现步骤如下：

步骤一状态值初始化：假设初始状态均值为p_0|0为初始方差。用选取的满足要求的一组容积点计算高斯权重分布，并解算容积点集(ξ_i,ω_i)。

步骤二更新预测值：和p_k运算之后得容积点X_i,k，借助状态函数得到由预测状态及预测误差协方差如下：

由于模型中存在一些外部的影响因素使模型信息不准确，从而影响滤波器的鲁棒性，为了改善滤波器的鲁棒性。将λ_k+1代入到初始预测误差协方差得：

然后利用和按步骤一计算容积点其通过量测函数得到再由可得量测及自协方差和互协方差

已知和以更新后的λ_k+1为基础，用和P_k+1|k依照步骤一对容积点X_i,k+1|k重新计算，再由量测函数得到Z_i,k+1|k，再引入λ_k+1之后的自协与互协方差如下：

Z_i,k+1|k＝h(X_i,k+1|k)+r_k+1 (43)

步骤三更新量测值：

在得到新z_i,k+1|k后，对强跟踪容积卡尔曼滤波做如下更新

7、交互模型概率更新

模型j的似然函数：

滤波产生新息为

新息的协方差为

滤波器的总输出是多个滤波器模型j的概率为c为归一化常数，且

8、输出状态估计和协方差估计

总的状态估计为：

总的协方差估计如下：

9、步骤实现

利用步骤1光学相机采集光学信息，根据空间坐标的转换关系，利用步骤2 建立探测器的动力学方程和姿态动力学方程。根据航天器的动力学方程和姿态动力学方程，利用步骤3定义航天器的状态向量，建立导航滤波模型。根据得到的导航滤波状态方程，利用步骤4求解滤波的离散方程，利用步骤4得到的离散方程。建立步骤5中的交互多状态模型，对系统的状态模型进行优化。利用步骤6 进行卡尔曼滤波，并通过步骤7进行状态模型的概率更新，最后通过步骤8输出模型交互信息并更新信息，然后进入下一时刻滤波过程。

Claims (3)

1.一种基于光学信息的交互多模型强跟踪容积卡尔曼滤波组合导航方法，其特征在于包括下列几个步骤：

在航天器的运动过程中建立航天器的三维空间坐标系，其坐标系示意图如附图2所示。通过光学导航相机采用坐标图方式对成像平面上有关特征路标的位置进行描述。在采集的图像信息中存在预定的特征点或是特征路标，在图像处理时对这些点进行识别和跟踪，能够获得其在相机系下所处位置的坐标值(u_i,v_i)。在相机系下，应用数学几何变换关系求取特征路标的视线角θ_x、θ_y。利用特征路标求得的视线角能够表示出激光测距仪方向，选取任意的特征点i，可得到在相机坐标系下特征点或特征路标所在的空间矢量定义为第i个特征点视线矢量。可得在焦平面上三维空间坐标点投影的测量方程z_i。

(2)根据步骤(1)的量测方程建立以及航天器的动力学方程和姿态动力学方程，在着陆点坐标系下用四元素法表示航天器的运动过程，定义探测器的状态向量，并求解得到航天器的状态方程。

(3)通过求解航天器的敏感测量矩阵h_k，并对探测器的状态方程做离散化处理，得到航天器的滤波过程中系统离散状态方程x_k+1和z_k。

(4)建立交互多状态模型，得到各个模型的模型状态信息。

(5)对航天器状态信息进行强跟踪容积卡尔曼滤波，通过时间更新和量测更新获得相关估计值与增益K。

(6)多个模型概率更新，输出交互多模型状态估计。

2.根据权利要求1所述的一种基于光学信息的交互多模型强跟踪容积卡尔曼滤波导航方法，其特征在于：步骤(4)引入交互多状态模型，在强跟踪容积卡尔曼滤波的开始和结束处采用多个不同的状态模型(机动模型和不同参数的非机动模型)对同一目标轨迹进行强跟踪容积卡尔曼滤波。交互多模型的相关参数信息如下所示。

模型j的预测概率为

模型j到模型ij的混合概率为

模型j的混合估计状态为

模型j的混合协方差估计为

其中，模型i到模型j的转移概率为p^(ij)，模型j在k-1时刻的概率为表示k-1时刻模型i的协方差估计，表示k-1时刻模型i的状态估计。

3.根据权利要求1所述的基于光学信息的交互多模型强跟踪容积卡尔曼滤波方法，其特征在于：步骤(5)强跟踪容积卡尔曼滤波和步骤(4)系统交互多模型的建立、步骤(6)系统的交互模型的状态估计的输出。通过建立系统的交互多模型，利用交互多模型间的关系对滤波后的系统状态进行状态估计，得到当前时刻的系统状态估计值。系统状态的统计特性如下。

总的状态估计：

总的协方差估计为：

其中，表示k时刻模型j的估计协方差，

由得到的当前时刻的系统状态的估计值预测下一时刻的系统状态的估计值，直至完成整个滤波过程。