航位推算在导航定位中的应用

航位推算在导航定位中的应用摘要：只限于GPS导航往往会有盲点，DR导航的自主性恰能弥补GPS导航的不足。本文阐述了在得到一组GPS导航数据的基础上如何利用航位推算进行导航。通过卡尔曼滤波对航向角数据进行处理，比较滤波前后载体的运动轨迹，分析产生误差的主要原因。结果表明：matlab仿真出的载体运动轨迹与实验时的罗盘方向轨迹相同，利用卡尔曼滤波能够有效减小误差。1引言DR导航时一种自主式导航，成本相对GPS较低，在短时间内能够提供比较精确的导航参数，抗干扰能力强，隐蔽性好。但是误差随导航时间积累是DR导航的致命缺点，因此DR导航不适合长时间的单独导航。GPS导航精度高、功能强、使用方便，但在应用于车辆导航时，当车辆在树荫下、涵洞、隧道里、深山峡谷内，特别在城市里行驶时，高楼大厦会挡住卫星的信号，使卫星导航接收机不能正常接收卫星信号，不能正常定位[1]。由此可见，将两种导航方式结合起来共同完成导航功能是提高导航精度的一个主要手段，具有重大意义。卡尔曼滤波在导航方面有很重要的作用，最传统的导航融合是集中式Kalman滤波，现在应用比较广泛的是自适应扩展卡尔曼滤波，利用系统预测量、系统状态噪声量、观测量与观测噪声几个量实现滤波，在信息融合中，利用信息分配原理，自适应调整GPS和DR观测量在滤波中的权重。自适应滤波根据自适应因子作用范围不同分为单因子自适应滤波和多因子自适应滤波[2]。东北θ(xi,yi)cosivsiniviv图1航位推算原理2航位推算原理航位推算原理是利用载体的速度、航向以及上一时刻的位置来估计下一时刻载体的位置。利用载体的速度和航向能够得出速度在当地水平坐标轴上分向速度，将分向速度和载体所经过的时间相乘便可以得到载体在坐标轴上增加的坐标值，与前一时刻的坐标值求和便可以得到此时的坐标值。原理图如图1所示。推算如公式(1)。错误!未找到引用源。…………………..…………(1)其中，(错误!未找到引用源。)为载体在错误!未找到引用源。时刻的初始位置，(错误!未找到引用源。)为载体在错误!未找到引用源。时刻位置，错误!未找到引用源。、错误!未找到引用源。为载体在错误!未找到引用源。时刻的速度与航向角（航向角为载体运动方向与正东方向的夹角）。错误!未找到引用源。为罗盘相邻两次推算所用的时间差。3坐标系之间的转化3.1所涉坐标系的种类3.1.1当地水平坐标系当地水平坐标系又名东北天坐标系，它的坐标原点为载体的质心，X轴指向当地北子午线，Y轴垂直X轴指向东，Z轴垂直XOY面构成右手直角坐标系。3.1.2大地坐标系地球椭球的中心与地球质心重合，椭球的短轴与地球的自转轴重合，大地纬度B为地面点的椭球法线与椭球赤道面的夹角，大地经度L为过地面点的椭球子午面与格林威治起始子午面的夹角，大地高H为地面点沿椭球法线至椭球面的距离。地面一点可表示为(L,B,H)。如图3.1所示。0起初子午面赤道BLHA（B，L，H）图3.1大地坐标系B:大地纬度，通过观测站的参考椭球面的法线与赤道面的夹角。L:大地经度，通过观测站的大地子午面与本初子午面得夹角。H:大地高度，观测站沿法线到参考椭球面的距离。3.1.3WGS-84坐标系GPS使用的坐标系，该坐标系属于协议地球坐标系，其原点位于地球质心，Z轴指向BIH1984.0定义的协议地级方向，X轴指向BIH1984.0的零子午面和CTP赤道的交点，Y轴与Z轴构成右手坐标系[3]。GPS定位结果属于协议地球地心坐标系，即84坐标系，且通常以空间直角坐标（X，Y，Z），或以椭球大地坐标（B,L,H）的形式给出。而实用的常规地面测量成果或是属于国家的参心大地坐标系，或是属于地方独立坐标系。因此必须实现GPS成果的坐标系的转换。3.2坐标系转换3.2.1当地水平坐标系转化为空间直角坐标系当地水平坐标系与84坐标系同属于直角坐标系，故相互转化时需要利用地心大地坐标系的参数，将84坐标系下（以大地坐标（B,L,H））测得的GPS量作为载体的质心，然后经过旋转矩阵将东北天坐标系（当地水平坐标系、站心坐标系）转化为空间直角坐标系。旋转点作为载体的质心，以（B,L,H）的值为起点。将坐标系分别绕Z、X、Y轴逆时针旋转α、β、γ角所使用的旋转矩阵如下：错误!未找到引用源。=错误!未找到引用源。…………………………………..(2)错误!未找到引用源。=错误!未找到引用源。…………………………………(3)错误!未找到引用源。=错误!未找到引用源。………………………..………(4)由于航位推算是在东北天坐标系下进行的，所以需要需变换到大地坐标系下，显示经纬度信息，先将东北天坐标系转换到直角坐标系下，然后再有直角坐标系转化为大地坐标系，将在84坐标系（大地坐标（B、L、H）形式表示）下测得的GPS值转化为直角坐标系（X、Y、Z）下表示，作为直角坐标系下初值，转化公式如下：错误!未找到引用源。………………………..…….(5)现将Y轴反向得错误!未找到引用源。轴。绕错误!未找到引用源。轴逆时针旋转错误!未找到引用源。)，再绕Z轴旋转错误!未找到引用源。)，即可将东北天坐标系（地平站心坐标系）转化为站心赤道直角坐标系，公式如下：错误!未找到引用源。………..………..(6)R=错误!未找到引用源。=错误!未找到引用源。*错误!未找到引用源。*错误!未找到引用源。=错误!未找到引用源。所以空间直角坐标系和站心赤道直角坐标系之间有简单的平移关系，所以将站心赤道坐标系转化为空间直角坐标系之间的转化关系为：错误!未找到引用源。=错误!未找到引用源。………………..(7)所以与东北天坐标系之间的转化关系为：错误!未找到引用源。=错误!未找到引用源。(8)3.2.2直角坐标系转化为大地坐标系直角坐标系转化为大地坐标系的转化公式为：错误!未找到引用源。…………………………………….(8)其中：错误!未找到引用源。，N为该点的卯酉圈曲率半径；错误!未找到引用源。，a、b、e分别为该大地坐标系对应参考椭球的长半轴、短半轴和第一偏心率。长半轴a=63781372m，短半轴b=6356.7523142km，90130066943799.02e。此方法H和B需要迭代求得，而且应用条件受到很大限制，如要求大地高度需要小于1000m时，才能使B、L迭代四次之后精度分别达到错误!未找到引用源。和0.001m，所以本文一种计算精度高，公式形式简便的方法来计算B和L[4]。错误!未找到引用源。……………..(9)4电子罗盘HMR30004.1概述HMR3000包含三个磁阻传感器和一个双轴的液体横滚传感器，并带有A/D转换电路和微处理器。微处理器控制传感器的测量顺序，控制器操作的参数存储在EEPROM中，输出3类NMEA0813标准语句(HDG、HDT和XDR)，3类专用语句(HPR、RCD和CCD)，HPR是最常用的格式，采用RS232接口功能模块如图4-1所示。两轴倾斜传感器三轴磁场计模拟驱动电路稳压电路单片机RS-232EEPROMRXGNDTXVCC输入12V图4-1HMR3000功能框图4.2HMR3000特性数字电子罗盘HMR3000是Honeywell公司的产品。它可以为导航定位系统提供航向、俯仰、横滚等数据。其中航向的精度为±错误!未找到引用源。，分辨率可达±错误!未找到引用源。。横滚和俯仰的精度为±错误!未找到引用源。，分辨率为±错误!未找到引用源。，输出端口采用RS232。HMR3000的串行通信是根据NMEA0183标准制定的简单、异步的ASCII协议[5]。ASCII码的传输和接收使用1位起始位、8位数据位，无奇偶校验位和1位停止位。每一个码有10位。波特率可选择1200,2400,4800,9600或38400。HMR3000支持NMEA0183和专用的信息。在测量模式中有HMR3000按照EEPROM中的编程的速率主动发送NMEA信息。本次试验输出是专用HPR句子，它包含了航向、俯仰、横滚等信息。格式如下：HPRHeading,Pitch&Roll（航向，俯仰和横滚）$PTNTHPR,x.x,a,x.x,a,x.x,a*hhcrlf这个句子把HMR3000的三个重要的测量结果和有用的状态、俯仰、俯仰状态、横滚、横滚状态。如本次试验的一个句子为：$PTNTHPR,199.3,N,0.3,N,0.2,N*37此句说明：罗盘所测航向角为：错误!未找到引用源。，正常状态，俯仰角为错误!未找到引用源。，正常状态，横滚角为错误!未找到引用源。，正常状态。5卡尔曼滤波1960年，R.E.Kalman首次提出的卡尔曼滤波是一种线性最小方差估计。卡尔曼滤波理论成功地解决了美国阿波罗登月中的多导航传感器组合导航的难题，引起工程界的广泛重视。之后，卡尔曼滤波理论开始在导航系统中得到普遍的应用。卡尔曼滤波的基本思想：以最小均方误差为估计准则，采用信号与误差的状态空间模型，利用前一时刻的估计值和当前的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的的估计[6]。由于卡尔曼滤波方程具有易编程的优点，所以卡尔曼滤波器在导航方面应用十分广泛，且能够很好地提高导航的精度。卡尔曼滤波是一种最优化自回归数据处理方式，在了解状态方程、观测方程以及系统噪声的情况下，通过卡尔曼滤波的五个公式实现罗盘航向角的最优化回归处理，将k-1时刻的预测值以及预测偏差和k时刻的量测值以及量测偏差结合起来，通过计算卡尔曼增益将这5个式子结合起来实现递推。下面详细介绍一下这五个式子的含义：首先我们要根据k-1时刻的航向角来预测k时刻的航向角，根据k-1时刻的状态方程可以得到k时刻的预测航向角如公式(10)：错误!未找到引用源。……………………………………(10)其中错误!未找到引用源。是根据k-1时刻航向角预测k时刻的航向角，A、B为系统参数矩阵，错误!未找到引用源。为k-1时刻的航向角，错误!未找到引用源。为控制量，此时为零向量。我们用错误!未找到引用源。(covariance)来表示通过k-1时刻方差阵预测的k时刻方差阵（预测值的偏差），错误!未找到引用源。表示k-1时刻的航向角的偏差，他们的关系如公式(11)：错误!未找到引用源。………………………………………(11)Q为过程噪声的covariance，假设k时刻航向角的量测值为错误!未找到引用源。，现在我们可以得到k时刻的最优化航向角估计值如公式(12)：错误!未找到引用源。……………………………………(12)H为测量系统参数，错误!未找到引用源。为k时刻的卡尔曼增益，求解如公式13：错误!未找到引用源。……………………………(13)R为观测噪声的covariance，为了能够以此往下面迭代，故必须求出k时刻航向角的偏差，即求出错误!未找到引用源。。如公式(14)：错误!未找到引用源。(1-错误!未找到引用源。)*错误!未找到引用源。……………………………………(14)然后再根据k时刻的参数值求解k+1时刻的航向角，这样就可以实时求解航向角的大小。至于编程时错误!未找到引用源。初始参数的确定可以任意确定一个就行，因为卡尔曼滤波算法会使这个参数逐渐收敛的，但是错误!未找到引用源。最好别取零，防止卡尔曼滤波把这个当成最优解。实验中，采用卡尔曼滤波器对罗盘输出的航向值进行滤波，滤波器的输出如图5.1所示。010002000300040005000600070008000900010000-50050100150200250300350航向角滤波数据与原始数据对比滤波数据原始数据图5.1航向角滤波前后对比滤波后航向角的相对误差如图5.2所示。010002000300040005000600070008000900010000024681012141618滤波数据与原始数据相对误差图5.2滤波数据与原始数据相对误差从图5.1可以看出，在滤波时给出的初值和刚开始的航向角值差别比较大，卡尔曼滤波能很快收敛到精确的航向角，在罗盘每次改变方向的时候，卡尔曼滤波