采用SLAM方法的电脑鼠

苏 豪，黄心汉，王贞炎

(1. 华中科技大学 自动化学院，湖北 武汉 430074；2. 华中科技大学 电气与电子工程学院，湖北 武汉 430074)



采用SLAM方法的电脑鼠

苏 豪1，黄心汉1，王贞炎2

(1. 华中科技大学 自动化学院，湖北 武汉 430074；2. 华中科技大学 电气与电子工程学院，湖北 武汉 430074)

与绝大部分采用红外对管进行简单测距的电脑鼠不同，在电脑鼠上使用线型激光器和摄像头搭建了一个小型激光测距系统并进行SLAM。摄像头以240 f/s的帧率输出分辨率为188×120的图像，使用Zynq-7010作为主控制器，在其可编程逻辑部分搭建了图像采集系统，在其双核处理器Cortex-A9部分搭建了用于运行图像处理、传感器数据融合和解迷宫算法的实时Linux操作系统。制作了一只重量为100.06 g、车体大小为7.45 cm(宽)×6.6 cm(长)的电脑鼠，能高速稳定地在迷宫中运动。

电脑鼠；SLAM；激光测距；Zynq

0 引言

电脑鼠是一种典型的小型轮式移动机器人，国际IEEE电脑鼠走迷宫比赛规则要求其能在大小为180 mm×180 mm单元组成的迷宫中进行自主移动并求解迷宫。目前绝大部分的电脑鼠都是使用红外对管在迷宫中进行测距定位[1-3]，然而使用红外对管测距的方式需要针对不同的场地环境进行复杂的调整，导致使用这种方式的电脑鼠场地适应性很差，限制了其使用范围。

通过对外部环境进行识别和分析，创建地图，同时利用地图进行定位和导航，是移动机器人的同时定位与地图创建(SLAM)问题[4]。由于电脑鼠体积较小，一般要求其体积小于90 mm×90 mm×3 mm，较大的体积不利于其在迷宫中高速运动，而较小的体积则不利于进行复杂的电路设计和机械构造，因此在电脑鼠身上应用SLAM具有很大难度。

随着FPGA集成度和可编程逻辑资源数目的不断提高，FPGA越来越广泛地应用于实时图像处理领域[5]。本文使用高速成像芯片MT9V034和线型红外激光器构建了一个小型二维激光测距系统，使用Xilinx的Zynq-7010可扩展处理平台对图像数据进行实时处理，针对迷宫特点进行SLAM，应用在电脑鼠上。

1 系统硬件结构

1.1 整体系统结构

系统由电源电路、传感系统、主控制器部分和电机等机械结构构成。硬件系统结构如图1所示，主控制器部分包括Zynq-7010可扩展处理平台、数据存储单元DDR3和用于无线数据传输的SDIO WiFi模块；传感系统包括由高速成像芯片MT9V034和线型红外激光器构成的激光测距系统以及能测三轴角速度的惯性传感器LSM330；机械结构包括一对Faulhaber 1717-006SR空心杯直流电机、传动件、轮毂、轮胎和支撑件等。

Zynq-7010可扩展处理平台作为整个实时处理系统的核心，它包括Processing System(PS)和Programmable Logic(PL)两大部分，其中PS部分包含了最高可运行在866 MHz的双Cortex-A9核，PL部分包含了传统意义的FPGA逻辑单元和DSP资源[6]。

电脑鼠所设计的最高运动速度为4 m/s，为了在高速移动时仍能保持准确定位，本系统所使用的高速成像芯片MT9V034能在188×120的分辨率下以240 fps的速率输出图像。即使电脑鼠以4 m/s的速度进行移动，在240 f/s的速率下，每一帧间隔内的移动距离也仅为16.7 mm，在可接受范围内。

1.2 激光测距结构

激光测距的结构和原理如图1和图2所示。线型激光器向前方发射线型的激光束，照射在物体表面，经过反射后传入到摄像头中，在摄像头内部的图像传感器形成光斑。物体与激光器的相对位置不同，激光在图像传感器中形成光斑的位置也不同，据此可以确定物体的位置。

图1 激光测距结构图

图2 激光测距原理图

对于理想的针孔摄像头模型，根据三角形相似：

由于u和l均为常数，因此可以得到障碍物距离d和障碍物在图像中的成像位置s的关系。

s对d求偏导，得：

可见，障碍物距离原点越远，障碍物距离的分辨率越小，由于图像传感器分辨率有限，在超出一定距离后，分辨率会小于一个像素，需要在图像处理过程中进一步处理。

