未知環境地圖的情況下,機器人將利用紅外反射傳感器獲取近處障礙物的 信息,并以20CM為邊長的正方形作為柵格大小,建立柵格地圖。柵格地圖的單位域主要包 含了以下信息類型:
[0078] 柵格的位置坐標;
[0079] 柵格是否可通行;
[0080] 柵格在路徑規劃中的特征標識;
[0081] 柵格在單元區域分割的序號。
[0082] 步驟五:在已知環境地圖和實時位置時,可以利用先進路徑規劃算法自動規劃出 最優清掃路徑。
[0083] 利用已有的柵格地圖規劃最優清掃路徑,實現清潔時間最省。
[0084] 地圖已知情況下的單元域域內規劃遵循的基本原則是:修掃邊界,制造矩形,平 行鋪掃,兼顧障礙。
[0085] 即使用效率最高的覆蓋方一一S型的規整掃地方式掃過地面,即機器人橫坐標不 變,沿著縱坐標從小到大方向移動,到達頂端后,機器人沿著水平方向移動一個柵格,再縱 坐標從大到小方向移動,依次進行。這種方式對于矩形區域來說效率是最高,可是任務區域 往往并非理想的矩形,采用這種方法在曲折的邊界過多轉彎會影響速度,故在柵格地圖中 將待清潔的區域先大致分塊,分割成單元區域,如圖4中的單元域1至單元域4,在具體路徑 規劃過程中優先沿著單元區域的邊界清掃,然后在整塊的矩形區域采用S型的規整掃地方 式。
[0086] 步驟六:按規劃出的最優清掃路徑運行,進行清掃;利用含三軸加速度計的陀螺 儀模塊與增量型旋轉編碼器配合算法來完成航跡推算定位。該方法通過對陀螺儀模塊提供 的加速度和角速度和增量型旋轉編碼器提供的雙輪加速度進行二重積分,并依據航跡推算 基本算法,來對自身進行精確定位,確定其本身在柵格地圖中的位置。
[0087] 601、基于陀螺儀的航跡推算公式如下:
[0088]
[0089]
[0090] 其中a(t)表示t時刻的加速度,W(t)表示t時刻的角速度,S表示t時刻的位移,Θ表示t時刻轉過角度。且s(o) =〇,Θ(〇) =0。用數值分析的方法,將利用梯形數值積 分公式,化簡運算公式為:
[0094] 表示第i次測量得到的加速度,Vla表示實時速度,Sla表示第i次Δt時間內的 位移,Wi表示第i次測量得到的角速度,Θia表示通過式(5)計算得到的單位時間內機器人
[0091]
[0092]
[0093] 所轉過的角度,△t表示測量時間間隔,為單位時間;參數角標i,表示第i次測量得到的數 據。其中%、Vla和Sla都包含水平和豎直兩個分量。
[0095] 由于上一次的積分得到的速度、位置結果中已經包含之前所有運動歷史數據的信 息,所以在進行下一次航跡推算時,只需在上一次的速度、位置上進行遞推即可。
[0096]Via= 0· 5(a;i+ajΔt (6)
[0097]Sla= i+V,)At (7)
[0098]Θia= i+ff,)At(8)
[0099] 由上可得:
[0100]ΔΧ13= slacosΘia (9)
[0101]ΔYia= SlasinΘia (10)
[0102] (Xa,Ya) = (Xlal+AXla,Ylal+AYia) (11)
[0103] 其中,AXla表示X坐標的增量,AYla表示Y坐標的增量。(Xa,Ya)表示用陀螺儀和 加速度計得到的機器人當前坐標;參數角標i,表示第i次測量得到的數據。
[0104] 602、基于增量型旋轉編碼器的航跡推算公式如下:
[0105]
[0106]
[0107]AXib=SibcosΘib (17);ΔYib=SibsinΘib (18);
[0108] (Xb,Yb) = (Xlbl+AXib,Yibl+AYib) (19)。
[0109]du為單位時間內左輪所走過的路程,nu為左輪側的增量型旋轉編碼器單位時間 內發出的脈沖數,m為增量型旋轉編碼器轉一圈所發出的脈沖數,R表示車輪的半徑;dRl為 單位時間內右輪所走過的路程,nRl為右輪側的增量型旋轉編碼器單位時間內發出的脈沖 數;△山為單位時間內左輪和右輪的路程差。
[0110]Θlb表示通過式(17)計算得到的單位時間內機器人所轉過的角度,r為左輪和右 輪之間的距離;Slb為單位時間內左輪和右輪的路程平均值;ΔXib表示X坐標的增量,ΔYib 表示Y坐標的增量;(Xb,Yb)表示用增量型旋轉編碼器得到的機器人當前坐標;參數角標i, 表示第i次測量得到的數據。
[0111] 603、機器人的精確坐標為:
[0112]
[0113]
[0114]
[0115] (X,Y) = (AXa+BXb,AYa+BYb) (26)
[0116]A是陀螺儀的置信度值,B是增量型旋轉編碼器的置信度值;Θia是單位時間內用 陀螺儀得到的機器人所轉過的角度,Θib是單位時間內用增量型旋轉編碼器得到的機器人 所轉過的角度,93為用陀螺儀得到的當前位置與原點沿X軸的夾角,用增量型旋轉 編碼器得到的當前位置與原點沿X軸的夾角,j為93與Θ的差值。k為Θb與Θ的差 值。Θ為機身上的超聲波測距傳感器在能夠收到來自基座的超聲波信號時,通過超聲波經 過四個超聲波測距傳感器的不同時間,精確的測量出機器人和充電基座與X軸之間的實時 夾角。利用這個精確的夾角,來和陀螺儀與增量型旋轉編碼器多次疊加所得到的夾角比較, 可以得到每個傳感器的偏差。當一個傳感器出現錯誤的時候,其偏差會很大,其置信度就 會變得很小,所以本發明可以減少單個傳感器的誤差,也增加了傳感器系統的容錯率;在機 身上的超聲波測距傳感器無法收到來自基座的超聲波信號時,Α和Β設為最近的一次取值。 (X,Y)表示計算得到的機器人精確坐標。
[0117] 步驟七:在清掃過程中,進行路障掃描,遇到障礙時進入避障程序同時把障礙物所 在位置信息記錄進地圖數據中。其中遇到障礙物時的避障礙策略分為三種,即墻形障礙策 略、小型障礙策略、大型障礙策略。參見圖4,詳細說明如下:
[0118] 當遇到障礙后首先依靠側翼的紅外反射傳感器沿著障礙物邊沿前進,若障礙物邊 長小于25cm則定義為小型障礙,若障礙物邊長大于25cm但小于100cm則定義為大型障礙, 若障礙物邊長大于100cm則定義為墻型障礙。
[0119] 墻形障礙策略:首先依靠側翼的紅外反射傳感器,在距離墻形障礙1cm處,沿邊清 掃,直到遇到明顯轉角處,此時切出障礙清掃策略,沿原路徑返回起始點,繼續之前清掃路 線。
[0120] 小型障礙策略:遇到小型障礙時,在距離小型障礙5cm處,環繞小型障礙清掃一 圈,繼續沿原清掃路線前進。
[0121] 大型障礙策略:遇到大型障礙時,在距離大型障礙5cm處清掃一圈,最后在距離大 型障礙物15cm處清掃一圈。在此過程中通過航跡推算法實時獲得機器人相對于充電基座 的坐標,并記錄最后一圈清掃路徑上水平坐標和縱坐標的最大值與最小值。以所記錄得到 的水平坐標的最大值和最小值的平均值,作為中心點的水平坐標。以所記錄得到的縱坐標 的最大值和最小值的平均值,作為中心點的縱坐標。在得到障礙物的中心點后,過中心點生 成一條平行于X軸的線段和一條平行于Y軸的線段。這兩條線段將除去大型障礙物的未清 掃區域劃分為四個區塊。再以這四個區塊為清掃的單元域,逐個執行整齊的S型清掃。
[0122] 在清掃同時進行紅外掃描并對紅外近距傳感器傳回數據檢測,檢測車體是否離開 地面。若檢測到車體離開地面,則停止所有功能進入待機狀態。
[0123] 步驟八:判斷是否打掃完全部可清潔柵格,若清掃完成則進入步驟九;若沒有結 束,返回步驟