多房間、高精度的室內(nèi)機器人SLMN導(dǎo)航算法

蘭 勇, 馬 力, 馮 曉, 夏國廷

(青島大學(xué) 電氣工程學(xué)院，山東 青島 266071)

0 引 言

機器人在室內(nèi)應(yīng)達到在短時間內(nèi)實現(xiàn)室內(nèi)高效率打掃，減少重復(fù)清掃。傳統(tǒng)的掃地機采用隨機清掃模式，覆蓋面積小、清掃效率低、易碰撞家具。采用z字型全覆蓋算法可有效減少隨機清掃帶來的弊端，但前提是掃地機在室內(nèi)能穩(wěn)定走直線。mpu6050陀螺儀存在累計誤差和漂移問題，利用PID控制的雙輪也存在機械誤差和打滑等問題，使得掃地機的位置和方向發(fā)生偏移，單獨依靠陀螺儀無法實現(xiàn)正常全屋清掃。因此，研究了室內(nèi)機器人SLMN導(dǎo)航算法。

文獻[1]中提出了一種基于地圖匹配的慣性導(dǎo)航軌跡推算算法(Improved Pedestrian Dead Reckoning, IPDR)。但是，由于傳感器累計誤差較大，導(dǎo)致其導(dǎo)航精確度不高。Dardari等[2-3]提出了Map Matching算法，與IPDR相比成本更低且算法也更加的高效。在此基礎(chǔ)上，文獻[4-5]中提出磁圖匹配算法，但是該算法復(fù)雜度過高，精確度與之前相比并未得到明顯改善且路徑改變困難。文獻[6-7]中采用基于濾波方案的貝葉斯算法(Bayesian Inference based Filtering Methods, BIFM)的WiFi和藍牙方案，精確度得到改善，但WiFi路由器定位偏差依舊較大且成本較高。文獻[8-9]中設(shè)備通過在一個預(yù)先確定的路徑位置進行校準(zhǔn)，但需要大量的數(shù)據(jù)集進行校準(zhǔn)，很難進行管理，且校準(zhǔn)結(jié)果依賴于定位精度，還需要精確的無線電地圖。文獻[10-11]中部分系統(tǒng)選擇校準(zhǔn)自由的方法(Calibration Free Approaches, CFA)，雖然成本得到了控制，但在遞歸式學(xué)習(xí)系統(tǒng)(Recursive Style Learning Systems, RSLS)中存在例如估計最大化(Estimation-maximization)或SLAM等不確定性和噪音。這些算法收斂速度比輸入輸出樣式的學(xué)習(xí)算法慢，當(dāng)初始誤差或數(shù)據(jù)噪聲太大時，可能收斂于一個錯誤的解決方案。文獻[12-14]中提出基于WiFi的指紋定位系統(tǒng)，需要對信號強度進行采樣，并提前構(gòu)建無線電地圖，可靠地使用WiFi覆蓋信息和加速度計讀數(shù)，但是WiFi的指紋識別需要大量的初始化工作，收益甚微。文獻[15]中提出SLAM視覺導(dǎo)航，該方案靈活度高、可視化效果好，但是受環(huán)境光線的影響較大且位置的準(zhǔn)確性也較為一般，由于引入了Kinect景深攝像機，成本也大大提高。

在保證高覆蓋率的情況下充分考慮成本因素，本文選擇了利用紅外線和超聲波輔助陀螺儀進行定位導(dǎo)航的方案：陀螺儀獲取掃地機自身姿態(tài)信息執(zhí)行z字清掃算法，再利用紅外和超聲波進行輔助定位和糾偏，效率提高近70.8%。與現(xiàn)有方案相比，該方案能夠?qū)崿F(xiàn)機器人直線運動、高精度且多房間導(dǎo)航。除此之外，該方案還具有成本低、不確定性和噪音低、易管理、靈活度高、可視化效果好等優(yōu)點，并且在光線較暗的環(huán)境下依然可以高效工作。

1 機器人平臺

為了驗證導(dǎo)航算法，本文采用了智能掃地機和燈塔式導(dǎo)航儀的雙設(shè)備導(dǎo)航平臺。其中，智能掃地機選用了由意法半導(dǎo)體公司推出的基于Cotex-M3架構(gòu)的32位微控制器STM32作為主控芯片；導(dǎo)航儀選用了美國德州儀器公司的msp430超低功耗單片機作為主控芯片。掃地機有兩個由減速電動機控制的輪子，配備了測速片和光電碼盤用來測定雙輪速度，主板上配備了mpu6050六軸陀螺儀以解算自身姿態(tài)。掃地機和導(dǎo)航儀分別在頂部配置了紅外收發(fā)模塊進行通信，機身配備紅外傳感器進行直線校準(zhǔn)、超聲波距離感應(yīng)器以獲取兩者之間的距離。如圖1所示。

(a)(b)

