SINS/GNSS组合导航：组合导航设计（卡尔曼滤波）

SINS/GNSS组合导航将SINS输出信息与GNSS信息进行组合，状态量为

卡尔曼滤波原理参照

卡尔曼滤波（KF）与扩展卡尔曼滤波（EKF）讲解与推导

，核心就是五个递推方程迭代递推出系统状态变量，SINS/GNSS组合导航的卡尔曼滤波原理如下。

0 松组合和紧组合

GNSS 定位的长期稳定性与 SINS 系统定位的短期精确性具有近乎完美的互补特性，将两者进行组合可以显著提高导航精度。目前，大多数 SINS/GNSS 组合导航系统是利用 Kalman 滤波器将两者组合起来的。因此，组合系统的滤波器状态、量测信息、实现方式以及系统校正方式和组合深度都会影响到 SINS/GNSS 组合系统的工作性能。GNSS 与 SINS 的组合导航主要有三种模式：松组合、紧组合、深组合。

松组合的组合模式较为简单。在 GNSS 正常工作时，将 SINS 与其位置、速度之差作为量测信息送入卡尔曼滤波器，然后通过滤波器估计并校正 SINS 误差。此组合模式具有简单实用，便于实现，系统流程如下图所示。

紧组合利用伪距、伪距率进行组合。伪距、伪距率为 GPS 接收机给出的信息， SINS 无法直接给出，因此需要根据 SINS 输出位置和速度信息，结合 GPS 接收的星历信息，计算出相应于 SINS 的伪距、伪距率，然后将两个系统伪距、伪距率的差值作为组合滤波器的输入。最后利用卡尔曼滤波器估计并反馈 SINS 误差，同时还对接收机钟差等参数进行估计。系统流程图如下图所示：

相对于松组合模式，紧组合最大优点是在可见星的个数少于 4 颗时也可以使用，而且理论和工程应用都表明其组合后的位置、速度精度也略高。深组合设计传感器设计内容，尚在研究。本文以松组合为例介绍SINS/GNSS组合系统。

1 状态方程

以SINS的误差模型建立状态方程，参考

SINS/GNSS组合导航：SINS误差模型

，状态量为18维：有位置误差（经纬高）
$\delta p = [\delta L,\delta \lambda ,\delta h]$
、速度误差（东北天向速度）
$\delta v =[\delta v_E, \delta v_N, \delta v_U]$
、姿态角误差
$\phi=[\phi_E,\phi_N, \phi_U]$
、陀螺仪常值漂移误差
$\varepsilon^b = [\varepsilon _x^b,\varepsilon _y^b, \varepsilon _z^b]$
，陀螺仪一阶马尔科夫漂移误差
$\varepsilon^r= [\varepsilon _x^r,\varepsilon _y^r, \varepsilon _z^r]$
，加速度计一阶马尔科夫漂移误差
$\bigtriangledown ^b=[\bigtriangledown _x,\bigtriangledown _y,\bigtriangledown _z]$
，上述连接推到了误差模型，可以得到状态方程如下：

$\dot{X}(t)=F(t)X(t)+G(t)W(t)$
（1-1）

其中F为：

$F = \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & -C_b^n& 0_{3\times3}\\ M_{va} & M_{vv}& M_{vp} & 0_{3\times3} & 0_{3\times3} & C_b^n\\ 0_{3\times3} & M_{pv} & M_{pp} & 0_{3\times3} &0_{3\times3} & 0_{3\times3} \\ 0_{3\times3}& 0_{3\times3}& 0_{3\times3} &0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3}& 0_{3\times3}& 0_{3\times3} &0_{3\times3} & a_{g} & 0_{3\times3}\\ 0_{3\times3} & 0_{3\times3}& 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & a_{f} \end{bmatrix}$
（1-2）

将加速度计和陀螺误差漂移项建模为马尔科夫模型，该模型通常可满足高端 IMU 器件建模的精度要求。而 MEMS-IMU 相关时间很长，因此相关漂移项又可近似为随机游走，则系统状态方程变为:

$F = \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & 0_{3\times3}\\ M_{va} & M_{vv}& M_{vp} & 0_{3\times3} & C_b^n\\ 0_{3\times3} & M_{pv} & M_{pp} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3}& 0_{3\times3}& 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \end{bmatrix}$
（1-4）

F矩阵是已知的。参考

SINS/GNSS组合导航：SINS误差模型

中的惯性器件误差，设W(t)为白噪声，6维，表示如下：

$W(t)=[w_b,w_g,w_a]$
（1-5）

$w_b=[w_{bx},w_{by},w_{bz}]$
为三轴陀螺仪随机漂移误差，
$w_b$
为陀螺仪一阶马尔科夫过程漂移，
$w_b$
为加速度计一阶马尔科夫过程漂移。

噪声矩阵G(t)如下：

$G(t)=\begin{bmatrix} -C_b^n & 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3} & 0_{3\times 3} &C_b^n \\ 0_{3\times 3} &0_{3\times 3} & 0_{3\times 3}\\ 0_{3\times 3} & I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3} & 0_{3\times 3} & I_{3\times 3}\end{bmatrix}$
（1-6）

利用随机线性连续系统和随机线性离散系统的关系：

$\\ \Phi _{k+1|k}=I+F(t_k)\bigtriangleup T+o(\bigtriangleup T^2)\\ \Gamma _{k+1|k}=F(t_k)\bigtriangleup T+o(\bigtriangleup T^2)\\$
（1-7）

忽略二阶高次项得到：

$\\ \Phi _{k+1|k}=I+F(t_k)\bigtriangleup T\\ \Gamma _{k+1|k}=(I+F(t_k)\bigtriangleup T/2)G\bigtriangleup T\\$
（1-8）

