基于状态矩阵卡尔曼滤波的姿态估计算法研究

王 冰，隋立芬，吴江飞，张 宇

（1.信息工程大学 地理空间信息学院，河南 郑州 450052；2.66240部队，北京 100042）

GPS接收机除用于导航定位、精确着陆之外，还可用于航天、航空、航海以及地面等各种载体的姿态测定。GPS姿态测量系统与传统测量系统相比，具有结构简单、价格低廉、精度稳定、体积小、固体化和可靠性好等优点，可将其作为一种新的姿态测量方法代替或者辅助传统的姿态测量［1］。基于GPS估计载体姿态算法有确定性算法和状态估计算法两大类［2］。确定性算法是只利用GPS的观测信息进行姿态估计，状态估计算法是结合载体运动模型与观测信息进行姿态估计。

众所周知，载体姿态确定的精度不仅取决于姿态测量系统硬件配置的性能与精度，还与所采用的姿态估计算法密切相关［3］。在姿态估计算法中，常用的姿态参数主要有方向余弦矩阵、欧拉角、姿态四元数、旋转矢量、罗德里格参数等。而这些参数或者存在奇异、不连续问题，或者存在冗余性问题［4］。四元数以其计算量小、非奇异性、可全姿态工作等优点，而在实际系统中得到广泛应用。但四元数存在冗余问题，文献［5－6］提出的乘性四元EKF算法通过构造乘性误差四元数来计算四元数协方差，不仅避免了四元数规范化的限制，而且解决了四元数协方差奇异性问题。然而当系统非线性程度较强时，EKF算法的截断误差将大大降低滤波精度甚至会导致滤波发散，针对此，不少学者已经开展了相应的研究工作，提出了基于四元数的二阶EKF算法［7］、无迹滤波［8］（unscented Kalman filter，UKF）、粒子滤波（particle filter，PF）［9］以及非线性预测 滤 波［10］（nonlinear predictive filter，NPF）等方法。

本文给出了一种矩阵卡尔曼滤波算法，该方法选择方向余弦矩阵作为姿态参数，以此构造的姿态系统状态方程和测量方程均为线性方程，可以避免基于误差四元数的EKF滤波中存在的线性误差。

1 载体姿态估计滤波器设计

1.1 系统状态方程

定义卫星本体系相对惯性系的方向余弦矩阵为R，由其表达的姿态运动学方程为［11］

其中：ω为载体坐标系相对惯性系的角速度在载体坐标系的坐标，

假设历元间隔Δt＝tk＋1－tk足够小，对式（1）进行离散化处理［12］，得到

为了推导方便，假设陀螺测量系与本体坐标系重合，陀螺测量模型建立如下：

其中：为陀螺的测量输出值，ω为载体相对惯性空间的姿态角速度，ηv为陀螺测量噪声。

将式（3）代入式（2）得到状态方程

其中

由于状态模型误差Wk为3×3维矩阵，建立其协方差阵需要将其向量化

其中，vec［ηv×］可以表示为列向量ηv的线性形式

其中：ej（j＝1，2，3）为3×1维向量，位置j元素为1，其余元素为0。

将式（7）代入式（6），得到

根据误差协方差传播定律，建立状态模型误差协方差阵

1.2 GPS观测方程

假设载体上一短基线同时观测两颗卫星，双差载波相位观测方程描述为

其中：▽Δφ为双差载波相位观测值，z为双差整周模糊度，λ为L1载波波长。由于载体上天线之间的距离很短，可假设同一历元载体上所有天线接收中心到同一颗卫星的视向量相同，记为s12。bE为基线在ECEF（Earth－Centered，Earth－Fixed）坐标系中的向量。ε为双差载波相位观测噪声。

若载体上安装m＋1个天线，共视卫星数为n＋1个，选择某一天线作为主天线，仰角最高的卫星作为参考星（以符号r表示），将基线i的双差载波相位观测量表示为向量yi形式

基线i的观测方程可以表示为

其中，系数阵A，S分别表示如下：

其中：In为n维单位向量；为基线i关于卫星j，r的视向量。

根据式（12）将载体上m条基线观测量组合，得到如下观测方程：

假设BB表示为BE在载体坐标系中的相应向量，两者之间的关系以姿态矩阵表示为RBB＝BE。多基线观测方程可以表示为姿态矩阵形式

根据文献［13］，多基线载波相位观测量方差－协方差阵表示为

2 基于SMKF滤波的姿态估计算法

可以看出基于方向余弦矩阵的状态方程、观测方程均为线性方程，无需线性化处理，但其未知参数及观测量均以矩阵形式出现，若采用kalman滤波对其进行估计，需要对其进行向量化。根据矩阵向量化定理［14］：vec（ABC）＝（CT⊗A）vecB，由状态方程、观测方程可以得到

由向量化后的系统方程式（17）、式（18）可以看出，其与线性kalman滤波系统方程类似，基于状态矩阵的kalman滤波（SMKF）算法步骤归纳如下：

1）对状态参数及其协方差阵进行初始化

2）计算预测状态向量

3）计算预测状态协方差阵

4）计算信息向量及其协方差阵

5）计算增益矩阵

