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

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

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

3、算原理2 航位推算原理航位推算原理是利用载体的速度、航向以及上一时刻的位置来估计下一时刻载体的位置。利用载体的速度和航向能够得出速度在当地水平坐标轴上分向速度，将分向速度和载体所经过的时间相乘便可以得到载体在坐标轴上增加的坐标值，与前一时刻的坐标值求和便可以得到此时的坐标值。原理图如图1所示。推算如公式(1)。.(1)其中，()为载体在时刻的初始位置，()为载体在时刻位置，、为载体在时刻的速度与航向角（航向角为载体运动方向与正东方向的夹角）。为罗盘相邻两次推算所用的时间差。3 坐标系之间的转化3.1 所涉坐标系的种类3.1.1 当地水平坐标系当地水平坐标系又名东北天坐标系，它的坐标原点为载体的

4、质心，X轴指向当地北子午线，Y轴垂直X轴指向东，Z轴垂直XOY面构成右手直角坐标系。3.1.2大地坐标系地球椭球的中心与地球质心重合，椭球的短轴与地球的自转轴重合，大地纬度B为地面点的椭球法线与椭球赤道面的夹角，大地经度L为过地面点的椭球子午面与格林威治起始子午面的夹角，大地高H为地面点沿椭球法线至椭球面的距离。地面一点可表示为( L,B,H)。如图3.1所示。图3.1 大地坐标系B:大地纬度，通过观测站的参考椭球面的法线与赤道面的夹角。L:大地经度，通过观测站的大地子午面与本初子午面得夹角。H:大地高度，观测站沿法线到参考椭球面的距离。3.1.3 WGS-84坐标系GPS使用的坐标系，该坐标

5、系属于协议地球坐标系，其原点位于地球质心，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,

6、L,H）测得的GPS量作为载体的质心，然后经过旋转矩阵将东北天坐标系（当地水平坐标系、站心坐标系）转化为空间直角坐标系。旋转点作为载体的质心，以（B,L,H）的值为起点。将坐标系分别绕Z、X、Y轴逆时针旋转、角所使用的旋转矩阵如下：=.(2)= (3)= .(4)由于航位推算是在东北天坐标系下进行的，所以需要需变换到大地坐标系下，显示经纬度信息，先将东北天坐标系转换到直角坐标系下，然后再有直角坐标系转化为大地坐标系，将在84坐标系（大地坐标（B、L、H）形式表示）下测得的GPS值转化为直角坐标系（X、Y、Z）下表示，作为直角坐标系下初值，转化公式如下：.(5)现将Y轴反向得轴。绕轴逆时针旋转)

7、，再绕Z轴旋转)，即可将东北天坐标系（地平站心坐标系）转化为站心赤道直角坐标系，公式如下：.(6)R=*=所以空间直角坐标系和站心赤道直角坐标系之间有简单的平移关系，所以将站心赤道坐标系转化为空间直角坐标系之间的转化关系为：=.(7)所以与东北天坐标系之间的转化关系为：=(8) 3.2.2 直角坐标系转化为大地坐标系直角坐标系转化为大地坐标系的转化公式为：.(8)其中：，N为该点的卯酉圈曲率半径；，a、b、e分别为该大地坐标系对应参考椭球的长半轴、短半轴和第一偏心率。长半轴a=63781372m，短半轴b=6356.7523142km，。此方法H和B需要迭代求得，而且应用条件受到很大限制，如要

8、求大地高度需要小于1000m时，才能使B、L迭代四次之后精度分别达到和0.001m，所以本文一种计算精度高，公式形式简便的方法来计算B和L4。.(9)4 电子罗盘HMR30004.1 概述HMR3000包含三个磁阻传感器和一个双轴的液体横滚传感器，并带有A/D转换电路和微处理器。微处理器控制传感器的测量顺序，控制器操作的参数存储在EEPROM中，输出3类NMEA0813标准语句(HDG、HDT和XDR)，3类专用语句(HPR、RCD和CCD)，HPR是最常用的格式，采用RS232接口功能模块如图4-1所示。图4-1 HMR3000功能框图4.2 HMR3000特性数字电子罗盘HMR3000是H

