基于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)