邱春凱，王利恒

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

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

1 機器人的定位和環(huán)境的識別

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

1.1 機器人定位

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

圖1 機器人掃描環(huán)境Fig.1 Robot scanning environment

1.2 環(huán)境識別

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

圖2 識別環(huán)境Fig.2 Identify environment

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

圖3 機器人環(huán)境識別判斷Fig.3 Robot environment recognition and judgment

2 路徑規(guī)劃算法

2.1 環(huán)境模型的建立

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

拓撲法[8]是以環(huán)境的幾何特征為依據(jù)，將消毒機器人的整個消毒空間劃分成許多個子空間，然后將這些子空間連接起來。拓撲法通過使用拓撲特征顯著地縮小了機器人的探索空間，并且對機器人的定位精度要求相對較低?？蓪崿F(xiàn)從高維向低維的轉(zhuǎn)化，但是無法區(qū)分相似區(qū)域。

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

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

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

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

柵格的行列號用坐標（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]，但這些算法會有很大的重復率，機器人完成任務時間長，機器人進去死區(qū)后很難走到新區(qū)域，本文設計一套算法使機器人能夠高效完成任務。

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

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

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

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

2.3 回溯點的銜接方法

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

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

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

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

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

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

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

圖8 A*算法程序結(jié)構(gòu)框圖Fig.8 Program structure block diagram of A* algorithm for robot environment recognition and judgment

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

圖9 Theta*算法程序結(jié)構(gòu)框圖Fig.9 Program structure block diagram of Theta*algorithm for robot environment recognition and judgment

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

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

2.4 全覆蓋路徑規(guī)劃

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

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

3 仿真實驗

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

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

4 結(jié)語

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