本發(fā)明屬于機(jī)器人導(dǎo)航領(lǐng)域，尤其涉及一種動態(tài)環(huán)境中機(jī)器人安全自主規(guī)劃導(dǎo)航方法。

背景技術(shù)：

無論是自主巡航的機(jī)動車還是無人駕駛飛機(jī)，幾乎所有的機(jī)器人都應(yīng)該具備在人，寵物，汽車等移動個體面前從一個地方到另一個地方安全導(dǎo)航的能力。為了實(shí)現(xiàn)這個功能，機(jī)器人應(yīng)該對動態(tài)障礙物近期可能處于的位置進(jìn)行預(yù)判并同時產(chǎn)生一段的能夠避免碰撞的路徑。同時，由于機(jī)器人對動態(tài)障礙物軌跡的預(yù)測在持續(xù)變化，因此機(jī)器人也需要在很短的時間里來規(guī)劃這些路徑。如何在動態(tài)環(huán)境中以有限的時間規(guī)劃可靠的路徑顯得至關(guān)重要。動態(tài)環(huán)境在時間域規(guī)劃上構(gòu)成實(shí)時約束。如果一個規(guī)劃方案需要很長的時間來得到一個新路徑，那么機(jī)器人很有可能和一個移動的障礙物發(fā)生碰撞。

許多對動態(tài)障礙物的處理方法是將它們模型化成初始運(yùn)行軌跡下的具有高代價(jià)的短窗口靜態(tài)障礙物。這個方法非常有效率，但存在次優(yōu)性的缺點(diǎn)；還有的方法是只在執(zhí)行路徑規(guī)劃的情形下和使用相鄰障礙物避免方法下來考慮動態(tài)障礙物(仍然將它們看作靜態(tài)障礙物)，但是可能在局部小范圍會出現(xiàn)停滯和卡頓；另一個對局部規(guī)劃方案的替代方法是采用速度障礙物，速度障礙物能夠決定和引導(dǎo)與移動障礙物碰撞的控制，盡管這個方法相對來說更加準(zhǔn)確，但是它仍然會引起局部小范圍問題；

有一些方法在全面的時間-空間規(guī)劃尋找路徑。silver’shca*算法是用來為多個機(jī)器人實(shí)現(xiàn)規(guī)劃，但是只適用于很少的動態(tài)障礙物情形下，正如sipp方法下的實(shí)驗(yàn)結(jié)果，而且該算法所花費(fèi)的時間比實(shí)時時間要長。sipp在動態(tài)環(huán)境下是一種快速的優(yōu)化方案，由于對每個空間位置來說時間步長的數(shù)量是無限的，因此在狀態(tài)空間中對時間進(jìn)行劃分來作為一個明確的維度相對相對較慢。sipp把時間步長壓縮成相鄰的安全時間間隔指數(shù)。因此對于每個安全時間間隔和每個空間位置只有一個單獨(dú)的狀態(tài)。

在線實(shí)現(xiàn)用時間來進(jìn)行規(guī)劃并同時需要處理動態(tài)障礙物是十分困難的，因?yàn)槎我?guī)劃的連續(xù)需求在執(zhí)行周期中強(qiáng)制形成嚴(yán)格的約束條件。為了明確實(shí)時約束條件，一系列的方法被用于犧牲其次優(yōu)性來保證效率。rrt-variants在能夠處理更多復(fù)雜機(jī)器人動態(tài)運(yùn)動學(xué)約束下的高維度搜索空間來實(shí)現(xiàn)快速規(guī)劃。然而，這些基于采樣的方法不能夠保證所爭取的優(yōu)化性。rrt*是一種近期任何時候基于采樣的方法，這種方法在限制條件下能夠得出優(yōu)化方案。然而，當(dāng)規(guī)劃時間結(jié)束時，這個算法不能提供關(guān)于這個方案到底有多合適的任何相關(guān)信息，另一方面，對于每個解決方案，實(shí)時搜索策略在 次優(yōu)性與優(yōu)化方案相比下有數(shù)值的上界。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于針對上述技術(shù)中存在的缺陷，提出了一種動態(tài)環(huán)境中機(jī)器人安全自主規(guī)劃導(dǎo)航方法。通過結(jié)合ara*(實(shí)時a*規(guī)劃方案)和用sipp運(yùn)行weighta*搜索能力把sipp擴(kuò)展成實(shí)時搜索，在實(shí)時sipp規(guī)劃方案之上，從時間界限網(wǎng)格中采取時間范圍。顯著提高了機(jī)器人在大規(guī)模環(huán)境中規(guī)劃速度；本發(fā)明除了提供實(shí)時規(guī)劃方案之外，還提供了對解決方案代價(jià)的完整性和次優(yōu)性理論的保證。

