导航原理_组合导航

1、 组合导航组合导航2. 最优综合导航系统最优综合导航系统n采用卡尔曼滤波器的组合方法采用卡尔曼滤波器的组合方法n卡尔曼滤波是一种递推线性最小方差估计，它卡尔曼滤波是一种递推线性最小方差估计，它用用“状态状态”表征系统的各个物理量，而以表征系统的各个物理量，而以“状状态方程态方程”和和“观测方程观测方程”描述系统的动力学特描述系统的动力学特性。它要求应用对象是线性系统，且已知系统性。它要求应用对象是线性系统，且已知系统的某些先验知识，如系统噪声和测量噪声的统的某些先验知识，如系统噪声和测量噪声的统计特性。综合导航系统基本满足这些条件，因计特性。综合导航系统基本满足这些条件，因而适合采用卡尔曼滤波

2、。而适合采用卡尔曼滤波。n根据根据KF所估计的状态不同，所估计的状态不同，Kalman滤波在组滤波在组合导航中的应用有直接法与间接法之分。合导航中的应用有直接法与间接法之分。n直接法估计导航参数本身，间接法是估计导航直接法估计导航参数本身，间接法是估计导航参数的误差。参数的误差。n直接法的直接法的KF接收惯导系统测量的比力、角速度接收惯导系统测量的比力、角速度和其他导航系统计算的某些导航参数，经过滤和其他导航系统计算的某些导航参数，经过滤波，给出有关导航参数的最优估值。波，给出有关导航参数的最优估值。4.3 最优组合导航系统最优组合导航系统 -Kalman滤波在组合导航中的应用滤波在组合导航中

3、的应用间接法的间接法的KF，接收的信号是惯导系统和其他导航，接收的信号是惯导系统和其他导航参数的差值，经过计算给出有关误差的最优估计参数的差值，经过计算给出有关误差的最优估计值。值。n利用直接法进行估计时，状态方程和测利用直接法进行估计时，状态方程和测量方程有可能是非线性的，由于运动体量方程有可能是非线性的，由于运动体的导航参数一般不是小量，方程线性化的导航参数一般不是小量，方程线性化会带来较大误差，且滤波计算需花费较会带来较大误差，且滤波计算需花费较多时间，这使得导航参数的刷新周期不多时间，这使得导航参数的刷新周期不可能太快，难以满足动态载体对导航参可能太快，难以满足动态载体对导航参数更新的

4、要求。数更新的要求。n因此，在组合导航系统中，直接法较少因此，在组合导航系统中，直接法较少采用。采用。间接法估计时，所谓间接法估计时，所谓“系统系统”实际就是导实际就是导航系统的各种误差的航系统的各种误差的“组合组合”，系统状态，系统状态均为小量，方程线性化带来的误差较小。均为小量，方程线性化带来的误差较小。n在滤波计算时，不参与原系统的计算流在滤波计算时，不参与原系统的计算流程，对原系统来讲，除了接受误差估值程，对原系统来讲，除了接受误差估值的校正外，原导航系统保持其工作的独的校正外，原导航系统保持其工作的独立性。立性。n这使得间接法能充分发挥各个系统的特这使得间接法能充分发挥各个系统的特点

5、（例如惯导系统具有较快的参数更新点（例如惯导系统具有较快的参数更新率），因而被广泛采用。率），因而被广泛采用。间接法估计的状态都是误差状态，即滤波间接法估计的状态都是误差状态，即滤波方程中的状态矢量是导航参数误差状态和方程中的状态矢量是导航参数误差状态和其它误差状态的集合（用其它误差状态的集合（用 来表示）。来表示）。n利用状态估值利用状态估值 去对原系统进行校正去对原系统进行校正也有两种方法，即输出校正和反馈校正。也有两种方法，即输出校正和反馈校正。 1、输出校正、输出校正 以惯导系统和其它某一导航系统组合为以惯导系统和其它某一导航系统组合为例，间接法的组合导航卡尔曼滤波器将例，间接法的组合

