十二、构建一个基本的组合导航系统

一、概述

在之前的文章里，我们从一个基本的IMU模块开始讲起（二），利用Allan方差分析方法（三、四），得出了其误差参数，

又通过标定（五、六、七、八）、温补（九）提高了原始数据精度，通过导航解算（十）得出了状态预测值，通过分析惯性导航误差传播规律（十一）写出了状态方程，

最后使用滤波器（十二）把状态、观测融合在一起。

然而，这其实并不完整，首先初始对准还没有做，其次融合是为了得到更精确的位置、速度、姿态，但是导航误差和IMU自身的内参是耦合在一起的，

要估得更精确，强行忽视可不行，但是那么多内参都应该放在状态量里吗，真的是估的越多越好吗？这就牵扯到状态量的可观测性分析。

本篇文章就着重解决初始对准和可观测性分析两个问题。由于理解初值对准要先理解可观测性分析，因此先讲后者。

 

二、可观测性分析方法

虽然组合导航的内容很多，但是如果要让我挑一个最核心的技术点，我认为就是可观测性分析，因为它和所有的估计问题绑定在一起，

是导航这个毛线团的毛线头，抓住这个毛线头，绝大多数融合问题就都可以迎刃而解。广义上讲，分析可观测性，就是分析各状态量之间的耦合关系，

虽然我们之前没有提过这个概念，但是却在标定、温补等环节分析过各变量的耦合及解耦方法。狭义上讲，由于观测性这个名词是从现代控制理论里面来的，

随后被应用到kalman滤波(控制理论里的可观测性分析方法就是kalman本人提出的)，所以一般导航问题里说可观测性分析，仅指滤波过程中的耦合分析，

具体包括系统级标定、初始对准、组合导航。

1. 理解可观测性

之前介绍耦合问题的时候，以简单的方程为例子，这里不妨使用可观测性分析的方法从新审视一下这些例子。现在假设有状态量 

，它的各量之间满足如下关系

简单的方程可以这样直接观察得到结果，但当方程变得复杂，观察不出来的时候，就需要想别的办法。可观测性问题是现代控制理论里的重要问题，

在 kalman 本人发明 kalman 滤波器之前就已经解决了这个问题。如果一个系统可以用如下方程表示

其中A是状态矩阵，C是观测矩阵，那么当下面的矩阵满秩的时候，整个系统就是完全可观测的，也即所有参数都是可解的。

其中n是A矩阵的维数；这个矩阵也被称作可观测性矩阵。回到上面的例子，在第一个例子里，有

那么它的观测性矩阵可以写为

显然是满秩的，所以它完全可观测。在第二个例子里，有

它对应的观测性矩阵为

不满秩，所以不完全可观测。

2. 改变可观测性

上面的分析让我们知道什么样的系统是完全可观测的，什么样的是不完全可观测的。但这只是发现问题，我们还要解决问题，

即把所有系统都按照完全可观测标准去设计。在组合导航的问题里，是想可观就可观的吗？有时候是有时候不是，等这一节看完你会明白这句话的意思。

通过上面简单的方程例子，可以发现，对它做一些改造，是可以满足可观测性的。

在这里，虽然大家会发现无论a和b取任何值，这个模型都是不完全可观的，但是kalman是一个迭代估计过程，如果a和b在估计过程中是时变的，

即不是常值，那么它就可以完全可观测。得出这个结论的方法叫 分时段可观测性分析方法(PWCS)。用数学形式可以表达为，既然a和b是变化的，

那么A就是变化的，进而引起`$\wp$`的变化，可以得出k时刻的观测性矩阵为

PWCS指的是把所有时刻的观测性矩阵放在一起（如下式），如果它是满秩的，那么整个系统也同样是完全可观测的。

这个和我们以前讲的耦合其实是一回事，比如在标定的例子里，当方程组不可解的时候，就转动IMU，等同于构建新的方程组，

当方程组个数足够解出所有变量的时候，这个就叫 完全可观测

回到组合导航的问题，如果有这样可改变的'a'和'b'，那么就容易把系统由不可观变成可观。通过前面几篇文章的分析，可以知道，

惯性导航系统的误差方程为

理解这段话的意思

 

三、初始对准

其实理解了上面的问题，那么初始对准问题就迎刃而解了。初始对准是根据gps提供的位置、速度作观测，使用kalman滤波估计出失准角，

从而得到精确姿态的过程。为了失准角是可观测的，必须通过旋转改变的值，也就是说车要转向才行。我们经常看到组合导航在进入

导航状态之前要绕八字，就是这个原因。另外，组合导航在绕八字之前会先走一段直线，这是因为状态方程里有

，即姿态矩阵，如果它的初始值不准，那么就会带来很大的模型误差，从而使滤波器发散，走直线的时候，gps的航向角或者航迹角可以提供

一个比较好的初始航向值，从而解决这个问题，这个过程可以成为粗对准；相对的，使用滤波器精确估计失准角的过程可以称为精对准。

这只是分析了完全可观测的情况，对着滤波模型估就行了，很简单。但是上面还提到，静止情况下，它是不完全可观测的。真正需要深入

分析的是这种。我们同样通过方程的例子，来解释这个问题。比如下面这个方程组

显然这个方程不可解，即不完全可观测。但是我们可以得到

也就是说，如果真把上面这个模型扔给一个kalman滤波器去估计，x是会收敛到真值的，那y和z会收敛到哪呢？答案是不确定。

在组合导航的模型里，这样的状况会带来什么后果呢？假设y是IMU的方位失准角，z是东向陀螺仪的零偏(因为在IMU静止的时候，

这两个量是耦合的)，那么你的姿态误差有多大，是不可控的。解决办法当然有，还用这个方程组的例子。假如我把z从方程组里面删除掉，那么就会得到

显然这个y是不准的，y的误差

即z有多大值，y就有多大误差。但是如果z比较小，那么让y里面有一个小的确定性误差，也比让它不知道会收敛到哪里去要更好。在初始对准的模型里，

如果IMU真的只能静止不动，那么往往把陀螺仪的零位从状态模型里删除掉，接受姿态里有这样一个确定性的误差。当然前提是陀螺仪的精度要足够高，

如果只是一个低精度的mems，这种做法显然是不合理的，必须使用旋转的方法把零偏估出来补偿掉才行。如果你接到任务，让你在只有gps的位置、

速度作观测的前提下，使用低精度IMU在静止不动状态下完成初始对准，你可以直接置之不理； 另外，对于低精度mems，即使在初始对准时候估计出了零偏，

由于零偏不稳定性比较大，在组合导航过程中这个零偏会发生变化，所以需要在组合的时候也一直去估计它。但是估计又需要车辆有转向，否则它和姿

态失准角之间的耦合作用会导致姿态估计不准，进而引起大的导航误差。所以使用过组合导航的朋友们会发现，在车一直沿直线行驶或停车的时候，

导航误差会变大，就是这个原因，这个被称作退化现象

 

四、搭建基本的组合导航系统

完成了上面两个模块的分析，现在从一个IMU到一个基本的组合导航系统的流程就算是走通了，这个过程可以用下面的框图概括。

当然，这里只是完成一个最基本的系统，想要它变得更稳定，需要在滤波过程中对滤波状态做一些判断，并屏蔽掉不良观测。进一步，

还应该添加更多的约束，比如车辆的运动模型，让系统精度更高。还可以添加更多传感器，比如轮速计、磁力计，甚至雷达、摄像头等，

从而形成一套多传感器融合系统，后面我会们一起来讨论这些事情应该怎么做