二維碼修正EKF-SLAM定位的室內無人駕駛小車

尚明超，楊斌，張翠芳

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

二維碼修正EKF-SLAM定位的室內無人駕駛小車

尚明超，楊斌，張翠芳

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

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

二維碼人工路標；EKF-SLAM；數據融合；MEMS；STM32F205；MINNOWBOARD

引 言

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

1 系統(tǒng)總體結構

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

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

圖1 實驗平臺總體結構

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

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

2 算法設計

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

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

圖2 EKF-SLAM設計框圖

設定系統(tǒng)的狀態(tài)向量為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ài)預測方程為(T為采樣時間)：

(1)

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

(2)

(2) 更 新

設定觀測模型為二維向量

(3)

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

(4)

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

卡爾曼增益：

(5)

狀態(tài)更新：

(6)

更新協(xié)方差：

(7)

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

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

3 二維碼識別過程

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

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

圖3 二維碼人工路標

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

表1 二維碼人工路標數據

4 實驗測試結果

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

4.1 航向角的數據仿真結果

圖4是小車運行4圈的航向角的仿真圖，共產生14 000個點，平均每圈為一個周期，每周期小車發(fā)生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技術的估計運動軌跡與真實軌跡基本重合，定位誤差也相對較小。而未采用二維碼作為人工路標，小車只運行幾秒，它的估計軌跡與真實軌跡發(fā)生很大的偏差，實驗結果驗證了二維碼人工路標修正EKF-SLAM定位的可行性與實用性。

圖5 小車行走軌跡圖

結 語

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