基于Zynq平臺的室內機器人航向測量系統設計

嚴浩，樊旭，何頂新

(華中科技大學 自動化學院，武漢 430074)

基于Zynq平臺的室內機器人航向測量系統設計

嚴浩，樊旭，何頂新

(華中科技大學 自動化學院，武漢 430074)

為獲得室內移動機器人的準確航向，利用慣性傳感器與磁力計搭建了一套基于Zynq的航向測量系統。該系統使用ARM和FPGA相結合的形式,實現了高性能、高集成度的控制算法。本系統中FPGA部分實現了計算性能要求高的擴展卡爾曼濾波算法，ARM部分實現了可移植性強的數據采集模塊與數據濾波算法等。實驗結果表明，該航向測量系統準確可靠，并且算法執行效率相較于傳統方案有很大提高，取得了較好的效果。

航向測量系統；擴展卡爾曼濾波算法；軟硬件協同設計；XC7Z020-CLG484

引 言

基于GPS與圖像視覺的移動機器人航向測量系統易受到遮擋、光線等因素干擾，而基于慣性傳感器的航向測量系統對外界環境依賴性不強，適合用在短距離室內機器人定位中[1]。目前基于慣性傳感器的航向測量算法中精度最高的算法為EKF[2](Extended Kalman Filter)算法，EKF算法是一種動態自適應算法，可以提供高精度的解算結果，并有效地抑制傳感器噪聲，但是EKF算法需要進行大規模的矩陣運算，對MCU的性能要求很高[3]。

EKF算法復雜度高，使用純軟件算法無法提供高頻率的解算信息，而使用純硬件方案實現的開發難度大、周期長、靈活性欠佳，針對這一矛盾，目前廣泛提出的解決方案是使用軟硬件協同設計的方法進行系統設計。出于低成本、高可靠性、高擴展性的考慮，設計中使用了異構多核平臺XC7Z020-CLG484作為實現平臺，采用了軟硬件協同設計的方案，通過功能劃分合理地分配軟硬件的任務。另外，針對FPGA[4]開發難度大的特點，設計中通過Vivado HLS[5]提供的高層次綜合的方法，使用C語言對FPGA進行開發，大大縮短了開發周期。

1 系統主控平臺介紹

1.1 異構多核平臺Zynq

Zynq平臺(XC7Z020-CLG484)是由Xilinx公司推出的一款全可編程的異構多核片上系統。Xilinx公司推出該芯片旨在將其應用在復雜圖像處理、高性能視頻處理、高性能工業控制和高級汽車電子等方面[7]。Zynq平臺的結構如圖1所示，Zynq平臺分為處理器系統(PS)和可編程邏輯(PL)兩部分，其中處理器系統包含了運行主頻為667 MHz的Cortex-A9雙核ARM、輸入/輸出接口、內部總線、以及存儲器接口等單元；可編程邏輯部分為Artix-7系列FPGA，最高主頻可達250 MHz，包含有85K邏輯單元、53.2K查找表(LUT)、106K觸發器、220個DSP單元。PS和PL之間通過片內AXI總線進行通信，該總線是一種主從模式的數據通信總線，用于CPU與IP CORE之間的通信并支持多種通信機制，具備突發數據傳輸、DMA數據傳輸等功能，其按照吞吐量與接口類型可被分為AXI_HP、AXI_ACP、AXI_GP三種，可以靈活地按照系統的實際需求進行配置。

1.2 基于Zynq平臺的系統開發

基于Zynq平臺的系統開發流程如圖1所示，首先根據系統需求進行軟硬件劃分，其中軟件系統的開發方式與一般軟件開發方式相同，可以使用Xilinx公司的軟件開發工具 SDK進行開發。在軟件、硬件劃分完成之后，被劃分為硬件部分的系統會按照實際設計的形式進行模塊劃分，其中功能固定、實現難度小的部分使用HDL代碼的形式實現，而其中功能復雜，實現難度大的部分使用HLS工具設計實現，然后將兩部分設計通過Xilinx公司的Vivado軟件完成綜合、實現。

圖1 Zynq平臺開發流程

