尚明超，楊斌，張翠芳

(西南交通大學(xué) 智能系統(tǒng)與控制實(shí)驗(yàn)室，成都 611756)

二維碼修正EKF-SLAM定位的室內(nèi)無(wú)人駕駛小車(chē)

尚明超，楊斌，張翠芳

(西南交通大學(xué) 智能系統(tǒng)與控制實(shí)驗(yàn)室，成都 611756)

無(wú)人車(chē)的室內(nèi)自主駕駛中常用到EKF-SLAM(Simultaneous Localization and Mapping)技術(shù)。單純的編碼器SLAM技術(shù)，由于其長(zhǎng)時(shí)間的運(yùn)行會(huì)導(dǎo)致累計(jì)誤差過(guò)大，使得定位非常不準(zhǔn)確，所以需要一種技術(shù)，對(duì)位置信息的定位方式加以輔助，考慮到二維碼識(shí)別技術(shù)的方便性以及易用性，本文采用二維碼人工路標(biāo)作為絕對(duì)定位方式的標(biāo)簽，提升EKF-SLAM的定位準(zhǔn)度，并利用擴(kuò)展卡爾曼濾波進(jìn)行多數(shù)據(jù)融合，通過(guò)實(shí)驗(yàn)驗(yàn)證了實(shí)驗(yàn)該方案的可行性與實(shí)用性。

二維碼人工路標(biāo)；EKF-SLAM；數(shù)據(jù)融合；MEMS；STM32F205；MINNOWBOARD

引 言

無(wú)人車(chē)的自主駕駛定位分為室內(nèi)定位和室外定位。室外定位中常用GPS/北斗提供的絕對(duì)定位信息輔助車(chē)子自身的相對(duì)定位；在室內(nèi)，由于接收不到衛(wèi)星的信號(hào)，需要采用其他傳感器提供絕對(duì)位置信息，常用的室內(nèi)的定位方式有RFID、RSSI、WUB等，RFID和RSSI方式的定位精度在幾米或者幾分米內(nèi)，無(wú)法滿足室內(nèi)定位的需要，而WUB需要借助基站才能達(dá)到理想的定位效果，價(jià)格高昂且設(shè)備安裝存在一定局限。由于二維碼識(shí)別方便易行，視覺(jué)定位可以提供可靠的定位精度，故本文采用二維碼作為人工路標(biāo)，作為絕對(duì)定位的定位標(biāo)簽，并結(jié)合擴(kuò)展卡爾曼濾波進(jìn)行標(biāo)簽式的定位建圖，同時(shí)再利用擴(kuò)展卡爾曼融合9軸MEMS，計(jì)算小車(chē)的航向角，進(jìn)而實(shí)現(xiàn)小車(chē)的自主定位。

1 系統(tǒng)總體結(jié)構(gòu)

如圖1所示，本文的實(shí)驗(yàn)平臺(tái)總體結(jié)構(gòu)分為以Intel minnowboard為主的上層嵌入式應(yīng)用平臺(tái)和基于STM32F205單片機(jī)的下層嵌入式平臺(tái)。整車(chē)以算法主體-控制決策(控制命令)-反饋信息(里程數(shù)據(jù))形成完整的閉環(huán)控制。

Intel的minnowboard開(kāi)發(fā)板搭載主頻為1.33 GHz的雙核x86凌動(dòng)CPU，通過(guò)外接無(wú)線網(wǎng)卡與數(shù)據(jù)監(jiān)測(cè)終端(本文實(shí)驗(yàn)中利用PC監(jiān)測(cè))組成局域網(wǎng)，用戶或者開(kāi)發(fā)人員可以通過(guò)監(jiān)測(cè)終端對(duì)車(chē)子的實(shí)時(shí)采集數(shù)據(jù)進(jìn)行監(jiān)測(cè)。開(kāi)發(fā)板裝有的Ubuntu14.04系統(tǒng)作為主要的軟件開(kāi)發(fā)環(huán)境。在小車(chē)的運(yùn)行過(guò)程中，攝像頭采集二維碼圖像信息，通過(guò)USB接口傳輸給嵌入式開(kāi)發(fā)板，并由二維碼定位識(shí)別算法。