為了實(shí)現(xiàn)上述目的，本發(fā)明提供的技術(shù)方案如下：一種動態(tài)環(huán)境中機(jī)器人安全自主規(guī)劃導(dǎo)航方法，其特征在于包括如下步驟：

步驟1、動態(tài)障礙物表示法：用特定的方法來表示環(huán)境中動態(tài)障礙物并統(tǒng)一這一表示；

所述特定方法為：假設(shè)存在另一個能夠跟蹤環(huán)境中動態(tài)障礙物的系統(tǒng)，該系統(tǒng)存在一系列動態(tài)障礙物并且把每個障礙物看成具有運(yùn)動軌跡和半徑的球體，一條軌跡是由一系列的點(diǎn)組成，而每個點(diǎn)有表明其構(gòu)成和時間的狀態(tài)變量，這些軌跡中的點(diǎn)按照時間順序排列，通過讀取這些點(diǎn)的順序來預(yù)測未來一段時間障礙物的運(yùn)動軌跡并進(jìn)行統(tǒng)一表示；

步驟2、表示法和假設(shè)：該步驟中描述了如何表示和解釋機(jī)器人的路徑規(guī)劃算法；

步驟2.1、描述了機(jī)器人在環(huán)境中的空間構(gòu)型以及表示方法，假設(shè)規(guī)劃問題是用圖來表示，每個在圖中的狀態(tài)量s由兩部分組成：機(jī)器人的空間構(gòu)型向量記作x(s)和標(biāo)量安全時間間隔指數(shù)記作interval(s)；

步驟2.2、定義了機(jī)器人在搜尋路徑過程中的一系列參數(shù)：其中包括狀態(tài)量s的定義，在機(jī)器人的搜尋路徑過程中，每個狀態(tài)量s有一個變量記作g(s)，這個變量是從初始狀態(tài)到s狀態(tài)的最出名路徑所花費(fèi)的代價(jià)；

啟發(fā)式函數(shù)h(s)是從s狀態(tài)到目標(biāo)狀態(tài)的代價(jià)估計(jì)，假設(shè)這個啟發(fā)式函數(shù)是一個常量，這意味著它不會大于到目的點(diǎn)的代價(jià)并且滿足三角不等式；

狀態(tài)轉(zhuǎn)變所花費(fèi)的時間代價(jià)c(s，s’)，從s狀態(tài)到其中一個繼承狀態(tài)s’的轉(zhuǎn)變或者邊緣代價(jià)記作c(s，s’)；

步驟2.3、表明asipp算法和sipp具備相同的假設(shè)前提：c(s，s’)等價(jià)于從s狀態(tài)到s’狀態(tài)的行動執(zhí)行時間；

步驟3、asipp算法：asipp算法是ara*算法和sipp算法二者的結(jié)合并進(jìn)行一些擴(kuò)展和修改的算法，緊接著提出了兩個算法；

步驟3.1、算法1是ara*算法的主體循環(huán)并得出目前所得到的解決方案的次優(yōu)性范圍；

步驟3.1.1、初始顯示用戶提供的初始參數(shù)ε的初始搜索機(jī)制，之后減少ε的值(用戶可以自定義減少的值)并且再運(yùn)行搜索直到到達(dá)最優(yōu)解(ε＝1)；

