CN103900608A - 一种基于四元数ckf的低精度惯导初始对准方法 - Google Patents

Abstract

本发明提供的是一种基于四元数CKF的低精度惯导初始对准方法。首先利用加速度计和磁强计的测量值进行粗对准，得到粗略的姿态矩阵。然后利用GPS提供的准确外界信息，取位置和速度误差作为量测量，通过四元数CKF进行非线性滤波，得到姿态误差矩阵。最后利用姿态误差矩阵进行校正，得到精确的姿态矩阵。本发明利用加速度计和磁强计分别对重力加速度和磁力的量测值，进行粗对准。解决了在低精度惯性器件的条件下，由于陀螺性能低，无法正确敏感地球自转角速率，不能应用传统粗对准方法的问题；另一方面，本方法中提出的四元数CKF能适用于大角度误差的非线性模型，可以在大方位失准角的条件下依然很好的完成对准，且对准精度高。

Description

一种基于四元数CKF的低精度惯导初始对准方法

技术领域

本发明涉及的是一种捷联惯导系统初始对准方法，尤其涉及一种基于四元数CKF的低精度捷联惯导的非线性初始对准方法。

背景技术

静基座条件下，捷联惯导系统（SINS）可利用加速度计和陀螺分别对重力加速度和地球自转角速度的量测值，粗略计算捷联惯导的姿态矩阵以完成粗对准，并在此基础上进行精对准。但在使用低精度惯性器件的条件下，由于陀螺精度低（其精度都在每小时零点几度到上百度），无法正确敏感地球自转角速率，以及其他各种环境因素的干扰，很难根据陀螺和加速度计的输出完成具有一定精度的粗对准。此时精对准往往需要在大姿态误差角的情况进行，特别是方位角误差可能达到几十度。传统捷联惯导的线性误差方程是在姿态误差为小角度的基础上推导得到的，当姿态误差角较大时无法准确刻画捷联惯导的误差传播特性。因此，需要研究能适用于大角度误差的非线性模型及相应的非线性滤波算法。

初始对准是捷联惯导系统中的关键技术之一，初始对准的时间和精度直接影响捷联惯导系统的工作性能。捷联惯导误差模型和滤波算法在初始对准中扮演着重要的角色。常用的SINS误差模型主要有φ-angle误差模型、ψ-angle误差模型、旋转矢量误差模型(RVE)、四元数误差模型(QE)等。其中四元数法因为其计算量小、精度高、非奇异性和可全姿态工作而得到广泛的应用。

传统的非线性滤波EKF在处理非线性较强的系统时，由于高阶项截断误差，会导致滤波数值不稳定性且有可能发散。而最近由加拿大学者Arasaratnam提出的基于三阶spherical-radial cubature准则的cubature kalman filter则克服了EKF缺点。CKF类似于UKF都避免了对非线性模型的线性化处理，不依赖于具体系统模型的非线性方程，算法相对独立，适用于任何形式的非线性模型。

发明内容

本发明的目的在于提供一种能够解决低精度的惯导大方位失准角初始对准问题，并且估计精度高、所需对准时间短，可靠性高的基于四元数CKF的低精度惯导初始对准方法。

本发明的目的是这样实现的：

1、包括以下步骤：

(1)利用GPS确定载体的初始精度、纬度参数；

(2)采集加速度计和磁强计输出的载体三轴加速度和磁力信息；

(3)利用采集的载体三轴加速度信息，根据下式确定初始纵摇角α₀和初始横摇角β₀，