圖1 掃地機機器人(a)與導(dǎo)航儀(b)

2 室內(nèi)機器人定位與導(dǎo)航

燈塔導(dǎo)航儀位于房間中間，定時發(fā)出基本不散射的紅外信號，當(dāng)掃地機檢測到紅外信號所在直線超聲波時開始測得距離L1，同時開始記錄輪子滾過的距離L0，一小段時間后再次用超聲波測距，由于超聲波測距范圍為扇形，此時依然可以測得第2段距離L2，通過解算L0，L1，L2三邊構(gòu)成得三角形可以獲得掃地機當(dāng)前運行角度，與理論值進行比較后就可以糾正掃地機自身運行導(dǎo)致的偏差。如圖2所示。

圖2 機器人的定位與導(dǎo)航

2.1 保證走直線的方案設(shè)計

掃地機器人能夠穩(wěn)定執(zhí)行算法的前提是掃地機器人的兩個輪子本身能大致保持自身的直線運行。由于輪子的兩個電動機電氣特性不可能完全一致，同樣的PWM占空比下，兩個電動機的速度不會完全一致，長時間運行則誤差不斷累計，掃地機器人的運動會偏向一邊。因此，需要對兩邊電動機的速度進行調(diào)整，使電動機轉(zhuǎn)速一致，從而使機器人在輪子不打滑的情況下能夠保持自身的直線運動。本文中在電動機后面安裝兩個26孔光電測速碼盤，通過測量單位時間T內(nèi)光電碼盤產(chǎn)生的脈沖數(shù)N，結(jié)合輪子外圈的周長L，根據(jù)

v=(N/26)×L/T

(1)

就可以獲得左右輪各自的前進速度v。

得到前進速度v后就可以利用經(jīng)典自動控制領(lǐng)域中的PID控制算法對輪子的速度進行調(diào)節(jié)?？紤]到公式轉(zhuǎn)換可能增大計算量和產(chǎn)生誤差，所以可以直接對光電碼盤產(chǎn)生的脈沖數(shù)N進行PID調(diào)節(jié)。

位置型控制

(2)

增量型控制

Δu(n)=u(n)-u(n-1)=

(3)

式中：u(n)為PID輸出值；e(n)=r(t)-c(t)，e(n)為偏差量，r(t)為設(shè)定目標(biāo)值，c(t)=N為光電碼盤當(dāng)前測量值，即單位時間T內(nèi)光電碼盤產(chǎn)生脈沖數(shù)N,其與掃地機器人的兩個輪子前進速度的關(guān)系為式(1)。

本文設(shè)計的機器人采用直流電動機，直接控制占空比以控制速度，采用增量型控制。遞推公式為

u(n)=u(n-1)+Δu(n)=u(n-1)+

KPe(n-2)=u(n-1)+b0e(n)+

b1e(n-1)+b2e(n-2)

(4)

式中：u(n)是第n次給電動機PWM輸出值；u(n-1)是第n-1次給電動機PWM輸出值；e(n)，e(n-1)，e(n-2)分別是第n次、第n-1次、第n-2次目標(biāo)速度與實際速度的偏差量；T為采樣時間，一般為定值；KP，TI，TD為待整定的3個參數(shù)；

流程圖如圖3所示。圖中：uL(n)，uR(n)，eL(n)，eR(n)分別是左右輪的PID輸出量和偏差量。

圖3 PID算法的實現(xiàn)流程圖

通過PID算法，可以消除兩輪的機械誤差，盡可能保證機器人保持自身直線運行。

2.2 定位導(dǎo)航方案設(shè)計

機器人的兩個后輪由獨立的兩個電動機驅(qū)動，雖然采用PID調(diào)節(jié)方式控制可保證兩個輪子的速度相同，但由于打滑和機械誤差，運行一定距離后機器人仍會發(fā)生偏移，無法保證直線運行。本文提出一種基于紅外傳感與定方向噪聲波的角度和位置糾正的方法，如圖4所示。

圖4 基于紅外傳感與定方向噪聲波的角度和位置糾正

導(dǎo)航儀放在房間一側(cè)墻壁中部，垂直該側(cè)墻壁發(fā)射紅外線及兩側(cè)的扇形噪聲波。機器人的運行角度總是與墻壁平行且從上到下、從右向左運行，機器人在房間的中間位置都可收到紅外線和噪聲波，結(jié)合機身裝置判斷自身的位置，若發(fā)現(xiàn)機身偏離直線，便可及時糾正。

基本工作原理：導(dǎo)航儀發(fā)射單束紅外線做參考基準(zhǔn)線，發(fā)射具有一定角度的噪聲波起測距作用。根據(jù)參考基準(zhǔn)線和距離，機器人可以測定位置。判定的原理如下：

步驟1機器人到達紅外參考線后，測得與導(dǎo)航儀相距LOA，也即圓半徑r。