步驟3.1.2、在步驟3.1.1每次搜索過程中，incons列表會被找到的成本更低的路徑的狀態(tài)填滿，由于它們已經(jīng)很接近因此不會被用于再擴(kuò)展；

步驟3.1.3、步驟3.1.2中所述成本更低的路徑的狀態(tài)需要在下一次搜索迭代中被再次擴(kuò)展，同時，這些被放入open列表的狀態(tài)有機(jī)會在下一次搜索迭代中被擴(kuò)展，最后計(jì)算ε’的值，得出目前所找到的解決方案的次優(yōu)性范圍。

步驟3.2、算法2是一個單獨(dú)的在sipp下實(shí)現(xiàn)的weighteda*搜索機(jī)制；

步驟3.2.1、通常在weighteda*中，在open列表的狀態(tài)中，有著最小f值的狀態(tài)被擴(kuò)展直到目標(biāo)值在open列表中有代價(jià)最小的f值，一個狀態(tài)s的f值是g(s)和h(s)的一個函數(shù)，當(dāng)s在open列表中，起擴(kuò)展?fàn)顟B(tài)s的優(yōu)先權(quán)的作用；

步驟3.2.2、一個狀態(tài)s擴(kuò)展過程開始時，此處迭代循環(huán)所有可能的移動基元，這些移動基元能夠在沒有與靜態(tài)環(huán)境發(fā)生碰撞的情況下被采用，變量x’是進(jìn)行移動m后到狀態(tài)s的空間構(gòu)型x(s)之后的機(jī)器人結(jié)束時所處的空間構(gòu)型；

步驟3.2.3、創(chuàng)建了最小的和最大的機(jī)器人能夠基于它能夠離開安全間隔s最早和最晚的次數(shù)來到達(dá)x’次數(shù)；

步驟3.2.4、算法2通過一個等待操作，然后再移動m，將一個后繼在盡可能早的時間放到位于x’的每個安全間隔；

步驟4、理論分析：主要是通過提出兩個定理來驗(yàn)證算法的完整性和最優(yōu)性；

步驟4.1、定理1：任何時候sipp算法是完整的，當(dāng)算法結(jié)束時，它對于動態(tài)障礙物和靜態(tài)障礙物分別返回了一個到目標(biāo)點(diǎn)的安全距離，其前提是這樣的一條路徑存在于給定規(guī)劃問題的圖形表示；

步驟4.2、定理2：當(dāng)規(guī)劃時間結(jié)束時，通過任何時候sipp算法返回的結(jié)果所花費(fèi)的代價(jià)不會比ε’倍最優(yōu)解所花費(fèi)的代價(jià)大；

步驟5、時限圖表示：當(dāng)規(guī)劃方案處于動態(tài)或者不確定的環(huán)境中，它通常不會在未來很長的時間內(nèi)依賴于預(yù)測的障礙物行為，時限圖就是基于上述理論創(chuàng)建出一個由兩種狀態(tài)組成的圖：一個狀態(tài)是由結(jié)構(gòu)和時間間隔所決定，即高維度狀態(tài)；另一個狀態(tài)只由空間構(gòu)型決定，即低維度狀態(tài)。

所述的步驟5中還可以考慮用sipp和任何時候sipp把時限表達(dá)方式放進(jìn)基于安全區(qū)間的圖形表示方法中，限制對應(yīng)的截?cái)鄤討B(tài)障礙物預(yù)測軌跡的持續(xù)時間的sipp中的時間范圍以達(dá)到不超過時間范圍的目的。

本發(fā)明具有如下有益效果：

1)本發(fā)明通過結(jié)合ara*(實(shí)時a*規(guī)劃方案)和用sipp運(yùn)行weighta*搜索能力把sipp擴(kuò)展成實(shí)時搜索，在實(shí)時sipp規(guī)劃方案之上，從時間界限網(wǎng)格中采取時間范圍；在時間范圍之后的狀態(tài)用來為它們的空間坐標(biāo)做規(guī)劃，由于時間維度只存在機(jī)器人時間附近，因此以這種方式在大規(guī)模環(huán)境中規(guī)劃速度會得到顯著地提高。

