實(shí)現(xiàn)智能型吸塵器自動(dòng)歸位功能　MCU動(dòng)態(tài)坐標(biāo)精確計(jì)算

市面上已有居家小幫手吸塵器相關(guān)產(chǎn)品，其中較高等級產(chǎn)品具有自動(dòng)歸位充電功能，也就是當(dāng)小幫手吸塵器工作至電量不足時(shí)，會自動(dòng)尋找原點(diǎn)的充電位置以完成自動(dòng)歸位與充電功能，如此對于居家生活有實(shí)質(zhì)幫助。主人可于外出或上班前設(shè)定小幫手吸塵器工作模式，即可自動(dòng)完成居家的吸塵工作并歸位完成自動(dòng)充電，該功能對于忙碌現(xiàn)代人而言幫助非常大，故有其市場價(jià)值。

然市場上號稱具有自動(dòng)歸位功能的小幫手吸塵器，深入了解為利用靠右墻壁，以循壁方式逆時(shí)針尋找直到回到原位為止，但該法有使用盲點(diǎn)，若小幫手吸塵器開始要?dú)w位時(shí)剛好在原位的右邊墻壁，則小幫手吸塵器須繞一大圈才能回到原位，若運(yùn)氣不好，可能因距離太遠(yuǎn)而在電量耗盡前還無法找到原位。另一個(gè)缺點(diǎn)為若小幫手吸塵器開始要?dú)w位時(shí)，剛好遇見如箱柜四面均靠墻，則小幫手吸塵器會繞著該障礙物箱柜順時(shí)針旋轉(zhuǎn)，永遠(yuǎn)無法找到原位。

居家小幫手若用于室內(nèi)，應(yīng)不能使用全球衛(wèi)星定位系統(tǒng)(GPS)為定位功能，因?yàn)槭覂?nèi)環(huán)境無法定位，須于室外見天環(huán)境才可定位。另也有考慮用無線射頻辨識系統(tǒng)(RFID)定位，雖然可達(dá)到精確定位，但須于使用時(shí)對居家環(huán)境作RFID標(biāo)簽設(shè)定，對用戶來說并不實(shí)用。若使用紅外線定位亦是可行方法，但遇到紅外線被環(huán)境地物遮蔽時(shí)，小幫手吸塵器則無法順利歸位。

基于上述說明，本文擬提出坐標(biāo)定位法記錄小幫手吸塵器的動(dòng)態(tài)坐標(biāo)，包含X、Y軸坐標(biāo)與小幫手吸塵器的方向角，據(jù)此即可達(dá)成最短距離自動(dòng)歸位和提高歸位成功率。

動(dòng)態(tài)坐標(biāo)計(jì)算方法介紹

小幫手吸塵器除具自動(dòng)吸塵功能外，若要方便用戶使用，最重要具備的功能為當(dāng)小幫手吸塵器電量不足須充電時(shí)，要能自動(dòng)歸位充電，且歸位動(dòng)作須具有快速與準(zhǔn)確特性，且考慮商品化須有低成本優(yōu)點(diǎn)。本文即以此為重點(diǎn)聚焦于小幫手吸塵器自動(dòng)歸位功能的動(dòng)態(tài)坐標(biāo)法則設(shè)計(jì)。

圖1為智能型居家小幫手吸塵器工作示意圖，小幫手吸塵器由原點(diǎn)坐標(biāo)(0,0)出發(fā)，小幫手吸塵器的正前方定義為正X軸方向，及初始時(shí)小幫手吸塵器方向角為0度，接著依小幫手吸塵器工作模式(不在本文討論范圍)先以θ1角度前進(jìn)至P1點(diǎn)，接著轉(zhuǎn)至θ2角度前進(jìn)至P2點(diǎn)，再轉(zhuǎn)θ3角度前進(jìn)至P3點(diǎn)，最后到達(dá)Pn點(diǎn)，假設(shè)到達(dá)Pn點(diǎn)后小幫手吸塵器電量呈現(xiàn)不足，須直接由Pn點(diǎn)回到原點(diǎn)充電。