9、oneywell公司的产品。它可以为导航定位系统提供航向、俯仰、横滚等数据。其中航向的精度为±，分辨率可达±。横滚和俯仰的精度为±，分辨率为±，输出端口采用RS232。HMR3000的串行通信是根据NMEA0183标准制定的简单、异步的ASCII协议5。ASCII码的传输和接收使用1位起始位、8位数据位，无奇偶校验位和1位停止位。每一个码有10位。波特率可选择1200,2400,4800,9600或38400。HMR3000支持NMEA0183和专用的信息。在测量模式中有HMR3000按照EEPROM中的编程的速率主动发送NMEA信息。本次试验输出是专用

10、HPR句子，它包含了航向、俯仰、横滚等信息。格式如下：HPR Heading, Pitch & Roll（航向，俯仰和横滚）$PTNTHPR,x.x,a,x.x,a,x.x,a*hh <cr> <lf>这个句子把HMR3000的三个重要的测量结果和有用的状态、俯仰、俯仰状态、横滚、横滚状态。如本次试验的一个句子为：$PTNTHPR,199.3,N,0.3,N,0.2,N*37此句说明：罗盘所测航向角为：，正常状态，俯仰角为，正常状态，横滚角为，正常状态。5 卡尔曼滤波1960年，R.E.Kalman首次提出的卡尔曼滤波是一种线性最小方差估计。卡尔曼滤波理论成功地

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

12、化回归处理，将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，假

13、设k时刻航向角的量测值为，现在我们可以得到k时刻的最优化航向角估计值如公式(12)：(12)H为测量系统参数，为k时刻的卡尔曼增益，求解如公式13：(13)R为观测噪声的covariance，为了能够以此往下面迭代，故必须求出k时刻航向角的偏差，即求出。如公式(14)：(1-)*(14)然后再根据k时刻的参数值求解k+1时刻的航向角，这样就可以实时求解航向角的大小。至于编程时初始参数的确定可以任意确定一个就行，因为卡尔曼滤波算法会使这个参数逐渐收敛的，但是最好别取零，防止卡尔曼滤波把这个当成最优解。实验中，采用卡尔曼滤波器对罗盘输出的航向值进行滤波，滤波器的输出如图5.1所示。图5.1 航向角

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

15、分钟，通过串口调试助手观测并保存罗盘数据为“txt”格式。6.2 数据处理（1）将GPS测得经纬度信息取平均得到初始B=32.0304，L=118.8574，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 东北天坐标系下运动轨迹从上图可以看

16、出用滤波后的数据进行航位推算得到的轨迹与原始数据进行航位推算得到的轨迹大致相同。说明在短时间内罗盘的导航精度很高。（3）将东北天坐标系转化为直角坐标系，在直角坐标系中推算的轨迹如图6.2所示，因为X-Y面更能清楚的反映运动情况，故采用平面直角坐标系。图6.2 直角空间坐标系X-Y面然后再将直角空间坐标系转换为大地坐标系，在大地坐标经纬度面的轨迹如图6.3所示。图6.3 大地坐标系经纬度面在大地坐标系（B,L,H）下的轨迹如图6.4所示。图6.4 大地坐标系X-Y-Z7 误差分析7.1电子罗盘误差电子罗盘的误差具体可以分为两类：系统误差和外界环境误差。系统误差包括罗盘自身的器件精度误差以及安装误

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

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

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

20、ing;m,n=size(Z);T=0.1167;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'*sigam2;%过程噪声方差初值f=Z(:,1);for k=1:m Xk=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*

21、Xk);%状态更新 C=Ck-K*H*Ck;%方差更新 Xe(:,k)=X;%滤波输出 endfigure(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 short e1=(63781402-63567552)/63781402;N=6378140/(sqrt(1-e1*(sind(B)2); X=(N+H)*cosd(B)*cosd(L);Y=(N+H)*cosd(B)*sin

22、d(L);Z=(N*(1-e1)+H)*sind(B); c=Xe(1,:);n3 n4=size(Xe);for i=1:(n4+1) if i=1 x2(i)=0; y2(i)=0; else d1=c(i-1)+5; x2(i)=x2(i-1)+4*0.1167*cosd(d1); y2(i)=y2(i-1)+4*0.1167*sind(d1); end endfor i=1:(n4+1) if i=1 x3(i)=0; y3(i)=0; else d2=f(i-1)-5; x3(i)=x3(i-1)+4*0.1167*cosd(d2); y3(i)=y3(i-1)+4*0.1167*sind(d2); end end for i=1:(n4)d3(:,i)=abs(f(i)-c(i)/f(i);endfigure(2)plot(d3(1,:),'r');grid on;hold on;figure(3) plot(x2,y2,x3,y3,'r')hold on;xlabel('正东/(m)');ylabel('正北/(m)')