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

1、航位推算在导航定位中的应用摘要：只限于GPS导航往往会有盲点，DR导航的自主性恰能弥补GPS导航的 不足。本文阐述了在得到一组GPS导航数据的基础上如何利用航位推算进行导 航。通过卡尔曼滤波对航向角数据进行处理，比较滤波前后载体的运动轨迹， 分析产生误差的主要原因。结果表明：matlab仿真出的载体运动轨迹与实验时 的罗盘方向轨迹相同，利用卡尔曼滤波能够有效减小误差。1引言DR导航时一种自主式导航，成本相对GPS较低，在短时间内能够提供比 较精确的导航参数，抗干扰能力强，隐蔽性好。但是误差随导航时间积累是 DR导航的致命缺点，因此DR导航不适合长时间的单独导航。GPS导航精度 高、功能强、使用

2、方便，但在应用于车辆导航时，当车辆在树荫下、涵洞、隧 道里、深山峡谷内，特别在城市里行驶时，高楼大厦会挡住卫星的信号，使卫 星导航接收机不能正常接收卫星信号，不能正常定位1。由此可见，将两种导 航方式结合起来共同完成导航功能是提高导航精度的一个主要手段，具有重大 意义。卡尔曼滤波在导航方面有很重要的作用，最传统的导航融合是集中式 Kalman滤波，现在应用比较广泛的是自适应扩展卡尔曼滤波，利用系统预测 量、系统状态噪声量、观测量与观测噪声几个量实现滤波，在信息融合中，利 用信息分配原理，自适应调整GPS和DR观测量在滤波中的权重。自适应滤波 根据自适应因子作用范围不同分为单因子自适应滤波和多因

3、子自适应滤波。图1航位推算原理2航位推算原理航位推算原理是利用载体的速度、航向以及上一时刻的位置来估计下一时 刻载体的位置。利用载体的速度和航向能够得出速度在当地水平坐标轴上分向 速度，将分向速度和载体所经过的时间相乘便可以得到载体在坐标轴上增加的 坐标值，与前一时刻的坐标值求和便可以得到此时的坐标值。原理图如图1所 示。推算如公式(1)。错误！未找到引用源。 （1）其中，（错误！未找到引用源。）为载体在错误！未找到引用源。时刻的初始位 置，（错误！未找到引用源。）为载体在错误！未找到引用源。时刻位置，错 误！未找到引用源。、错误！未找到引用源。为载体在错误！未找到引用源。时刻的速度与航向角（

4、航向角为载体运动方向与正东方向的夹角）。错误！未找 到引用源。为罗盘相邻两次推算所用的时间差。3坐标系之间的转化3.1所涉坐标系的种类3.1.1当地水平坐标系当地水平坐标系又名东北天坐标系，它的坐标原点为载体的质心，X轴指 向当地北子午线，Y轴垂直X轴指向东，Z轴垂直XOY面构成右手直角坐标 系。3.1.2大地坐标系地球椭球的中心与地球质心重合，椭球的短轴与地球的自转轴重合，大地 纬度B为地面点的椭球法线与椭球赤道面的夹角，大地经度L为过地面点的椭 球子午面与格林威治起始子午面的夹角，大地高H为地面点沿椭球法线至椭球 面的距离。地面一点可表示为（L,B,H）。如图3.1所示。图3.1大地坐标系

5、B:大地纬度，通过观测站的参考椭球面的法线与赤道面的夹角。L:大地经度，通过观测站的大地子午面与本初子午面得夹角。H:大地高度，观测站沿法线到参考椭球面的距离。3.1.3 WGS-84 坐标系GPS使用的坐标系，该坐标系属于协议地球坐标系，其原点位于地球质 心，Z轴指向BIH1984.0定义的协议地级方向，X轴指向BIH1984.0的零子午面和 CTP赤道的交点，Y轴与Z轴构成右手坐标系。GPS定位结果属于协议地球地心坐标系，即84坐标系，且通常以空间直角坐标（X, Y, Z）,或以椭球大地 坐标（B,L,H ）的形式给出。而实用的常规地面测量成果或是属于国家的参心大 地坐标系，或是属于地方独