$\left\{\begin{array}{c}{\mathrm{\α}}_0=\arcsin \left(\frac{f_x^b}{g}\right)\\ {\mathrm{\β}}_0=\arcsin (-\frac{f_y^b}{g\cos {\mathrm{\α}}_0})\end{array}\right.$

式中：

和

分别是载体的x、y轴的加速度，g表示地球重力加速度；

(4)利用载体的三轴磁力信息以及步骤(3)中确定的载体初始纵摇角α₀和初始横摇角β₀，根据下式确定初始方位角γ₀；

${\mathrm{\γ}}_0=\arctan \frac{m_y\cos {\mathrm{\β}}_0+m_z\sin {\mathrm{\β}}_0}{m_x\cos {\mathrm{\α}}_0+m_y\sin {\mathrm{\α}}_0\sin {\mathrm{\β}}_0-m_z\sin {\mathrm{\α}}_0\cos {\mathrm{\β}}_0}$

式中：m_x、m_y和m_z分别为载体x、y和z轴的磁力量；

(5)利用步骤(3)、(4)中确定的载体初始纵摇角α₀和初始横摇角β₀和初始方位角γ₀，采用解析法来进行系统的粗对准，粗略的得到载体坐标系到计算地理坐标系的姿态矩阵

$C_b^p\left(0\right)=\left(\begin{array}{ccc}\cos {\mathrm{\β}}_0\cos {\mathrm{\γ}}_0-\sin {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0& -\cos {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0& \sin {\mathrm{\β}}_0\cos {\mathrm{\γ}}_0+\cos {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0\\ \cos {\mathrm{\β}}_0\sin {\mathrm{\γ}}_0+\sin {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0& \cos {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0& \sin {\mathrm{\β}}_0\sin {\mathrm{\γ}}_0-\cos {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0\\ -\sin {\mathrm{\β}}_0\cos {\mathrm{\α}}_0& \sin {\mathrm{\α}}_0& \cos {\mathrm{\β}}_0\cos {\mathrm{\α}}_0\end{array}\right);$

(6)建立捷联惯导系统初始对准非线性状态空间模型，取系统的状态变量为

取速度误差和位置误差作为观测量

当地地理坐标选取东北天坐标系，q₀、q₁、q₂和q₃分别是误差四元数δQ的四个元素，其中q0为标量部分，δv_x、δv_y分别为东向和北向速度误差，δλ分别为纬度和经度误差，

和

分别是三个方向上的陀螺仪常值偏移，

和分别是加速度计常值零偏；

(7)根据捷联惯导系统静基座下的姿态四元数误差方程、速度误差方程和位置误差方程，建立捷联惯导系统在大方位失准角下的状态空间模型：

$\left\{\begin{array}{c}\stackrel{\·}{X}=f\left(X\right)+G\left(X\right)W\\ Z=\mathrm{HX}+V\end{array}\right.$

式中：f(X)为系统矩阵，G(X)为噪声驱动阵，W为系统噪声阵，H为观测矩阵，V为观测噪声阵，其中：

H=[0_4×4 I_4×4 0_4×5]；

(8)滤波初始化，在初始时刻即k=0时，对步骤(6)中的变量初始化

式中：

为初始状态估计值，P₀为初始状态误差协方差阵，E表示求取期望；

(9)当k≥1时，将GPS提供的载体速度、位置与惯导解算的速度、位置的差值作为滤波的观测量，通过四元数CKF对状态量进行估计，直到估计出精确的误差四元数δQ；

(10)利用步骤(9)估计出的误差四元数δQ，根据下式确定姿态误差矩阵

$C_n^p=C_n^p\left(\mathrm{\δQ}\right)=\left[\begin{array}{ccc}{2q}_0^2+{2q}_1^2-1& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {2q}_0^2+{2q}_2^2-1& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {2q}_0^2+{2q}_3^2-1\end{array}\right]$

(11)利用步骤(10)中确定的姿态误差矩阵

校正粗略姿态矩阵

得到精确的姿态矩阵

完成初始对准。

本发明首先利用加速度计和磁强计的测量值进行粗对准，得到粗略的姿态矩阵。然后利用GPS提供的准确外界信息，取位置和速度误差作为量测量，通过四元数CKF进行非线性滤波，得到姿态误差矩阵。最后利用姿态误差矩阵进行校正，得到精确的姿态矩阵。

本发明与现有的技术相比的优点在于：本方法利用加速度计和磁强计分别对重力加速度和磁力的量测值，进行粗对准。解决了在低精度惯性器件的条件下，由于陀螺性能低，无法正确敏感地球自转角速率，不能应用传统粗对准方法的问题；另一方面，本方法中提出的的四元数CKF能适用于大角度误差的非线性模型，可以在大方位失准角的条件下依然很好的完成对准，且对准精度高。而传统的线性卡尔曼滤波在大角度误差的非线性模型的情况下，对准精度较低。

对本发明的有益效果说明如下：

对本发明中的方法进行仿真验证，假设载体初始位置：北纬45.7996°，东经126.6705°；地球自转角速率（弧度/秒）：ω_ie=7.2921158e-5；地球赤道半径：R=6378393m；常数：π=3.1415926；地球表面重力加速度：g₀=9.78049(m/s)；MEMS陀螺仪常值漂移：5度/小时；MEMS加速度计零偏：10^-4g₀；初始失准角：φ_x=1°,φ_y=1°,φ_x=15°。为了模拟实际捷联惯导系统三个姿态角的变化，假设惯导系统在静基座上以基座原点为中心做如下摇摆：

ψ=10°+6°sin(2πt/4+π/4)

θ=8°sin(2πt/5+π/5)

η=10°sin(2πt/6+π/6)

根据所设置的参数，利用本发明的基于四元数CKF的初始对准方法，可以得到如图2、图3、图4所示的方位失准角，纵摇失准角和横摇失准角的误差曲线。结果表明在粗对准结束时存在大方位失准角且陀螺常值偏移很大的情况下，采用本发明的方法可以获得很高的对准精度，有效地解决了低精度惯导静基座非线性初始对准的相关问题。

附图说明

图1为本发明所述初始对准方法的具体流程图；

图2为本发明具体实施方式的方位失准角误差曲线图；

图3为本发明具体实施方式的纵摇角失准角误差曲线图；

图4为本发明具体实施方式的横摇角失准角误差曲线图。

具体实施方式

下面结合附图对本发明作进一步说明：

如图1所示：本发明包括以下步骤：

(1)利用GPS确定载体的初始精度、纬度参数；

(2)采集加速度计和磁强计输出的载体三轴加速度和磁力信息；

(3)利用采集的载体三轴加速度信息，根据下式确定初始纵摇角α₀和初始横摇角β₀；

$\left\{\begin{array}{c}{\mathrm{\α}}_0=\arcsin \left(\frac{f_x^b}{g}\right)\\ {\mathrm{\β}}_0=\arcsin (-\frac{f_y^b}{g\cos {\mathrm{\α}}_0})\end{array}\right.$

式中：

和

分别是载体的x、y轴的加速度，g表示地球重力加速度；

(4)利用载体的三轴磁力信息以及步骤(3)中确定的载体初始纵摇角α₀和初始横摇角β₀，根据下式确定初始方位角γ₀，

${\mathrm{\γ}}_0=\arctan \frac{m_y\cos {\mathrm{\β}}_0+m_z\sin {\mathrm{\β}}_0}{m_x\cos {\mathrm{\α}}_0+m_y\sin {\mathrm{\α}}_0\sin {\mathrm{\β}}_0-m_z\sin {\mathrm{\α}}_0\cos {\mathrm{\β}}_0}$

式中：m_x、m_y和m_z分别为载体x、y和z轴的磁力量；

(5)利用步骤(3)、(4)中确定的载体初始纵摇角α₀和初始横摇角β₀和初始方位角γ₀，采用解析法来进行系统的粗对准，粗略的得到载体坐标系到计算地理坐标系的姿态矩阵

$C_b^p\left(0\right)=\left(\begin{array}{ccc}\cos {\mathrm{\β}}_0\cos {\mathrm{\γ}}_0-\sin {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0& -\cos {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0& \sin {\mathrm{\β}}_0\cos {\mathrm{\γ}}_0+\cos {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0\\ \cos {\mathrm{\β}}_0\sin {\mathrm{\γ}}_0+\sin {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0& \cos {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0& \sin {\mathrm{\β}}_0\sin {\mathrm{\γ}}_0-\cos {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0\\ -\sin {\mathrm{\β}}_0\cos {\mathrm{\α}}_0& \sin {\mathrm{\α}}_0& \cos {\mathrm{\β}}_0\cos {\mathrm{\α}}_0\end{array}\right);$

(6)建立捷联惯导系统初始对准非线性状态空间模型，取系统的状态变量为

取速度误差和位置误差作为观测量

当地地理坐标选取东北天坐标系，q₀、q₁、q₂和q₃分别是误差四元数δQ的四个元素，其中q₀为标量部分，δv_x、δv_y分别为东向和北向速度误差，

δλ分别为纬度和经度误差，

和

分别是三个方向上的陀螺仪常值偏移，

和

分别是加速度计常值零偏；

(7)根据捷联惯导系统静基座下的姿态四元数误差方程、速度误差方程和位置误差方程，建立捷联惯导系统在大方位失准角下的状态空间模型：

$\left\{\begin{array}{c}\stackrel{\·}{X}=f\left(X\right)+G\left(X\right)W\\ Z=\mathrm{HX}+V\end{array}\right.$

式中：f(X)为系统矩阵，G(X)为噪声驱动阵，W为系统噪声阵，H为观测矩阵，V为观测噪声阵，其中：

H=[0_4×4 I_4×4 0_4×5]；

(8)滤波初始化，在初始时刻即k=0时，对步骤(6)中的变量初始化

式中：

为初始状态估计值，P₀为初始状态误差协方差阵，E表示求取期望；

(9)当k≥1时，将GPS提供的载体速度、位置与惯导解算的速度、位置的差值作为滤波的观测量，通过四元数CKF对状态量进行估计，直到估计出精确的误差四元数δQ；

(10)利用步骤(9)估计出的误差四元数δQ，根据下式确定姿态误差矩阵

$C_n^p=C_n^p\left(\mathrm{\δQ}\right)=\left[\begin{array}{ccc}{2q}_0^2+{2q}_1^2-1& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {2q}_0^2+{2q}_2^2-1& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {2q}_0^2+{2q}_3^2-1\end{array}\right]$

(11)利用步骤(10)中确定的姿态误差矩阵校正粗略姿态矩阵

得到精确的姿态矩阵

完成初始对准。

Claims (1)

1.一种基于四元数CKF的低精度惯导初始对准方法，其特征在于包括以下步骤：

(1)利用GPS确定载体的初始精度、纬度参数；

(2)采集加速度计和磁强计输出的载体三轴加速度和磁力信息；

(3)利用采集的载体三轴加速度信息，根据下式确定初始纵摇角α₀和初始横摇角β₀，

$\left\{\begin{array}{c}{\mathrm{\α}}_0=\arcsin \left(\frac{f_x^b}{g}\right)\\ {\mathrm{\β}}_0=\arcsin (-\frac{f_y^b}{g\cos {\mathrm{\α}}_0})\end{array}\right.$

式中：

和

分别是载体的x、y轴的加速度，g表示地球重力加速度；

(4)利用载体的三轴磁力信息以及步骤(3)中确定的载体初始纵摇角α₀和初始横摇角β₀，根据下式确定初始方位角γ₀；

${\mathrm{\γ}}_0=\arctan \frac{m_y\cos {\mathrm{\β}}_0+m_z\sin {\mathrm{\β}}_0}{m_x\cos {\mathrm{\α}}_0+m_y\sin {\mathrm{\α}}_0\sin {\mathrm{\β}}_0-m_z\sin {\mathrm{\α}}_0\cos {\mathrm{\β}}_0}$

式中：m_x、m_y和m_z分别为载体x、y和z轴的磁力量；

(5)利用步骤(3)、(4)中确定的载体初始纵摇角α₀和初始横摇角β₀和初始方位角γ₀，采用解析法来进行系统的粗对准，粗略的得到载体坐标系到计算地理坐标系的姿态矩阵

$C_b^p\left(0\right)=\left(\begin{array}{ccc}\cos {\mathrm{\β}}_0\cos {\mathrm{\γ}}_0-\sin {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0& -\cos {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0& \sin {\mathrm{\β}}_0\cos {\mathrm{\γ}}_0+\cos {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\sin {\mathrm{\γ}}_0\\ \cos {\mathrm{\β}}_0\sin {\mathrm{\γ}}_0+\sin {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0& \cos {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0& \sin {\mathrm{\β}}_0\sin {\mathrm{\γ}}_0-\cos {\mathrm{\β}}_0\sin {\mathrm{\α}}_0\cos {\mathrm{\γ}}_0\\ -\sin {\mathrm{\β}}_0\cos {\mathrm{\α}}_0& \sin {\mathrm{\α}}_0& \cos {\mathrm{\β}}_0\cos {\mathrm{\α}}_0\end{array}\right);$

(6)建立捷联惯导系统初始对准非线性状态空间模型，取系统的状态变量为

取速度误差和位置误差作为观测量

当地地理坐标选取东北天坐标系，q₀、q₁、q₂和q₃分别是误差四元数δQ的四个元素，其中q₀为标量部分，δv_x、δv_y分别为东向和北向速度误差，δλ分别为纬度和经度误差，和

分别是三个方向上的陀螺仪常值偏移，

和

分别是加速度计常值零偏；

(7)根据捷联惯导系统静基座下的姿态四元数误差方程、速度误差方程和位置误差方程，建立捷联惯导系统在大方位失准角下的状态空间模型：

$\left\{\begin{array}{c}\stackrel{\·}{X}=f\left(X\right)+G\left(X\right)W\\ Z=\mathrm{HX}+V\end{array}\right.$

式中：f(X)为系统矩阵，G(X)为噪声驱动阵，W为系统噪声阵，H为观测矩阵，V为观测噪声阵，其中：

H=[0_4×4 I_4×4 0_4×5]；

(8)滤波初始化，在初始时刻即k=0时，对步骤(6)中的变量初始化

式中：为初始状态估计值，P₀为初始状态误差协方差阵，E表示求取期望；

(9)当k≥1时，将GPS提供的载体速度、位置与惯导解算的速度、位置的差值作为滤波的观测量，通过四元数CKF对状态量进行估计，直到估计出精确的误差四元数δQ；

(10)利用步骤(9)估计出的误差四元数δQ，根据下式确定姿态误差矩阵

$C_n^p=C_n^p\left(\mathrm{\δQ}\right)=\left[\begin{array}{ccc}{2q}_0^2+{2q}_1^2-1& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& {2q}_0^2+{2q}_2^2-1& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& {2q}_0^2+{2q}_3^2-1\end{array}\right]$

(11)利用步骤(10)中确定的姿态误差矩阵

校正粗略姿态矩阵

得到精确的姿态矩阵

完成初始对准。

Images