基于DSP+FPGA的超紧组合导航系统研究*

余 威 陈 帅 陆 帅 王磊杰

南京理工大学，南京210094

基于DSP+FPGA的硬件平台搭建分布式超紧组合导航系统，利用惯性信息辅助GNSS接收机进行信号跟踪，提高了系统的鲁棒性。从提高动态条件下GNSS接收机跟踪环路性能的角度出发，通过曲线拟合的方法对惯性辅助信息进行数据递推以消除加速度残差，并对该方法进行了仿真验证。对于少星情况下系统观测量维数降低、组合导航精度下降的问题，提出了一种通过引入外部高度信息、采用全维滤波方式进行信息融合的算法。

1 系统方案设计与硬件实现

1.1 方案设计

分布式GNSS/SINS超紧组合导航系统根据多普勒效应，利用卫星的星历信息解算出的卫星位置和速度以及组合导航输出的载体位置和速度，计算由于载体和卫星之间的相对运动所造成的载波多普勒频移，并对载波环进行辅助，以提高跟踪环路的动态性能。同时为了降低收星数过少带来的影响，引入了外部高度表信息作为滤波器输入，重新构建观测方程。整体结构如图1所示。

1.2 硬件实现

系统基于DSP+FPGA硬件平台搭建，按功能划分为以下模块:INS模块、接收机模块、高度表模块、数据通信模块、数据处理模块和上位机监控模块。其中，INS模块和接收机模块分别指MEMS惯导和GNSS接收机;高度表模块采用气压式高度表;数据通信模块分为UART模块和FIFO模块，由FPGA硬件实现;数据处理模块为TI公司生产的数字信号处理器TMS320C6713;上位机监控模块由PC机和监控软件组成。

在系统中INS模块使用RS-422传输数据，而高度表模块、接收机模块和上位机模块的信息传输采用RS-232，面临信息源多，数据量大的问题。为了减轻DSP负担，采用FPGA对串口进行扩展并通过EMIF总线接口与DSP通信，使得DSP能专注于数据处理，提高实时性。硬件结构如图2所示。

图1 系统总体结构图

图2 系统硬件结构图

2 系统状态方程与观测方程

组合导航系统通过卡尔曼滤波器对系统中的误差量进行估计，并将估计值作为校正量反馈到系统中，实现组合校正。系统状态方程与观测方程如下［1-3］:

基于伪距和伪距率的超紧组合导航系统在收星数少于4颗的情况下仍能继续工作，但由于观测维数的减少削弱了状态变量的观测性，导致定位精度出现下降。高度表是一种在飞行器上广泛使用的测量器件，其中气压式高度表利用大气静压随高度的变化规律间接测量高度，具有自主性好，低空测量准确等优点。由于接收机天线受到遮挡基本上出现在近地面附近，所以气压式高度表不仅可以在少星情况下增强系统的观测性，还弥补了GNSS接收机在近地面时高度信息准确度相对较差的缺点。

引入外部高度信息之后，系统的状态方程不变，观测方程增加了高度误差信息作为观测量。设惯导解算得到的高度为hins，气压式高度表测得的高度为halt，则高度信息的观测量h为:

3 组合导航系统工作模式

3.1 收星数大于4颗

当收星数大于4颗时系统将面临2种选择:通过选星算法选择4颗卫星作为导航星;使用全部可见星的观测量作为滤波器输入。

由于精度因子表征了定位误差的放大倍数，选星算法通过计算得到一组使精度因子最佳的卫星组合，可以在不增加滤波器系统维数的情况下提高导航精度。缺点是此时其他卫星提供的有效信息会被浪费，从而降低系统的能观性。与之相反，全维滤波使用所有可见卫星进行组合滤波，增强了系统的观测性，但观测维数的增加导致滤波时间加长。

