用于定位導(dǎo)航授時(shí)微終端的SoC系統(tǒng)設(shè)計(jì)

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

（清華大學(xué)，北京100084）

0 引言

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

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

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

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

1 定位導(dǎo)航微終端總體方案

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

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

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

2.1 硬件設(shè)計(jì)

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

2.1.1 外設(shè)地址與中斷分配

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

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

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

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

圖3 外設(shè)地址配置Fig.3 Configuration of peripheral address

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

2.1.2 存儲芯片讀寫時(shí)序控制

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

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

圖4為SSRAM的讀寫控制時(shí)序。CK為時(shí)鐘

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

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

2.1.3 通信接口工作流程

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

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

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

2.2 算法簡介

算法目前融合了MIMU信息與GNSS信息，主要計(jì)算過程有初始對準(zhǔn)、基于GNSS/MIMU的組合導(dǎo)航Kalman濾波算法。

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

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

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

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

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

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

（2）觀測方程建立

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

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

（3）濾波計(jì)算過程

主要采用標(biāo)準(zhǔn)Kalman濾波進(jìn)行算法求解，其具體過程如下

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

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

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

3 結(jié)果驗(yàn)證與分析

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

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

3.1 算法運(yùn)行時(shí)間測試

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

圖8 算法運(yùn)行時(shí)間測試Fig.8 Runtime test of algorithm

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

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

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

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

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

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

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

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

4 結(jié)論

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