消毒機器人全覆蓋路徑算法設計

邱春凱，王利恒

（武漢工程大學 電氣信息學院，武漢430205）

隨著科技的發展，機器人技術越來越成熟，新型冠狀病毒的爆發使消毒機器人的使用越發頻繁。如何讓機器人高效率、低成本地執行任務體現了其智能化水平。其中關鍵的一步是機器完成消毒工作時的路徑規劃問題。路徑規劃就是在消毒機器人在工作范圍內，從初始點開始工作，找到一條將所有位置覆蓋并且可連續的路徑，消毒機器人可在無人看守的情況下自主完成室內消毒工作，使某種性能指標達到最優。此技術的實現使得消毒機器人能代替人深入病房，徹底而快速地做好消毒工作，衛生死角都能妥善處理。不僅解決了前線人員不足等問題，還有效防止各類病原體在醫院內的傳播，降低了感染傳播的風險，減少了不必要的人力和物力損耗。所以就目前整個社會對消毒機器人的需求是非常大的。其智能化程度對于人類的醫療工作意義非凡。為此，環境柵格地圖[1]構建與路徑規劃算法是實現此目標的兩項關鍵技術。本文使用柵格法對地圖環境進行環境建模，采取SPDF 算法[2]與Theta* 算法[3]相結合進行路徑規劃與控制實現。

1 機器人的定位和環境的識別

機器人采用的是激光雷達[4]進行環境的識別，機器人通過自己機身的旋轉和雷達托盤的旋轉進行周圍環境的掃描獲取周圍的環境信息。

1.1 機器人定位

以機器人為原點，通過雷達掃描得到周邊各點到原點的距離，連接各點就可以構成周邊區域的輪廓，通過坐標變換，就可以確定當前機器人相對周邊環境的位置，如圖1所示。

圖1 機器人掃描環境Fig.1 Robot scanning environment

1.2 環境識別

環境的識別主要是確定空間內的障礙物，為后面的路徑規劃提供設計輸入。在地圖重建[5]過程中，根據邊界計算的布爾運算，可以確定外聯通區域和內聯通區域。外聯通區域可以確定為邊界，內聯通區域可以確定為障礙物，如圖2所示。

圖2 識別環境Fig.2 Identify environment

由于消毒機器人自身有高度，雷達掃描的是一個三維的立體環境[6]，考慮到機器人自身高度，高于機器人的障礙物，下空高可以讓機器人順利通過的，在地圖建模中按自由空間處理，而下空高低于機器人高度的，按照障礙物處理，如圖3所示。

圖3 機器人環境識別判斷Fig.3 Robot environment recognition and judgment

2 路徑規劃算法

2.1 環境模型的建立

環境建模[7]是將機器人工作的現實環境情況轉化為自身可以識別的信息的過程，合適的環境建模方式將會明顯縮減路徑規劃算法研究的難度。目前常用的有拓撲法、可視圖法、自由空間法以及柵格法等多種環境建模方法。

拓撲法[8]是以環境的幾何特征為依據，將消毒機器人的整個消毒空間劃分成許多個子空間，然后將這些子空間連接起來。拓撲法通過使用拓撲特征顯著地縮小了機器人的探索空間，并且對機器人的定位精度要求相對較低。可實現從高維向低維的轉化，但是無法區分相似區域。

可視圖法[9]在進行環境建模時，是將機器人看做一個質點，將障礙物轉化為規則的多邊形，然后利用直線把起點、障礙物多邊形的頂點和目標點連結起來同時保證連接線不與障礙物多邊形相交，由此得到的是可視圖。該方法具有原理簡單，易于操作等優點。但是隨著障礙物增多，將會導致頂點之間的連線增多，進而導致路徑規劃的復雜度上升和消耗的時間增多。除此之外，如果可視圖中頂點位置發生變化，則需要重新構建可視圖。

自由空間法[10]原理簡單，便于應用，并且構建出的連通圖并不會隨著起始點和目標點位置的變化而改變。但是其復雜程度與障礙物個數成正比，當障礙物數量較大時，構建連通圖將變得十分困難，同時無法確保能否獲得最優路徑。

對于柵格法[11]而言，其原理簡單，便于后續路徑規劃算法設計等優點。此外，柵格法有一個十分重要的參數，這個參數就是柵格尺寸大小，當選擇柵格法去完成環境建模任務時，應該選擇合適的柵格尺寸。

本文選用柵格法建立模型，需要考慮到4 個影響因子：①房間清掃面積及障礙物數量；②根據路徑規劃方法制定具體路徑和柵格地圖大小；③環境地圖中柵格尺寸大小；④計算機的運算能力和反應能力。

柵格的行列號用坐標（x，y）表示，或者用索引值表示，如圖4（a）所示，黃點可用坐標（2，2）或者索引值13 表示。索引值表示柵格位置如圖4（c）所示，這里選擇消毒機機器人的輪廓的大小。格柵表示方法柵格分為自由格柵與有障礙物格柵，這里將自由柵格設置為“1”，障礙物柵格設置為“2”，機器人覆蓋后的柵格設置為“3”，如圖4（b）所示。起初自由柵格為白色，障礙物為黑色，當機器人遍歷自由柵格后，由柵格變成灰色，如圖4（a）所示。

圖4 柵格法Fig.4 Grid method

2.2 算法設計

通常機器人全覆蓋的時候采用的是隨機算法[12]，回旋算法[13]，但這些算法會有很大的重復率，機器人完成任務時間長，機器人進去死區后很難走到新區域，本文設計一套算法使機器人能夠高效完成任務。