对于“DSP+FPGA”平台，数据的接收和发送均由FPGA完成，DSP与FPGA间通过EMIF连接，传输耗时可忽略不计。不妨设卡尔曼滤波器全维滤波时间为tk，数据的同步时间为tc，惯导解算时间为tins，其他数据运算时间为tother。若惯导的解算周期为T，那么需满足下列公式成立:

组合导航系统中各项所占时间如表1所示。

表1 时间测试

表2 不同收星数下的滤波时间

若惯导的解算周期设为10ms，从上表可以看到，即使是全维滤波，总时间仍小于解算周期，说明采用全维滤波完全可行。从提高系统性能的角度出发，选用全维滤波方式进行组合导航。跑车试验中2种方式的误差对比如图3和4所示。

由图可见:全维滤波位置误差在5m以内，速度误差在1m/s以内;而采用选星算法位置误差在10m以内，速度误差在2m/s以内。试验结果表明采用全维滤波可改善导航精度。

图3 位置误差对比图

图4 速度误差对比图

3.2 收星数小于4颗

当可见卫星数为3颗时，系统状态变量的能观性遭到削弱，此时加入高度表信息可增强观测性。在静态试验中位置、速度信息长时间内无发散，且短时间内仍能保持较高定位精度。

导航误差随时间积累，当收星数为2颗时，系统的能观性进一步下降，以该种情况作为典型的少星情况予以分析。静态实验结果如图5和6所示。

由图5和6可知，在无高度表信息辅助情况下，200s后纬度误差累积到50m，经度误差达到90m，高度误差达到40m，速度误差超过1m/s;加入高度表信息后200s以内纬度误差小于10m，经度误差小于10m，高度误差小于2m，速度误差在0.2m/s以内。试验结果表明引入外部高度信息可改善少星情况下的导航精度。

图5 无高度表辅助的位置曲线图

图6 高度表辅助下的位置曲线图

4 惯性信息辅助跟踪环路

4.1 惯性辅助载波跟踪环路数学模型

接收机载波环是一个由鉴相器、环路滤波器和数控振荡器构成的负反馈闭环控制系统。将惯性辅助环路加入该系统后得到的数学模型如图7［4-8］。

图7 惯性辅助载波环数学模型

4.2 高动态条件下的动态应力误差

从上式我们可以看出:当载体作加加速度运动时，由于载波环路无法跟踪高阶次的运动，使得辅助间隔内锁相环相位误差累积，可能导致环路失锁，因此需要对辅助信息进行插值或拟合。

4.3 加加速度曲线拟合方法

对于上式可使用最小二乘法求解［9］。此外，由于高动态往往发生在载体飞行的主动段，发动机的工作由火控系统控制，飞行阻力与大气稀薄程度和速度有关。通过对发动机推力和飞行阻力变化的合理建模，也可以在一定范围内对加加速度进行拟合，减小其带来的不利影响。载体运动数学模型如下:

式中，Ju为多项式拟合得到的加加速度;αu，Vu，Pu分别是采用串口实时接收的载体三维、速度、位置信息;t为传输延迟时间;α'u，V'u，P'u分别为根据数学模型递推得到的观测时刻载体三维加速度、速度和位置信息。在每次跟踪环路更新时，利用递推得到的惯性辅助全信息在辅助间隔内对跟踪环路进行辅助，从而消除加速度残差。

4.4 惯性信息辅助跟踪环路仿真分析

出于简化考虑，载体的运动选择最简单的匀加加速运动，此时仅需要对惯性辅助信息进行线性拟合即可。在DSP+FPGA硬件平台的基础上利用卫星导航信号模拟器仿真相应的运动轨迹和卫星信号，并搭建半实物仿真系统进行高动态实验。接收机定位后载体的实际运动轨迹如图8～10所示。

由图8～10可知，载体在接收机定位后首先以100m/s的速度匀速飞行220s;然后以20g/s的加加速度作匀加加速度运动3s;再以60g的加速度作匀加速运动7s;最后转为匀速飞行。

组合导航系统定位结果如图11～13所示。

