一种新的姿态仪表优化估计算法

　　1　引　　言

　　姿态测量是舰船定向、钻井设备测斜、飞行器姿态控制等许多领域的一个重要问题。采用三轴重力加速度计和三轴磁强计为传感器的姿态仪表,以其固态化、捷联式、低成本、高自主性等特点,得到了广泛应用[1～2]。出于自主性测量的考虑,这类仪表中普遍采用基于几何关系直接计算的算法,即首先根据重力矢量观测值计算出倾斜角,然后结合地磁矢量观测值计算出航向角[1～2]。这种方法没有利用系统动态信息和历史观测数据进行优化处理,可能产生较大的误差,更不能同步提供姿态速率信息。为提高姿态仪表的测量精度,扩展其测量能力,在保证自主性前提下,研究了一种基于加权统计线性回归及卡尔曼滤波技术的优化估计新算法,具有现实意义。

　　2　系统动态模型

　　设载体姿态欧拉角为Θ=(Ψ　θ　)^T,其中Ψ为偏航(航向)角,θ为俯仰角,为横滚角。用Ω=Θ·和Ω·分别表示欧拉角的角速度和角加速度矢量。为避免使用不确定性的载体角动态模型,可以合理地假设Ω·的动态模型为一阶高斯-马尔科夫过程[3],即:

　　其中: 为对应轴指数相关时间常数,w为零均值的白噪声,其方差矩阵为可按照角加速度的概率模型确定。定义系统的状态向量为:  根据(1)式可写出系统状态方程,经离散化后的系统过程方程为:

　　其中:wk为零均值过程噪声向量,其相关矩阵Qk由Qw决定。

　　系统的直接测量值为地球重力场和地磁场在载体坐标系三轴上的分量。如将其直接选取为观测向量,则观测方程将具有严重的非线性,且需要确知参考坐标系中的重力矢量和地磁矢量。通常,重力矢量可以认为是已知的,而在要求完全自主测姿时,不能认为地磁矢量准确已知。为满足自主性要求,并将问题纳入线性卡尔曼滤波框架进行处理,选取观测向量y=Θ,则可写出观测方程为:

　　3　观测噪声随机模型

　　对方程(2)、(3)构成的系统,如能获得观测值和观测噪声向量的相关矩阵,就可采用标准的卡尔曼滤波算法进行处理。方程(3)中定义的观测向量需要由直接观测值通过(4)式表示的非线性变换得到:

　　其中: 和 分别为载体坐标系中的重力矢量和地磁场矢量,g为重力加速度。设为传感器直接观测向量z对应的真值,则有:

　　其中:n为零均值的白噪声向量,其相关矩阵Pz为由各传感器的测量精度决定的对角正定阵。

　　系统观测值可根据直接观测量由(4)式计算,而观测噪声相关矩阵的获取实际上是随机向量经非线性变换后其统计参量的传递问题。一般情况下,对它只能采用近似(常用一阶近似)的方法求解,如EKF中通过泰勒级数展开将非线性方程线性化,因未考虑随机变量的散布情况,加之其强非线性,这种近似可能引入大的误差。加权统计线性回归(WSLR)[4]提供了另一种解决方法。它根据随机变量先验分布选取一些确定性的点,利用真正的非线性函数计算各西格马点对应的值,然后利用加权线性回归技术获得随机变量经非线性变换后的统计参量。这一统计近似技术考虑了随机变量的先验统计特性,而且对采样点用非线性函数准确计算,因此与截断泰勒级数的方法相比,它可以获得更高的精度。西格马点个数、位置及相应加权值的选取应遵循捕获随机变量最重要统计特性的基本原则,即满足下列形式的条件: