黎 超，郭美鳳，周 斌，張 嶸，侯 波

（清華大學，北京100084）

0 引言

隨著信息技術的發(fā)展，人們對實時位置信息獲取的要求越來越高。目前，最常見的定位方式依賴于衛(wèi)星定位導航系統(tǒng)（如GPS、北斗等）。然而，在城市、叢林等有遮擋、信號弱的地方，衛(wèi)星無法給出定位信息［1-2］。為了解決遮擋情況下的實時定位問題，不依賴于衛(wèi)星的自主導航成為了近年來各國的研究熱點。美國DARPA提出了微型定位導航授時（Micro-PNT）方案，其目標是研發(fā)尺寸小、質(zhì)量輕、功耗低的慣性器件和時鐘芯片，以作為GNSS混淆環(huán)境的增強、校驗、延續(xù)的后援PNT解決方案［3-4］；國內(nèi)一些單位也提出了相關的定位導航授時微終端（Micro Positioning Navigation and Timing Terminal， MPNTT）方案［4］。

MPNTT方案擬采用微系統(tǒng)集成技術，將高性能微慣性測量單元、衛(wèi)星導航系統(tǒng)、時鐘、輔助傳感器接口、信息融合處理器等集成于一個小型化模塊，經(jīng)過多源信息的融合，為終端用戶提供精確可用、完好及時、連續(xù)安全的定位導航服務［3-4］。該方案的重點在于兩部分：其一是高精度慣性傳感器的研制，其二是微集成技術與多源信息融合算法。微半球諧振陀螺精度高、對稱性好、可靠性高［5］，是未來高精度傳感器的代表，也是微終端的研究重點，其校準、補償對微終端處理器提出了高處理能力的要求。微集成技術與多源信息融合算法需要將多種傳感器集成在小尺寸內(nèi)，并將各種信息融合濾波進行導航，這也對處理器系統(tǒng)提出了集成化、高精度、可擴展的要求。

目前，用于導航系統(tǒng)的處理器方案主要有DSP［6］、 DSP+FPGA［7-9］、 DSP+ARM［10］等， 這些方案在芯片級間存在數(shù)據(jù)傳輸速度的限制，無法支持大量傳感器信息的接入與傳輸，且占用面積較大。為解決微終端方案對處理器系統(tǒng)提出的高精度、集成化、可擴展的要求，本文選擇了片上系統(tǒng)（System on Chip，SoC）解決方案，該方案兼有FPGA的靈活性和ARM編程的易用性。芯片內(nèi)部的ARM與FPGA之間使用了AXI總線通信，傳輸速度高且占用面積小。此種方案在驗證后還可根據(jù)外設情況制作數(shù)字專用集成電路（Application Specific Integrated Circuit，ASIC），以進一步降低功耗和電路集成面積。

本文在第1部分介紹了定位導航微終端總體方案；在第2部分介紹了SoC系統(tǒng)設計，其中包含了硬件設計方案和算法流程；在第3部分介紹了系統(tǒng)的驗證與分析，包含硬件計算速度測試和嵌入式軟件與仿真對比測試。最后，根據(jù)結果給出了分析和展望。

1 定位導航微終端總體方案

如圖1（a）所示，定位導航授時微終端包含了4個部分，即：衛(wèi)星導航系統(tǒng)、微慣性測量單元、微型原子鐘和SoC信息處理器。其中，微慣性測量單元（Micro Inertial Measurement Unit， MIMU）由 3個MEMS陀螺與3個MEMS加速度計組成，采用旋轉(zhuǎn)調(diào)制方案，可通過旋轉(zhuǎn)積分消除隨機誤差，提高了初始對準尋北精度，其具體集成如圖1（b）所示。旋轉(zhuǎn)調(diào)制MIMU與衛(wèi)星導航系統(tǒng)通過RS422和RS232接口與處理器進行通信。當接收到慣性測量信息與GNSS信息時，處理器將采用多源信息融合算法進行導航解算，并將結果通過RS422對外接口發(fā)出。此系統(tǒng)還集成了CAN總線接口，處理器內(nèi)部預置了多種傳感器驅(qū)動，支持相同協(xié)議外源傳感器即插即用，可融合磁強計、氣壓計、激光雷達、里程計等多種傳感器信息，增強定位精度與可靠性。而微型原子鐘授時精度高，產(chǎn)生的秒脈沖信號可長時間支持衛(wèi)星導航系統(tǒng)的無信號狀態(tài)運行，也為MIMU和衛(wèi)星導航系統(tǒng)提供了統(tǒng)一時間標。定位導航授時微終端的整體集成如圖 1（c）所示。

