一種用于定位導(dǎo)航授時微終端的SoC系統(tǒng)的設(shè)計與實現(xiàn)

摘要：為了解決遮擋情況下的實時定位問題，美國提出了Micro-PNT方案，我國也提出了定位導(dǎo)航授時微終端（Micro Positioning Navigation and Timing Terminal，MPNTT）方案。定位導(dǎo)航授時微終端集成了衛(wèi)星導(dǎo)航系統(tǒng)、微慣性測量單元、微型原子鐘及處理器系統(tǒng)，可為終端用戶提供精確可用、完好及時、連續(xù)安全的定位導(dǎo)航服務(wù)。介紹了一種用于定位導(dǎo)航授時微終端的SoC系統(tǒng)設(shè)計，其包括了基于SoC FPGA的硬件設(shè)計和基于GNSS/MIMU的組合導(dǎo)航濾波算法。SoC系統(tǒng)集成了Flash、SSRAM等存儲芯片，通過RS422、RS232、CAN等通信接口接收GNSS、MIMU及外源傳感器信息，并在ARM核中完成組合導(dǎo)航算法，以得到導(dǎo)航結(jié)果。SoC芯片單片實現(xiàn)了ARM與FPGA的功能，系統(tǒng)集成面積滿足小型化需求，為后續(xù)移植為ASIC芯片提供了基礎(chǔ)。對組合導(dǎo)航濾波算法進行嵌入式軟件移植并測試，結(jié)果表明：SoC系統(tǒng)單次慣導(dǎo)解算時間為7ms，實測與仿真輸出的導(dǎo)航位置差距在0.05m以內(nèi)，俯仰角差和橫滾角差在0.005°以內(nèi)，航向角差在0.05°以內(nèi)。本文設(shè)計的SoC系統(tǒng)高精度、集成化、可擴展，滿足了微終端的要求。

0引言

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

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

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

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

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

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

圖１ 定位導(dǎo)航微終端總體方案與部件集成

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

2.1 硬件設(shè)計

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

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

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

圖２ SoC系統(tǒng)總體設(shè)計方案

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

圖3 外設(shè)地址配置

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

表1 外設(shè)中斷優(yōu)先級

2.1.2 存儲芯片讀寫時序控制

外接存儲芯片包含SSRAM、并行Flash和串行Flash。其中，SRAM、并行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的總?cè)萘繛?MB，用于存儲系統(tǒng)程序，其中FPGA程序占用了4.5MB左右，剩余為ARM軟件程序所用。本系統(tǒng)設(shè)置為從FPGA啟動，當(dāng)系統(tǒng)上電時將運行FPGA端ROM中的preloader程序，將硬件在FPGA中進行部署，再將軟件程序及數(shù)據(jù)從串行Flash中拷貝到SSRAM中運行，上述過程即為整個系統(tǒng)的啟動。

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

圖4為SSRAM的讀寫控制時序。CK為時鐘信號，ADSC_________為地址選通信號，A0-An為并行地址，BW______為字節(jié)寫入使能（低寫入高讀?。?，Ba____-Bd____為寫入a-d的4個區(qū)域的字節(jié)寫入使能，E1____為片選信號，G___為輸出使能，DQa_______-DQd________為a-d的4個區(qū)域并行數(shù)據(jù)輸入輸出（Q為讀出，D為寫入）?？刂菩盘枎蟿澗€表示低有效。圖4的前半部分為讀取時序，當(dāng)讀取完成時，先拉高E1____取消選擇，再拉低E1____選通芯片進行下一步操作。圖4的后半部分為寫入時序，系統(tǒng)依靠BW______控制讀寫方向，依靠G___控制數(shù)據(jù)輸入輸出方向，依靠地址A0-An控制讀寫位置。

圖4 SSRAM的讀寫時序控制

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

2.1.3 通信接口工作流程

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

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

圖5 接口數(shù)據(jù)接收流程

2.2 算法簡介

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

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

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

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

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

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

式中，F(xiàn)eINS為狀態(tài)轉(zhuǎn)移矩陣，G為噪聲轉(zhuǎn)移矩陣，W為狀態(tài)噪聲。

（2）觀測方程建立

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

式中，Z為觀測量矩陣，H為量測矩陣，V為量測噪聲。

（3）濾波計算過程

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

式中，k=1，2，3，…，Φ為一步轉(zhuǎn)移陣，P為預(yù)測協(xié)方差陣，Q為系統(tǒng)噪聲協(xié)方差陣，K為Kalman增益，H為觀測矩陣，R為測量噪聲協(xié)方差陣。

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

圖6 融合算法工作流程

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

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

圖7 系統(tǒng)集成實物圖及測試環(huán)境

3.1 算法運行時間測試

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

圖8 算法運行時間測試

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

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

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

圖9 實測與仿真的經(jīng)緯度差和高程差

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

圖10 實測與仿真各方向速度差

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

圖11 實測與仿真的姿態(tài)角差

4結(jié)論

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