圖1 實(shí)驗(yàn)平臺(tái)總體結(jié)構(gòu)

電動(dòng)小車(chē)的航向角由STM32F103(即MEMS慣導(dǎo))融合三軸加速度計(jì)、三軸磁力計(jì)、三軸陀螺儀計(jì)算得到。為了得到較為準(zhǔn)確的編碼器采樣數(shù)據(jù)，本文中每隔70 ms進(jìn)行一次編碼器數(shù)據(jù)(車(chē)速)的采樣，而MEMS(慣導(dǎo))每隔35 ms進(jìn)行一次數(shù)據(jù)融合，在EKF-SLAM進(jìn)行多數(shù)據(jù)源融合的過(guò)程中，主算法每執(zhí)行一個(gè)周期，MEMS需要進(jìn)行兩次數(shù)據(jù)融合,并通過(guò)串口上傳小車(chē)的航向角數(shù)據(jù)。運(yùn)行在嵌入式開(kāi)發(fā)板內(nèi)的,EKF-SLAM算法將識(shí)別后的二維碼位置信息、編碼器的里程信息和MEMS提供的小車(chē)的航向角度信息進(jìn)行數(shù)據(jù)融合和建圖定位，并將決策信息(小車(chē)要行駛的速度和角度)通過(guò)串口下傳給單片機(jī)，再由單片機(jī)驅(qū)動(dòng)底層電路。

底層單片機(jī)STM32F205通過(guò)串口接收上層應(yīng)用平臺(tái)接收過(guò)來(lái)的控制命令(速度信息和角度信息)，并將其轉(zhuǎn)化為PWM控制信號(hào)，通過(guò)電機(jī)驅(qū)動(dòng)器驅(qū)動(dòng)左右車(chē)輪，因?yàn)楸緦?shí)驗(yàn)平臺(tái)是通過(guò)左右輪差速改變方向，所以通過(guò)控制左右輪的轉(zhuǎn)速進(jìn)行方向的改變，無(wú)需通過(guò)舵機(jī)來(lái)控制車(chē)的方向。同時(shí)，單片機(jī)采集左右車(chē)輪編碼器的數(shù)據(jù)，形成反饋數(shù)據(jù)通過(guò)串口上傳至嵌入式開(kāi)發(fā)板。

2 算法設(shè)計(jì)

SLAM作為室內(nèi)定位方法有很多種，有ORB-SLAM、RGBD-SLAM、LSD-SLAM、RBPF-SLAM方式等，但是其中大部分方式依賴(lài)攝像頭，計(jì)算量大，且極其消耗計(jì)算資源，考慮到嵌入式開(kāi)發(fā)平臺(tái)的計(jì)算性能，這些SLAM方法很難在嵌入式平臺(tái)滿足實(shí)時(shí)要求?；谝陨峡紤]，本文使用基于人工路標(biāo)的EKF-SLAM方法。常用的人工路標(biāo)的定位方式有WiFi、RSSI、RFID等，但是信號(hào)類(lèi)的定位方式常會(huì)受到電磁信號(hào)的干擾以及原本比較差的定位精度(分米或者分米以上)，并不適合被用作人工標(biāo)簽。二維碼具有簡(jiǎn)單易行的操作性、低廉的價(jià)格和較準(zhǔn)確的相機(jī)定位方法，所以本文使用二維碼作為人工路標(biāo)并結(jié)合EKF-SLAM設(shè)計(jì)定位方法。

圖2是利用二維碼人工路標(biāo)的EKF-SLAM設(shè)計(jì)框圖。首先利用擴(kuò)展卡爾曼濾波計(jì)算MEMS的姿態(tài)角,EKF-SLAM融合二維碼的位置信息、編碼器數(shù)據(jù)、MEMS的航向角，實(shí)現(xiàn)小車(chē)的定位和建圖。

圖2 EKF-SLAM設(shè)計(jì)框圖

