基于重力加速度矢量匹配的非线性初始对准方法

摘要

本发明提供了一种基于重力加速度矢量匹配的非线性初始对准方法。主要步骤包括：捷联惯性导航系统粗对准；建立基于导航坐标系重力加速度矢量匹配的非线性滤波连续系统模型及其离散化；采用容积卡尔曼非线性滤波器进行滤波，完成初始对准。优点在于本发明中状态量采用欧拉角和惯性器件常值误差，以欧拉角微分方程和惯性器件误差模型为状态方程，量测量采用导航坐标系中的重力加速度矢量，以速度微分方程为量测方程，并采用容积卡尔曼非线性滤波算法进行滤波估计，滤波输出直接为导航参数，在滤波时间更新过程中同步实现捷联惯导系统的导航参数更新，不需要单独的导航解算过程，算法非常简单，为捷联惯性导航系统初始对准提供了一种新方案。

主权项

基于重力加速度矢量匹配的非线性初始对准方法，包括下列步骤：步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的线加速度信息，忽略惯性器件的测量误差，使用基于惯性系重力加速度的凝固解析粗对准算法求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；步骤2、在载体线速度为零的前提下，根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及以导航坐标系重力加速度矢量为量测量的量测方程，选定9维系统状态量和3维量测量，建立基于重力加速度矢量匹配的非线性连续系统模型，其状态方程和量测方程均为非线性；具体为：以东北天地理坐标系作为导航坐标系，即n系，以载体右前上方向矢量右手定则构成的坐标系作为载体坐标系，即b系，假设载体线速度为零；捷联惯性导航系统的欧拉角微分方程为：$\left[\begin{array}{c}\stackrel{·}{\theta }\\ \stackrel{·}{\gamma }\\ \stackrel{·}{\psi }\end{array}\right]=\frac{1}{\cos \theta }\left[\begin{array}{ccc}\cos \theta \cos \gamma & 0& \sin \gamma \cos \theta \\ \sin \theta \sin \gamma & \cos \theta & -\cos \gamma \sin \theta \\ -\sin \gamma & 0& \cos \gamma \end{array}\right]{\omega }_{nb}^b$式中，θ为纵摇角；γ为横摇角；ψ为航向角；为载体坐标系相对导航坐标系的角速度矢量在载体坐标系轴向的分量构成的列矩阵，在载体线速度为零的情况下，为载体坐标系轴向三只陀螺仪无误差的测量值，${\omega }_{ib}^b={[{\tilde{\omega }}_{ibx}^b-{\mathrm{\delta \omega }}_{ibx}^b,{\tilde{\omega }}_{iby}^b-{\mathrm{\delta \omega }}_{iby}^b,{\tilde{\omega }}_{ibz}^b-{\mathrm{\delta \omega }}_{ibz}^b]}^T,$和为载体坐标系轴向三只陀螺仪测量值，和为三只陀螺仪相应的测量误差，${\mathrm{\delta \omega }}_{ibx}^b={ϵ}_{gx}+w_{gx},{\mathrm{\delta \omega }}_{iby}^b={ϵ}_{gy}+w_{gy},{\mathrm{\delta \omega }}_{ibz}^b={ϵ}_{gz}+w_{gz},$ε_gx、ε_gy和ε_gz为三只陀螺仪测量误差的随机常值部分，w_gx、w_gy和w_gz为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；为姿态矩阵，$C_n^b=\left[\begin{array}{ccc}\cos \gamma \cos \psi -\sin \theta \sin \gamma \sin \psi & \cos \gamma \sin \psi +\sin \theta \sin \gamma \cos \psi & -\cos \theta \sin \gamma \\ -\cos \theta \sin \psi & \cos \theta \cos \psi & \sin \theta \\ \sin \gamma \cos \psi +\sin \theta \cos \gamma \sin \psi & \sin \gamma \sin \psi -\sin \theta \cos \gamma \cos \psi & \cos \theta \cos \gamma \end{array}\right];$为地球自转角速度在导航坐标系中的投影，ω_ie为地球自转角速度，L为当地地理纬度；捷联惯性导航系统的惯性器件测量误差的随机常值部分模型为：${\stackrel{·}{▿}}_{ax}=0,{\stackrel{·}{▿}}_{ay}=0,{\stackrel{·}{▿}}_{az}=0,{\stackrel{·}{ϵ}}_{gx}=0,{\stackrel{·}{ϵ}}_{gy}=0,{\stackrel{·}{ϵ}}_{gz}=0;$选定的系统状态向量为：$X={[\theta ,\gamma ,\psi ,{▿}_{ax},{▿}_{ay},{▿}_{az},{ϵ}_{gx},{ϵ}_{gy},{ϵ}_{gz}]}^T$式中，x为系统状态向量，9维；系统噪声向量为：w＝[w_gx,w_gy,w_gz,0,0,0,0,0,0]^T式中，w为系统噪声向量；在载体线速度为零的情况下，以导航系重力加速度矢量为量测量的量测方程为：gⁿ＝‑fⁿ式中，gⁿ为重力加速度矢量在n系中投影，其中gⁿ＝[0,0,‑g]^T，g为重力加速度；fⁿ为加速度计无误差的测量值在n系中投影，其中为载体系坐标轴方向三只加速度计无误差的测量值，其中$f_{ib}^b={[{\tilde{f}}_{ibx}^b-{\mathrm{\delta f}}_{ibx}^b,{\tilde{f}}_{iby}^b-{\mathrm{\delta f}}_{iby}^b,{\tilde{f}}_{ibz}^b-{\mathrm{\delta f}}_{ibz}^b]}^T,$和分别为三只加速度计相应的测量值；和分别为三只加速度计相应的测量误差，其中${\mathrm{\delta f}}_{ibx}^b={▿}_{ax}+w_{ax},{\mathrm{\delta f}}_{iby}^b={▿}_{ay}+w_{ay},{\mathrm{\delta f}}_{ibz}^b={▿}_{az}+w_{az},$和为三只加速度计测量误差的随机常值部分，w_ax、w_ay和w_az为三只加速度计测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；选择gⁿ为量测量，则3维量测量为：z＝[0,0,‑g]^T式中，z为量测向量；量测噪声向量为：η＝[w_ax,w_ay,w_az]^T式中，η为量测噪声向量；根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及量测方程建立非线性滤波连续系统模型如下：$\left\{\begin{array}{c}\stackrel{·}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+U\left(x\right)\eta \end{array}\right.$式中，F(x)为非线性状态转移函数，根据选定的状态向量参照欧拉角微分方程和惯性器件误差模型写出；H(x)为非线性量测函数，根据选定的状态向量和量测向量参照量测方程写出；G(x)为系统噪声输入函数，根据选定的状态向量和系统噪声向量参照欧拉角微分方程写出；U(x)为量测噪声输入函数，根据选定的量测向量和量测噪声向量参照量测方程写出；步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型，非线性离散系统模型的状态方程与量测方程均为非线性；非线性离散系统模型为：$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\eta }_k\end{array}\right.$式中，x_k和x_k‑1分别为t_k和t_k‑1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k‑1)为t_k‑1时刻的非线性状态转移函数，离散化时，采用四阶龙格库塔方法；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k‑1分别为t_k和t_k‑1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足：$\left\{\begin{array}{c}E\left[w_k\right]=0,E\left[w_k{w}_j^T\right]=Q_k{\delta }_{kj}\\ E\left[{\eta }_k\right]=0,E\left[{\eta }_k{\eta }_j^T\right]=R_k{\delta }_{kj}\\ E\left[w_k{\eta }_j^T\right]=0\end{array}\right.$式中，δ_kj是δ函数；离散系统噪声方差强度阵为：Q_k＝G(x_k)QG(x_k)^TT_f式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_f为滤波周期；离散量测噪声方差强度阵为：R_k＝U(x_k)RU(x_k)^T/T_f式中，R_k为离散系统噪声η_k的方差强度阵；U(x_k)为t_k时刻的系统量测噪声输入函数；R为连续系统噪声η的方差强度阵；步骤4、针对步骤3中建立的非线性离散系统模型，采用容积卡尔曼非线性滤波器进行滤波，滤波周期与惯性器件的数据更新周期一致，周期为T_f；其中容积卡尔曼非线性滤波器的滤波步骤为：1)初始化系统状态向量及其方差矩阵：${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$式中，和P₀分别为t₀时刻的系统状态向量估计值及其方差阵；2)时间更新：分解前一步的方差阵：$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$式中，P_k‑1|k‑1和S_k‑1|k‑1分别为t_k‑1时刻的系统状态向量方差阵及其平方根阵；计算状态容积点：${\chi }_{i,k-1|k-1}=S_{k-1|k-1}{\xi }_i+{\hat{x}}_{k-1|k-1}$式中，χ_{i,k‑1|k‑1}为t_k‑1时刻的系统状态向量的第i个容积点；为t_k‑1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；计算非线性状态函数传播的容积点：${\chi }_{i,k|k-1}^{*}=F\left({\chi }_{i,k-1|k-1}\right)$式中，为χ_{i,k‑1|k‑1}通过非线性状态函数F(χ_{i,k‑1|k‑1})传播的容积点；计算状态的一步预测值：${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Sigma }}{\chi }_{i,k|k-1}^{*}{a}_i$式中，为一步预测的t_k时刻系统状态向量；a_i为容积点ξ_i对应的权重；m为容积点总个数；i为容积点的序号；计算一步预测方差阵：$P_{k|k-1}=\underset{i=1}{\overset{m}{\Sigma }}{a}_i{\chi }_{i,k|k-1}^{*}{\chi }_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$式中，P_k|k‑1为一步预测的t_k时刻系统状态向量方差阵；Q_k‑1为t_k‑1时刻的系统噪声方差强度阵；3)量测更新：分解一步预测方差阵：$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$式中，S_k|k‑1为一步预测的t_k时刻系统状态向量方差阵的平方根阵；计算一步预测容积点：${\chi }_{i,k|k-1}=S_{k|k-1}{\xi }_i+{\hat{x}}_{k|k-1}$式中，χ_i,k|k‑1为一步预测的t_k时刻系统状态向量的第i个容积点；计算通过非线性量测函数传播的量测容积点：Z_i,k|k‑1＝H(χ_i,k|k‑1)式中，Z_i,k|k‑1为χ_i,k|k‑1通过非线性量测函数H(χ_i,k|k‑1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；计算一步预测量测值：${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Sigma }}{Z}_{i,k|k-1}{a}_i$式中，为一步预测的t_k时刻量测向量；计算一步预测量测方差阵：$P_{zz,k|k-1}=\underset{i=1}{\overset{m}{\Sigma }}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$式中，P_zz,k|k‑1为一步预测的t_k时刻量测方差阵；计算一步预测协方差阵：$P_{xz,k|k-1}=\underset{i=1}{\overset{m}{\Sigma }}{a}_i{\chi }_{i,k|k-1}{Z}_{i,k|k-1}^T{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$式中，P_xz,k|k‑1为一步预测的t_k时刻协方差阵；计算卡尔曼滤波增益：$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$式中，K_k为t_k时刻卡尔曼滤波增益阵；更新状态：${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$更新状态方差阵：$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T.$