图8 轨迹位置曲线图

图9 轨迹速度曲线图

图10 轨迹合速度与加速度曲线图

图11 接收机位置曲线图

从图8～9和图11～12的对比可以看到:当载体进行高动态飞行时，接收机仍然能保持对卫星信号的稳定跟踪，实现正常定位;以第4号卫星为例，从图13可以看到，在加入线性拟合后解算多普勒频移曲线与实际多普勒频移曲线基本重合，两者的误差保持在10Hz以内。

图12 接收机速度曲线图

图13 多普勒频移及误差曲线图

5 结束语

设计了一种基于DSP+FPGA的GNSS/SINS超紧组合导航系统。针对导航过程中出现的高动态、障碍遮挡等情况提出了相应的解决方法。通过相关试验验证，为下一步的工程实现提供了重要依据。结果表明:采用惯性辅助载波跟踪环路可改善接收机的动态性能，引入外部高度信息可改善少星情况下的导航精度。

［1］ 罗建军，马卫华，袁建平，岳晓奎.组合导航原理与运用［M］.西安:西北工业大学出版社，2012.(Luo Jianjun，Ma Weihua，Yuan Jianping，Yue Xiaokui.Integrated Navigation System:Principle and Application［M］.Xi'an:Northwestern Polytechnical University Press Lo.Ltd，2012.)

［2］ 叶萍.MEMS IMU/GNSS超紧组合导航技术研究［D］.上海交通大学，2011:70-77.(Ye Ping.Research on M-EMS IMU/GNSS Ultra-tight Integration Navigation Technology ［D］.Shanghai Jiao Tong University，2011:70-77.)

［3］ 袁信，俞济祥，陈哲.导航系统［M］.北京:航空工业出版社，1993.(Yuan Xin，Yu Jixiang，Chen Zhe.Navigation System ［M］.Beijing:Aviation Industry Press，1993.)

［4］ 余忠清，张捷，龚诚.GNSS接收机在惯性速度辅助下跟踪环路的研究［J］.无线通信技术，2008，17(1):34-36，41.(Yu Zhongqing，Zhang Jie，Gong Cheng.Research on GNSS receiver tracking loop using inertial velocity［J］.Wireless Communication Technolog，2008，17(1):34-36，41.)

［5］ 董晓明，孙迅.GPS/INS超紧耦合方法及其应用分析［J］. 遥测遥控，2010，31(6):6-10.(Dong Xiaoming，Sun Xun，GPS/INSUltra-Tightly coupled technique and its application analysis［J］.Journal of Telemetry;Tracking and Command，2010，31(6):6-10.)

［6］ 谢钢.GPS原理与接收机设计［M］.北京:电子工业出版社，2009.(Xie Gang，Principles of GPS and Receiver Dsign［M］.Beijing:Publishing House of Electronics Industry，2009.)

［7］ 马云峰.MSINS/GPS组合导航系统及其数据融合技术研究［D］.东南大学，2006:82-84.(Ma Yunfeng.Research on MSINS/GPS Integrated Navigation System and Its Data FusionTechnology［D］.Southeast University，2006:82-84.)

［8］ 于洁，王新龙.SINS辅助GPS跟踪环路超紧耦合系统设计［J］.北京航空航天大学学报，2010，36(5):606-609.(Yu Jie，Wang Xinlong.Ultra-tightly coupled navigation system design based on SINS aiding GPS tracking loops［J］.Journal of Beijing University of Aeronautics and Astronautics，2010，36(5):606-609.

［9］ 蔡山，张浩，陈洪辉，沙基昌.基于最小二乘法的分段三次曲线拟合方法研究［J］.科学技术与工程，2007，7(3):352-355.(Cai Shan，Zhang Hao，Chen Hong hui，Sha Jichang.Research of piecewise cubic curvefitting method based on least-square principle［J］.Science Technology and Engineering，2007，7(3):352-355.