6、立坐标系。因此必须实现 GPS成果的坐标系的转 换。3.2坐标系转换3.2.1当地水平坐标系转化为空间直角坐标系当地水平坐标系与84坐标系同属于直角坐标系，故相互转化时需要利用地 心大地坐标系的参数，将84坐标系下（以大地坐标（B,L,H）测得的GPS量 作为载体的质心，然后经过旋转矩阵将东北天坐标系（当地水平坐标系、站心 坐标系）转化为空间直角坐标系。旋转点作为载体的质心，以（B,L,H）的值为 起点。将坐标系分别绕Z、X、Y轴逆时针旋转a、0、y角所使用的旋转矩阵如 下： TOC o 1-5 h z 错误！未找到引用源。=错误！未找到引用源。（2）错误！未找到引用源。=错误！未找到引用源。

7、（3）错误！未找到引用源。=错误！未找到引用源。（4）由于航位推算是在东北天坐标系下进行的，所以需要需变换到大地坐标系下， 显示经纬度信息，先将东北天坐标系转换到直角坐标系下，然后再有直角坐标 系转化为大地坐标系，将在84坐标系（大地坐标（B、L、H）形式表示）下测 得的GPS值转化为直角坐标系（X、Y、Z）下表示，作为直角坐标系下初值， 转化公式如下：错误！未找到引用源。（5）现将Y轴反向得错误！未找到引用源。轴。绕错误！未找到引用源。轴逆时针 旋转错误！未找到引用源。），再绕Z轴旋转错误！未找到引用源。），即可将 东北天坐标系（地平站心坐标系）转化为站心赤道直角坐标系，公式如下：错误！未找

8、到引用源。R=错误！未找到引用源。=错误！未找到引用源。*错误！未找到引用源。*错误！未找到引用源。=错误！未找到引用源。-sinBcasL sinBsinL cosBsinLcosL-sinBcasL sinBsinL cosBsinLcosLcos B cost cosBsinL sinB空间直角坐标系和站心赤道直角坐标系之间有简单的平移关系，所以将站心赤 道坐标系转化为空间直角坐标系之间的转化关系为：错误！未找到引用源。=错误！未找到引用源。所以与东北天坐标系之间的转化关系为：错误！未找到引用源。=错误！未找到引用源。（8）3.2.2直角坐标系转化为大地坐标系直角坐标系转化为大地坐标系的

9、转化公式为：错误！未找到引用源。（8）其中：错误！未找到引用源。，N为该点的卯酉圈曲率半径；错误！未找到引 用源。，a、b、e分别为该大地坐标系对应参考椭球的长半轴、短半轴和第一偏 心率。长半轴 a=6378137 + 2m， 短半轴 b=6356.7523142km， e2 = 0.00669437999013。此方法H和B需要迭代求得，而且应用条件受到很大限制，如要求大地高 度需要小于1000m时，才能使B、L迭代四次之后精度分别达到错误！未找到 引用源。和0.001m，所以本文一种计算精度高，公式形式简便的方法来计算B 和 L4。错误！未找到引用源。（9）4电子罗盘HMR30004.1概

10、述HMR3000包含三个磁阻传感器和一个双轴的液体横滚传感器，并带有A/D 转换电路和微处理器。微处理器控制传感器的测量顺序，控制器操作的参数存 储在EEPROM中，输出3类NMEA0813标准语句（HDG、HDT和XDR），3类 专用语句（HPR、RCD和CCD），HPR是最常用的格式，采用RS232接口功能模 块如图4-1所示。图4-1 HMR3000功能框图4.2 HMR3000 特性数字电子罗盘HMR3000是Honeywell公司的产品。它可以为导航定位系统 提供航向、俯仰、横滚等数据。其中航向的精度为土错误！未找到引用源。，分 辨率可达土错误！未找到引用源。横滚和俯仰的精度为土错误