6、导航卡尔曼滤波器将惯导系统和其它导航系统各自计算的某惯导系统和其它导航系统各自计算的某些导航参数（分别用些导航参数（分别用 表示）进表示）进行比较，行比较，n其差值就包含了惯导某些导航参数误差其差值就包含了惯导某些导航参数误差 和其它导航系统的误差和其它导航系统的误差 ,即即 n滤波器将这种差值作为测量值，经过滤滤波器将这种差值作为测量值，经过滤波计算，得到滤波器状态（也即包括波计算，得到滤波器状态（也即包括 和和 在内的各种误差状态）的估值。在内的各种误差状态）的估值。其结构如图其结构如图6.4所示。所示。IXNXIXNXn上式说明，组合导航系统的导航参数的误差就上式说明，组合导航系统的导航

7、参数的误差就是惯导系统导航参数误差估值的估计误差。是惯导系统导航参数误差估值的估计误差。所谓输出校正，就是用导航参数误差的所谓输出校正，就是用导航参数误差的估值去校正系统输出的导航参数，得到估值去校正系统输出的导航参数，得到组合导航系统的导航参数估值组合导航系统的导航参数估值2、反馈校正、反馈校正n采用反馈校正的间接法估计是将导航参数误差采用反馈校正的间接法估计是将导航参数误差的估值反馈到各导航系统内，对误差状态进行的估值反馈到各导航系统内，对误差状态进行校正。反馈校正的滤波示意图如图校正。反馈校正的滤波示意图如图6.5所示所示输出校正和反馈校正的分析输出校正和反馈校正的分析n从形式看，输出校

8、正只是校正系统的输出量，而从形式看，输出校正只是校正系统的输出量，而反馈校正则校正系统内部状态，但可以证明，如反馈校正则校正系统内部状态，但可以证明，如果滤波器是最优滤波器，则两种校正方式的结果果滤波器是最优滤波器，则两种校正方式的结果是一样的。然而，真正意义上的是一样的。然而，真正意义上的“最优滤波器最优滤波器”工程上是不存在的。未校正系统导航参数的误差工程上是不存在的。未校正系统导航参数的误差会随时间而增大，因而输出校正方式下的滤波器会随时间而增大，因而输出校正方式下的滤波器状态值会越来越大。这使得方程线性化等近似计状态值会越来越大。这使得方程线性化等近似计算误差不断增大，从而滤波效果变差

9、。算误差不断增大，从而滤波效果变差。n由于以上原因，对实际系统（尤其是长时间工作由于以上原因，对实际系统（尤其是长时间工作的系统）来说，只要状态能够通过具体实施反馈的系统）来说，只要状态能够通过具体实施反馈校正来实现，综合导航系统就尽可能采用反馈校校正来实现，综合导航系统就尽可能采用反馈校正的滤波方法。正的滤波方法。综合导航卡尔曼滤波器的设计综合导航卡尔曼滤波器的设计n根据综合导航系统设计任务的要求不同，综合根据综合导航系统设计任务的要求不同，综合卡尔曼滤波器的设计步骤也不尽相同，但大体卡尔曼滤波器的设计步骤也不尽相同，但大体可分为三个阶段可分为三个阶段：n第一阶段：系统分析第一阶段：系统分析

10、n第二阶段：数值仿真第二阶段：数值仿真 n第三阶段：实验检验和完善设计第三阶段：实验检验和完善设计 综合卡尔曼滤波器设计中几个需要注意的综合卡尔曼滤波器设计中几个需要注意的问题：问题：n1、坐标系问题、坐标系问题n2、滤波器状态的可观测性问题、滤波器状态的可观测性问题n3、滤波器初值的确定、滤波器初值的确定n4、提高综合滤波器的自适应能力、提高综合滤波器的自适应能力2、滤波器状态的可观测性问题、滤波器状态的可观测性问题n惯性导航为主体的综合导航系统，其一部分状惯性导航为主体的综合导航系统，其一部分状态（如位置、速度误差等）可观测性好，有些态（如位置、速度误差等）可观测性好，有些状态（如加速度计

