蘭 勇, 馬 力, 馮 曉, 夏國(guó)廷

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

0 引 言

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

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

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

1 機(jī)器人平臺(tái)

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

(a)(b)

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

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

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

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

2.1 保證走直線(xiàn)的方案設(shè)計(jì)

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

v=(N/26)×L/T

(1)

就可以獲得左右輪各自的前進(jìn)速度v。

得到前進(jìn)速度v后就可以利用經(jīng)典自動(dòng)控制領(lǐng)域中的PID控制算法對(duì)輪子的速度進(jìn)行調(diào)節(jié)?？紤]到公式轉(zhuǎn)換可能增大計(jì)算量和產(chǎn)生誤差，所以可以直接對(duì)光電碼盤(pán)產(chǎn)生的脈沖數(shù)N進(jì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為光電碼盤(pán)當(dāng)前測(cè)量值，即單位時(shí)間T內(nèi)光電碼盤(pán)產(chǎn)生脈沖數(shù)N,其與掃地機(jī)器人的兩個(gè)輪子前進(jìn)速度的關(guān)系為式(1)。

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

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次給電動(dòng)機(jī)PWM輸出值；u(n-1)是第n-1次給電動(dòng)機(jī)PWM輸出值；e(n)，e(n-1)，e(n-2)分別是第n次、第n-1次、第n-2次目標(biāo)速度與實(shí)際速度的偏差量；T為采樣時(shí)間，一般為定值；KP，TI，TD為待整定的3個(gè)參數(shù)；

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

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

通過(guò)PID算法，可以消除兩輪的機(jī)械誤差，盡可能保證機(jī)器人保持自身直線(xiàn)運(yùn)行。

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

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

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

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

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

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

步驟2機(jī)器人向前運(yùn)行LAC后，再測(cè)出與導(dǎo)航儀之間的距離為L(zhǎng)OC。

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

(5)

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

(6)

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

圖5 偏移直線(xiàn)運(yùn)行的角度為10°時(shí)的相對(duì)誤差關(guān)系

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

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

(7)

對(duì)于式(6)，整個(gè)等式的時(shí)間：

t+20t=21t

(8)

然后得到：

(9)

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

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

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

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

(3) 機(jī)器人進(jìn)入房間，機(jī)器人接收到信號(hào)后記錄自身位置。

(4) 按照預(yù)定路線(xiàn)前進(jìn)，到達(dá)兩房間交界時(shí)可收到兩個(gè)ID信號(hào)，則判斷這兩個(gè)房間相鄰，并按序記錄。

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

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

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

此處定義：機(jī)器人從該房間進(jìn)入另一房間，則該房間為另一房間的上一級(jí)房間。

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

3 實(shí)驗(yàn)與仿真

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

圖7 機(jī)器人模型

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

圖8 機(jī)器人的局部軌跡

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

圖9 偏差的變化趨勢(shì)

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

表1 測(cè)試結(jié)果

4 結(jié) 語(yǔ)

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