2)本發(fā)明除了提供實(shí)時規(guī)劃方案之外，還提供了對解決方案代價(jià)的完整性和次優(yōu)性理論的保證；在實(shí)際操作上，本發(fā)明展示了規(guī)劃方案在無人駕駛飛機(jī)領(lǐng)域?qū)崟r能力，在有著50個動態(tài)障礙物的巨大地圖上規(guī)劃時間并參數(shù)化路徑，結(jié)果顯示本發(fā)明能夠基于時間范圍為0.05秒內(nèi)得到初始解決方案并且能夠給予充足的時間來提升解決方案使其達(dá)到最優(yōu)化。

附圖說明

圖1為本發(fā)明所述的動態(tài)環(huán)境中機(jī)器人安全自主規(guī)劃導(dǎo)航方法的流程圖。

具體實(shí)施方式

下面結(jié)合附圖對本發(fā)明做進(jìn)一步的說明。

一種動態(tài)環(huán)境中機(jī)器人安全自主規(guī)劃導(dǎo)航方法，該方法包括以下步驟：如附圖1所示，步驟1、動態(tài)障礙物表示法：用特定的方法來表示環(huán)境中動態(tài)障礙物并統(tǒng)一這一表示；本發(fā)明中假設(shè)存在另一個能夠跟蹤環(huán)境中動態(tài)障礙物的系統(tǒng)，該系統(tǒng)存在一系列動態(tài)障礙物并且把每個障礙物看成具有運(yùn)動軌跡和半徑的球體，一條軌跡是由一系列的點(diǎn)組成，而每個點(diǎn)有表明其構(gòu)成和時間的狀態(tài)變量，這些軌跡中的點(diǎn)按照時間順序排列，通過讀取這些點(diǎn)的順序來預(yù)測未來一段時間障礙物的運(yùn)動軌跡并進(jìn)行統(tǒng)一表示；

步驟2、表示法和假設(shè)：該步驟中描述了如何表示和解釋機(jī)器人的路徑規(guī)劃算法；

步驟2.1、描述了機(jī)器人在環(huán)境中的空間構(gòu)型以及表示方法，假設(shè)規(guī)劃問題是用圖來表示，每個在圖中的狀態(tài)量s由兩部分組成：機(jī)器人的空間構(gòu)型向量記作x(s)和標(biāo)量安全時間間隔指數(shù)記作interval(s)；連續(xù)安全時間間隔指數(shù)是指一段時間內(nèi)沒有動態(tài)障礙物的特定位置的時間間隔。例如，如果只有一個動態(tài)障礙物經(jīng)過一個特定位置x(s)，那么此處只有兩個安全時間間隔(在障礙物經(jīng)過之前和之后)。這意味著只存在兩個狀態(tài)有x(s)的值，一個狀態(tài)是interval(s)＝0，另一個是interval(s)＝1。對于一個給定的x(s)時間步長，這大大低于許多狀態(tài)副本。狀態(tài)之間的邊緣是連接對應(yīng)構(gòu)型的短運(yùn)動，基于構(gòu)型空間的表達(dá)方式，這些可能是適用于動態(tài)運(yùn)動的移動基元或者是有著常曲率的 曲線的簡單片段。

步驟2.2、定義了機(jī)器人在搜尋路徑過程中的一系列參數(shù)：其中包括狀態(tài)量s的定義，在機(jī)器人的搜尋路徑過程中，每個狀態(tài)量s有一個變量記作g(s)，這個變量是從初始狀態(tài)到s狀態(tài)的最出名路徑所花費(fèi)的代價(jià)；

啟發(fā)式函數(shù)h(s)是從s狀態(tài)到目標(biāo)狀態(tài)的代價(jià)估計(jì)，假設(shè)這個啟發(fā)式函數(shù)是一個常量，這意味著它不會大于到目的點(diǎn)的代價(jià)并且滿足三角不等式；