圖1 定位導航微終端總體方案與部件集成Fig.1 MPNTT overall scheme and its component integration

2 微終端處理器SoC系統(tǒng)設計

2.1 硬件設計

SoC系統(tǒng)的整體設計方案如圖2所示，ARM處理器通過AXI總線與外接存儲芯片控制器、通信接口相連。ARM處理器為SoC系統(tǒng)提供了優(yōu)秀的處理能力，外接存儲芯片為大規(guī)模信息融合算法提供了存儲與運行空間，對外通信接口接收的傳感器信息可用于數(shù)據(jù)融合。下面將介紹關鍵部分的詳細設計。

2.1.1 外設地址與中斷分配

處理器芯片采用Altera公司的Cyclone V SoC芯片，其最高主頻為925MHz。該系列芯片含有2個Cortex-A9核，通過2條寬度為64bit的AXI總線及1條寬度為32bit的輕量級AXI總線與FPGA端的IP核相連。AXI總線將地址、讀數(shù)據(jù)、寫數(shù)據(jù)、握手信號在不同通道中進行單向傳輸，具有性能高、延遲低的特點，較AHB等總線提高了數(shù)據(jù)傳輸效率。

圖2 SoC系統(tǒng)總體設計方案Fig.2 Overall design scheme of SoC system

圖3為外設地址配置。本系統(tǒng)將大容量、快速的外設如RAM、SSRAM、Flash等，掛載到正常的AXI總線，采用了100MHz時鐘；將低速的外設如UART、TIMER（定時器）等，掛載到輕量級的AXI總線，采用了25MHz時鐘。

系統(tǒng)的中斷捕獲模塊（Interrupt Capture Module）根據(jù)表 1的中斷優(yōu)先級將 JTAG、串行 Flash、UART、TIMER等外設的中斷信號反饋給ARM進行處理，并保證程序存儲芯片的中斷優(yōu)先級最高，JTAG調(diào)試器第二，定時器最低。

圖3 外設地址配置Fig.3 Configuration of peripheral address

表1 外設中斷優(yōu)先級Table 1 Interruption priority of peripheral

2.1.2 存儲芯片讀寫時序控制

外接存儲芯片包含SSRAM、并行Flash和串行Flash。其中，SSRAM、并行Flash由三態(tài)控制器（Tri-State Controller）進行控制，串行 Flash由串行Flash控制器（Serial Flash Controller）進行控制。 上述3個控制器均通過地址范圍擴展器（Address Span Extender）掛載到64bit AXI總線上。地址范圍擴展器創(chuàng)建了一個窗口橋，可以允許ARM通過總線訪問更大的地址映射。SSRAM的大小為1M×32bit，用作系統(tǒng)運行內(nèi)存；并行Flash的大小為1M×16bit，用于掉電時的數(shù)據(jù)存儲；串行Flash的總容量為8MB，用于存儲系統(tǒng)程序，其中FPGA程序占用了4.5MB左右，剩余為ARM軟件程序所用。本系統(tǒng)設置為從FPGA啟動，當系統(tǒng)上電時將運行FPGA端ROM中的preloader程序，將硬件在FPGA中進行部署，再將軟件程序及數(shù)據(jù)從串行Flash中拷貝到SSRAM中運行，上述過程即為整個系統(tǒng)的啟動。

串行Flash的控制接口為SPI接口，其讀寫控制方法簡單。并行Flash與SSRAM的控制方式根據(jù)地址與片選、讀寫使能信號進行，此處以SSRAM為例說明存儲芯片的讀寫控制。