在進行軟硬件劃分后，PS與PL部分可分別進行開發。其中PS部分可被視作一個特殊的包含了完善的外部通信接口與豐富系統資源的IP核，在對其進行開發之前，需要通過Vivado工具對其進行配置。在Vivado工具中，PS部分被封裝為ZYNQ7 Processing System模塊，通過圖形化的配置界面，可以對I/O接口、AIX總線、內存接口、時鐘等進行配置，并將配置信息導入BSP(Board Support Package)中。隨后在SDK軟件中導入BSP，就可在SDK中按照一般的Cortex-A9的開發方法對PS部分進行軟件開發。

PL部分使用Xilinx公司的7系列FPGA實現，因此可以使用Xilinx公司的標準 FPGA 開發套件和工具實現代碼開發、綜合實現以及可執行文件的生成。針對FPGA開發難度大、開發周期長的缺點，PL部分可以使用高層次綜合的方式進行開發，使用Vivado HLS工具將無時序約束的高層次代碼C、C++轉換成完全按照時序運行的HDL代碼。使用Vivado HLS工具開發的主要流程如下：

① 使用C、C++實現所需功能，但是由于設計的代碼最終要與實際的 FPGA 硬件相對應，所以不是所有的數據類型與系統調用都能被成功轉化，因此在代碼編寫時應使用HLS 工具支持的數據類型與函數方法。另外，為了節約FPGA的資源，應盡量縮短各數據的存儲空間。

② 使用Vivado HLS工具將編譯無誤的代碼綜合為HDL代碼并生成報告，其中綜合報告中包含有通用信息、性能與時間估計、資源使用率等信息，性能報告中給出了設計中各部分代碼執行的執行順序與時間消耗。

③ 按照性能報告與綜合報告的信息對原始的代碼進行優化，即在C、C++設計中增加優化條件。循環展開是最常用的優化方法，Vivado HLS工具提供了多種展開的策略，如PIPELINE展開與UNROLL展開等，不同的策略加速效果不同，資源消耗也不同，用戶可以按照需求與資源消耗進行選擇。另外，Vivado HLS工具中也可以對變量的存儲類型與交換策略進行優化，例如可以將頻繁使用的數據存入RAM之中，還可以把數組接口綜合為單口RAM、雙口RAM、FIFO或者把它們展開為多個獨立的端口。

④ 多次進行代碼綜合并查看報告，按照報告進行優化，直至設計滿足要求。最后，使用Vivado HLS工具將生成的HDL代碼導出為硬件IP核的形式以供使用。

2 航向測量系統原理

2.1 航向測量系統模型

航向測量系統最終得到的是系統相對于地理坐標系的航向信息，可被包含于一個表示了三維空間旋轉全部信息的方向余弦矩陣之中。

設地理坐標系為n系，載體坐標系為b系，則根據坐標系的旋轉關系有：

(1)

(2)

考慮載體運動時旋轉矩陣的變化率有：

(3)

(4)

(5)

其中,R(Δt)可認為由3個繞xb,yb,zb軸旋轉的微小旋轉a、b、c組成，忽略高次項系數有：

(6)

由矩陣運算可知，坐標軸分解的旋轉順序將不會影響結果，結合式(3)、(5)、(6)可得：

(7)

其中S(ω)為角速度矩陣：

(8)

由上式可知，機器人的位姿信息變化率可由角速度積分得到。

使用旋轉矩陣表示的計算量大，實時計算困難，工程中使用四元素表述位姿信息。四元素是一種高階復數q=(q0+q1×i+q2×j+q3×k)，常用在捷聯慣導系統中，表示一個繞(q1,q2,q3)向量旋轉q0角度的旋轉變換。四元素可以和旋轉矩陣表示法進行相互轉換：

(9)

通過四元素法也可以對姿態進行更新，根據參考文獻[8]與式(7)，可以得到連續系統下四元素的更新律為：

(10)

(11)

將此連續系統離散化，并設每次算法更新的時間間隔為T，可以得到離散系統的四元素法更新律為：

(12)

2.2 基于EKF算法的航向測量系統解算原理

航向測量系統解算的根本思想是利用陀螺儀所提供的角加速度,結合航向測量系統模型進行迭代積分運算，但是由于陀螺儀糟糕的低頻特性將會產生很大的漂移，因此需要使用加速度計與磁力計對低頻數據進行修正，從而改善航向測量系統計算中的靜態特性。EKF是目前廣泛使用在多傳感器融合領域的算法，其主要算法如下：