11、零偏、水平偏差角等）的可状态（如加速度计零偏、水平偏差角等）的可观测性差。这里由于这些状态之间存在较强的观测性差。这里由于这些状态之间存在较强的线性相关性，如果载体运动过程中姿态角变化线性相关性，如果载体运动过程中姿态角变化不大（如车辆、水面舰船），则加速度计零偏不大（如车辆、水面舰船），则加速度计零偏和水平姿态角几乎是不可分辨的。因此，在确和水平姿态角几乎是不可分辨的。因此，在确定滤波器的状态时，要充分注意这一点。根据定滤波器的状态时，要充分注意这一点。根据具体情况，舍弃某些难于观测又相对次要的状具体情况，舍弃某些难于观测又相对次要的状态。滤波系统的可观测性，可通过可观测矩阵态。滤波系统的可

12、观测性，可通过可观测矩阵加以判断。加以判断。3、滤波器初值的确定、滤波器初值的确定n理论上，应取理论上，应取n但一般但一般 和和 为未知，可选取为未知，可选取 ， 阵中各对角线元素可按系统状态的可阵中各对角线元素可按系统状态的可能分布情况选取。例如陀螺漂移和加速能分布情况选取。例如陀螺漂移和加速度计的零偏的大致分布范围我们是知道度计的零偏的大致分布范围我们是知道的，如果初始状态间有相联关系，则的，如果初始状态间有相联关系，则 阵中相应的非对角线元素不为零。阵中相应的非对角线元素不为零。000 xMXEX000XCXVarP0 xM0XC00X0P0P4、提高综合滤波器的自适应能力、提高综合滤波

13、器的自适应能力n由于设计的卡尔曼滤波器数学模型与实际系统的真实由于设计的卡尔曼滤波器数学模型与实际系统的真实模型不可能完全一致，因此滤波的实际效果（甚致仿模型不可能完全一致，因此滤波的实际效果（甚致仿真效果）往往不太理想。真效果）往往不太理想。n原因：滤波器的工作状态受实际物理系统的影响，而原因：滤波器的工作状态受实际物理系统的影响，而物理系统又随运动状态而有所变化，这种变化比较集物理系统又随运动状态而有所变化，这种变化比较集中地反映在系统噪声和测量噪声上。噪声方差甚至噪中地反映在系统噪声和测量噪声上。噪声方差甚至噪声性质都会发生变化，这可能导致滤波稳定性变坏。声性质都会发生变化，这可能导致滤

14、波稳定性变坏。n措施：可以设计一个简单的噪声方差估计器，估计器措施：可以设计一个简单的噪声方差估计器，估计器的输入是惯性传感器的输出，利用估计结果，动态调的输入是惯性传感器的输出，利用估计结果，动态调解滤波中的噪声方差，使卡尔曼滤波器具有自适应滤解滤波中的噪声方差，使卡尔曼滤波器具有自适应滤波性质，可使滤波效果得到改善。波性质，可使滤波效果得到改善。 4.4 GPS/惯性组合导航系统惯性组合导航系统 n4.4.1 GPS/惯性组合导航模式惯性组合导航模式n惯性导航系统由于其工作的完全自主性和导航惯性导航系统由于其工作的完全自主性和导航功能的完备性决定了在完全综合导航系统中的功能的完备性决定了在

15、完全综合导航系统中的主体地位。而主体地位。而GPS全球定位系统以其优良的测全球定位系统以其优良的测速定位性能、用户部分（速定位性能、用户部分（GPS接收机）的造价接收机）的造价低廉而作为综合导航系统的辅助系统，成为设低廉而作为综合导航系统的辅助系统，成为设计者的最佳选择。计者的最佳选择。GPS/惯性组合作为一种颇惯性组合作为一种颇为理想的综合方案而得到广泛应用。为理想的综合方案而得到广泛应用。nGPS/惯性综合系统克服了各自缺点，取长补惯性综合系统克服了各自缺点，取长补短，使综合后的导航精度高于两个系统单独工短，使综合后的导航精度高于两个系统单独工作的精度。作的精度。n综合系统的优点表现为：对