圖4為SSRAM的讀寫控制時序。CK為時鐘

圖4 SSRAM的讀寫時序控制Fig.4 Read-write sequence control of SSRAM

外接存儲芯片為大規(guī)模組合導航算法提供了程序存儲與運行空間，為實現(xiàn)更高精度的融合算法提供了平臺。

2.1.3 通信接口工作流程

SoC系統(tǒng)的通信接口分為兩部分，一部分為2個UART接口（RS232）和1個CAN總線接口。這些接口被直接掛載在HPS端（即ARM處理器），便于捕捉數(shù)據(jù)狀態(tài)的變化。其中，UART負責與衛(wèi)星導航系統(tǒng)進行通信，接收GNSS信號，返回定位導航結果；CAN總線接口用于接收其他外源傳感器的信號，并在接收到數(shù)據(jù)時根據(jù)數(shù)據(jù)幀頭判斷傳感器的類型，支持即插即用。另一部分為2個UART接口（RS422），接口掛載在FPGA端，通過AXI總線與ARM核相連，負責與IMU進行通信及輸出導航結果。多種通信接口為實現(xiàn)傳感器的信息融合提供了硬件支持，接口與軟件配合可實現(xiàn)傳感器的即插即用，完成導航解算。

通信接口數(shù)據(jù)的接收流程如圖5所示。數(shù)據(jù)要經(jīng)過有效性檢驗，若數(shù)據(jù)無丟包、錯包現(xiàn)象，則進行數(shù)據(jù)編碼。解析數(shù)據(jù)與變量一一對應，并可存儲到相應的buffer，將接收狀態(tài)設置為1，等待處理器調(diào)用。

圖5 接口數(shù)據(jù)接收流程Fig.5 Receiving flow of interface data

2.2 算法簡介

算法目前融合了MIMU信息與GNSS信息，主要計算過程有初始對準、基于GNSS/MIMU的組合導航Kalman濾波算法。

初始對準可分為粗對準與精對準。在載體靜止時，粗對準利用載體三軸加速度計的輸出等于重力加速度在載體坐標系的投影的原理，可求出水平方向的俯仰角與橫滾角。完成粗對準后進行精對準，需要衛(wèi)星導航系統(tǒng)提供載體運動信息。根據(jù)位置、速度等信息列出誤差方程，可求解出航向角，即完成了導航初始化［11］。

組合導航算法主要可分為狀態(tài)方程、觀測方程建立與濾波計算兩部分。

（1）狀態(tài)方程建立

在ECEF坐標系下，組合導航算法中的Kalman濾波誤差狀態(tài)向量主要包括姿態(tài)誤差三軸加速度計零偏ba、三軸陀螺零偏bg， 其具體表述如下

對其列狀態(tài)方程如下

式中，F(xiàn)e INS為狀態(tài)轉(zhuǎn)移矩陣，G為噪聲轉(zhuǎn)移矩陣，W為狀態(tài)噪聲。具體的矩陣求解過程詳見文獻［12］。

（2）觀測方程建立

系統(tǒng)的觀測量為IMU解算的位置和速度與GNSS獲取的位置和速度的差值，據(jù)此建立觀測方程

式中，Z為觀測量矩陣，H為量測矩陣，V為量測噪聲。具體的矩陣求解過程詳見文獻［12］。

（3）濾波計算過程

主要采用標準Kalman濾波進行算法求解，其具體過程如下

式中，k＝1，2，3，…，Φ為一步轉(zhuǎn)移陣，P為預測協(xié)方差陣，Q為系統(tǒng)噪聲協(xié)方差陣，K為Kalman增益，H為觀測矩陣，R為測量噪聲協(xié)方差陣。具體矩陣求解過程詳見文獻［13］。

