二维码修正EKF-SLAM定位的室内无人驾驶小车

尚明超，杨斌，张翠芳

(西南交通大学 智能系统与控制实验室，成都 611756)

二维码修正EKF-SLAM定位的室内无人驾驶小车

尚明超，杨斌，张翠芳

(西南交通大学 智能系统与控制实验室，成都 611756)

无人车的室内自主驾驶中常用到EKF-SLAM(Simultaneous Localization and Mapping)技术。单纯的编码器SLAM技术，由于其长时间的运行会导致累计误差过大，使得定位非常不准确，所以需要一种技术，对位置信息的定位方式加以辅助，考虑到二维码识别技术的方便性以及易用性，本文采用二维码人工路标作为绝对定位方式的标签，提升EKF-SLAM的定位准度，并利用扩展卡尔曼滤波进行多数据融合，通过实验验证了实验该方案的可行性与实用性。

二维码人工路标；EKF-SLAM；数据融合；MEMS；STM32F205；MINNOWBOARD

引 言

无人车的自主驾驶定位分为室内定位和室外定位。室外定位中常用GPS/北斗提供的绝对定位信息辅助车子自身的相对定位；在室内，由于接收不到卫星的信号，需要采用其他传感器提供绝对位置信息，常用的室内的定位方式有RFID、RSSI、WUB等，RFID和RSSI方式的定位精度在几米或者几分米内，无法满足室内定位的需要，而WUB需要借助基站才能达到理想的定位效果，价格高昂且设备安装存在一定局限。由于二维码识别方便易行，视觉定位可以提供可靠的定位精度，故本文采用二维码作为人工路标，作为绝对定位的定位标签，并结合扩展卡尔曼滤波进行标签式的定位建图，同时再利用扩展卡尔曼融合9轴MEMS，计算小车的航向角，进而实现小车的自主定位。

1 系统总体结构

如图1所示，本文的实验平台总体结构分为以Intel minnowboard为主的上层嵌入式应用平台和基于STM32F205单片机的下层嵌入式平台。整车以算法主体-控制决策(控制命令)-反馈信息(里程数据)形成完整的闭环控制。

Intel的minnowboard开发板搭载主频为1.33 GHz的双核x86凌动CPU，通过外接无线网卡与数据监测终端(本文实验中利用PC监测)组成局域网，用户或者开发人员可以通过监测终端对车子的实时采集数据进行监测。开发板装有的Ubuntu14.04系统作为主要的软件开发环境。在小车的运行过程中，摄像头采集二维码图像信息，通过USB接口传输给嵌入式开发板，并由二维码定位识别算法。

图1 实验平台总体结构

电动小车的航向角由STM32F103(即MEMS惯导)融合三轴加速度计、三轴磁力计、三轴陀螺仪计算得到。为了得到较为准确的编码器采样数据，本文中每隔70 ms进行一次编码器数据(车速)的采样，而MEMS(惯导)每隔35 ms进行一次数据融合，在EKF-SLAM进行多数据源融合的过程中，主算法每执行一个周期，MEMS需要进行两次数据融合,并通过串口上传小车的航向角数据。运行在嵌入式开发板内的,EKF-SLAM算法将识别后的二维码位置信息、编码器的里程信息和MEMS提供的小车的航向角度信息进行数据融合和建图定位，并将决策信息(小车要行驶的速度和角度)通过串口下传给单片机，再由单片机驱动底层电路。

底层单片机STM32F205通过串口接收上层应用平台接收过来的控制命令(速度信息和角度信息)，并将其转化为PWM控制信号，通过电机驱动器驱动左右车轮，因为本实验平台是通过左右轮差速改变方向，所以通过控制左右轮的转速进行方向的改变，无需通过舵机来控制车的方向。同时，单片机采集左右车轮编码器的数据，形成反馈数据通过串口上传至嵌入式开发板。

2 算法设计

SLAM作为室内定位方法有很多种，有ORB-SLAM、RGBD-SLAM、LSD-SLAM、RBPF-SLAM方式等，但是其中大部分方式依赖摄像头，计算量大，且极其消耗计算资源，考虑到嵌入式开发平台的计算性能，这些SLAM方法很难在嵌入式平台满足实时要求。基于以上考虑，本文使用基于人工路标的EKF-SLAM方法。常用的人工路标的定位方式有WiFi、RSSI、RFID等，但是信号类的定位方式常会受到电磁信号的干扰以及原本比较差的定位精度(分米或者分米以上)，并不适合被用作人工标签。二维码具有简单易行的操作性、低廉的价格和较准确的相机定位方法，所以本文使用二维码作为人工路标并结合EKF-SLAM设计定位方法。

图2是利用二维码人工路标的EKF-SLAM设计框图。首先利用扩展卡尔曼滤波计算MEMS的姿态角,EKF-SLAM融合二维码的位置信息、编码器数据、MEMS的航向角，实现小车的定位和建图。

图2 EKF-SLAM设计框图

设定系统的状态向量为x=[xk,yk,yawk,l1,l2...lm]，控制量为uk=(vk,wk)T，其中(xk,yk),yawk为小车在k时刻的二维坐标和航向角，li=[xi,yi]为第i个Landmarks在二维平面上的全局坐标。vk,wk为车子在k到k+1时刻车子行驶的速度和角速度(由正交编码器和MEMS计算得来)。算法模型如下：

(1) 预 测

由小车的运动学方程，离散时间下的状态预测方程为(T为采样时间)：

(1)

预测协方差(其中F(k)为转移矩阵，w(k)为状态噪声)：