圖1 智能型居家小幫手吸塵器工作示意圖

要由Pn點(diǎn)直接往原點(diǎn)坐標(biāo)(0,0)歸位，小幫手吸塵器須計(jì)算其行走至每一點(diǎn)之坐標(biāo)值及其方向角，才能正確判斷原點(diǎn)方向。當(dāng)然在小幫手吸塵器歸位路徑上也許會有障礙物阻擋小幫手吸塵器前進(jìn)，此時(shí)須設(shè)計(jì)避開障礙物功能，此情形并不在本文討論范圍。

圖1中定義原點(diǎn)坐標(biāo)為(0,0)，正X軸方向?yàn)樾褪治鼔m器正前方，假設(shè)點(diǎn)P1、P2、P3、…Pi、…Pn的坐標(biāo)分別為(X1,Y1)、(X2,Y2)、(X3,Y3)、…(Xi,Yi)、…與(Xn,Yn)，而小幫手吸塵器方向角轉(zhuǎn)動(dòng)角度為θ1、θ2、θ3、…θi、…與θn，由原點(diǎn)至P1點(diǎn)距離為Z1、由P1點(diǎn)至P2點(diǎn)距離為Z2、由P2點(diǎn)至P3點(diǎn)距離為Z3以此類推，其須計(jì)算每一點(diǎn)坐標(biāo)值與方位角度。以圖1為例，假設(shè)矩陣Tyi(y,d)、Tzi(z,θ)與Txi(x,a)功能為將行走一段距離轉(zhuǎn)換為原坐標(biāo)，矩陣Tyi(y,d)處理Y軸距離，矩陣Tzi(z,θ)處理坐標(biāo)旋轉(zhuǎn)角度θi，與矩陣Txi(x,a)處理X軸距離，定義矩陣Tyi(y,d)、Tzi(z,θ)與Txi(x,a)如方程式(1)～(3)所示。

則Pi點(diǎn)對應(yīng)至原坐標(biāo)系統(tǒng)坐標(biāo)值如(4)式所示。坐標(biāo)(Pxi,Pyi)即為Pi點(diǎn)對應(yīng)至原坐標(biāo)系統(tǒng)之坐標(biāo)值。

[Pix Piy 0 1]T=Tyi(y,d)Tzi(z,θ)Txi(x,a)[0 0 0 1]T ……………………………(4)

小幫手吸塵器坐標(biāo)改變可歸納三種模式，直線行走、原地旋轉(zhuǎn)與行走轉(zhuǎn)彎模式。直線行走只改變小幫手吸塵器坐標(biāo)位置并不影響其方向角，原地旋轉(zhuǎn)只改變小幫手吸塵器方向角并不影響其坐標(biāo)位置，行走轉(zhuǎn)彎同時(shí)會改變小幫手吸塵器坐標(biāo)位置與其方向角，此三種模式改變小幫手吸塵器坐標(biāo)位置與其方向角其計(jì)算方法如下。   

采直線行走模式計(jì)算坐標(biāo)位置  

圖2為直線行走模式坐標(biāo)位置計(jì)算方法示意圖，小幫手吸塵器具有左右輪，其左右輪之間距離為Dw，左右輪之半徑為Rw，因?yàn)橹本€行走模式故左右輪轉(zhuǎn)動(dòng)方式與速度相同，此模式可使小幫手吸塵器前進(jìn)或后退，只要計(jì)算出小幫手吸塵器前進(jìn)或后退之距離帶入(1)～(4)式即可求出新坐標(biāo)，而小幫手吸塵器前進(jìn)或后退距離可由式(5)求出。

圖2 直線行走模式坐標(biāo)位置計(jì)算方法示意圖

Zi=2π*Rw*Cw ………………………………(5)

其中Cw為輪子轉(zhuǎn)動(dòng)圈數(shù)。

藉原地旋轉(zhuǎn)模式計(jì)算方向角

若小幫手吸塵器原地旋轉(zhuǎn)及左右輪以相同速度但方向相反轉(zhuǎn)動(dòng)，則其旋轉(zhuǎn)角度計(jì)算方法如式(6)所示。