11、！未找到引用 源。，分辨率为土错误！未找到引用源。，输出端口采用RS232。HMR3000的 串行通信是根据NMEA0183标准制定的简单、异步的ASCII协议。ASCII码 的传输和接收使用1位起始位、8位数据位，无奇偶校验位和1位停止位。每一 个码有10位。波特率可选择1200,2400,4800,9600或38400。HMR3000支持NMEA0183和专用的信息。在测量模式中有HMR3000按照 EEPROM中的编程的速率主动发送NMEA信息。本次试验输出是专用HPR句 子，它包含了航向、俯仰、横滚等信息。格式如下：HPR Heading, Pitch & Roll （航向，俯仰和横滚

12、）$PTNTHPR,x.x,a,x.x,a,x.x,a*hh 这个句子把HMR3000的三个重要的测量结果和有用的状态、俯仰、俯仰状 态、横滚、横滚状态。如本次试验的一个句子为：$PTNTHPR,199.3,N,0.3,N,0.2,N*37此句说明：罗盘所测航向角为：错误！未找到引用源。，正常状态，俯仰角为 错误！未找到引用源。，正常状态，横滚角为错误！未找到引用源。，正常状 态。5卡尔曼滤波1960年，R.E.Kalman首次提出的卡尔曼滤波是一种线性最小方差估计。卡 尔曼滤波理论成功地解决了美国阿波罗登月中的多导航传感器组合导航的难 题，引起工程界的广泛重视。之后，卡尔曼滤波理论开始在导航

13、系统中得到普 遍的应用。卡尔曼滤波的基本思想：以最小均方误差为估计准则，采用信号与 误差的状态空间模型，利用前一时刻的估计值和当前的观测值来更新对状态变 量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需 要处理的信号做出满足最小均方误差的的估计。由于卡尔曼滤波方程具有易编程的优点，所以卡尔曼滤波器在导航方面应 用十分广泛，且能够很好地提高导航的精度。卡尔曼滤波是一种最优化自回归 数据处理方式，在了解状态方程、观测方程以及系统噪声的情况下，通过卡尔 曼滤波的五个公式实现罗盘航向角的最优化回归处理，将k-1时刻的预测值以 及预测偏差和k时刻的量测值以及量测偏差结合起来，通过计算

14、卡尔曼增益将 这5个式子结合起来实现递推。下面详细介绍一下这五个式子的含义：首先我们要根据k-1时刻的航向角来预测k时刻的航向角，根据k-1时刻的 状态方程可以得到k时刻的预测航向角如公式(10)：错误！未找到引用源。(10)其中错误！未找到引用源。是根据k-1时刻航向角预测k时刻的航向角，A、B 为系统参数矩阵，错误！未找到引用源。为k-1时刻的航向角，错误！未找到 引用源。为控制量，此时为零向量。我们用错误！未找到引用源。(covariance) 来表示通过k-1时刻方差阵预测的k时刻方差阵(预测值的偏差)，错误！未 找到引用源。表示k-1时刻的航向角的偏差，他们的关系如公式(11)： T

15、OC o 1-5 h z 错误！未找到引用源。(11)Q为过程噪声的covariance，假设k时刻航向角的量测值为错误！未找到引用 源。，现在我们可以得到k时刻的最优化航向角估计值如公式(12)：错误！未找到引用源。(12)H为测量系统参数，错误！未找到引用源。为k时刻的卡尔曼增益，求解如公 式13：错误！未找到引用源。(13)R为观测噪声的covariance，为了能够以此往下面迭代，故必须求出k时刻航向 角的偏差，即求出错误！未找到引用源。如公式(14)：错误！未找到引用源。(1-错误！未找到引用源。)*错误！未找到引用 源。(14)然后再根据k时刻的参数值求解k+1时刻的航向角，这样就

