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

尚明超，楊斌，張翠芳

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

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

尚明超，楊斌，張翠芳

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

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

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

引 言

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

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

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

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

圖1 實驗平臺總體結(jié)構(gòu)

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

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

2 算法設(shè)計

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

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

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

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

(1) 預(yù) 測

由小車的運動學(xué)方程，離散時間下的狀態(tài)預(yù)測方程為(T為采樣時間)：

(1)

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

(2)

(2) 更 新

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

(3)

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

(4)

xc、yc、zc為每個二維碼檢測符點在相機坐標(biāo)系下的坐標(biāo)。

卡爾曼增益：

(5)

狀態(tài)更新：

(6)

更新協(xié)方差：

(7)

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

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

3 二維碼識別過程

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

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

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

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

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

4 實驗測試結(jié)果

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

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

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

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

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

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

圖5 小車行走軌跡圖

結(jié) 語

本文使用的二維碼人工路標(biāo)有效地提升了EKF-SLAM的定位精度，并且基本可以達(dá)到實時性要求，解決了其他算法的高計算量和其他類傳感器安裝不便、價格高昂等問題，但是通過實驗結(jié)果可以看出定位精度仍然沒有達(dá)到cm級別，在定位過程中當(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