16、惯导系统可以实现综合系统的优点表现为：对惯导系统可以实现惯性传感器的校准等，从而可以有效地提高惯惯性传感器的校准等，从而可以有效地提高惯导系统的性能和精度；而惯导系统对导系统的性能和精度；而惯导系统对GPS的辅的辅助，提高了其跟踪卫星的能力，从而提高接收助，提高了其跟踪卫星的能力，从而提高接收机的动态特性和抗干扰能力机的动态特性和抗干扰能力。nGPS接收机和惯性导航系统的综合，根据不同接收机和惯性导航系统的综合，根据不同的应用技术，可以有不同方式的综合，即综合的应用技术，可以有不同方式的综合，即综合的深度不同。按照综合深度，可以把综合系统的深度不同。按照综合深度，可以把综合系统大体分为两类：一

17、类叫松散综合（大体分为两类：一类叫松散综合（Loose Coupling）或称简易综合（）或称简易综合（Easily Integration），另一类叫紧密综合（），另一类叫紧密综合（Tight Coupling）。）。1、松散综合、松散综合n这是一种低水平的综合，其主要特点是这是一种低水平的综合，其主要特点是GPS和和惯导仍独立工作，综合工作仅表现在用惯导仍独立工作，综合工作仅表现在用GPS辅辅助惯导。属于这类综合的有两种方式。助惯导。属于这类综合的有两种方式。n一、输出校正方式一、输出校正方式n1）用）用GPS给出的位置、速度信息直接调整惯给出的位置、速度信息直接调整惯导系统的输出。实际上

18、，就是在导系统的输出。实际上，就是在GPS工作期间，工作期间，惯导显示的是惯导显示的是GPS的位置和速度：的位置和速度：GPS停止工停止工作时，惯导在原显示的基础上变化，即作时，惯导在原显示的基础上变化，即GPS停停止工作瞬时的位置和速度作为惯导系统的初值。止工作瞬时的位置和速度作为惯导系统的初值。n2）把惯导和）把惯导和GPS输出的位置和速度信息进输出的位置和速度信息进行加权平均，其原理框图如图行加权平均，其原理框图如图6.6所示。在所示。在短时间工作的情况下，惯导精度较高。而长短时间工作的情况下，惯导精度较高。而长时间工作时，由于惯导误差随时间增长，因时间工作时，由于惯导误差随时间增长，因

19、此惯导输出的权随工作时间增加而减小。此惯导输出的权随工作时间增加而减小。二、用位置、速度信息综合（反馈校正方二、用位置、速度信息综合（反馈校正方式）式）n这是采用综合卡尔曼滤波器的一种综合模式，这是采用综合卡尔曼滤波器的一种综合模式，其原理框图如图其原理框图如图6.7所示。用所示。用GPS和惯导输出的和惯导输出的位置和速度信息的差值作为量测值，经综合卡位置和速度信息的差值作为量测值，经综合卡尔曼滤波，估计惯导系统的误差，然后对惯导尔曼滤波，估计惯导系统的误差，然后对惯导系统进行校正。系统进行校正。n紧密综合是一种高水平的综合，其主要特点是紧密综合是一种高水平的综合，其主要特点是GPS接收机和惯

