多房間、高精度的室內機器人SLMN導航算法

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

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

0 引 言

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

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

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

1 機器人平臺

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

(a)(b)

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

2 室內機器人定位與導航

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

圖2 機器人的定位與導航

2.1 保證走直線的方案設計

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

v=(N/26)×L/T

(1)

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

得到前進速度v后就可以利用經典自動控制領域中的PID控制算法對輪子的速度進行調節。考慮到公式轉換可能增大計算量和產生誤差，所以可以直接對光電碼盤產生的脈沖數N進行PID調節。

位置型控制

(2)

增量型控制

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

(3)

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

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

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次目標速度與實際速度的偏差量；T為采樣時間，一般為定值；KP，TI，TD為待整定的3個參數；

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

圖3 PID算法的實現流程圖

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

2.2 定位導航方案設計

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

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

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

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

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

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

步驟3計算機器人水平姿態角。由幾何關系，前行了LAC距離，機器人水平姿態角為

(5)

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

(6)

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

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

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

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

(7)

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

t+20t=21t

(8)

然后得到：

(9)

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

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

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

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

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

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

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

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

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

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

圖6 多房間之間的導航

3 實驗與仿真

在10.8 m×10 m室內環境中測試了機器人SLMN導航算法，機器人尺寸為0.8 m×0.4 m，如圖7所示。

圖7 機器人模型

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

圖8 機器人的局部軌跡

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

圖9 偏差的變化趨勢

成本和準確性是我們關注的主要問題，通過分析幾個候選算法的導航問題，可以確定一個最有效的算法。本文將比較分析結果與其他算法進行了對比，如表1所示。可以看出，兩個SLAM系統具有相似的精確度且是最高的。然而在相同的電池容量下它們的工作時間也是最短的，所以它需要更多的能量來保持工作。雖然慣性導航系統成本低，但其誤差太大無法正常工作。因此與其他算法相比，本文提出的SLMN導航算法將機器人實現室內全覆蓋導航成本降低14.3%，在滿足實際應用中的能耗成本較低。

表1 測試結果

4 結 語

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