6）求解新的状态估计值

7）计算状态估计的协方差矩阵k

考虑到方向余弦矩阵的正交化性质，在得到滤波的估计值＾Rk及其协方差阵Σ＾Rk后，需要对其进行正交化处理，即解算最优化问题

文献［15］详细介绍了基于拉格朗日的正交矩阵最优化算法，本文在此不作赘述。

3 仿真实验

根据陀螺角速度、测量噪声仿真得出陀螺观测量及载体每个历元的姿态。这里假设载体角速度及 初 值 为ω＝［cos（10Ω0t），cos（8Ω0t），cos（5.7Ω0t）］rad/s，Ω0＝0.001rad/s。载体上安装3条基线，其在载体坐标系中的坐标为：b1＝［2 ，0，0］T，b2＝［1 ，1.7，0］T，b3＝［1，0.6，1.6 ］T，设定载波相位观测噪声为0.006m，利用VISUAL软件［16］，基于2008真实的GPS卫星星座分布、当前历元的姿态及GPS载波相位观测噪声仿真得到GPS观测值。

为了验证MKF姿态估计算法的有效性，采用两种方案对表1中4组仿真数据进行解算。方案如下：

方案1：基于乘性四元数卡尔曼滤波。

方案2：基于方向余弦矩阵的卡尔曼滤波。

表1给出不同仿真条件下两种方案姿态解算统计结果。图1为第4组数据两种方案的参数估计误差曲线图（其中虚线为方案1结果，实线为方案2结果）。

表1 各方案结果统计分析

图1 姿态角估计误差曲线

1）由表1可以看出，不同条件数据下，基于方向余弦矩阵的kalman滤波较基于误差四元数的EKF算法的解算精度均有所提高，提高的幅度不等。在初始误差为5°，采样频率为1Hz的情况下，横滚角的解算精度提高了近4倍。基于误差四元数的EKF算法的线性化误差受采样频率、初始值误差的影响较大，而基于方向余弦矩阵的kalman滤波无需进行线性化处理。

2）由图1可以看出，两种方案均在20历元附近得到收敛，20历元后的解算结果基本相同，而20历元内的解算结果方案2要优于方案1，这也从一定程度上说明基于状态矩阵kalman滤波的姿态估计算法对初始姿态误差具有较好的鲁棒性。

4 结束语

本文用方向余弦矩阵描述姿态，提出了基于状态矩阵kalman滤波的姿态估计算法，以方向余弦矩阵表示的姿态系统状态方程及观测方程均为线性方程，避免了线性化误差。仿真结果表明：新算法对初始姿态误差更具有较好的鲁棒性，较基于误差四元数的EKF滤波算法的精度高，稳定性强。

［1］王磊，梁开龙.四元数在GPS船姿测量中的应用［J］.测绘工程，2004，13（3）：32－34.

［2］周朝阳.基于陀螺和星敏感器的卫星姿态确定研究［D］.哈尔滨：哈尔滨工业大学，2008：4－7.

［3］牟忠凯.卫星姿态确定的非线性滤波方法研究［D］.郑州：信息工程大学，2010.

［4］SHUSTER M D.A survey of attitude representations［J］.Journal of the Astronautical Sciences，1993，41（4）：439－517.

［5］MARKLEY F L.Attitude Error Representations for Kalman Filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26，（2）：311－317.

［6］LEFFERTS E J，MARKLEY F L，SHUSTER M D.Kalman Filtering for Spacecraft Attitude Estimation［J］.Journal of Guidance，Control，and Dynamics，1982，5（5）：417－429.

［7］VATHSAL S.Spacecraft attitude determination using a second－order nonlinear filter［J］.Journal of Guidance，Control，and Dynamics，1987，10（5）：559－566.

［8］牟忠凯，隋立芬，范澎湃，等.基于MRPs估计航天器姿态的IUKF算法及其改进［J］.测绘科学技术学报，2009，26（4）：250－253.

［9］OSHMAN Y，CARMI A.Attitude Estimation from Vector Observations Using Genetic Algorithm Embedded Quaternion Particle Filter［J］，Journal of Guidance，Control，and Dynamics，2006，29（4）：879－891.

［10］CRASSIDIS J L，MARKLEY F L.Predictive filter attitude estimation without rate sensors［J］.Journal of Guidance，Control and Dynamics，1997，20：522－527.

［11］章仁为.卫星轨道姿态动力学与控制［M］.北京：北京航空航天大学出版社，1998：147－148.

［12］付梦印，邓志红，闫莉萍.Kalman滤波理论及其在导航系统中的应用［M］.北京：科学出版社，2010：13－17.

［13］许国昌.GPS理论、算法与应用［M］.李强，译.北京：清华大学出版社，2011.

［14］张贤达.矩阵分析与应用［M］.北京：清华大学出版社，2004：638－642.

［15］ALAN EDELMAN，ARIAS T A，STEVEN T S.The Geometry of algorithms with orthogonality constraints［J］.SIAM Journal on Matrix Analysis and Applications，1998，20（2）：303－353.

［16］VERHAGEN S.Visualization of GNSS－related design parameters：Manual for the Matlab user interface VISUAL［S］.2006.