首先在機器人遍歷消毒階段設置8 個方向，分別是North，South，West，East，North West，North East、South West 以及South East，如圖5所示，并且設置移動方向的順序規則，水平方向和豎直方向優先于傾斜方向，趨向于起始點方向的優先級高于遠離起始點方向的優先級，滿足以上條件的同時，水平方向優先級權重高于豎直方向優先級權重，也就是West 方向和South 方向都是趨向于起始點的方向，East 方向和North 方向都是遠離起始點的方向，所以方向的優先級依次為West、South、North、East。

圖5 機器人移動方向Fig.5 Direction of robot moving

假設機器人起始點為室內的左下角，West 方向和South 方向是趨向于起始點的方向，因此方向的優先級順序是West、South、North、East、South West、North West、South East、North East；當消毒機器人在室內右上角時，East 方向和North 方向是趨向于起始點的方向，因此方向的優先級順序是 East、North、South、West、North East、South East、North West、South West。

這里就是按照SPDF 算法的工作原理，從機器人起始點出發，在當前位置上根據優先規則選出此刻的移動方向并沿著這個方向移動1 個柵格，循環這個規律一直運動到死點位置。在這個時候，機器人并未將所有位置遍歷，需要機器人移動到一個新的位置重新工作。

2.3 回溯點的銜接方法

當運動到死點位置時，需要移動到另一個子區域空間進行消毒，在這個過程中，消毒機器人不進行消毒工作，在這個時候建立回溯機制，記錄未覆蓋的子區域和尋找子區域銜接的路徑目標點。利用權值和代價值找到最近的未遍歷點設置為回溯點，設A（XA，YA），B（XB，YB）權值和代價值的公式為

找到最優回溯點后，機器人重新使用SPDF 算法進行全覆蓋。這樣依次循環直到機器人遍歷所有點。這里連接回溯點使用的是Theta*算法。

式（3）歐氏距離公式[11]比較適用于機器人可以沿著任意方向進行移動的情況，具有更加接近于真實距離的優點，但是需要開方導致計算量增大，這里區域銜接采用的是Theta* 算法[13]，機器人也可以向著任意角度移動，所以十分符合。

A*算法[15]中每個位置可以沿著East、North、South、West、North East、South East、North West、South West 8 個方向延伸，即除了邊界節點之外，每個節點都有8 個相鄰。如圖6 從起點（S）到終點（G）的搜索過程中，最終形成的路徑為C1-C2-B3-A4。而在Theta*算法中，由于取消了對于搜索節點必須沿相鄰節點傳播的限制，路徑可以沿任意角度轉換與延伸，因而最終搜索形成的路徑為C1-B3-A4，如圖7所示。

圖6 A*算法Fig.6 A* algorithm

圖7 Theta*算法Fig.7 Theta* algorithm

傳統的A* 算法是一種啟發的搜索算法，其算法流程為從初始點向附近點進行擴展，將不超過地圖的可行區域子節點放入openlist，然后在openlist中選取最優點作為下一個父節點，并將此點放入closelist 中，重復此過程直至目標節點，完成路徑規劃，流程如圖8所示。

圖8 A*算法程序結構框圖Fig.8 Program structure block diagram of A* algorithm for robot environment recognition and judgment

根據A* 算法得出來的路徑可以進行優化，得到Theta* 算法的路徑，從初始點開始與后面的路徑點進行連接判斷，如果該點不存在障礙物，則判斷下一個點，直到某點存在障礙物，則設該點的前一個放入節點P 中，并設置為新得父節點，一次循環，直到目標點，其算法流程如圖9所示。

圖9 Theta*算法程序結構框圖Fig.9 Program structure block diagram of Theta*algorithm for robot environment recognition and judgment

圖10所示為用A* 算法和Theta* 算法從黃點到紫點的兩條路徑，從圖中看出，很明顯存在更短的路徑抵達終點。圖中藍色線路路徑是根據A* 算法路徑優化的路徑，路徑長度d=21.49，根據兩點之間直線最短的法則，顯然得出藍色路徑更短，經過計算可以得出藍色優化后的路徑長度d=20。

圖10 Theta*仿真實驗圖Fig.10 Theta* simulation experiment diagram

2.4 全覆蓋路徑規劃

首先機器人建立環境模型，采用SPDF 局部區域覆蓋路徑規劃算法進行覆蓋，局部覆蓋的過程中更新環境空間模型和回溯點，一直到局部區域的死點。利用回溯點列表篩選出最優回溯點，移動到最近的一個子區域空間中，并且更新當前位置，直到回溯列表中無回溯點，則表示空間被全覆蓋，程序流程如圖11所示。

圖11 機器人全覆蓋程序框圖Fig.11 Robot full coverage program block diagram

3 仿真實驗

圖12所示為10×10 柵格的全局路徑規劃圖，黑色區域表示障礙物，白色區域表示自由可移動區域且是未覆蓋的，灰色區域表示已覆蓋區域，設起點為（9，2），黑色粗線條為全覆蓋路徑，黑點為當機器人移動到死點區域時最短的銜接點，此時消毒機器人的路徑覆蓋率到達無死角全覆蓋，并且保證了在銜接路徑時最短。

圖12 全覆蓋仿真實驗圖Fig.12 Simulation diagram of full coverage algorithm

4 結語

本文利用雷達上下角度擺動，將二維坐標系轉化為了三維立體的坐標系，根據消毒機器人的高度來判斷機器人的通過性，然后利用柵格法建立環境地圖柵格模型，結合全覆蓋路徑規劃算法和Theta*算法進行全局路徑規劃，并對室內進行避障路徑規劃和仿真實驗。結果表明，在靜態的環境中機器人對室內可達到全面的消毒，覆蓋率高，路徑短，同時可以避開障礙物。