設(shè)定系統(tǒng)的狀態(tài)向量為x=[xk,yk,yawk,l1,l2...lm]，控制量為uk=(vk,wk)T，其中(xk,yk),yawk為小車(chē)在k時(shí)刻的二維坐標(biāo)和航向角，li=[xi,yi]為第i個(gè)Landmarks在二維平面上的全局坐標(biāo)。vk,wk為車(chē)子在k到k+1時(shí)刻車(chē)子行駛的速度和角速度(由正交編碼器和MEMS計(jì)算得來(lái))。算法模型如下：

(1) 預(yù) 測(cè)

由小車(chē)的運(yùn)動(dòng)學(xué)方程，離散時(shí)間下的狀態(tài)預(yù)測(cè)方程為(T為采樣時(shí)間)：

(1)

預(yù)測(cè)協(xié)方差(其中F(k)為轉(zhuǎn)移矩陣，w(k)為狀態(tài)噪聲)：

(2)

(2) 更 新

設(shè)定觀測(cè)模型為二維向量

(3)

其中：(xl,yl)為本次迭代觀測(cè)到的第l個(gè)Landmarks的全局坐標(biāo)(已存儲(chǔ)，且已知)，(x,y)為小車(chē)此時(shí)的全局二維坐標(biāo)，θ為此時(shí)刻的航向角。假定相機(jī)到二維碼人工路標(biāo)的距離和方向角為：

(4)

xc、yc、zc為每個(gè)二維碼檢測(cè)符點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)。

卡爾曼增益：

(5)

狀態(tài)更新：

(6)

更新協(xié)方差：

(7)

其中，H(k)為雅克比矩陣，v(k)為觀測(cè)誤差，R(k)為過(guò)程噪聲。

小車(chē)在二維平面的航向角的計(jì)算由MEMS提供，考慮到濾波效果和數(shù)據(jù)的非線性，這部分?jǐn)?shù)據(jù)融合仍然采用擴(kuò)展卡爾曼濾波，至此整個(gè)算法流程已經(jīng)全部給出。

3 二維碼識(shí)別過(guò)程

實(shí)驗(yàn)中利用PnP算法計(jì)算相機(jī)相對(duì)二維碼的距離，如圖3所示，將二維碼中的A、B、C、D、E、F檢測(cè)符點(diǎn)作為PnP算法的控制點(diǎn)，且每組檢測(cè)符點(diǎn)的世界坐標(biāo)位置已知(實(shí)驗(yàn)全部測(cè)量)，根據(jù)二維碼中符點(diǎn)深色-淺色-深色-淺色-深色(相應(yīng)元素所占比例為1:1:3:1:1)的規(guī)律，檢測(cè)出每組二維碼中的6個(gè)定位符點(diǎn)，再根據(jù)世界坐標(biāo)系與相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系即可求出每個(gè)檢測(cè)符點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)xc、yc、zc。式(4)中的未知數(shù)全部給出，通過(guò)擴(kuò)展卡爾曼濾波的迭代即可完成EKF-SLAM的數(shù)據(jù)融合和建圖過(guò)程。其中一組檢測(cè)效果圖：

圖3是小車(chē)在(0.50,2.10,0)處觀測(cè)到的二維碼人工路標(biāo)，此時(shí)相機(jī)的坐標(biāo)為(0.50,2.10,0.84)。

圖3 二維碼人工路標(biāo)

表1為此二維碼人工路標(biāo)在世界坐標(biāo)系下的坐標(biāo)和相應(yīng)相機(jī)坐標(biāo)系下的坐標(biāo)以及相應(yīng)的誤差,左半部分是在世界坐標(biāo)系下的坐標(biāo)值，右半部分是在相機(jī)坐標(biāo)系下的坐標(biāo)值。

表1 二維碼人工路標(biāo)數(shù)據(jù)

4 實(shí)驗(yàn)測(cè)試結(jié)果