(13)

根據上一時刻的協方差計算當前先驗預測的協方差:

(14)

式中Rt為引入的高斯噪聲的方差。使用加速度計與磁力計的值Zt=[Da,Db]T+RK=[ax,ay,az,mx,my,mz]T作為系統的觀測量，式中RK為估計誤差，則系統的測量方程為：

(15)

(16)

其中，bx與bz為地磁場在世界坐標系中的值，具體數值可以根據當地經緯度查詢。為了求取卡爾曼增益，需要計算量測方程的雅可比矩陣：

(17)

可以得到系統的卡爾曼增益KE與四元素后驗估計qt為：

(18)

(19)

至此,可以得到當前時刻位姿的最優估計，為了使迭代算法可以繼續，需要計算方差后驗估計：

(20)

當誤差收斂后，可以得到穩定的四元素值，進一步可將其轉換為歐拉角，其中yaw角即為系統相對于地理坐標系的航向角：

(21)

3 系統設計

3.1 整體架構

系統采用軟硬件協同方法，結合處理器與可編程硬件的優勢，PS端進行整體流程控制以及交互控制功能的實現，通過USART端口與上位機進行通信，使用SPI接口獲得傳感器數據，并將運行時產生的數據存儲在外置大容量SD卡中。

PL端中運行EKF算法，使用并行計算以縮短EKF算法所需的時間。PS與PL之間使用片內高速AXI-BUS總線進行連接，由于PS與PL的數據交互量不大，在保證性能的情況下，使用資源占用量小、吞吐率較小的AXI_GP[9]總線，其數據吞吐率可達600MB/s。

傳感器平臺由兩部分組成： 6自由度IMU傳感器和電子羅盤。IMU傳感器選用了ST公司iNEMO系列高性能慣性傳感器lsm6dsltr，該傳感器在一個極小的封裝中集成了一個3自由度陀螺儀與一個3自由度加速度計。電子羅盤選用了Honeywell公司的HMC5983，HMC5983是一款為汽車和個人導航應用設計的，帶溫度補償的3自由度磁力計，具有極低的成本與很高的精確度。整體架構如圖2所示。

圖2 整體架構

3.2 ARM部分軟件設計

ARM部分通過SPI總線和lsm6dsltr芯片與HMC5983芯片進行連接，完成了數據采集并在進行數據采集后，對原始的傳感器數據采用了平滑濾波與零點修正濾波，去除了靜態偏置并抑制了高頻隨機噪聲的干擾，最后將傳感器數據傳送給PL端。

慣性傳感器lsm6dsltr可被配置成不同狀態，可以改變芯片的輸出頻率、量程、片內補償等，其配置接口支持SPI總線接口和I2C總線接口。本設計采用SPI接口來配置lsm6dsltr，系統被用于移動機器人，考慮實際的機器人運行情況，選用加速度量程為±8g，角速度量程為±500 dps。在進行傳感器數據的預處理時會損失傳感器數據的輸出頻率，因此設置lsm6dsltr的輸出頻率為最高，其中加速度的輸出頻率為6 664 Hz，陀螺儀的輸出頻率為3 332 Hz。

電子羅盤HMC5983也通過SPI總線對內部寄存器進行配置，結合實際情況，配置其輸出頻率為220 Hz，輸出范圍為±2 gauss。為了降低漂移所帶來的誤差，在讀取磁力計的數據后，需要補償傾斜、硬鐵和軟鐵效應帶來的畸變。

3.3 FPGA部分軟件設計

FPGA端的主要工作是設計EKF算法的IP核，由于算法邏輯復雜，使用Verilog語言實現的難度較高，因此采用高層次綜合方法(HLS)實現，即用C語言完成算法流程，并由 Vivado HLS工具轉化為Verilog。使用FPGA進行開發的關鍵在于充分利用FPGA資源以減少運算時間，使用空間換取時間[10]。本設計中主要對以下3個方面進行了優化：