将连续状态方程（1-1）离散化：

$X_{k+1}=\Phi _{k+1|k}X_k+\Gamma _{k+1|k} W_k$
（1-9）

至此得到了观测方程。

2 观测方程

GNSS/SINS 松组合系统选取 SINS 解算的位置、速度同 GNSS 接收机得到的位置、速度之差作为观测量，即：

$Z_{k+1}=\begin{bmatrix} p^n_{INS}-p^n_{GNSS} \\ v^n_{INS}-v^n_{GNSS} \end{bmatrix}=H_kX_k+V_k$
（2-1）

$V_k=[\eta _p^T,\eta_v^T]^T$
为高斯观测白噪声，H矩阵如下：

$H_k=\begin{bmatrix} I_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3} & I_{3\times3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} \end{bmatrix}$
（2-2）

3 开环卡尔曼滤波模型

卡尔曼滤波原理参照

卡尔曼滤波（KF）与扩展卡尔曼滤波（EKF）讲解与推导

。根据状态方程（1-9）与观测方程（2-1）可以得到卡尔曼滤波方程：

$\\ \overline{X_k} &= \Phi _{k+1|k}\hat{X}_{k-1}\\ \overline{P_k} & = \Phi _{k+1|k}\hat{P}_{k-1} \Phi _{k+1|k}^T + Q_k\\ K_k &= \overline{P}_kH_k^T(H_k\overline{P_k}H_k^T+R_k)^{-1}\\ \hat{X_k}&=\overline{X}_k+K_k(Z_k-H_k\overline{X_k})\\ \hat{P_k}&=(I-K_kH_k)\overline{P}_k$
（3-1）

4 闭环卡尔曼滤波模型

实际上，对于SINS/GNSS组合导航系统而言，必须要对导航系统进行反馈校正。原因是前述组合导航系统的数学模型是建立在误差状态为小量的基础上的，而SINS 的误差随时间积累迅速，如果不进行反馈校正，即使在短时间内，惯导误差也不再是小量。此时，卡尔曼滤波器的模型出现误差，滤波误差增大，系统甚至无法工作。

导航系统的反馈校正，就是将组合卡尔曼滤波器估计的 SINS 参数误差
$X_k$
反馈到惯导系统内部，对误差状态进行校正，从而形成闭环卡尔曼滤波器，如本文第一幅图所示的 SINS 误差反馈校正环节。校正环节框图简化如下：

相比开环系统状态方程（1-9），闭环系统状态方程增加了控制项，结合测量方程可以表示如下：

$\\ X_{k+1}=\Phi _{k+1|k}X_k+B_kU_k+\Gamma _{k+1|k} W_k\\ Z_{k+1}=H_kX_k+V_k$
（4-1）

观测方程不变，卡尔曼滤波方程如下：

$\\ \overline{X_k} &= \Phi _{k+1|k}\hat{X}_{k-1} + B_{k-1}U_{k-1}\\ \overline{P_k} & = \Phi _{k+1|k}\hat{P}_{k-1} \Phi _{k+1|k}^T + Q_k\\ K_k &= \overline{P}_kH_k^T(H_k\overline{P_k}H_k^T+R_k)^{-1}\\ \hat{X_k}&=\overline{X}_k+K_k(Z_k-H_k\overline{X_k})\\ \hat{P_k}&=(I-K_kH_k)\overline{P}_k$
（4-2）

相比开环系统，区别只是预测部分增加了控制项。根据现代控制理论中的分离定理：针对具有二次型性能指标的随机动态系统的最优控制问题，首先可以不必考虑控制，而是在随机干扰下尽量精确地求解出状态估计（最优估计），其次将最优估计视为已知量，专门求解最优控制规律。

针对（4-1）描述的的随机离散系统的最优问题，设系统的二次型性能指标为：

$J_N=\sum_{N}^{j=1}(X_j^TQ_j^0X_j+U_j^TR_j^0U_j)+X_N^TS^0X_N$
（4-3）

优化问题通常按照
$J_N$
的期望为0这一约束条件进行求解：

$minE(J_N)=min(\sum_{N}^{j=1}(X_j^TQ_j^0X_j+U_j^TR_j^0U_j)+X_N^TS^0X_N)$
（4-4）

应用动态规划法求解得到：

$U^{*}_k=-A_kX_k$
（4-5）

$A_k$
为控制矩阵满足：

$A_k=[R_k^0+B_k^TP_{k+1}^0B_k]^{-1}B_k^TP_{k+1}^0\Phi _{k+1|k}$
（4-6）

在二次性能指标中令
$R_{k}^0=0$
，不考虑控制能量得到：

$A_k=B_k^{-1}\Phi _{k+1|k}$
（4-7）

（4-6）代入（4-4）得到：

$U^{*}_k=-B_k^{-1}\Phi _{k+1|k}X_k$
（4-8）

用
$\hat{X}_{k}$
来估计
$X_k$
得到：

$U^{*}_k=-B_k^{-1}\Phi _{k+1|k}\hat{X}_k$
（4-9）

将（4-8）代入闭环卡尔曼方程（4-2）得到：

$\\ \overline{X_k} &= 0\\ \overline{P_k} & = \Phi _{k+1|k}\hat{P}_{k-1} \Phi _{k+1|k}^T + Q_k\\ K_k &= \overline{P}_kH_k^T(H_k\overline{P_k}H_k^T+R_k)^{-1}\\ \hat{X_k}&=K_kZ_k\\ \hat{P_k}&=(I-K_kH_k)\overline{P}_k$
（4-10）

原文链接：https://blog.csdn.net/LittleEmperor/article/details/105377416

你可能也喜欢