20、导系统相互辅助。为了更好地接收机和惯导系统相互辅助。为了更好地实现两者之间的相互辅助，最好是把实现两者之间的相互辅助，最好是把GPS和惯和惯导系统按综合的要求进行一体化设计。属于紧导系统按综合的要求进行一体化设计。属于紧密综合的基本模式是伪距、伪距率的综合，以密综合的基本模式是伪距、伪距率的综合，以及在伪距、伪距率综合基础上再加上用惯导位及在伪距、伪距率综合基础上再加上用惯导位置和速度对置和速度对GPS接收机跟踪环路进行辅助，也接收机跟踪环路进行辅助，也可以再增加对可以再增加对GPS接收机导航功能的辅助。用接收机导航功能的辅助。用在高动态飞行器上的在高动态飞行器上的GPS/惯性综合系统通常惯性

21、综合系统通常都是采用紧密综合模式。都是采用紧密综合模式。2、紧密综合（或称深综合）、紧密综合（或称深综合）（1）用伪距、伪距率综合）用伪距、伪距率综合4.4.2 位置速度综合（位置速度综合（Loose Coupling）n4.4.2.1 综合系统的数学模型综合系统的数学模型n1、系统的状态方程、系统的状态方程n当综合系统采用线性卡尔曼滤波器时，则取系当综合系统采用线性卡尔曼滤波器时，则取系统的误差作为状态。统的误差作为状态。n（1）平台误差角方程）平台误差角方程n平台误差角方程在讲惯导系统时已经进行了讨平台误差角方程在讲惯导系统时已经进行了讨论。当考虑飞行高度论。当考虑飞行高度h和地球为旋转椭