步驟2機器人向前運行LAC后，再測出與導(dǎo)航儀之間的距離為LOC。

步驟3計算機器人水平姿態(tài)角。由幾何關(guān)系，前行了LAC距離，機器人水平姿態(tài)角為

(5)

為提高測定精度，LAC一般較小，則LAB分別與LOA、LOB垂直，LBC=LOC-LOB=LOC-LOA。由幾何關(guān)系得，β=arcsin(LBC/LAC)。則

(6)

相比上一算法降低了運算量，提高了運算速度，且在LAC較小的情況下滿足實際運算精度。圖5是偏移直線運行的角度為10°時的相對誤差關(guān)系。

圖5 偏移直線運行的角度為10°時的相對誤差關(guān)系

由于式(5)和式(6)是反三角函數(shù)，可以直接考慮內(nèi)部公式。將加法和減法的時間定義為t，乘法為10t，除法為20t。對于式(5)，整個等式的時間：

10t+10t+10t+2t+20t+10t+10t=72t

(7)

對于式(6)，整個等式的時間：

t+20t=21t

(8)

然后得到：

(9)

與式(5)、(6)相比，降低了計算復(fù)雜度，效率提高了近70.8%。

為了提高清潔工作的自動化程度，應(yīng)使機器人可跨不同房間工作。其導(dǎo)航算法(見圖6)步驟如下：

(1) 對多個房間進行編碼，房間里的導(dǎo)航儀具有與房間編碼一一對應(yīng)的編碼信號。

(2) 所在房間導(dǎo)航儀發(fā)出獨特的編碼信號。

(3) 機器人進入房間，機器人接收到信號后記錄自身位置。

(4) 按照預(yù)定路線前進，到達兩房間交界時可收到兩個ID信號，則判斷這兩個房間相鄰，并按序記錄。

(5) 待完成本房間清掃后，調(diào)取按序記錄的相鄰房間編號，若無相鄰房間則轉(zhuǎn)到(6)；若有相鄰房間則轉(zhuǎn)到(3)。

(6) 若無上一級房間則轉(zhuǎn)到(7)，若有則返回首次進入該房間時的上一級房間，并轉(zhuǎn)到(3)。

(7) 機器人返回起始地點，此次清潔結(jié)束。

此處定義：機器人從該房間進入另一房間，則該房間為另一房間的上一級房間。

圖6 多房間之間的導(dǎo)航

3 實驗與仿真

在10.8 m×10 m室內(nèi)環(huán)境中測試了機器人SLMN導(dǎo)航算法，機器人尺寸為0.8 m×0.4 m，如圖7所示。

圖7 機器人模型

在完成工作運行后，機器人局部軌跡如圖8所示。紅色和藍色線分別表示機器人的真實軌跡和理想軌跡。因此可以發(fā)現(xiàn)當(dāng)機器人由導(dǎo)航器通過一個紅外線參考線(=5 m)，偏離軌跡將有效地糾正。

圖8 機器人的局部軌跡

通過計算絕對值，可以創(chuàng)建此偏差的良好視圖，出現(xiàn)的偏差可以得到有效的更正，如圖9所示。

圖9 偏差的變化趨勢

成本和準(zhǔn)確性是我們關(guān)注的主要問題，通過分析幾個候選算法的導(dǎo)航問題，可以確定一個最有效的算法。本文將比較分析結(jié)果與其他算法進行了對比，如表1所示?？梢钥闯觯瑑蓚€SLAM系統(tǒng)具有相似的精確度且是最高的。然而在相同的電池容量下它們的工作時間也是最短的，所以它需要更多的能量來保持工作。雖然慣性導(dǎo)航系統(tǒng)成本低，但其誤差太大無法正常工作。因此與其他算法相比，本文提出的SLMN導(dǎo)航算法將機器人實現(xiàn)室內(nèi)全覆蓋導(dǎo)航成本降低14.3%，在滿足實際應(yīng)用中的能耗成本較低。

表1 測試結(jié)果

4 結(jié) 語

本文提出了一種高效的室內(nèi)機器人導(dǎo)航方案，導(dǎo)航器和機器人之間的兩個測量值和機器人運動距離可以形成一個三角形，然后通過解析幾何原理求解正向角并校正前進速度方向。通過紅外線校準(zhǔn)以及超聲波的兩次測距構(gòu)成三角形，利用余弦定理進行角度解算和糾偏，最后實現(xiàn)了室內(nèi)機器人的高精度直線導(dǎo)航，將效率提高近70.8%，并在此基礎(chǔ)上提出了多房間導(dǎo)航算法。經(jīng)過仿真與實際測試，該算法可將機器人實現(xiàn)室內(nèi)全覆蓋導(dǎo)航成本降低14.3%，解決了算法過于復(fù)雜的問題且夜間也能良好工作，能夠?qū)崿F(xiàn)機器人直線運動、精度高且多房間導(dǎo)航。