16、可以实时求解 航向角的大小。至于编程时错误！未找到引用源。初始参数的确定可以任意确 定一个就行，因为卡尔曼滤波算法会使这个参数逐渐收敛的，但是错误！未找 到引用源。最好别取零，防止卡尔曼滤波把这个当成最优解。实验中，采用卡尔曼滤波器对罗盘输出的航向值进行滤波，滤波器的输出 如图5.1所示。35030025020015010050 0-50010002000300040005000600070008000900010000图5.1航向角滤波前后对比滤波后航向角的相对误差如图5.2所示。18图5.2滤波数据与原始数据相对误差从图5.1可以看出，在滤波时给出的初值和刚开始的航向角值差别比较 大，卡尔

17、曼滤波能很快收敛到精确的航向角，在罗盘每次改变方向的时候，卡 尔曼滤波因为要根据预测量和测量值来估计下一时刻值，所以滤波时航向角失 调量要大些。6实验、数据处理及其仿真6.1实验内容（1）用NT3000GPS接收板卡测得五分钟的数据，得到一组实验场地的经纬度 信息，然后取平均得到一组精度相对较高的初始经纬度信息。通过串口调试助 手观测并保存GPS数据为“txt”格式。（2）用HMR3000电子罗盘绕一周测4个方向的航向角信息，每个方向大概测 5分钟，通过串口调试助手观测并保存罗盘数据为“txt”格式。6.2数据处理（1）将GPS测得经纬度信息取平均得到初始B=32.0304，L=118.857

18、4，H=5.3。（2）用matlab读取罗盘信息，在东北天坐标系下进行航位推算，因为磁偏角 的存在使电子罗盘存在误差，所以将测得的航向角加上当地的磁偏角便得到实 际的航向角（南京的磁偏角为5），我们这组我取速度为4 m/s,采样时间用四 组的平均采用时间，即错误！未找到引用源。/4（15）错误！未找到引用源。=（0.1170+0.1157+0.1172+0.1170）/4=0.1167按DR算法可以得到如图6.1所示运动轨迹图：图6.1东北天坐标系下运动轨迹从上图可以看出用滤波后的数据进行航位推算得到的轨迹与原始数据进行 航位推算得到的轨迹大致相同。说明在短时间内罗盘的导航精度很高。（3）将东

19、北天坐标系转化为直角坐标系，在直角坐标系中推算的轨迹如图6.2 所示，因为X-Y面更能清楚的反映运动情况，故采用平面直角坐标系。4.7402 -2.6128-2.6126-2.6124-2.6122-2.612 -2.6118-2.6116-2.6114-2.6112-2.611x 10 4.7402 -2.6128-2.6126-2.6124-2.6122-2.612 -2.6118-2.6116-2.6114-2.6112-2.611x 10 6X/（m）图6.2直角空间坐标系X-Y面然后再将直角空间坐标系转换为大地坐标系，在大地坐标经纬度面的轨迹如图 6.3所示。图6.3大地坐标系经纬度

20、面在大地坐标系（B,L,H）下的轨迹如图6.4所示。大地坐标系X-Y-Z-1.1763-1.1764-1.1765-1.176732.04118.85118.845经度纬度-1.1768118.865度高-1.1766大地坐标系X-Y-Z-1.1763-1.1764-1.1765-1.176732.04118.85118.845经度纬度-1.1768118.865度高-1.176632.02532.0232.015图6.4大地坐标系X-Y-Z7误差分析7.1电子罗盘误差电子罗盘的误差具体可以分为两类：系统误差和外界环境误差。系统误差 包括罗盘自身的器件精度误差以及安装误差，主要受罗盘内部的三轴