22、圆球体和地球为旋转椭圆球体时，可重写如下：时，可重写如下：NNMNENEieNEieNEuNUMNENEieieNNrEUNEieNNEieNNEhRVhRVLLLhRVLLhRVhRVLhRVLLLhRVhRVLLhRVLhRV)cos()seccos(tg)tgsin(sin)cos()tgsin(2ENU式中角注式中角注E、N、U、代表东、北、天；代表东、北、天； )sin321 (2LffRReM)sin1 (2LfRReNmRe6378137257.298/1f速度误差方程速度误差方程（3）位置误差方程）位置误差方程位置误差方程位置误差方程（4）惯性仪表误差）惯性仪表误差n惯性仪表误

23、差包括安装误差、刻度系数误差和惯性仪表误差包括安装误差、刻度系数误差和随机误差。为了简单起见，这里只考虑随机误随机误差。为了简单起见，这里只考虑随机误差。差。n 陀螺漂移误差模型陀螺漂移误差模型n平台误差角误差方程中的陀螺漂移，是沿平台误差角误差方程中的陀螺漂移，是沿“东、东、北、天北、天”地坐标系的陀螺漂移。对平台式惯导地坐标系的陀螺漂移。对平台式惯导系统，当系统采用东北天地理坐标系时，则式系统，当系统采用东北天地理坐标系时，则式中的陀螺漂移即为实际陀螺的漂移。而对捷联中的陀螺漂移即为实际陀螺的漂移。而对捷联式惯导系统，则式中的陀螺漂移为从机体系变式惯导系统，则式中的陀螺漂移为从机体系变换到

24、地理系的等效陀螺漂移。换到地理系的等效陀螺漂移。 加速度计误差模型加速度计误差模型 n考虑为一阶马尔柯夫过程，且假定三个考虑为一阶马尔柯夫过程，且假定三个轴的加速度计的误差模型相同，均为轴的加速度计的误差模型相同，均为n式中式中 为相关时间。为相关时间。aT（5）GPS误差误差GPS接收机给出的位置和速度误差一般是时接收机给出的位置和速度误差一般是时间相关的，在位置、速度综合模式中这些误间相关的，在位置、速度综合模式中这些误差是量测噪声，所以噪声特性是有色的，而差是量测噪声，所以噪声特性是有色的，而且建模比较困难，不能用状态扩充法加以处且建模比较困难，不能用状态扩充法加以处理。常用的处理方法是

25、加大综合滤波器的迭理。常用的处理方法是加大综合滤波器的迭代周期。代周期。综合系统的状态方程为综合系统的状态方程为)()()()()(11111tWtGtXtFtXTzyxrzryrxbzbybxUNEUNEhLVVVX,TazayaxbzbybxgzgygxW,3333333333333939393333331000000000IIIG（对平台式系统）（对平台式系统） 2、系统的量测方程、系统的量测方程n在位置、速度综合模式中，其量测值有两组。在位置、速度综合模式中，其量测值有两组。一组为位置量测值，即惯导系统给出的经度、一组为位置量测值，即惯导系统给出的经度、纬度、高度信息和纬度、高度信息和

26、GPS接收机给出的相应信息接收机给出的相应信息的差值为一组量测值。而两个系统给出的速度的差值为一组量测值。而两个系统给出的速度差值为另一组量测值。差值为另一组量测值。n表示惯导系统的位置信息为表示惯导系统的位置信息为n表示表示GPS接收机给出位置信息为接收机给出位置信息为:n式中式中 为真实的位置，为真实的位置， 为为GPS接收机沿东、北、天方向的位置误差。接收机沿东、北、天方向的位置误差。tttLh、 、ENUNNN、定位位置量测矢量为定位位置量测矢量为:量测噪声作为白噪声处理，其方差分别为：量测噪声作为白噪声处理，其方差分别为：式中为伪距测量误差。表示惯导系统的速度信息式中为伪距测量误差。

27、表示惯导系统的速度信息为：为：n式中式中 是飞行器沿地理坐标系各轴的是飞行器沿地理坐标系各轴的真实速度。真实速度。GPS接收机给出的速度信息为接收机给出的速度信息为式中 为GPS接收机测量速误差定义速度量测矢量为用用 表示表示GPS接收机伪距率测量误差，则东、接收机伪距率测量误差，则东、北、天方向的速度误差标准差为：北、天方向的速度误差标准差为： n把位置量测矢量和速度量测矢量合在一起，得把位置量测矢量和速度量测矢量合在一起，得3、状态方程和量测方程的离散化、状态方程和量测方程的离散化n把状态方程式和量测方程式离散，可得把状态方程式和量测方程式离散，可得 n式中：式中：n式中式中T为迭代周期为

28、迭代周期在实际计算时取有限项即可在实际计算时取有限项即可状态方程和量测方程中的系统噪声和量测噪声具状态方程和量测方程中的系统噪声和量测噪声具有如下性质有如下性质 4.4.2.2 综合卡尔曼滤波器综合卡尔曼滤波器n综合卡尔曼滤波器是综合导航系统的核心。根综合卡尔曼滤波器是综合导航系统的核心。根据对系统校正方式不同，卡尔曼滤波器有开环据对系统校正方式不同，卡尔曼滤波器有开环校正即输出校正和闭环校正即反馈校正之分。校正即输出校正和闭环校正即反馈校正之分。n开环卡尔曼滤波器的状态方程中设有控制项，开环卡尔曼滤波器的状态方程中设有控制项，用卡尔曼滤波器对惯导系统的校正采用开环方用卡尔曼滤波器对惯导系统的

29、校正采用开环方式即输出校正，如图式即输出校正，如图6.9所示。惯导系统输出所示。惯导系统输出误差状态用误差状态用 表示，卡尔曼滤波器的估计值表示，卡尔曼滤波器的估计值用用 表示，则开环校正后的综合系统误差为表示，则开环校正后的综合系统误差为IXIXIIXXX 如果用滤波估计如果用滤波估计 进行开环校正后的系统误差为进行开环校正后的系统误差为n显然，显然， 也是卡尔曼滤波器的滤波估计误差。也是卡尔曼滤波器的滤波估计误差。即用滤波估计对系统进行开环校正，校正后的即用滤波估计对系统进行开环校正，校正后的系统精度和卡尔曼滤波器的精度相同系统精度和卡尔曼滤波器的精度相同.所以可所以可用卡尔曼滤波器的协方差来描述开环校正后的用卡尔曼滤波器的协方差来描述开环校正后的系统精度。这就是通常的协方差分析方法。系统精度。这就是通常的协方差分析方