实际应用时，需要准确得到障碍物在图像中的相应位置，但是环境中还存在不相关的景物，因此需要尽可能过滤掉不相关景物。线型激光器发射出808 nm的红外激光，在镜头前加装了一个808 nm红外窄带滤波片，接收经过障碍物反射回来的红外激光，由可以接收红外光线的MT9V034图像传感器采集生成包含环境障碍物信息的图像。

2 系统软件设计

2.1 整体系统软件结构

如图3所示，系统软件包括PL部分和PS部分。其中PL部分包括MT9V034的驱动与数据采集、编码盘数据采集和电机驱动。PS部分包括对采集后的图像数据进行处理，惯性传感器LSM330数据的获取与处理，融合图像、惯性传感器和编码盘数据的姿态解算，运动路径规划，解迷宫算法等。

图3 系统软件结构图

可见PS需要处理很多任务，考虑到系统调试的便捷性、现有资源的可重复利用性、各个软件模块的协调运行，系统在PS部分搭建了嵌入式实时Linux操作系统，并接入一个SDIO WiFi作为运行时的调试接口。

2.2 图像采集

MT9V034与Zynq-7010的接口位于PL侧，考虑到与PS的数据通信问题，本文在PL部分设计了一个自定义IP核，实现了MT9V034与PS的数据交互。

如图3系统软件结构图内的数据流所示，PS使用AXI_GP接口，通过AXI-Lite协议，使用Memory Map的方式读写自定义IP核的寄存器和AXI DMA，控制MT9V034和启动DMA进行数据传输。

自定义IP核使用Verilog HDL语言实现，通过检测MT9V034的帧同步、行同步、像素时钟等信息，把MT9V034的图像数据缓存于一个异步时钟FIFO中，异步时钟FIFO的写时钟由MT9V034的像素时钟提供。AXI DMA提供异步时钟FIFO的读时钟，请求一次读操作时从异步时钟FIFO中弹出一个数据写入到AXI DMA中。

2.3 图像处理

要实现一个激光测距系统，需要对物体反射的激光图像进行处理。尽管在硬件中通过加装红外滤光片能过滤掉绝大部分无用的环境光，但环境中仍会存在干扰源，需要在图像处理中把干扰源去掉，获取真实的激光图像。

图4为图像传感器采集得到的原始的图像，由于线型激光器发射的激光平面与x轴平行，图像中每一列数据最多有一个真实的激光光斑，对每一列图像数据进行局部峰值检测，结果如图5所示。由于图像分辨率的限制，为了提高测距精度，光斑的峰值位置采用加权的方法得到其亚像素位置。

图4 原始图像

图5 峰值检测结果

图像中每一个光斑位置与载体坐标系中的一个位置一一对应，需要通过数据标定来获取该对应关系。

通过标定的数据对应关系得到了点云数据后，对点云数据进行邻近点聚类、线段分割，删除不合理或距离原点太远的数据，识别出墙壁位置。

2.4 定位算法

得到墙壁位置信息后，需要利用墙壁位置信息估计电脑鼠的实际位置。国际IEEE电脑鼠走迷宫比赛规则中的场地为由180 mm×180 mm的单元组成的迷宫，设迷宫坐标原点与迷宫的左下角重合，则迷宫具有以下特点：

(1)墙壁端点坐标为180 mm的整数倍；

(2)墙壁与坐标轴(x轴或y轴)平行；

(3)墙壁与间隔为180 mm的正方形网格重合。

根据上述特点，可以使用最小二乘法估计电脑鼠在迷宫中的姿态。

2.4.1 估计电脑鼠的偏航角

估计电脑鼠的偏航角可以看作一个最优化问题：给定电脑鼠上一时刻偏航角θN，计算一个尽可能接近于0的θ，使得通过图像处理后得到的线段特征在绕着原点旋转(θN+θ)后与坐标轴(x轴或y轴)平行，则当前时刻的电脑鼠偏航角为(θN+θ)。

首先考虑如下表示单一线段的点云数据的角度匹配问题，已知点云数据pi=(xi，yi)，i∈{0, 1, … ,Np-1}表示该点云数据的线段已经绕原点旋转θN，因此表示该点云数据的线段已经与x轴接近平行，求角度θ，使得点云数据继续绕原点旋转θ后，点云数据与x轴尽可能平行，即目标函数：

取最小值，其中：

