實現智能型吸塵器自動歸位功能　MCU動態坐標精確計算

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

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

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

基于上述說明，本文擬提出坐標定位法記錄小幫手吸塵器的動態坐標，包含X、Y軸坐標與小幫手吸塵器的方向角，據此即可達成最短距離自動歸位和提高歸位成功率。

動態坐標計算方法介紹

小幫手吸塵器除具自動吸塵功能外，若要方便用戶使用，最重要具備的功能為當小幫手吸塵器電量不足須充電時，要能自動歸位充電，且歸位動作須具有快速與準確特性，且考慮商品化須有低成本優點。本文即以此為重點聚焦于小幫手吸塵器自動歸位功能的動態坐標法則設計。

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

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

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

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

則Pi點對應至原坐標系統坐標值如(4)式所示。坐標(Pxi,Pyi)即為Pi點對應至原坐標系統之坐標值。

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

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

采直線行走模式計算坐標位置  

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

圖2 直線行走模式坐標位置計算方法示意圖

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

其中Cw為輪子轉動圈數。

藉原地旋轉模式計算方向角

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

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

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

行走轉彎模式坐標位置/方向角計算解析

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

圖3 行走轉彎模式坐標位置/方向角計算方法示意圖

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

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

其中Cwr為右輪轉動的圈數，Cwl為左輪轉動的圈數。而小幫手新坐標計算方法為：

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

其中Ｒc為小幫手轉彎內半徑，則新坐標為

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

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

計算小幫手吸塵器坐標之方法步驟依序為判斷小幫手吸塵器工作模式；若為直線前進或后退模式時，計算前進或后退之行走距離，利用坐標轉換計算新坐標；若為原地旋轉模式時，計算小幫手之新方向角；若為前進或后退轉彎模式時，計算小幫手的新方向角及前進或后退的行走距離；若須歸位回到原點時，計算最短距離方向以規畫為行進方向，若遇障礙物時，則以左彎或右彎以避開障礙物回到原點。

硬件架構面面觀

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

{$page$}

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

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

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

應用動態坐標計算法　小幫手吸塵器漸成主流

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

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

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

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

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

圖8 智能型居家小幫手實驗用實體照相圖

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