① 對于矩陣中的元素采用非阻塞的并行賦值方法，設計中對矩陣賦值的函數使用高并行度的UNROLL策略進行循環展開，并設置矩陣的存儲類型為RAM_2P，使用并行處理的方法大幅提高賦值運算的速率。

② Vivado HLS工具提供了循環展開的優化策略，可化串行運算為并行運算。設計中針對算法中最多的矩陣乘法運算使用了循環展開的優化策略，對其進行了并行加速，由于矩陣乘法運算的規模很大，使用了較為節省資源的PIPELINE策略進行循環展開。

③ 設計中使用Gauss-Jordan消元法求解矩陣的逆矩陣，該算法有著整個EKF算法中最高的時間復雜度與空間復雜度。為了加快運算速度，提前為中間變量開辟了RAM_1P類型的存儲空間，并將行初等變化的算法采用并行加速，考慮到LUT(查找表)的不足，使用較為節省資源的PIPELINE策略進行循環展開。

4 實驗分析

4.1 誤差分析

將傳感器板固定在高精度手動R軸旋轉臺上，調節傳感器初始位置在零點，航向測量系統運算周期為1 kHz。在初始條件下令q=[1,0,0,0]T，取當地的重力加速度數值|g|=9.7936 m/s2，當地的地磁場強度近似為B=[3.41×10-50 3.57×10-5]T，系統的協方差矩陣Rt為：

首先測試系統的靜態性能，將系統靜置120s后，各傳感器的輸出如圖3～圖5所示。可以看出陀螺儀有一個很小的靜態偏移，如果不使用加速度計與磁力計對其進行修正，那么系統在積分的作用下將會產生很大的誤差。另外加速度計與磁力計的數據穩定、靜態性能優秀，可以很好地起到修正作用。

圖3 陀螺儀輸出的角速度值

圖4 加速度計輸出的加速度值

圖5 磁力計輸出的磁場值

通過解算后可以得到靜止時姿態角輸出,如圖6所示，其中最大漂移誤差0.384 8°，方差為0.003 5，反映出系統具有很好的靜態特性。

圖6 靜止時姿態角輸出

為了測試本航向系統算法性能，將旋轉臺順時針旋轉180°與逆時針旋轉180°，每隔30°記錄下航向系統的輸出，重復做10組實驗，取平均值列表如表1所列。

經過多次實驗，在較為穩定的磁場環境中，本系統的航向角定向精度為±1.5°，數據的一致性高，性能可滿足需求。

4.2 性能分析

系統性能主要考慮EKF算法的執行效率，實驗中測試了相同的求解算法在嵌入式系統上的執行效率，并與Zynq平臺進行了對比，結果如表2所列。

從對比測試可以看出，本文提出的方案，時間消耗降低了3個數量級，大大提升了計算速度，證實了該方案的優越性。

從表3中FPGA資源使用可以看出， LUT的數量不足是制約算法速率提升的主要因素，這是因為在進行算法優化時，大量的循環展開運算將消耗大量的LUT資源。但是系統中對BRAM_18K的利用率并不高，因此想要進一步提高解算速度，可以將大量重復使用的數據提前運算并存儲到BRAM_18K中，減少重復運算的時間。

表1 航向角誤差測試結果

表2 時間消耗對比

表3 FPGA資源使用

結 語

[1] Geiger W, Bartholomeyczik J, Breng U, et al. MEMS IMU for AHRS applications[C]//2008 IEEE/ION Position, Location and Navigation Symposium,Monterey CA USA, 2008:225-231.

[2] Sabatelli S, Galgani M, Fanucci L, et al. A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU[J]. Instrumentation & Measurement IEEE Transactions on, 2013, 62(3):590-598.

[3] Cavallo A, Cirillo A, Cirillo P, et al. Experimental Comparison of Sensor Fusion Algorithms for Attitude Estimation[J]. IFAC Proceedings Volumes, 2014, 47(3):7585-7591.

[4] Kung Y S, Shu G S. Development of a FPGA-based motion control IC for robot arm[C]//2005 IEEE International Conference on Industrial Technology ,Hong Kong，2005:1397-1402.

[5] Xilinx. Zynq-7000 All Programmable SoC Technical Reference Manual [EB/OL].[2017-02].http://www.xilinx.com/support/documentation/user_guides/ug585-Zynq-7000-TRM.pdf.

