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

0引言

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

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

目前，用于導(dǎo)航系統(tǒng)的處理器方案主要有DSP、DSP+FPGA、DSP+ARM等，這些方案在芯片級(jí)間存在數(shù)據(jù)傳輸速度的限制，無(wú)法支持大量傳感器信息的接入與傳輸，且占用面積較大。為解決微終端方案對(duì)處理器系統(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ì)算速度測(cè)試和嵌入式軟件與仿真對(duì)比測(cè)試。最后，根據(jù)結(jié)果給出了分析和展望。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖4 SSRAM的讀寫時(shí)序控制

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

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信號(hào)，返回定位導(dǎo)航結(jié)果；CAN總線接口用于接收其他外源傳感器的信號(hào)，并在接收到數(shù)據(jù)時(shí)根據(jù)數(shù)據(jù)幀頭判斷傳感器的類型，支持即插即用。另一部分為2個(gè)UART接口（RS422），接口掛載在FPGA端，通過(guò)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)過(guò)有效性檢驗(yàn)，若數(shù)據(jù)無(wú)丟包、錯(cuò)包現(xiàn)象，則進(jìn)行數(shù)據(jù)編碼。解析數(shù)據(jù)與變量一一對(duì)應(yīng)，并可存儲(chǔ)到相應(yīng)的buffer，將接收狀態(tài)設(shè)置為1，等待處理器調(diào)用。

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

2.2 算法簡(jiǎn)介

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

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

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

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

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

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

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

（2）觀測(cè)方程建立

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

式中，Z為觀測(cè)量矩陣，H為量測(cè)矩陣，V為量測(cè)噪聲。

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

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

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

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

圖6 融合算法工作流程

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

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

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

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

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

圖8 算法運(yùn)行時(shí)間測(cè)試

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

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

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

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

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

圖10 實(shí)測(cè)與仿真各方向速度差

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

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

4結(jié)論

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