導航算法的工作流程如圖6所示，IMU信息與GNSS信息分別由兩個串口中斷接收。當收到GNSS信息時，整個算法進行初始對準。初始化完成后，在接收到IMU信息時進行機械編排，得到慣性導航系統(tǒng)（Inertial Navigation System，INS）的推算結果。將此推算結果與GNSS信號進行時空基準統(tǒng)一，將相同時間數(shù)據(jù)轉(zhuǎn)換到同一坐標系。隨后，對傳感器數(shù)據(jù)做出有效性判斷。若數(shù)據(jù)無效則去掉數(shù)據(jù)，若數(shù)據(jù)有效則執(zhí)行最優(yōu)融合濾波算法，修正INS機械編排結果，并輸出導航結果。

圖6 融合算法工作流程Fig.6 Workflow of fusion algorithm

3 結果驗證與分析

圖7為SoC系統(tǒng)集成實物圖及其測試環(huán)境。將系統(tǒng)集成在一塊53mm×59mm大小的PCB板上，各信號通過一塊外接板引出。用上位機將IMU數(shù)據(jù)和GNSS數(shù)據(jù)按照時序發(fā)送給SoC處理器進行測試，測試結果如下。

圖7 系統(tǒng)集成實物圖及測試環(huán)境Fig.7 System integrated physical map and its test environment

3.1 算法運行時間測試

在算法函數(shù)入口及出口處設置計數(shù)器，并在周期為1ms的定時器中斷函數(shù)中進行累加，對算法在SoC系統(tǒng)中的運行時間進行測試。圖8展現(xiàn)了SoC系統(tǒng)在啟動后一段時間內(nèi)的測試結果。當未接收到運動的GNSS信息時，算法的運行時間小于1ms；當接收到運動的GNSS信息時，進行初始對準，算法單次運行時間約為12ms；在初始化完成后，慣性導航的單次解算時間約為7ms，可支持100Hz的慣性導航信息融合計算。

圖8 算法運行時間測試Fig.8 Runtime test of algorithm

3.2 系統(tǒng)實測與仿真對比

將IMU數(shù)據(jù)與GNSS數(shù)據(jù)按照時序發(fā)送給SoC系統(tǒng)，進行40min導航解算，將返回的解算結果與仿真結果對應點做差，結果如圖9～圖11所示。

由圖9可知，測試結果與仿真結果的經(jīng)緯度差在5×10-7°以下，轉(zhuǎn)換為距離約在0.05m以下，高程差小于0.01m。

圖9 實測與仿真的經(jīng)緯度差和高程差Fig.9 Longitude and latitude difference and elevation difference between measurement and simulation

由圖10可知，測試結果與仿真結果在東向、北向的速度差在0.01m/s以下，在天向的速度差在 0.001m/s以下。

圖10 實測與仿真各方向速度差Fig.10 Velocity difference in all directions between measurement and simulation

由圖11可知，測試結果與仿真結果的俯仰角差和橫滾角差在0.005°以下，航向角差在0.05°以下。ARM核為單精度浮點型運算，仿真為雙精度浮點型運算，實測與仿真計算結果偏差基本在10-7數(shù)量級，符合預期，可滿足微終端的需求。SoC系統(tǒng)可添加雙精度浮點型DSP協(xié)處理器，提高計算速度與精度。

圖11 實測與仿真的姿態(tài)角差Fig.11 Attitude angle difference between measurement and simulation attitude

4 結論

本文介紹了一種用于定位導航授時微終端的SoC系統(tǒng)的設計與實現(xiàn)，基于SoC FPGA的硬件設計，滿足了微終端對于集成化、高精度、可擴展的要求。基于GNSS/MIMU的組合導航濾波算法具有可擴展性，可融合更多種類的傳感器信息，提高系統(tǒng)導航精度。測試結果顯示：SoC系統(tǒng)的單次慣導解算時間為7ms，速度較慢，可增加DSP協(xié)處理器，增強計算能力，提高算法運行速度。實測與仿真的位置差在0.05m以內(nèi)，俯仰角差和橫滾角差在0.005°以內(nèi)，航向角差在0.05°以內(nèi)，滿足了微終端的要求。未來，可根據(jù)此SoC系統(tǒng)設計ASIC芯片，進一步降低功耗與集成面積。