[6] Xilinx. Vivado Design Suite User Guide High-Level Synthesis [EB/OL].[2017-02].http://www.xilinx.com/support/documentation/sw_manuals/xilinx2016_2/ug902-vivado-high-level-synthesis.pdf.

[7] 任慰. 以實時操作系統為中心的嵌入式系統平臺化設計研究[D].武漢：華中科技大學,2013.

[8] Vicci L. Quaternions and Rotations in 3-Space: The Algebra and its Geometric Interpretation[J]. Physiological Research, 2001, 59(2):305-308.

[9] 牛盼情, 康翔宇, 何頂新. 基于Zynq的PMSM驅動控制系統設計[J]. 單片機與嵌入式系統應用, 2015(5):65-68.

[10] 王曉璐. 基于Zynq的LS-SVM算法加速器設計[D]. 哈爾濱：哈爾濱工業大學, 2015.

嚴浩(碩士研究生)，主要研究方向為運動控制，智能機器人；樊旭(碩士研究生)，主要研究方向是模式識別、嵌入式系統；何頂新(副教授)，主要研究方向是嵌入式系統、智能機器人。

參考文獻

[1] 任曉鋒.全自動立體車庫控制系統的研究與構建[D].西安:陜西科技大學,2014.

[2] 邢飛,郭文成.利用計算機視覺庫的Android平臺系統道路識別[J].單片機與嵌入式系統應用,2014(2):35-37.

[3] 師艷偉,楊晶東.移動機器人目標識別算法[J].光學技術,2015(1):27-33.

[4] 黃佳.基于OPENCV的計算機視覺技術研究[D].上海:華東理工大學,2013.

[5] Alex Krizhevsky,Ilya Sutskever，Geoffrey E Hinton.Imagenet classification with deep convolutional neural networks[C]//Advances in Neural Information Processing Systems(NIPS2012),2012:1097-1105.

[6] DAHL G E,Yu D,Deng L,et al.Context-dependent pre-trained deep neural networks for large-vocabulary speech recognition[J].IEEE Transactions on Audio,Speech and Language Processing,2012,20(1):30-42.

[7] Russakovsky O,Deng J,Su H,et al.ImageNet large scale visual recognition challenge[J].International Journal of Computer Vision,2015,115(3):211-252.

[8] HINTON G E,OSINDERO S,TEH Y W.A fast learning algorithm for deep belief nets[J].Neural Computation,2006,18(7):1527-1554．

[9] 尹寶才,王文通,王立春.深度學習研究綜述[J].北京工業大學學報,2015(1):48-59.

[10] 王茜,張海仙.基于深度神經網絡的汽車車型識別[J].現代計算機:專業版,2015(35):61-64.

[11] HUBEL D H,WIESEL T N.Receptive fields,binocular interaction and functional architecture in the cat's visual cortex[J].The Journal of Physiology,1962,160(1):106.

[12] Gando G,Yamada T,Sato H,et al.Fine-tuning deep convolutional neural networks for distinguishing illustrations from photographs[J].expert systems with applications,2016,66(10):295-301.

[13] 趙永科.深度學習:21天實戰Caffe[M].北京:電子工業出版社,2016.

茅正沖(副教授)，主要從事機器人視聽覺識別的研究；韓毅(碩士研究生)，主要研究方向為控制工程及應用。

(責任編輯：薛士然 收稿日期：2017-03-15)

Indoor Robot's Heading Measurement System Based on Zynq Platform

Yan Hao,Fan Xu,He Dingxin

(School of Automation,Huazhong University of Science and Technology,Wuhan 430074,China)

In order to obtain the accurate course of indoor mobile robot,a set of Zynq-based on heading measurement system is built using the inertial sensor and magnetometer.In the system, ARM and FPGA are combinated to realize the high integration of the high performance control algorithm.In this system,the extended Kalman filter algorithm is realized in FPGA for high computational performance requirements,while high data acquisition module and data filtering algorithm are realized in ARM for high scalability.The experiment results show that the heading system is accurate and reliable,and it is more efficient compared with traditional methods.

heading measurement system;EKF algorithm;software and hardware co-design;XC7Z020-CLG484

TP249

A

?迪娜

2017-02-21)