21、磁阻传感 器影响。外界环境误差是指罗盘工作的环境所带来的误差，如载体倾斜带来的 误差、磁罗差误差等。本实验中罗盘放在计算机、示波器和工控机旁边易受到 干扰，所以罗差对罗盘的精度有很大的影响。虽然滤波后得到的轨迹比滤波前 精确些，但是罗盘每个方向的具体方位角并不精确，若想提高每个方向的方位 角精度，最好能对罗盘进行补偿，可以在因为载体倾斜带来的误差和磁罗差方 面分别进行补偿，因为本实验中没有理想的精确的方位角理论值，所以没有对 罗盘进行补偿，只对罗盘转动的轨迹进行了仿真。7.2航位推算本身误差航位推算一个致命的弱点就是误差随着时间而积累。所以推算的航向角是 发散的，航向角误差越来越大。因此航位推

22、算不能长时间的单独工作，必须借 助于其他的定位方式进行组合导航，比如GPS/DR组合导航，航位推算借助于 GPS定期传来的载体位置进行联合导航，精度更加精确。8总结在得到一组GPS导航数据的基础上利用航位推算进行导航，通过卡尔曼滤 波对航向角数据进行处理，减小误差。利用 matlab仿真出分别在东北天坐标 系、直角坐标系、大地坐标系下的运动轨迹，载体运动轨迹与实验时的罗盘向 轨迹相同。最后分析了航位推算导航定位的误差原因，并且希望以后在罗盘补 偿方面能做更进一步的研究。参考文献1.孙希延，纪元法与施浒立,卡尔曼滤波实现车载GPS/DR组合导航.现代电子技 术,2006(11):第 149-15

23、2页.吴富梅，杨元喜与田育民,GPS/DR组合导航自适应Kalman滤波算法.测绘科学 技术学报,2008(3):第206-208+212页.王珂,GPS测向算法与应用研究,2009,重庆大学.张华海，郑南山，王军，李景芝.由空间直角坐标计算大地坐标的简便公式. 全球定位系统.2002，4:9-12.彭树生,数字电子罗盘HMR3000的特性及应用.电子技术,2004(8):第39-43页.付梦印，邓忠红，张继伟.Kalman滤波理论及其在导航系统中的应用M. 北京:科学出版社，2003.附录clear all;load head.matZ=heading;m,n=size(Z);T=0.116

24、7;A=1 T;0 1;%系统参数矩阵(状态转移矩阵)H=1 0; %测量系统参数X=100;0; %状态变量初值C=diag(1,1);%误差协方差初值Rk=diag(1);%量测噪声方差Ta=T;1;%状态过程噪声阵sigam=1;Qk=Ta*Ta*sigam人2;%过程噪声方差初值f=Z(:,1);for k=1:mXk=A*X;%从k-1时刻预测k时刻Ck=A*C*A+Qk;%k-1时刻预测k时刻方差K=Ck*H*inv(H*Ck*H+Rk);%kalman 增益X=Xk+K*(Z(k,:)-H*Xk);% 状态更新C=Ck-K*H*Ck;% 方差更新Xe(:,k)=X;%滤波输出en

25、dfigure(1)plot(Xe(1,:),r);hold on;plot(heading,b);grid on;hold on;B=32.0304;L=118.8574;H=5.3;e2=0.00673949674227;format shorte1=(6378140人2-6356755人2)/6378140人2;N=6378140/(sqrt(1-e1*(sind(B)人2);X=(N+H)*cosd(B)*cosd(L);Y=(N+H)*cosd(B)*sind(L);Z=(N*(1-e1)+H)*sind(B);c=Xe(1,:);n3 n4=size(Xe);for i=1:(n4+1)if i=1x2(i)=0;y2(i)=0;elsed1=c(i-1)+5;x2(i)=x2(i-1)+4*0.1167*cosd(d1);y2(i)=y2(i-1)+4*0.1167*sind(d1);endendfor i=1:(n4+1)if i=1x3(i)=0;y3(i)=0;el