狀態(tài)轉(zhuǎn)變所花費(fèi)的時間代價(jià)c(s，s’)，從s狀態(tài)到其中一個繼承狀態(tài)s’的轉(zhuǎn)變或者邊緣代價(jià)記作c(s，s’)；

步驟2.3、表明asipp算法和sipp具備相同的假設(shè)前提：c(s，s’)等價(jià)于從s狀態(tài)到s’狀態(tài)的行動執(zhí)行時間；

步驟3、asipp算法：asipp算法是ara*算法和sipp算法二者的結(jié)合并進(jìn)行一些擴(kuò)展和修改的算法；

緊接著提出了兩個算法；

步驟3.1、算法1是ara*算法的主體循環(huán)并得出目前所得到的解決方案的次優(yōu)性范圍；首先用戶提供一個初始的ε的值并首次調(diào)用算法2的improvepath()函數(shù)方法,接著利用ε’＝min(ε,g(sgoal)/mins∈open∪incons(g(s)+h(s)))，計(jì)算對應(yīng)的ε’的值并且發(fā)布當(dāng)前次優(yōu)方案ε’，其中，ε’能得出在目前所找到的解決方案的次優(yōu)性范圍；如果ε的值大于1，那么以用戶定義的值減少ε的值，在此循環(huán)中，將狀態(tài)從incons列表轉(zhuǎn)移到open列表同時對于所有open列表中的所有的s∈open根據(jù)f(s)的值更新所有狀態(tài)的優(yōu)先順序，而這些被放入open列表的狀態(tài)可能在下一次迭代中被再次擴(kuò)展，當(dāng)ε＝1的時整個循環(huán)體結(jié)束。

步驟3.2、improvepath(算法2)是一個單獨(dú)的在sipp下實(shí)現(xiàn)的weighteda*搜索機(jī)制；通常在weighteda*中，在open列表中的狀態(tài)中有著最小f值的狀態(tài)被用于擴(kuò)展直到目標(biāo)值在open列表中有代價(jià)最小的f值。一個狀態(tài)s的f值是g(s)和h(s)的一個函數(shù)，當(dāng)s在open列表中，它能夠優(yōu)先擴(kuò)展?fàn)顟B(tài)s，首先迭代循環(huán)所有可能的移動基元，這些移動基元能夠在沒有與靜態(tài)環(huán)境發(fā)生碰撞的情況下被采用；變量x’是進(jìn)行移動m到狀態(tài)s的空間構(gòu)型x(s)之后的機(jī)器人結(jié)束時所處的空間構(gòu)型。

算法2接著創(chuàng)建了最小的和最大的機(jī)器人能夠基于它能夠離開安全間隔s最早和最晚的次數(shù)來到達(dá)x’次數(shù)；然后算法2將一個后繼在盡可能早的時間放到位于x’的每個安全間隔，其實(shí)現(xiàn)方法是采用一個等待操作，然后再移動m(等待時間可以為0因此它能夠馬上適用于機(jī)器人移動)。

為了保證當(dāng)ε>1時的完整性的次優(yōu)界定，算法對每個狀態(tài)采用兩個版本，最優(yōu)和次優(yōu)，如果擴(kuò)展的狀態(tài)是最優(yōu)的，那么對于每個后繼(s’和s”)，它會生成兩個相同的 版本：最優(yōu)變量o(s’)被設(shè)為真，另一個有o(s”)被設(shè)為假；如果擴(kuò)展的狀態(tài)是次優(yōu)的，那么它能夠保證更多的次優(yōu)狀態(tài)，次優(yōu)狀態(tài)通常以f(s’)＝g(s’)+ε*h(s’)被放入open列表里，而最優(yōu)狀態(tài)以f(s’)＝ε*(g(s’)+h(s’))被放入open列表里。open列表保存了在最優(yōu)狀態(tài)中的最優(yōu)擴(kuò)展順序(因?yàn)閒與一個標(biāo)量相乘)，但是也有將之后的最優(yōu)狀態(tài)放入open列表中涉及次佳的狀態(tài)，因?yàn)閮烧叨急粩U(kuò)大了而不是一個。這意味著在許多時間情況下，只有次優(yōu)狀態(tài)被擴(kuò)展，這樣的結(jié)果導(dǎo)致足夠多的最優(yōu)狀態(tài)被擴(kuò)展用于對weighteda*搜索期望的理論支持和保證(次優(yōu)性的完整和界定)。

