1、卫 星 导 航 系 统Global Navigation Satellite System哈尔滨工程大学卫星导航系统课程组自动化学院第八章 船用惯性导航系统 与GPS的组合本章内容安排 概述概述 船用惯性导航系统船用惯性导航系统 卡尔曼滤波器卡尔曼滤波器 GPS/INSGPS/INS组合导航组合导航 双差分双差分GPS/INSGPS/INS组合导航系统组合导航系统 推算船位与推算船位与GPSGPS的组合的组合概述惯性导航系统（惯性导航系统（INSINS）：）：优点：优点： 完全自主的导航完全自主的导航 可提供多种导航信息可提供多种导航信息缺点：缺点： 精度逐渐降低。精度逐渐降低。 初始对准时间
2、长初始对准时间长卫星导航系统（卫星导航系统（GPSGPS）：）：优点：优点： 定位精度高定位精度高 精度不随时间变化精度不随时间变化缺点：缺点： 卫星信号不易捕获和跟踪卫星信号不易捕获和跟踪 抗干扰能力差易产生周跳抗干扰能力差易产生周跳因而被认为是目前导航领域和大地测量领域因而被认为是目前导航领域和大地测量领域最理想的组合方式。最理想的组合方式。GPSGPS系统与惯导系统具有互补的特点系统与惯导系统具有互补的特点概述 GPS/INSGPS/INS组合对改善系统精度有利组合对改善系统精度有利 GPS/INSGPS/INS组合加强系统的抗干扰能力组合加强系统的抗干扰能力 惯导系统提高惯导系统提高G
3、PSGPS接收机的跟踪能力接收机的跟踪能力 解决周跳问题解决周跳问题 组合系统将降低对惯导系统的要求组合系统将降低对惯导系统的要求 组合导航优点船用惯性导航系统船用平台式惯性导航系统船用平台式惯性导航系统 一般采用水平指北方案。由陀螺仪（三个单自由或二个双一般采用水平指北方案。由陀螺仪（三个单自由或二个双自由）构成稳定平台，跟踪并稳定在当地地理坐标系内。在惯性自由）构成稳定平台，跟踪并稳定在当地地理坐标系内。在惯性平台上安装两个敏感轴互相垂直、分别沿东西向和南北向放置的平台上安装两个敏感轴互相垂直、分别沿东西向和南北向放置的加速度计，用以测量这两个方向的加速度信息。加速度计，用以测量这两个方向
4、的加速度信息。船用惯性导航系统加速度计测量的是比力加速度计测量的是比力 ，即载体惯性力与地球引力之差：，即载体惯性力与地球引力之差：CC ()nneniiieiieVrrrC G CGnniininiifrr 表示从惯性坐标系表示从惯性坐标系i i到地理坐标系到地理坐标系n n的方向余弦矩阵，以下类的方向余弦矩阵，以下类同；同；r r为载体的地心位置矢量；为载体的地心位置矢量；G G为地球引力。为地球引力。Cni地理坐标系内的速度及加速度地理坐标系内的速度及加速度 ：C (2 )C nniiiiniiiienienieieVrVr则：则：(2 )nnnnnneniefVVg 及及 分别代表载体
5、运动的哥氏加速度项和离心加速度项；分别代表载体运动的哥氏加速度项和离心加速度项； 为地球引力与离心力之差，即重力。为地球引力与离心力之差，即重力。2nnieVnnenVngC Gniiiniieier船用惯性导航系统考虑在地球表面运动的船舶，忽略垂直速度得到考虑在地球表面运动的船舶，忽略垂直速度得到(2 )sin(2 )sinENieEnNNEiefVVVfVV一次积分一次积分：d(0)tnnnoVtVV第二次积分：第二次积分：d(0 )secd(0 )tNotEoVtRVtR 、 、 分别表分别表示速度矢量、纬度及经度示速度矢量、纬度及经度的初始值的初始值。(0)nV(0)(0)船用惯性导航
6、系统船用捷联式惯导系统船用捷联式惯导系统 捷联式惯性导航系统就是将惯性敏感元件直接安装在运载捷联式惯性导航系统就是将惯性敏感元件直接安装在运载体上，不再需要稳定平台和常平架结构的惯导系统。它通过体上，不再需要稳定平台和常平架结构的惯导系统。它通过计算机内的姿态矩阵实时解析计算，起到计算机内的姿态矩阵实时解析计算，起到“数学解析平台数学解析平台”的作用，把惯性导航系统中加速度计测量到的比力信息，转的作用，把惯性导航系统中加速度计测量到的比力信息，转换到导航动参考坐标系，并从姿态矩阵的有关元素中提取舰换到导航动参考坐标系，并从姿态矩阵的有关元素中提取舰船的姿态角（艏向角、横摇角和纵摇角）。船的姿态
7、角（艏向角、横摇角和纵摇角）。AccelerometerAccelerometerGyroscopeGyroscopeNavigationNavigationComputerComputerVehicle Vehicle accelerationaccelerationVehicleVehicleangular rateangular ratePositionPositionVelocityVelocityAttitudeAttitudeRateRateIntegrationIntegrationRateRateAccelAccelQuaternionQuaternionCalculation
8、CalculationCoordinateCoordinateTransformTransformVelocityVelocityIntegrationIntegrationEuler AngleEuler AngleCalculationCalculationAccelerateAccelerateIntegrationIntegration船用惯性导航系统船用惯性导航系统误差方程的推导船用惯性导航系统误差方程的推导 1. 1.平台姿态误差方程平台姿态误差方程nn nnninin 21,sin,(seccos)tgnNEEinieieEVVVVRRRR n陀螺漂移陀螺漂移 在地理坐标系的投影
9、：在地理坐标系的投影：nnnbC平台惯导：平台惯导：捷联惯导：捷联惯导：nbC船体坐标系到地理坐标系的方向余弦阵：船体坐标系到地理坐标系的方向余弦阵：船用惯性导航系统2. 2. 速度误差方程速度误差方程 采用摄动法，得到速度误差方程：采用摄动法，得到速度误差方程：(2)(2)nnnnnnnnnenieenieVVVgf 重力无异常时，重力无异常时，0ngnnenie和nnenie和2tgsecnNEEenEVVVVRRRR ，0sincosnieieie，nf的反对称矩阵的反对称矩阵比力误差在地理坐标系的投影，主要是由加速度计零位误差和平比力误差在地理坐标系的投影，主要是由加速度计零位误差和平
10、台（或捷联式惯导的台（或捷联式惯导的“数学平台数学平台”）偏离地理坐标系所致）偏离地理坐标系所致 船用惯性导航系统3. 3.位置误差方程位置误差方程 : :采用摄动法，直接得到位置误差方程：采用摄动法，直接得到位置误差方程：cossectgNNEVRVVRR 卡尔曼滤波器 卡尔曼滤波算法是一种线性最小方差估计的递推算法，相对卡尔曼滤波算法是一种线性最小方差估计的递推算法，相对其它几种最优估计方法，卡尔曼滤波具有如下特点：其它几种最优估计方法，卡尔曼滤波具有如下特点：l算法是递推的算法是递推的l采用动力学方程描述被估计量的动态变化规律采用动力学方程描述被估计量的动态变化规律l卡尔曼滤波具有连续型
11、和离散型两类算法卡尔曼滤波具有连续型和离散型两类算法 卡尔曼滤波器离散系统的卡尔曼滤器 ,1111kk kkkkxx 1kkkkzH xk量测方程量测方程 动态方程动态方程 状态预测估计状态预测估计状态估计状态估计 方差迭代方差迭代滤波增益滤波增益 方差预测方差预测 /1,11k kk kkxx/1/1()kk kkkk k kxxK zH x1/1/1/1/1/1kk kk kkkk kkkkk kkkk kPPPH H PHRH PIK H P1/1/1kk kkkk kkkHPHH PHR/1,11,1111k kk kkk kkkkPPQ 卡尔曼滤波器卡尔曼滤波是一种递推算法，启动时必
12、须先给定初值和。卡尔曼滤波是一种递推算法，启动时必须先给定初值和。计算估计的均方误差阵有三种等价形式：计算估计的均方误差阵有三种等价形式：卡尔曼滤波估计卡尔曼滤波估计 是无偏估计是无偏估计 与与 成正比，而与成正比，而与 成反比成反比/1()()TTkkkk kkkkkkPIK HPIK HK R K/1()()TTkkkk kkkkkkPIK HPIK HK R K111/1Tkk kkkkPPH R HkxkK1kQkP卡尔曼滤波的性质卡尔曼滤波器白噪声情形下一般线性系统滤波 /1/1()kk kkkkkk kxxKzyH x/1,111111111()k kk kkkkkkkkkxxBu
13、JzyHx 1/1/1()kk kkkk kkkKPHH PHR/1()kkkk kPIK HP/1,1111,11111111111111()()k kk kkkkk kkkkkkkkkkkkkPJHPJHQJR JJSR 预测计算预测计算 滤波计算滤波计算 滤波增益滤波增益滤波误差协方差滤波误差协方差 预测误差协方差预测误差协方差 ,111111kk kkkkkkxxB ukkkkkzH xy动态方程动态方程量测方程量测方程 GPS/INS 组合系统 在船用惯性导航系统的使用中，通过对常值陀螺漂移的补偿，在船用惯性导航系统的使用中，通过对常值陀螺漂移的补偿，消除了常值陀螺漂移造成的系统位置
14、、速度和姿态误差，但随机消除了常值陀螺漂移造成的系统位置、速度和姿态误差，但随机漂移的影响依然存在。惯导和漂移的影响依然存在。惯导和GPSGPS组合滤波，在对系统输出作最组合滤波，在对系统输出作最优估计的同时，还可对随机的漂移进行估计和补偿，从而提高惯优估计的同时，还可对随机的漂移进行估计和补偿，从而提高惯导系统本身的精度。导系统本身的精度。GPS/INS 组合系统惯导系统的误差方程 22211tgsecsec112cossectg1(2sintg)2cossec( 2sin2tg )coNEEEieNENNEieNENyEENieExieENieVRVVRRVVV VVVRRVVVAgRVV
15、VVgAVRRVR 2ssintg11sinsintg1cossectgcosEEiexNieEieEyNEEieEiezVVRRVVVRRRVVVVRRRR 状态方程状态方程GPS/INS 组合系统 采用采用GPSGPS伪距误差为观测量，因此需对伪距误差为观测量，因此需对GPSGPS接收机的时钟接收机的时钟偏差进行估计。时钟偏差用一阶马尔可夫过程描述：偏差进行估计。时钟偏差用一阶马尔可夫过程描述：uuuutt 为白噪声 u于是，选择状态变量为:TENxyzuTINSGPSxVVtxx 组合系统的状态方程可以描述为组合系统的状态方程可以描述为: :( )( )( )( )x tF tx tw
16、t11 11( )0( )0( )INSGPSFtF tFt其中 GPS/INS 组合系统测量方程测量方程 GPS GPS伪距表达式为伪距表达式为: :12222()()() iiiiiiiGGGuiXXYYZZdtV采用变分法采用变分法, ,导出伪距误差方程：导出伪距误差方程： 123iiiiuieXeYeZtV12312222()()() iiiGiiiiiiGiiiiiiGiiiiiiiiiiGGGXXeXrYYeYrZZeZrrXXYYZZ为为GPSGPS接收机时钟偏差等效的距离误差接收机时钟偏差等效的距离误差 ， 为测量白噪声为测量白噪声 utiV其中其中 把伪距误差把伪距误差 表示
17、成经纬度坐标系中的位置误差表示成经纬度坐标系中的位置误差 、 的函数的函数有有 下式成立：下式成立：GPS/INS 组合系统i111211121321221222233132133233sin cossin cossin sincos coscos0CCeeeRRCCCeeeRRRCCeee 对舰船高度不进行估计,观测方程为：( )( ) ( )( )z tH t x tv t112233( )GCGCGCz t111221223 831321( )011CCH tCCCC ：GPSGPS接收机输出的用户至第接收机输出的用户至第i i颗卫星的伪距；颗卫星的伪距； ：惯导的定位信息和卫星星历提供
18、的卫星位置所确定的伪距；：惯导的定位信息和卫星星历提供的卫星位置所确定的伪距； ：高斯白噪声：高斯白噪声 。(1,2,3)iGi(1,2,3)iCi( )v t双差分GPS/INS组合导航系统 差分差分GPSGPS技术是利用技术是利用GPSGPS定位误差的相关特性，消去公共误差定位误差的相关特性，消去公共误差源的影响，从而达到提高定位精度的目的。源的影响，从而达到提高定位精度的目的。 伪距双差分伪距双差分GPSGPS的数学模型：的数学模型： klklrururuklklrruumnoisePPPd 11221122,mrmultumultrumltumultnoiserurudddddVVVV
19、V载波相位双差分GPS的数学模型: klklklklrurruumnoiserudNklklklrurruuNNNNN双差分GPS/INS组合导航系统状态方程：状态方程：( )( )( )( )x tFtx tt观测方程：观测方程：11( )()()( )klklklrurrucuczkPvk 用户接收机的位置误差用户接收机的位置误差 、 、 表示观测方程：表示观测方程：( XY)Z11111( )( )fffz kXYZv kXYZ1klkluukluufYyYyYYY 1klkluukluufXxXxXXX1klkluukluufZzZzZZZ双差分GPS/INS组合导航系统的设计 推算船
20、位与GPS的组合 推算船位和推算船位和GPSGPS构成的组合系统，由于其状态方程不再构成的组合系统，由于其状态方程不再是线性的，因而必须采用推广的卡尔曼滤波器处理。是线性的，因而必须采用推广的卡尔曼滤波器处理。推广卡尔曼滤器 推广的卡尔曼滤波器是通过对现时状态估计进行线性化推广的卡尔曼滤波器是通过对现时状态估计进行线性化的方法，解决非线性系统的滤波问题。的方法，解决非线性系统的滤波问题。应当指出，推广的卡应当指出，推广的卡尔曼滤波器实际上并不是最优的，使用时仍有可能发散。但尔曼滤波器实际上并不是最优的，使用时仍有可能发散。但由于它与普通卡尔曼滤波器的相似性，一般把推广的卡尔曼由于它与普通卡尔曼
21、滤波器的相似性，一般把推广的卡尔曼滤波器作为对任意非线性滤波问题首先试用的方法之一。滤波器作为对任意非线性滤波问题首先试用的方法之一。 推广卡尔曼滤器非线性系统的推广卡尔曼滤波递推非线性系统的推广卡尔曼滤波递推/1/1(, )kk kkkk kxxKzh xk/11(,1)k kkxxk 1/1/1/1/1/1()()TTkkkkk kk kkk kk kk khhhKPPRxxx11/1111111()(,1)(,1)TTkkk kkkkkkkPPxkQxkxx/1/1()kkkk kk khPIKPx0000,xEx PVarx初值推广卡尔曼滤器推广卡尔曼滤器结构图推广卡尔曼滤器结构图i.
22、c.uttki.c.增益更新状态更新协方差更新状态模型ut协方差模型Pxkx kPkP( )kx ( )kKkx ( )kx ( )kx ( )kP( )kP( )zkP( )kx ( )观测输出kt推算船位与GPS的组合推算船位与推算船位与GPSGPS组合系统建模组合系统建模 舰船运动的动态方程舰船运动的动态方程: :1234567cossinNEEENNVSHVSHVVVVSH 矢量形式：矢量形式：( )( ( ), )( )x tx ttW t 推算船位与GPS的组合初始条件初始条件1111(),()kkkktt1111(),()EkEkNkNkV tVV tV1111(),()kkkk
23、S tSH tH11()kkt状态转移矩阵状态转移矩阵111111,11100cossin010 10sincos00 000000 000000 0001000 000010 000001TkkkTkkkTk kTeHTSHTeHTSHTeet 当采样间隔较短时，当采样间隔较短时，H H与与S S通常变化很小，可以近似为常值通常变化很小，可以近似为常值推算船位与GPS的组合 根据水面舰船的常规配置，选择电磁计程仪的速度根据水面舰船的常规配置，选择电磁计程仪的速度VLVL、电罗、电罗经的航向信息经的航向信息HGHG以及以及GPSGPS的位置信息的位置信息 、 作为观测变量，其作为观测变量，其中位置信息需转换到地球坐标系：中位置信息需转换到地球坐标系：(m)mcoscmcmmRR观测方程：观测方程：1234ccLGVVVSVHHV式中式中V Vi i（i i=1=1，2 2，3 3，4 4）为测量噪声，反映观测量的随机干扰特性。）为测量噪声，反映观测量的随机干扰特性。推算船位与GPS的组合 计程仪和罗经的测量噪声较为简单，均可视为零均值白计程仪和罗经的测量噪声较为简单，均可视为零均值白噪声。于是推算船位噪声。于是推算船位/GPS/GPS组合系统观测方程的测量阵可写为组合系统观测方程的测量阵可写为1 0 000000 1 000000 0 001000 0 00010H