令J对θ的偏导为0，得：

(1)

对公式(1)左右同时除以cos2θ得：

Aw2-Bw-A=0

解此一元二次方程可得w，通过θ=arctan(w)即可求得θ。然而，此方程会有两个根，分别为w1和w2，它们所表示的角度相差90°，由于θ=arctan(w)为单调函数，且θ≈0，取w1和w2绝对值的较小者求θ。

同理，对于点云数据所表示的线段与y轴接近平行的角度匹配，亦可求得类似的形式。推广到表示若干线段的点云数据，需要判断其最近邻匹配角度(匹配到x轴还是y轴)，解方程，即得到最小二乘形式的θ。

2.4.2 估计电脑鼠的坐标

估计电脑鼠的坐标可以看作一个最优化问题：给定电脑鼠上一时刻在迷宫坐标系的坐标值(x，y)，计算一个尽可能接近于0的Δx和Δy，使得通过图像处理后得到的线段特征在平移(x+Δx，y+Δy)后与间隔为180 mm的网格重合。则当前时刻的电脑鼠坐标为(x+Δx，y+Δy)。

2.4.3 SLAM结果

图6所示为电脑鼠经过SLAM后的结果。其中黑色线条表示识别出该位置具有墙壁，虚线表示该位置尚未探索，梯形框表示电脑鼠的有效视野，有一根指向梯形框的黑色线条表示电脑鼠的位置与朝向，黑点则表示当前时刻的点云数据。

图6 SLAM结果

3 结论

成品电脑鼠总重量为100.06 g，车体大小为7.45 cm(宽)×6.6 cm(长)。与使用红外对管作为位置传感器的传统电脑鼠相比，本文的电脑鼠可以充分利用迷宫信息进行路径规划，以机械结构允许的最大速度运动且不与墙壁发生碰撞。其最高运动速度为3.5 m/s，最大加速度为15 m/s2，可以完成迷宫探索及快速冲刺功能。

[1] 张嘉夫,李迅波,余岷,等. 基于ARM的走迷宫电脑鼠的设计与实现[J]. 工业控制计算机,2013,26(11):136-138.

[2] 金余义,刘克申,任阳晖. 基于测距红外传感器的轮式迷宫机器人设计[J]. 单片机与嵌入式系统应用,2013(9):74-77.

[3] Zhang Haoming, LIAN S P, Wang Yinghai. Calibrate the turning of micromouse[J]. Advanced Materials Research, 2014, 971:1088-1091.

[4] THRUN S, LEONARO J J. Simultaneous localization and mapping[A]. Springer Handbook of Robotics[M]. 2007: 871-889.

[5] 吴长江,赵不贿,郑博,等. 基于FPGA的动态目标跟踪系统设计[J]. 电子技术应用,2010,36(3):45-50.

[6] Xilinx Inc. Zynq-7000 all programmable SoC technical reference manual (v1.6.1) [Z].2013.

Micromouse using SLAM

Su Hao1, Huang Xinhan1, Wang Zhenyan2

(1. School of Automation, Huazhong University of Science and Technology, Wuhan 430074, China;2. School of Electrical and Electronic Engineering, Huazhong University of Science and Technology, Wuhan 430074, China)

Most micromouses use infrared tubes as simpler position sensors, but this paper built a small laser ranging system with linear laser and camera on a micromouse and run simultaneous localization and mapping (SLAM) on it. The camera output images with resolution of 188×120 at speed of 240 f/s. The system used Zynq-7010 as main controller and built an image acquisition system on its programmable logic part. Also we built a real-time Linux system to realize the image processing, sensor data fusion and maze solver algorithm on its processing system part. We made a micromouse with total weight of 100.06 g and body size of 7.45 cm (width) × 6.6 cm (length), which can move fast and stably in the maze.

micromouse; SLAM; laser ranging; Zynq

TP242

A

10.19358/j.issn.1674- 7720.2017.12.016

苏豪，黄心汉，王贞炎.采用SLAM方法的电脑鼠[J].微型机与应用，2017,36(12)：54-56，60.

2016-12-22)

苏豪(1992-)，男，硕士研究生，主要研究方向：图像处理与计算机视觉，嵌入式系统。

黄心汉(1946-)，男，博士，教授，主要研究方向：智能控制、智能机器人、图像处理与模式识别、信息融合。

王贞炎(1984-)，男，硕士，工程师，主要研究方向：模拟信号调理、数字信号处理和FPGA应用技术。