步驟4、理論分析：主要是通過提出兩個定理來驗(yàn)證算法的完整性和最優(yōu)性；

步驟4.1、定理1：任何時候sipp算法是完整的，當(dāng)算法結(jié)束時，它對于動態(tài)障礙物和靜態(tài)障礙物分別返回了一個到目標(biāo)點(diǎn)的安全距離，其前提是這樣的一條路徑存在于給定規(guī)劃問題的圖形表示；

步驟4.2、定理2：當(dāng)規(guī)劃時間結(jié)束時，通過任何時候sipp算法返回的結(jié)果所花費(fèi)的代價(jià)不會比ε’倍最優(yōu)解所花費(fèi)的代價(jià)大；

步驟5、時限圖表示：當(dāng)規(guī)劃方案處于動態(tài)或者不確定的環(huán)境中，它通常不會在未來很長的時間內(nèi)依賴于預(yù)測的障礙物行為，時限圖就是基于上述理論創(chuàng)建出一個由兩種狀態(tài)組成的圖：一個狀態(tài)是由結(jié)構(gòu)和時間間隔所決定，即高維度狀態(tài)；另一個狀態(tài)只由空間構(gòu)型決定，即低維度狀態(tài)。高維度狀態(tài)出現(xiàn)在機(jī)器人附近，這些狀態(tài)只能在給定的時間范圍內(nèi)從機(jī)器人狀態(tài)到達(dá)。在時限網(wǎng)格圖中，時間步長與時間范圍相等的狀態(tài)能夠轉(zhuǎn)變成不包括時間的狀態(tài)，即低維度狀態(tài)。因此，一旦時間步長到達(dá)給定的時間范圍，狀態(tài)就會變成“不受時間影響的”。

在此步驟5中還可以考慮用sipp和任何時候sipp把時限表達(dá)方式放進(jìn)基于安全區(qū)間的圖形表示方法中，限制對應(yīng)的截?cái)鄤討B(tài)障礙物預(yù)測軌跡的持續(xù)時間的sipp中的時間范圍以達(dá)到不超過時間范圍的目的。因此，在sipp和任何sipp下，有限時間范圍的實(shí)現(xiàn)已經(jīng)通過截?cái)鄤討B(tài)障礙物的預(yù)測軌跡的持續(xù)時間實(shí)現(xiàn)。

任何時候規(guī)劃方案的一個重要的特點(diǎn)是它能夠很快地得到初始解決方案。當(dāng)我們改變初始的ε值和時間范圍時，算法得到初始解決方案總共所花費(fèi)的時間。表1的參數(shù)是經(jīng)過超過40次運(yùn)算后對于4種不同的時間范圍sipp和任何時候sipp的規(guī)劃時間的分布，這些結(jié)果都是利用最優(yōu)sipp(ε＝1)得到初始解決方案所花費(fèi)的時間。從表1中，可以很明顯的得出任何時候sipp提供規(guī)劃時間比最優(yōu)sipp至少少兩個數(shù)量級。表1的結(jié)果顯示：為了快速地得到一個解決方案，一個有權(quán)重的搜索是必不可少的，本發(fā)明中所省略的是擴(kuò)展?fàn)顟B(tài)的數(shù)量，因?yàn)檫@反映的是規(guī)劃時間(僅僅是標(biāo)量的乘積)。

如果規(guī)劃方案所需的時間比這個還要多，本發(fā)明將采用ara*算法提高解決方案的質(zhì)量。在這個實(shí)驗(yàn)中，次優(yōu)性范圍ε在連續(xù)的ara*算法迭代中減少了0.2。一旦ε 的值到達(dá)1，解決方案被證實(shí)是最優(yōu)的。

表1sipp和asipp規(guī)劃時間分布

以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)的所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。