用于定位導航授時微終端的SoC系統設計

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

（清華大學，北京100084）

0 引言

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

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

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

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

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

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

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

2 微終端處理器SoC系統設計

2.1 硬件設計

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

2.1.1 外設地址與中斷分配

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

圖2 SoC系統總體設計方案Fig.2 Overall design scheme of SoC system

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

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

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

表1 外設中斷優先級Table 1 Interruption priority of peripheral

2.1.2 存儲芯片讀寫時序控制

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

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

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

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

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

2.1.3 通信接口工作流程

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

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

圖5 接口數據接收流程Fig.5 Receiving flow of interface data

2.2 算法簡介

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

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

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

（1）狀態方程建立

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

對其列狀態方程如下

式中，Fe INS為狀態轉移矩陣，G為噪聲轉移矩陣，W為狀態噪聲。具體的矩陣求解過程詳見文獻［12］。

（2）觀測方程建立

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

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

（3）濾波計算過程

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

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

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

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

3 結果驗證與分析

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

圖7 系統集成實物圖及測試環境Fig.7 System integrated physical map and its test environment

3.1 算法運行時間測試

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

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

3.2 系統實測與仿真對比

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

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

圖9 實測與仿真的經緯度差和高程差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數量級，符合預期，可滿足微終端的需求。SoC系統可添加雙精度浮點型DSP協處理器，提高計算速度與精度。

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

4 結論

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