經(jīng)過(guò)改造的電動(dòng)小車(chē)搭載Intel公司的雙核處理器開(kāi)發(fā)板，底層驅(qū)動(dòng)核心板為STM32F205單片機(jī)。攝像頭安裝在車(chē)子的側(cè)面扶手處，并確定二維碼能夠出現(xiàn)在攝像頭的視角內(nèi)。本文實(shí)驗(yàn)環(huán)境大小為18 m×18 m，6組二維碼標(biāo)簽每隔約7 m粘貼在實(shí)驗(yàn)室的墻壁上和實(shí)驗(yàn)桌上。小車(chē)行駛在普通的光照條件下，行駛過(guò)程中，攝像頭一直處于打開(kāi)狀態(tài)并捕捉二維碼，因?yàn)榻柚鷝bar二維碼識(shí)別庫(kù)，所以只要不用強(qiáng)烈的光源照射攝像頭，攝像頭均可識(shí)別粘貼的每組二維碼上的檢測(cè)符點(diǎn)，實(shí)驗(yàn)中電動(dòng)小車(chē)行走了460 s，小車(chē)的行駛速度是0.3 m/s，共繞著實(shí)驗(yàn)室旋轉(zhuǎn)了4圈，小車(chē)在每組二維碼標(biāo)簽處發(fā)生轉(zhuǎn)向，平均每圈發(fā)生6次轉(zhuǎn)向。取觀測(cè)噪聲：σr為0.1，σb為0.05。

4.1 航向角的數(shù)據(jù)仿真結(jié)果

圖4是小車(chē)運(yùn)行4圈的航向角的仿真圖，共產(chǎn)生14 000個(gè)點(diǎn)，平均每圈為一個(gè)周期，每周期小車(chē)發(fā)生6次方向的改變。為計(jì)算方便，小車(chē)航向角的計(jì)算范圍為0°～360°。實(shí)驗(yàn)過(guò)程中小車(chē)的初始的航向角度為257.54°，由圖4可以看出小車(chē)運(yùn)行階段，MEMS沒(méi)有產(chǎn)生嚴(yán)重的偏移誤差。

4.2 小車(chē)的行走軌跡結(jié)果圖

本文實(shí)驗(yàn)是采集小車(chē)運(yùn)行時(shí)的里程數(shù)據(jù)和定位數(shù)據(jù)并進(jìn)行仿真，圖5(a)是將二維碼作為人工路標(biāo)的電動(dòng)小車(chē)運(yùn)動(dòng)的實(shí)驗(yàn)結(jié)果圖，其中藍(lán)色線是電動(dòng)小車(chē)的真實(shí)運(yùn)動(dòng)軌跡，紅色部分是EKF-SLAM技術(shù)估計(jì)的電動(dòng)小車(chē)的運(yùn)動(dòng)軌跡;圖5(b)是電動(dòng)小車(chē)未用二維碼作為人工路標(biāo)的小車(chē)的運(yùn)動(dòng)實(shí)驗(yàn)結(jié)果圖;圖5(c)上半部分是小車(chē)的定位偏差，偏差在0～0.4 m之間，下半部分中的紅色部分是

小車(chē)在x軸的定位偏差，誤差范圍為0.1～0.15 m，藍(lán)色部分是小車(chē)在y軸的定位偏差，誤差在0～0.15 m之間。圖5(d)是小車(chē)運(yùn)用的編碼器的偏移誤差，誤差在0～0.35 m之間。由此可以看出,在采用二維碼作為人工路標(biāo)的時(shí)候EKF-SLAM技術(shù)的估計(jì)運(yùn)動(dòng)軌跡與真實(shí)軌跡基本重合，定位誤差也相對(duì)較小。而未采用二維碼作為人工路標(biāo)，小車(chē)只運(yùn)行幾秒，它的估計(jì)軌跡與真實(shí)軌跡發(fā)生很大的偏差，實(shí)驗(yàn)結(jié)果驗(yàn)證了二維碼人工路標(biāo)修正EKF-SLAM定位的可行性與實(shí)用性。

圖5 小車(chē)行走軌跡圖

結(jié) 語(yǔ)

本文使用的二維碼人工路標(biāo)有效地提升了EKF-SLAM的定位精度，并且基本可以達(dá)到實(shí)時(shí)性要求，解決了其他算法的高計(jì)算量和其他類(lèi)傳感器安裝不便、價(jià)格高昂等問(wèn)題，但是通過(guò)實(shí)驗(yàn)結(jié)果可以看出定位精度仍然沒(méi)有達(dá)到cm級(jí)別，在定位過(guò)程中當(dāng)攝像頭連續(xù)被遮

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