θi=(2π*Rw*Cw/(Dw*π))*360=(2Rw*Cw/Dw)*360………………………………(6)

Cw為小幫手左右輪以相同速度但方向相反轉(zhuǎn)動(dòng)之轉(zhuǎn)動(dòng)圈數(shù)。式(6)角度值以度數(shù)(Degree)表示。

行走轉(zhuǎn)彎模式坐標(biāo)位置/方向角計(jì)算解析

圖3為行走轉(zhuǎn)彎模式坐標(biāo)位置與其方向角計(jì)算方法示意圖，若左輪與右輪均個(gè)別以等速前進(jìn)，且右輪速度高于左輪，則小幫手會往左轉(zhuǎn)彎如圖3所示，此時(shí)不但坐標(biāo)位置改變，其方向角亦隨之改變。

圖3 行走轉(zhuǎn)彎模式坐標(biāo)位置/方向角計(jì)算方法示意圖

計(jì)算小幫手吸塵器之旋轉(zhuǎn)角度如式(7)所示。

θ=360*( Rw*Cwr)/(Dw+Dw/(Cwr/Cwl-1))*2π……………………………………(7)

其中Cwr為右輪轉(zhuǎn)動(dòng)的圈數(shù)，Cwl為左輪轉(zhuǎn)動(dòng)的圈數(shù)。而小幫手新坐標(biāo)計(jì)算方法為：

Ｒc=Dw/[(Cwr/Cwl)-1]………………………(8)

其中Ｒc為小幫手轉(zhuǎn)彎內(nèi)半徑，則新坐標(biāo)為

