專利名稱：基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法
技術領域：
本發(fā)明涉及一種移動機器人路徑規(guī)劃方法，尤其是涉及一種基于局部環(huán)境建模和
自適應窗口的移動機器人實時路徑規(guī)劃方法。
背景技術：
路徑規(guī)劃是指找到一條從起始點到目標點之間的安全無碰撞路徑，是機器人導航 研究中的重要課題。按對環(huán)境知識的了解，可分為已知環(huán)境和未知環(huán)境下的路徑規(guī)劃。無 論機器人路徑規(guī)劃屬于哪種類別，采用何種規(guī)劃方法，基本上都要遵循以下步驟1)建立 環(huán)境模型，即將機器人所處的現(xiàn)實世界進行抽象后建立相關的模型；2)搜索無碰撞路徑， 即在某個模型的空間中尋找合乎條件的路徑的搜索算法。在全局規(guī)劃中，機器人周圍的環(huán) 境信息是完全已知的，因而可以離線地規(guī)劃出一條最優(yōu)路徑。最優(yōu)的標準可以是最短、最安 全或所需能量最小等。這方面已有許多解決方法，如單元分解法、投影法、路標法、柵格法、 可視圖法等。全局方法通?？梢詫ふ易顑?yōu)解，但是需要預先知道環(huán)境的準確信息，并且計算 量很大。與全局方法相比，實時的局部路徑規(guī)劃側重于考慮機器人當前探測到的環(huán)境信息， 讓機器人具有更好的避碰能力。這方面最具代表性的研究是人工勢場法、模糊邏輯法、滾動 窗口法等。 移動機器人的路徑規(guī)劃算法是伴隨著移動機器人的發(fā)展為滿足機器人的需求而 發(fā)展的。當今，無人地面、水下、空中機器人發(fā)展迅速，足球機器人比賽如火如荼，并且機器 人正朝著微小型化和多機器人協(xié)作方向發(fā)展。隨著星球探測和無人戰(zhàn)爭的需要，對機器人 的研究越來越注重于在復雜、非結構化的未知復雜環(huán)境中自主導航。為了滿足移動機器人 發(fā)展的需要，路徑規(guī)劃正在并且將會向高維自由度機器人、多機器人協(xié)調(diào)、動態(tài)未知環(huán)境中 的規(guī)劃發(fā)展。 公告號為CN1230478的發(fā)明專利提供一種機器人行走路徑規(guī)劃裝置和方法。該裝 置包括規(guī)劃器、低通濾波器、伺服控制器和伺服馬達。所述方法包括如下步驟收到對應于 預定目標位置的信號輸入以后，計算初始速度分布曲線；通過使用一個單級低通濾波器，對 初始速度分布曲線進行內(nèi)插處理而獲得經(jīng)濾波處理的速度分布曲線；以及利用經(jīng)濾波處理 的速度分布曲線來控制伺服馬達的加速和減速。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃 方法。 本發(fā)明包括以下步驟 1) —個進行多約束局部環(huán)境建模和分析的步驟； 2) —個可通過性分析的步驟； 3) —個安全性分析的步驟； 4) —個運動平穩(wěn)性分析的步驟
5) —個目標引導性分析的步驟； 6) —個采用自適應窗口的路徑規(guī)劃的步驟。
在步驟1)中，所述多約束局部環(huán)境建模和分析的步驟是機器人用激光傳感器感 知障礙物的信息，每一個障礙物探測點相對于機器人的位置可由激光傳感器直接得出，角 度掃描間隔為A伊，逆時針對激光掃描點進行標記為/,，/2,…，/,V",、，其中l(wèi)i以掃描角伊,和對 應的測量距離&來表示，即極坐標形式
/,"fe"如,(M)A一X) (1) 根據(jù)以上傳感器掃描探測到的局部環(huán)境信息，設計一個動態(tài)自適應規(guī)劃窗口，其 半徑rwin可根據(jù)環(huán)境信息動態(tài)調(diào)整，把t時刻測距范圍內(nèi)探測到的局部環(huán)境信息在半徑為 r由的半圓窗口中，表示為Win(r由(t)) = {1" 12，".，"，…，1181}，其中掃描點lt被重新 定義為
" . 、，《 /,,
(2) /,v(aH 一 ):' : (3) 在步驟2)中，所述可通過性分析的步驟是采用邊界線擴展法對障礙物進行膨脹 后，將機器人視為一個不記大小的質(zhì)點，定義li方向上的可通行代價函數(shù)為
「o若y;r^+w，連續(xù)yr、 yr、、…、/,旨均為0，且_/;;7=+00，則稱序列
li lj為局部窗口 Win(rwin(t))中的一個可通過方向域，lj、 li分別為此方向域的左右邊 界。 在步驟3)中，所述安全性分析的步驟是對可通過li序列來說，其與障礙物邊界 線的夾角越大，機器人沿此方向行進的安全性就越高，定義li的安全性度量函數(shù)為 fmi,孤-《，《-《■) (A =0 其中e，、 e"分別為FD(li)的左右邊界li肌、l"所對應的方向角。 在步驟4)中，所述運動平穩(wěn)性分析的步驟是機器人可以在360。范圍任意轉(zhuǎn)向， 引入路徑平滑性因素，定義li的平滑性代價函數(shù)為 (《)=-《I (5) 在步驟5)中，所述目標引導性分析的步驟是目標點Pe = (xe， ye)對于局部環(huán)境 中任一方向序列l(wèi)i的路徑引導函數(shù)為 yr1(《)—0—-《| (6) 其中9 goal = arctan (yG-yK/xG-xK)為目標點PG (xG， yG)相對于機器人的方向
(egoal g [i， ji])。 在步驟6)中，所述采用自適應窗口的路徑規(guī)劃的步驟是在環(huán)境建模和分析基礎 上，設計一種基于自適應窗口的實時路徑規(guī)劃方法，包括機器人的趨向目標行為和機器人 的沿墻行走行為。
所述機器人的趨向目標行為包括確定即時子目標和規(guī)劃窗口自適應調(diào)整。
所述確定即時子目標的方法為 規(guī)劃窗口為Win (rwin (t))，若附"( ^))，首先利用啟發(fā)式函數(shù)f (1》
g(li)+h(li)來選取使f(li)最小的掃描點作為子目標(記作l曲)，即
<formula>formula see original document page 8</formula> 式中g(li)——機器人從當前位置向li方向行進的代價，h(li)-進到目標點方向的代價，根據(jù)環(huán)境建模的分析，定義
g(/。= 柳=力一
(7)
-從li方向行
(8)
確定"h后，在其所在的可通過方向域FD(lJ中求安全性子目標L^和平穩(wěn)性子
目^示1 smooth
4rfe -Mg,1^,J 、(/"))
4nooth =叫^ 、(化》
(9)
(10) 至此，根據(jù)不同的性能指標要求得到位于可通過方向域FD("h)中的3種子目標 點:lgh = ( 9 gh， rwin) 、 lsafe = ( 9 safe， rwin) 、 l，oth = ( e s國th， rwin)。綜合考慮安全性和運動 平穩(wěn)性約束，對子目標方向進一步優(yōu)化選擇，最終即時子目標Psub為
(11)P。
p sub) I 9
kgh 9 gh+ks
e+ksmooth 9 smooth，
rwin(t)}
XE^^^^^ kgh、 ksafe、 ksmooth kgh+ksafe+ksmooth 1Q
所述規(guī)劃窗口自適應調(diào)整的方法為
動態(tài)窗口 Win(rwin(t))中總的可通過方向域和記為伊^，定義如下
Pro HS^sin" A =r—
(12) 在趨向目標行為中，Win(rwin(t))的調(diào)整原則為當Pro較大時，可以加大 Win(r^(t))，進一步擴展機器人的探測范圍，尋找更優(yōu)的子目標；當^^較小時，應該減小 Win(rwin(t))，縮小機器人的探測范圍，以便在狹窄空間中找到可行路徑，即




rwin(t+l) = rwin(t) + Ar (13) 其中Ar為^^的函數(shù)，定義為
△r = Armin (14a)
(14b)
當^FD,2 ^ Pro <伊FD,max時， Ar = Ar隨 (14c) 其中伊取。9^.2為設置的常數(shù)，《^，為最大值，Armin、 Ar^為定義的最大、最小值。 所述機器人的沿墻行走行為包括局部極小的判斷、確定即時子目標和規(guī)劃窗口的 動態(tài)調(diào)整。 所述局部極小的判斷方法為 當機器人陷入局部極小時，機器人運動軌跡會出現(xiàn)振蕩，或沿著某一閉合路徑運 動，從而不可能到達目標點。為了避免出現(xiàn)這種情況，需要提前預測機器人是否會進入局部 極小狀態(tài)，通過距離比較來判斷 D (Psub， PG) > min (D (PK， PG) ， Dleave) (15) 其中D(Psub， Pe)為子目標點與全局目標點之間的距離，D(PK， Pe)為機器人與全局 目標點之間的距離，D^e為趨向目標的保證距離，在離開沿墻走行為時定義，初始化為D一e =D(PS， Pe)。若條件(15)滿足，而且目標點位于機器人的左側，則follow—left—wall(沿 左墻)行為被激活；若條件(15)滿足，但目標點位于機器人的右側，則follow—right— wall(沿右墻)行為被激活。 一旦進入沿墻走行為，記錄機器人與目標點之間的最短距離 Dmin。使用沿墻走解死鎖算法而使移動機器人沿著障礙物的邊界運動，直到滿足以下退出條 件 D(PN，PG) <Dmin (16) 其中D (PN， Pe)為點PN和Pe之間的距離，PN為沿墻走行為中下一時刻的預計到達 位置。離開沿墻走行為時，更新DlMre = D(PK， Pe)。
所述確定即時子目標的方法為 當follOW_left_Wall激活時，采用Win(rwin(t))中左起第一個障礙物的右邊界線 擴展線對應的掃描點為即時子目標；當fo 1 low_right_wal 1激活時，采用Win (rwin (t))中右 起第一個障礙物的左邊界線擴展線對應的掃描點為即時子目標。機器人對確定的子目標點 進行跟蹤，即可沿著障礙物行走；當機器人沿障礙繞行的距離超過設定值Lmaxl時仍未退出 則自轉(zhuǎn)180°再向另一繞行模式運動，在新的繞行模式下若繞行的距離超過設定值Lmax2仍 未退出(取L_2 > 2L隨》，則停止路徑(認為目標不可達)。 在沿墻走行為中，若發(fā)現(xiàn)目標點Pe位于Win(r^(t))的可通過域中，則直接以Pe 作為當前的即時子目標。 所述規(guī)劃窗口的動態(tài)調(diào)整的方法為 在沿墻走行為中，若為followJeftjall，若/C =0，則設置~, (/ + 1) = ^^，否則
窗口大小不變；若為follow—rightjall，若/r =0，則設置1^&+1) = c^，否則窗口大小 不變。 與已有技術相比，本發(fā)明由于具有更好的環(huán)境適應性和避障能力，獲得較好的安 全性和可達性，并具有較高的計算實時性。因此本發(fā)明解決了不確定復雜環(huán)境下移動機器 人實時生成避障路徑的問題，提供了一種綜合最優(yōu)的的路徑選擇方法，較好地滿足移動機 器人的避障要求，實現(xiàn)機器人的實時路徑規(guī)劃和控制，為移動機器人的導航應用提供一個 有效的無碰撞路徑規(guī)劃方法。


圖1為子目標優(yōu)選示意圖。 圖2為只考慮可通行性的行走路徑。 圖3為考慮了安全性的行走路徑。 圖4為進一步考慮了運動平穩(wěn)性的行走路徑。 圖5為"回"形復雜環(huán)境下的行走路徑。 圖6為"U"形復雜環(huán)境下的行走路徑。 圖7為窗口半徑rwin = 1. 8m時的路徑。 圖8為窗口半徑rwin = 7m時的路徑。 圖9為窗口大小自適應調(diào)整時的路徑。 圖10為窗口半徑r^(t)的動態(tài)調(diào)整過程。在圖10中，橫坐標為時間t/s，縱坐標 為窗口半徑r^(t)m。 在圖2 9中，起始點為S，目標點為G。
具體實施例方式
下面結合具體附圖和實施例對本發(fā)明作進一步說明。
實施例1 1)多約束局部環(huán)境建模和分析 機器人用激光傳感器來感知障礙物的信息，每一個障礙物探測點相對于機器人 的位置可由激光傳感器直接得出，角度掃描間隔為A^ ，逆時針對激光掃描點進行標記為
d…，A,…，^ ，其中l(wèi)i以掃描角^和對應的測量距離&來表示，即極坐標形式
A = {fe, 4〗爐i = G _ 《"max } (1) 根據(jù)以上傳感器掃描探測到的局部環(huán)境信息，設計一個動態(tài)自適應規(guī)劃窗口 ，其 半徑rwin可根據(jù)環(huán)境信息動態(tài)調(diào)整，把t時刻測距范圍內(nèi)探測到的局部環(huán)境信息在半徑為 r由的半圓窗口中表示為Win(r由(t)) = {1"12， ".，"，， 1181}，其中掃描點lt被重新定
義為
d , cT < r
w, 5 w(、,糾 2)可通過性分析 采用邊界線擴展法對障礙物進行膨脹后，將機器人視為一個不記大小的質(zhì)點，定 義li方向上的可通行代價函數(shù)為 A =
(2)
■/r(A)=
+ 00若《、+w，連續(xù)/Tv、《
li lj為局部窗口 Win(rwin(t))中的-界。
(3)
、、…、/:均為0，且義;7=+00，則稱序列
-個可通過方向域，i,.、 ii分別為此方向域的左右邊
3)安全性分析 對可通過li序列來說，其與障礙物邊界線的夾角越大，機器人沿此方向行進的安 全性就越高，定義li的安全性度量函數(shù)為
,") (4) 其中e，、 e"分別為FD(li)的左右邊界li肌、l"所對應的方向角。 4)運動平穩(wěn)性分析 機器人可以在360°范圍任意轉(zhuǎn)向，但實際應用中總期望機器人在行進過程中能 夠減少轉(zhuǎn)向操作，盡量沿著比較平滑的軌跡行駛，這樣有利于車體定位操作，同時降低能量 消耗。 為了能將路徑的平穩(wěn)性反映到路徑規(guī)劃模型中，引入路徑平滑性因素，定義li的 平滑性代價函數(shù)為 y;咖。礎I (5) 5)目標引導性分析
目標點Pe = (Xe， ye)對于局部環(huán)境中任一方向序列l(wèi)i的路徑引導函數(shù)為
■/T,)H (6)
其中9 g。al = arctan (yG-yK/xG-XK)為目標點PG (xG， yG)相對于機器人的方向 (egoal G [-…])。
6)采用自適應窗口的路徑規(guī)劃 在環(huán)境建模和分析基礎上，設計一種基于自適應窗口的實時路徑規(guī)劃方法，包括 趨向目標和沿墻行走兩種行為如下
(1)機器人的趨向目標行為
①即時子目標的確定 規(guī)劃窗口為Win(^n(t))，若尸(7g附"(Avin(/))，首先利用啟發(fā)式函數(shù)f(li)=
g(li)+h(li)來選取使f(li)最小的掃描點作為子目標(記作l曲)，即

;=arg —"、、(化》1 式中g(li)——機器人從當前位置向li方向行進的代價，h(l》 進到目標點方向的代價。根據(jù)環(huán)境建模的分析，定義
柳= 柳=力—
(7)
-從li方向行
(8) 確定lgh后，在其所在的可通過方向域FD("h)中求安全性子目標L^和平穩(wěn)性子
目t示1 smooth :
*■ smooth
=org, —、(/(/》)
(9)
(10)
11
至此，根據(jù)不同的性能指標要求得到位于可通過方向域FD("h)中的3種子目標 點:lgh = ( 9 gh， rwin) 、 lsafe = ( 9 safe， rwin) 、 l，oth = ( e s國th， rwin)。綜合考慮安全性和運動 平穩(wěn)性約束，對子目標方向進一步優(yōu)化選擇，最終即時子目標Psub為
(11)
Psub { ( Q sub， P sub) I Q sub kgh ^ gh+ksafe ^ safe+ksmooth ^ smooth， P sub rwin (t))"
XE^^^^^ kgh、 ksafe、 ksmooth kgh+ksafe+ksmooth 1Q
②規(guī)劃窗口自適應調(diào)整
動態(tài)窗口 Win(rwin(t))中總的可通過方向域和記為P^，定義如下
=化、sin A= ^ ^ (12)
L '=i J 在趨向目標行為中，Win(rwin(t))的調(diào)整原則為當爐^較大時，可以加大 Win(r^(t))，進一步擴展機器人的探測范圍，尋找更優(yōu)的子目標；當^D較小時，應該減小 Win(rwin(t))，縮小機器人的探測范圍，以便在狹窄空間中找到可行路徑，即
rwin(t+l) = rwin(t) + Ar (13)
其中Ar為^^的函數(shù)，定義為當0《^pD 〈Proj時， Ar = Armin (14a) 當Pfw《Pro <伊ro,2時，
△r ,max二A:i _ $ 2) + △ r鵬 (14b)
^V化2 一 ^Vd,1
當^FD,2 ^ PFD < ^FD,max時， Ar = Ar隨 (14c) 其中伊取?！?，眠2為設置的常數(shù)，《^,鵬為最大值，Armin、 Ar^為定義的最大、最小值。 (2)機器人的沿墻行走行為
①局部極小的判斷方法 當機器人陷入局部極小時，機器人運動軌跡會出現(xiàn)振蕩，或沿著某一閉合路徑運 動，從而不可能到達目標點。為了避免出現(xiàn)這種情況，需要提前預測機器人是否會進入局部 極小狀態(tài)，通過距離比較來判斷D(Psub， PG) > min(D(P" PG) ， Dleave) (15) 其中D(Psub， Pe)為子目標點與全局目標點之間的距離，D(PK， Pe)為機器人與全局 目標點之間的距離，D^e為趨向目標的保證距離，在離開沿墻走行為時定義，初始化為D一e =D(PS， Pe)。若條件(15)滿足，而且目標點位于機器人的左側，則follow—left—wall(沿 左墻)行為被激活；若條件(15)滿足，但目標點位于機器人的右側，則follow—right— wall(沿右墻)行為被激活。 一旦進入沿墻走行為，記錄機器人與目標點之間的最短距離 Dmin。使用沿墻走解死鎖算法而使移動機器人沿著障礙物的邊界運動，直到滿足以下退出條 件
12
D(PN，PG) <Dmin (16) 其中D(P,， Pe)為點P,和Pe之間的距離，Pw為沿墻走行為中下一時刻的預計到達 位置。離開沿墻走行為時，更新DlMre = D(PK， Pe)。
②即時子目標的確定 當follOW_left_Wall激活時，采用Win(rwin(t))中左起第一個障礙物的右邊界線 擴展線對應的掃描點為即時子目標；當fo 1 low_right_wal 1激活時，采用Win (rwin (t))中右 起第一個障礙物的左邊界線擴展線對應的掃描點為即時子目標。機器人對確定的子目標點 進行跟蹤，即可沿著障礙物行走；當機器人沿障礙繞行的距離超過設定值Lmaxl時仍未退出 則自轉(zhuǎn)180°再向另一繞行模式運動，在新的繞行模式下若繞行的距離超過設定值Lmax2仍 未退出(取L_2 > 2L隨》，則停止路徑(認為目標不可達)。 在沿墻走行為中，若發(fā)現(xiàn)目標點Pe位于Win(r^(t))的可通過域中，則直接以Pe 作為當前的即時子目標。
規(guī)劃窗口的動態(tài)調(diào)整_在沿墻走行為中，若為follow—leftjall，若《7" = 0， 則設置Q(f + l)-^v,，否則窗口大小不變；若為follow—rightjall，若yr-0，則設置 rwin(t+l) = dp否則窗口大小不變。
實施例2 本發(fā)明基于環(huán)境建模和自適應窗口的移動機器人路徑規(guī)劃方法，其路徑規(guī)劃中的 行為決擇和執(zhí)行過程如下所述 1)對系統(tǒng)參數(shù)rwin、 kgh、 ksafe、 ksm。。th、 kFD、9Vzu 、 Pro,2 、伊現(xiàn)駆、A rmin、 A r隨、Dleave、 Lmaxl、 Lmax2進行初始化，機器人剛啟動時默認為MtG行為。 2)讀取傳感器數(shù)據(jù)，采用公式(1)的形式表示，再用公式(2)對li進行重新定義。
3)對障礙物A的邊界進行膨化處理，采用公式(3) (6)計算Win(rwin(t))中1, 的可通過性、安全性、運動平穩(wěn)性和目標吸引性函數(shù)。 4)判斷機器人當前處在哪種行為，若為趨向目標行為，轉(zhuǎn)St印5，否則轉(zhuǎn)St印6。
5)趨向目標運動(Motion toward Goal,MtG)(即5. 1 5. 3)，直到滿足以下條件 之一 當?shù)竭_目標點時，停止運動。 當發(fā)現(xiàn)局部極小(即滿足公式(15))時，轉(zhuǎn)步驟6)。 5. 1)如果目標點位于可通過域中，則直接以目標點作為當前的子目標，否則利用 公式(7) (10)計算子目標點lgh、 13^和1，。th，再用公式(11)對子目標進行優(yōu)選，得到 當前Win(r^(t))內(nèi)的即時子目標Psub，如圖1。 5.2)如果無法確定即時子目標，機器人根據(jù)目標方向自轉(zhuǎn)90。進行探測(即以 (eK± ji/2，0)為子目標點); 5. 3)根據(jù)環(huán)境信息，采用公式(12)計算機器人的可通過方向域，然后用公式(13) 和(14)對窗口 Win(rwin(t))的半徑進行調(diào)整，轉(zhuǎn)步驟2)。6)沿墻行走(Wall Following, WF)(即6. 1 6. 3)，直到滿足以下條件之一
當?shù)竭_目標點時，停止運動。 當滿足退出條件(即滿足公式(16))時，轉(zhuǎn)步驟5)。 當沿墻行走的距離大于設定值仍未退出時，則認為目標不可達，停止運動。
6. 1)根據(jù)目標點位置，選擇follow_left_wall或follow_right_wall ; 6.2)如果目標點位于可通過域中，則直接以目標點作為當前的子目標，否則根據(jù)
沿墻走行為，即f0ll0W_left_wall激活時，采用Win(rwin(t))中左起第一個障礙物的右邊
界線擴展線對應的掃描點為即時子目標，即圖1中的lflw ;當fo 1 low_right_wal 1激活時，采
用Win(r^(t))中右起第一個障礙物的左邊界線擴展線對應的掃描點為即時子目標，即圖
1中的1^。 6. 3)根據(jù)環(huán)境信息調(diào)整規(guī)劃窗口的半徑r^(t+l)，轉(zhuǎn)步驟2)。
實施例3 為驗證多約束環(huán)境模型中各種因素對路徑規(guī)劃的影B向，圖2 4給出簡單環(huán)境下 的路徑規(guī)劃結果。其中，圖2為kgg = l，ksafe = ksm。。th = 0時的路徑，只考慮了可通行性，所 以盡管其路徑最短，但離障礙物的距離卻最近，這種情況很不利于機器人的安全行駛；圖3 為kgg = 0. 70， ksaft = 0. 30， ksm。。th = 0時的路徑，考慮了安全性，與圖2相比安全性明顯得 到提高(路徑遠離障礙物);圖4為kgg = 0. 55，kdafe = 0. 20，ksm。。th = 0. 25時的路徑，進一 步考慮了運動平穩(wěn)性，有利于減少機器人的轉(zhuǎn)向操作。
實施例4 當障礙物形狀過于復雜或特殊時，機器人單靠實時探測的局部環(huán)境知識容易陷人 死鎖或振蕩狀態(tài)。本發(fā)明借助有效的WF行為可以脫離局部極小而順利到達目標點。圖5， 機器人先采用follow—left—wall，行走Lmaxl距離后未退出，再轉(zhuǎn)向folloW_right_wall行 為，最后達到目標點；圖6，機器人進入狹小的U形障礙物找不到子目標，自轉(zhuǎn)90°后進入 follow_left_wall行為，最后達到目標點。
實施例5 基于局部窗口的滾動規(guī)劃算法中，如果窗口太小，機器人則不能及時提前進行避 障行走；窗口太大，機器人則可能找不到可選子目標。如圖7所示，為窗口半徑較小時的路 徑，機器人不能及時進入MtG的繞行模式，所得路徑彎曲較大；圖8為窗口半徑較大時規(guī)劃 得到的行走路徑，機器人不能及時找到可行路徑，兩次陷入WF行為。
實施例6 采用基于自適應窗口的路徑規(guī)劃方法時，機器人能夠根據(jù)環(huán)境信息對局部規(guī)劃窗 口的大小進行動態(tài)調(diào)整，所得路徑如圖9所示，其窗口大小的調(diào)整過程如圖IO所示?？梢?看出，圖9中路徑明顯優(yōu)于圖7和圖8中的路徑，而且機器人路徑規(guī)劃的計算量與窗口大小 無關，不會隨著窗口的增大而增大每一次滾動規(guī)劃中的局部規(guī)劃時間。
權利要求
基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特征在于包括以下步驟1)一個進行多約束局部環(huán)境建模和分析的步驟；2)一個可通過性分析的步驟；3)一個安全性分析的步驟；4)一個運動平穩(wěn)性分析的步驟5)一個目標引導性分析的步驟；6)一個采用自適應窗口的路徑規(guī)劃的步驟。
2. 如權利要求1所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特 征在于在步驟1)中，所述多約束局部環(huán)境建模和分析的步驟是機器人用激光傳感器感知 障礙物的信息，每一個障礙物探測點相對于機器人的位置由激光傳感器直接得出，角度掃 描間隔為A^，逆時針對激光掃描點進行標記為115 12，…，li，…，、，其中l(wèi)i以掃描角R和 對應的測量距離&來表示，即極坐標形式 <formula>formula see original document page 2</formula>根據(jù)以上傳感器掃描探測到的局部環(huán)境信息，設計一個動態(tài)自適應規(guī)劃窗口 ，其半徑r^根據(jù)環(huán)境信息動態(tài)調(diào)整，把t時刻測距范圍內(nèi)探測到的局部環(huán)境信息在半徑為rwin的半 圓窗口中，表示為Win(r^(t)) = {lply…，li，…，1181}，其中掃描點li被重新定義為 <formula>formula see original document page 2</formula>
3. 如權利要求1所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特征在于在步驟2)中，所述可通過性分析的步驟是采用邊界線擴展法對障礙物進行膨脹 后，將機器人視為一個不記大小的質(zhì)點，定義li方向上的可通行代價函數(shù)為若/r^+①，連續(xù)<formula>formula see original document page 2</formula> 均為o，且<formula>formula see original document page 2</formula>則稱序列ii ij為局部窗口 Win(rwin(t))中的一個可通過方向域，lj、li分別為此方向域的左右邊界。
4. 如權利要求1所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特 征在于在步驟3)中，所述安全性分析的步驟是對可通過li序列來說，其與障礙物邊界線 的夾角越大，機器人沿此方向行進的安全性就越高，定義li的安全性度量函數(shù)為其中e，、 e"分別為FD(li)的左右邊界ir、l"所對應的方向角。
5. 如權利要求l所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特征在于在步驟4)中，所述運動平穩(wěn)性分析的步驟是機器人在360。范圍任意轉(zhuǎn)向，引入路 徑平滑性因素，定義li的平滑性代價函數(shù)為 <formula>formula see original document page 2</formula>
6. 如權利要求1所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特征在于在步驟5)中，所述目標引導性分析的步驟是目標點^= (xe， ye)對于局部環(huán)境中 任一方向序列l(wèi)i的路徑引導函數(shù)為其中0一 =arctan(^-y《—^ )為目標點相對于機器人的方向，<formula>formula see original document page 3</formula>
7. 如權利要求1所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特征在于在步驟6)中，所述采用自適應窗口的路徑規(guī)劃的步驟是在環(huán)境建模和分析基礎上，設計一種基于自適應窗口的實時路徑規(guī)劃方法，包括機器人的趨向目標行為和機器人 的沿墻行走行為。
8. 如權利要求7所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特 征在于所述機器人的趨向目標行為包括確定即時子目標和規(guī)劃窗口自適應調(diào)整。
9. 如權利要求8所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其特 征在于所述確定即時子目標的方法為規(guī)劃窗口為Win(rwin(t))，若PGg附"( in⑦)，首先利用啟發(fā)式函數(shù)f (li)= g(li)+h(li)來選取使f(li)最小的掃描點作為子目標，記作l曲，即 'gh <formula>formula see original document page 3</formula>式中g(li)——機器人從當前位置向li方向行進的代價，h(l》——從li方向行進到 目標點方向的代價，根據(jù)環(huán)境建模的分析，定義柳=/廣， /r1確定lgh后，在其所在的通過方向域FD(lgh)中求安全性子目標lsafe和平穩(wěn)性子目標<formula>formula see original document page 3</formula>至此，根據(jù)不同的性能指標要求得到位于可通過方向域FD("h)中的3種子目標點 lgh = ( 9 gh， O 、 lsafe = ( 9 safe， rwin) 、 l，th = ( e s國th， rwin)，綜合考慮安全性和運動 平穩(wěn)性約束，對子目標方向進一步優(yōu)化選擇，最終即時子目標Psub為Psub "I ( Q sub， P sub) I Q sub kgh ^ gh+kSafe ^ safe+kSmooth ^ smooth， P sub rwin (t) )" lE^^^^ kgh、 ksafe、 ksmooth kgh+ksafe+ksmooth1 ^所述規(guī)劃窗口自適應調(diào)整的方法為動態(tài)窗口 Win(rwin(t))中總的可通過方向域和記為 ^，定義如下<formula>formula see original document page 3</formula>在趨向目標行為中，Win(r^(t))的調(diào)整原則為當^D較大時，可以加大Win(r^(t))，進一步擴展機器人的探測范圍，尋找更優(yōu)的子目標；當9^較小時，應該減小Win(rwin(t))， 縮小機器人的探測范圍，以便在狹窄空間中找到可行路徑，即<formula>formula see original document page 4</formula>其中P剛、9V2為設置的常數(shù)，^Vmax為最大值，Armin、 Al^為定義的最大、最小值。
10.如權利要求7所述的基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，其 特征在于所述機器人的沿墻行走行為包括局部極小的判斷、確定即時子目標和規(guī)劃窗口的 動態(tài)調(diào)整；所述局部極小的判斷方法為當機器人陷入局部極小時，機器人運動軌跡會出現(xiàn)振蕩，或沿著某一閉合路徑運動，從 而不可能到達目標點，為了避免出現(xiàn)這種情況，需要提前預測機器人是否會進入局部極小 狀態(tài)，通過距離比較來判斷<formula>formula see original document page 4</formula>其中D(Psub， Pe)為子目標點與全局目標點之間的距離，D(PK， Pe)為機器人與全局目標 點之間的距離，Dleare為趨向目標的保證距離，在離開沿墻走行為時定義，初始化為DlMve = D(PS， P》，若條件(15)滿足，而且目標點位于機器人的左側，則沿左墻行為被激活；若條件 (15)滿足，但目標點位于機器人的右側，則沿右墻行為被激活， 一旦進入沿墻走行為，記錄 機器人與目標點之間的最短距離Dmin，使用沿墻走解死鎖算法而使移動機器人沿著障礙物 的邊界運動，直到滿足以下退出條件<formula>formula see original document page 4</formula>其中D(P,，P》為點P,和Pe之間的距離，P,為沿墻走行為中下一時刻的預計到達位置， 離開沿墻走行為時，更新DlMre = D(PK， Pe); 所述確定即時子目標的方法為當沿左墻激活時，采用Win(rwin(t))中左起第一個障礙物的右邊界線擴展線對應的掃 描點為即時子目標；當沿右墻激活時，采用Win(r^(t))中右起第一個障礙物的左邊界線 擴展線對應的掃描點為即時子目標，機器人對確定的子目標點進行跟蹤，即可沿著障礙物 行走；當機器人沿障礙繞行的距離超過設定值Lmaxl時仍未退出則自轉(zhuǎn)180°再向另一繞行模式運動，在新的繞行模式下若繞行的距離超過設定值Lm^仍未退出，取Lm^ > 2Lm^，則停止路徑；在沿墻走行為中，若發(fā)現(xiàn)目標點Pe位于Win (rwin(t))的可通過域中，則直接以Pe 作為當前的即時子目標；所述規(guī)劃窗口的動態(tài)調(diào)整的方法為在沿墻走行為中，若為沿左墻，若,;r =0，則設置^ (/ + 1) = ^ ，否則窗口大小不變；若為沿右墻，若y;r-o，則設置r^a+i) = dp否則窗口大小不變,
全文摘要
基于環(huán)境建模與自適應窗口的移動機器人路徑規(guī)劃方法，涉及一種移動機器人實時路徑規(guī)劃方法。一個進行多約束局部環(huán)境建模和分析的步驟；一個可通過性分析的步驟；一個安全性分析的步驟；一個運動平穩(wěn)性分析的步驟一個目標引導性分析的步驟；一個采用自適應窗口的路徑規(guī)劃的步驟。由于具有更好的環(huán)境適應性和避障能力，獲得較好的安全性和可達性，并具有較高的計算實時性，因此解決了不確定復雜環(huán)境下移動機器人實時生成避障路徑的問題，提供了一種綜合最優(yōu)的路徑選擇方法，較好地滿足移動機器人的避障要求，實現(xiàn)機器人的實時路徑規(guī)劃和控制，為移動機器人的導航應用提供一個有效的無碰撞路徑規(guī)劃方法。
文檔編號G01C21/34GK101738195SQ20091011308
公開日2010年6月16日 申請日期2009年12月24日 優(yōu)先權日2009年12月24日
發(fā)明者仲訓昱, 彭俠夫, 繆孟良 申請人:廈門大學