(2)

(2) 更 新

设定观测模型为二维向量

(3)

其中：(xl,yl)为本次迭代观测到的第l个Landmarks的全局坐标(已存储，且已知)，(x,y)为小车此时的全局二维坐标，θ为此时刻的航向角。假定相机到二维码人工路标的距离和方向角为：

(4)

xc、yc、zc为每个二维码检测符点在相机坐标系下的坐标。

卡尔曼增益：

(5)

状态更新：

(6)

更新协方差：

(7)

其中，H(k)为雅克比矩阵，v(k)为观测误差，R(k)为过程噪声。

小车在二维平面的航向角的计算由MEMS提供，考虑到滤波效果和数据的非线性，这部分数据融合仍然采用扩展卡尔曼滤波，至此整个算法流程已经全部给出。

3 二维码识别过程

实验中利用PnP算法计算相机相对二维码的距离，如图3所示，将二维码中的A、B、C、D、E、F检测符点作为PnP算法的控制点，且每组检测符点的世界坐标位置已知(实验全部测量)，根据二维码中符点深色-浅色-深色-浅色-深色(相应元素所占比例为1:1:3:1:1)的规律，检测出每组二维码中的6个定位符点，再根据世界坐标系与相机坐标系的转换关系即可求出每个检测符点在相机坐标系下的坐标xc、yc、zc。式(4)中的未知数全部给出，通过扩展卡尔曼滤波的迭代即可完成EKF-SLAM的数据融合和建图过程。其中一组检测效果图：

图3是小车在(0.50,2.10,0)处观测到的二维码人工路标，此时相机的坐标为(0.50,2.10,0.84)。

图3 二维码人工路标

表1为此二维码人工路标在世界坐标系下的坐标和相应相机坐标系下的坐标以及相应的误差,左半部分是在世界坐标系下的坐标值，右半部分是在相机坐标系下的坐标值。

表1 二维码人工路标数据

4 实验测试结果

经过改造的电动小车搭载Intel公司的双核处理器开发板，底层驱动核心板为STM32F205单片机。摄像头安装在车子的侧面扶手处，并确定二维码能够出现在摄像头的视角内。本文实验环境大小为18 m×18 m，6组二维码标签每隔约7 m粘贴在实验室的墙壁上和实验桌上。小车行驶在普通的光照条件下，行驶过程中，摄像头一直处于打开状态并捕捉二维码，因为借助zbar二维码识别库，所以只要不用强烈的光源照射摄像头，摄像头均可识别粘贴的每组二维码上的检测符点，实验中电动小车行走了460 s，小车的行驶速度是0.3 m/s，共绕着实验室旋转了4圈，小车在每组二维码标签处发生转向，平均每圈发生6次转向。取观测噪声：σr为0.1，σb为0.05。

4.1 航向角的数据仿真结果

图4是小车运行4圈的航向角的仿真图，共产生14 000个点，平均每圈为一个周期，每周期小车发生6次方向的改变。为计算方便，小车航向角的计算范围为0°～360°。实验过程中小车的初始的航向角度为257.54°，由图4可以看出小车运行阶段，MEMS没有产生严重的偏移误差。

4.2 小车的行走轨迹结果图

本文实验是采集小车运行时的里程数据和定位数据并进行仿真，图5(a)是将二维码作为人工路标的电动小车运动的实验结果图，其中蓝色线是电动小车的真实运动轨迹，红色部分是EKF-SLAM技术估计的电动小车的运动轨迹;图5(b)是电动小车未用二维码作为人工路标的小车的运动实验结果图;图5(c)上半部分是小车的定位偏差，偏差在0～0.4 m之间，下半部分中的红色部分是

小车在x轴的定位偏差，误差范围为0.1～0.15 m，蓝色部分是小车在y轴的定位偏差，误差在0～0.15 m之间。图5(d)是小车运用的编码器的偏移误差，误差在0～0.35 m之间。由此可以看出,在采用二维码作为人工路标的时候EKF-SLAM技术的估计运动轨迹与真实轨迹基本重合，定位误差也相对较小。而未采用二维码作为人工路标，小车只运行几秒，它的估计轨迹与真实轨迹发生很大的偏差，实验结果验证了二维码人工路标修正EKF-SLAM定位的可行性与实用性。

图5 小车行走轨迹图

结 语

本文使用的二维码人工路标有效地提升了EKF-SLAM的定位精度，并且基本可以达到实时性要求，解决了其他算法的高计算量和其他类传感器安装不便、价格高昂等问题，但是通过实验结果可以看出定位精度仍然没有达到cm级别，在定位过程中当摄像头连续被遮

Indoor Self-driving of QR Code Modified EKF-SLAM

Shang Mingchao,Yang Bin,Zhang Cuifang

(Intelligent System and Control Laboratory,Southwest Jiaotong University,Chengdu 611756,China)

EKF-SLAM(Simultaneous Localization and Mapping) is often used in the localization method of self-driving.If there is only encoder sensor in this way,the accumulated error will be greater and greater because of long-time running and the accuracy of positioning will become inaccuracy.So the location information may be secondary.Thanks to convenience and usability of QR code,the method of QR code artificial landmarks that support absolute position information is adopted in this paper,that increases the positioning accuracy.In the meanwhile,the Kalman filter is used to fusion the multiple data.The experiment results show that the scheme is feasibility and practicality.

QR code artificial landmarks;EKF-SLAM;data fusion;MEMS;STM32F205;MINNOWBOARD

TP242.6

A