x=[2π*Rw*(Cwr+Cwl)/2*cos(θ)……………(9)

y=[2π*Rw*(Cwr+Cwl)/2*sin(θ)…………(10)

計(jì)算小幫手吸塵器坐標(biāo)之方法步驟依序?yàn)榕袛嘈褪治鼔m器工作模式；若為直線前進(jìn)或后退模式時(shí)，計(jì)算前進(jìn)或后退之行走距離，利用坐標(biāo)轉(zhuǎn)換計(jì)算新坐標(biāo)；若為原地旋轉(zhuǎn)模式時(shí)，計(jì)算小幫手之新方向角；若為前進(jìn)或后退轉(zhuǎn)彎模式時(shí)，計(jì)算小幫手的新方向角及前進(jìn)或后退的行走距離；若須歸位回到原點(diǎn)時(shí)，計(jì)算最短距離方向以規(guī)畫為行進(jìn)方向，若遇障礙物時(shí)，則以左彎或右彎以避開障礙物回到原點(diǎn)。

硬件架構(gòu)面面觀

圖4為智能型居家小幫手核心控制芯片電路圖，本系統(tǒng)以HT46R24微控制器(MCU)為控制核心，因HT46R24具有高性能與低價(jià)位優(yōu)勢，且穩(wěn)定度佳具工業(yè)規(guī)格認(rèn)證。經(jīng)由MCU計(jì)算與分析左右輪行走距離，可得到小幫手吸塵器坐標(biāo)位置與其方向角，雖然誤差會造成歸位誤差，但程序設(shè)計(jì)小幫手吸塵器回到原位附近時(shí)，若無法實(shí)時(shí)找到Home的位置，可設(shè)計(jì)于附近尋找Home的位置，此時(shí)可加裝傳感器以確認(rèn)Home確實(shí)位置。

{$page$}

圖4 智能型居家小幫手核心控制芯片電路圖

圖5為智能型居家小幫手左右輪馬達(dá)控制電路圖，采單獨(dú)控制方式并加裝傳感器感以感測左右輪個(gè)別轉(zhuǎn)動(dòng)圈數(shù)。小幫手吸塵器由左右兩顆馬達(dá)驅(qū)動(dòng)，依驅(qū)動(dòng)馬達(dá)正反轉(zhuǎn)即可控制小幫手前進(jìn)、后退、左彎與右彎，又依其馬達(dá)正反轉(zhuǎn)、速度高低即可輕易控制小幫手運(yùn)動(dòng)行為。經(jīng)實(shí)際測試確實(shí)可達(dá)上述功能，若將小幫手設(shè)計(jì)為圓形且左右驅(qū)動(dòng)馬達(dá)置于其直徑上時(shí)，小幫手即可原地左右轉(zhuǎn)。

圖5 智能型居家小幫手左右輪馬達(dá)控制電路圖

應(yīng)用動(dòng)態(tài)坐標(biāo)計(jì)算法　小幫手吸塵器漸成主流

圖6為智能型居家小幫手實(shí)際實(shí)驗(yàn)行走路線圖，系統(tǒng)啟動(dòng)后MCU開始計(jì)算由編碼器計(jì)算馬達(dá)轉(zhuǎn)動(dòng)角度，再使用齊次坐標(biāo)(Homogeneous Coordinate)計(jì)算出智能型居家小幫手行走的路線距離與角度，并計(jì)算其坐標(biāo)位置。智能型居家小幫手由A原點(diǎn)出發(fā)，18.45度直行3.16公尺至B點(diǎn)，接著轉(zhuǎn)向52度執(zhí)行至C點(diǎn)，再由C點(diǎn)至D點(diǎn)、E點(diǎn)、F點(diǎn)、G點(diǎn)，至G點(diǎn)后，若智能型居家小幫手接收到歸位命令，MCU會計(jì)算歸位原點(diǎn)方向角與距離，計(jì)算核心會先控制智能型居家小幫手轉(zhuǎn)至面對原點(diǎn)的方向，接著控制直行歸位至原點(diǎn)的距離，如此智能型居家小幫手即可成功歸位。

圖6 智能型居家小幫手實(shí)際行走路線圖

圖7為智能型居家小幫手動(dòng)態(tài)坐標(biāo)定位系統(tǒng)計(jì)算的程序流程圖，系統(tǒng)啟動(dòng)后，MCU開始實(shí)時(shí)計(jì)算由編碼器送來的訊號，以計(jì)算出智能型居家小幫手當(dāng)下的坐標(biāo)位置。當(dāng)歸位模式開始執(zhí)行后，MCU計(jì)算原點(diǎn)方向與歸位時(shí)智能型居家小幫手須轉(zhuǎn)動(dòng)的角度，接著控制馬達(dá)并由編碼器量測是否已轉(zhuǎn)到歸位方向的角度，當(dāng)方向正確后，再直線前進(jìn)以達(dá)成歸位功能，歸位途中若遇到障礙物，將啟動(dòng)更換路線模式以尋求另一歸位路徑。

圖7 智能型居家小幫手歸位程序流程圖

圖8為智能型居家小幫手實(shí)驗(yàn)用實(shí)體照相圖，智能型居家小幫手具有五個(gè)超音波測距模塊用來量測障礙物的位置與距離。前方設(shè)有防碰撞機(jī)構(gòu)用來確保智能型居家小幫手吸塵時(shí)不會因碰撞而受損，而底部有紅外線傳感器，用來保護(hù)小幫手不會因掉落而損壞，智能型居家小幫手還有一組射頻(RF)遙控器，可以方便使用者選擇自己想要的工作模式。

圖8 智能型居家小幫手實(shí)驗(yàn)用實(shí)體照相圖

本文以HT46R24芯片為核心，利用動(dòng)態(tài)坐標(biāo)法計(jì)算小幫手吸塵器的實(shí)時(shí)坐標(biāo)位置與其方向角，并利用此坐標(biāo)位置與其方向角數(shù)據(jù)計(jì)算小幫手吸塵器歸位的最佳路徑與方向。相信本文所提方法具有高可靠度、低成本與具有市場性的居家特色，非常適合用于小幫手吸塵器自動(dòng)歸位充電用，且適合任何場所應(yīng)用，相信應(yīng)用動(dòng)態(tài)坐標(biāo)計(jì)算法可使小幫手吸塵器逐漸成為吸塵器市場主流。