移動(dòng)機(jī)器人最小快照軌跡尋優(yōu)方法研究

彭 偉，劉小方，吳玉彬，何 星，張憲宇

(火箭軍工程大學(xué)， 西安 710025)

1 引言

移動(dòng)機(jī)器人路徑規(guī)劃是集動(dòng)態(tài)環(huán)境檢測、動(dòng)態(tài)規(guī)劃與執(zhí)行、行為決策與控制等數(shù)項(xiàng)功能于一體的綜合化智能系統(tǒng)[1]。近年來已經(jīng)成功地應(yīng)用到工農(nóng)業(yè)生產(chǎn)的各個(gè)領(lǐng)域，在我們生活的周圍可以隨處找到它的影子，因而移動(dòng)機(jī)器人路徑規(guī)劃問題便引起了國內(nèi)外眾多學(xué)者的廣泛關(guān)注，獲得了一大批卓有成效的研究成果。

如何進(jìn)行路徑規(guī)劃是無人飛行器、水下機(jī)器人[2]等移動(dòng)機(jī)器人系統(tǒng)的一個(gè)重要問題，需要執(zhí)行者跟蹤移動(dòng)目標(biāo)，也就是要求移動(dòng)機(jī)器人追逐另一個(gè)移動(dòng)物體，例如潛艇[3]。除了機(jī)器人技術(shù)[4-5]之外，移動(dòng)目標(biāo)路徑規(guī)劃算法還可以解決諸如語音識別、路由、聯(lián)網(wǎng)[6-7]和視頻游戲[8-9]等各種問題。這些應(yīng)用程序都依賴于能夠在目標(biāo)改變時(shí)快速重新規(guī)劃路徑。例如，在網(wǎng)絡(luò)中，當(dāng)必須向某個(gè)位置發(fā)送數(shù)據(jù)并且該位置不確定或改變時(shí)，使用移動(dòng)目標(biāo)路徑規(guī)劃器[10]。我們正在處理的運(yùn)動(dòng)目標(biāo)路徑規(guī)劃問題在某些方面類似于通常應(yīng)用于導(dǎo)彈[11-12]的閉環(huán)尋的制導(dǎo)算法。主要的區(qū)別在于，我們的場景除了機(jī)動(dòng)目標(biāo)之外還有潛在的迷宮式障礙。

D.Mellinger等[13]提出了最小快照軌跡生成算法，但是針對最小快照軌跡生成的原因并沒有進(jìn)行解釋，以及最小快照在控制中的作用也鮮有涉獵。根據(jù)已有文獻(xiàn)研究內(nèi)容和其他一些相關(guān)學(xué)者的意見，平滑性標(biāo)準(zhǔn)一般可以轉(zhuǎn)化為最小化輸入變化率，這對于保持機(jī)載傳感器測量的質(zhì)量以及避免突然或過度的控制輸入帶來無法完成路徑尋優(yōu)任務(wù)是很有必要的。對于高動(dòng)態(tài)車輛和機(jī)器人，多項(xiàng)式軌跡的選擇是自然的[4-15]，因?yàn)樽鳛榧s束QP的解決方案，這些軌跡可以有效地獲得，并且約束QP使路徑導(dǎo)數(shù)的成本函數(shù)最小化。該優(yōu)化框架允許路徑段的端點(diǎn)可選地固定到期望值或保持自由，并且多項(xiàng)式可以聯(lián)合優(yōu)化，同時(shí)將導(dǎo)數(shù)的連續(xù)性保持到任意順序。對于導(dǎo)數(shù)，通過這些航路點(diǎn)的航點(diǎn)和時(shí)間進(jìn)而給出這個(gè)QP問題的約束。

盡管近年來針對移動(dòng)機(jī)器人路徑規(guī)劃問題已經(jīng)取得了很多卓有成效的研究成果，但就目前作者關(guān)注情況而言，對于未知?jiǎng)討B(tài)不確定環(huán)境下的移動(dòng)機(jī)器人路徑規(guī)劃尋優(yōu)問題還鮮有聽聞，眾所周知，未來的應(yīng)用環(huán)境是瞬息萬變的，這就要求移動(dòng)機(jī)器人的路徑規(guī)劃尋優(yōu)必須能夠做到與時(shí)俱進(jìn)地適應(yīng)任何苛刻的動(dòng)態(tài)環(huán)境。基于此，本文的研究重點(diǎn)就放在移動(dòng)障礙物時(shí)實(shí)現(xiàn)最小快照軌跡的生成。大多數(shù)研究人員處理這一問題的常用方法是嘗試添加路徑點(diǎn)。基本過程如下：如果在優(yōu)化后發(fā)現(xiàn)特定的軌跡段與障礙物相交，則在其兩端之間簡單地添加一個(gè)附加的路徑點(diǎn)，將該路段分成兩部分。這個(gè)中點(diǎn)被認(rèn)為是無碰撞的，因?yàn)樗挥谟伤阉魉惴ǚ祷氐淖顑?yōu)分段線性路徑上。用附加的路徑點(diǎn)重新優(yōu)化多項(xiàng)式，必要時(shí)重復(fù)該過程，直到多項(xiàng)式軌跡無碰撞為止。顯然，附加的路徑點(diǎn)增加了計(jì)算復(fù)雜度，尤其是在室內(nèi)密集環(huán)境中有移動(dòng)障礙物存在時(shí)。因此，本文嘗試分析添加路徑點(diǎn)時(shí)的具體影響，并試圖找出在避障成功的前提下可以降低計(jì)算復(fù)雜度的有效途徑。

2 模型建立

針對持續(xù)時(shí)間段是已知的情況，路徑可以描述如下。需要說明的是平坦輸出空間中兩點(diǎn)之間的單個(gè)軌跡段由獨(dú)立多項(xiàng)式f(t)組成。

(1)

處理多項(xiàng)式f(t)導(dǎo)數(shù)平方的代價(jià)函數(shù)可以復(fù)寫成如下：

(2)

其中c0,c1，…,cN是這些不同導(dǎo)數(shù)的系數(shù)。為了對這個(gè)快照之和進(jìn)行最小化，除了c4之外，代價(jià)函數(shù)中的所有導(dǎo)數(shù)懲罰將被設(shè)置為零。

于是，一個(gè)段的成本函數(shù)可以描述為如下：

(f(4)(t))2=

對于所有分段，m個(gè)多項(xiàng)式分段可以通過以塊對角格式連接它們的成本矩陣來聯(lián)合優(yōu)化。多項(xiàng)式優(yōu)化中的約束施加在每個(gè)軌跡段的端點(diǎn)上。這些約束允許將端點(diǎn)固定到空間中的已知位置，或者指定特定的速度、加速度和階躍等。

(4)

利用多項(xiàng)式的系數(shù)和端點(diǎn)導(dǎo)數(shù)之間的映射矩陣(Aj)，對第段軌跡的導(dǎo)數(shù)約束進(jìn)行公式化：

(5)

其中dj代表給定的第j段的位置信息。第j階導(dǎo)數(shù)表示路徑點(diǎn)約束。可以描述為如下。

(6)

對于聯(lián)合優(yōu)化問題，這些多段約束可以編譯為單個(gè)線性等式約束集。

(7)

兩段之間的連續(xù)性約束[16]：

如果特定導(dǎo)數(shù)未知，則必須施加連續(xù)性約束，以確保第j段末尾的導(dǎo)數(shù)與第j+1段開始處的導(dǎo)數(shù)匹配，然后，嘗試使用合適的方法來解決由此產(chǎn)生的約束QP。

雖然上述方法適用于單個(gè)分段和小的聯(lián)合優(yōu)化問題，但是這個(gè)公式在數(shù)值上是不穩(wěn)定的，并且對于多個(gè)分段、高階多項(xiàng)式以及涉及廣泛變化的分段時(shí)間時(shí)將會變得陷入病態(tài)。

AT, jpj=A0, j+1pj+1

(8)

對上述解的改進(jìn)與處理是利用替換技術(shù)[17-18]將問題轉(zhuǎn)化為無約束QP，直接用端點(diǎn)導(dǎo)數(shù)作為決策變量來求解，而不是用多項(xiàng)式系數(shù)求解。

(9)

首先，將約束替換為原始成本函數(shù)。

(10)

現(xiàn)在，這個(gè)新的二次成本函數(shù)中的決策變量是段的端點(diǎn)導(dǎo)數(shù)。通過一個(gè)組合零點(diǎn)矩陣和零點(diǎn)(C)將這些變量重新排序，即將固定的衍生項(xiàng)(dF)和自由的衍生項(xiàng)組合在一起(dP)。于是可以得到:

(11)

為了簡化符號，把塊對角矩陣寫為和。總成本的表達(dá)式可以在分區(qū)后寫入。

(12)

對J進(jìn)行微分，將J等同于零，利用固定導(dǎo)數(shù)和代價(jià)矩陣給出自由導(dǎo)數(shù)的最優(yōu)值向量。

(13)

現(xiàn)在可以從適當(dāng)約束方程的個(gè)別評估中恢復(fù)多項(xiàng)式，將導(dǎo)數(shù)映射到系數(shù)空間。

3 時(shí)間分配

在前面的導(dǎo)數(shù)處理中，設(shè)置T固定，這意味著機(jī)器人通過每個(gè)軌跡段的時(shí)間相同。前期沒有考慮時(shí)間分配問題。這些分段時(shí)間限制了解決方案的質(zhì)量，但是可以允許在成本函數(shù)方面改進(jìn)以提高總體解決方案的質(zhì)量。在一些實(shí)際應(yīng)用中，調(diào)整時(shí)間分配可以優(yōu)化軌跡。在本節(jié)中，將會考慮時(shí)間分配。通過仿真給出考慮時(shí)間分配的移動(dòng)機(jī)器人最小拍照軌跡生成實(shí)驗(yàn)結(jié)果。

如圖1所示，由上述解決方案生成簡單多段最小拍照軌跡。

圖1 簡單的最小拍照軌跡示意圖

4 通過添加航路點(diǎn)避免障礙物

對于最小拍照軌跡生成，避障也是實(shí)際應(yīng)用中需要解決的問題。這個(gè)問題的共同解決方案是增加航路點(diǎn)[19-20]。如果在優(yōu)化后發(fā)現(xiàn)特定的軌跡段與障礙物相交，則在其兩端之間簡單地添加一個(gè)附加的航路點(diǎn)，將該段分割成兩個(gè)。這個(gè)中點(diǎn)是已知的無碰撞的，因?yàn)樗挥谟伤阉魉惴ǚ祷氐淖顑?yōu)分段線性路徑上。使用附加的航路點(diǎn)重新優(yōu)化多項(xiàng)式，如果需要，重復(fù)該過程，直到多項(xiàng)式軌跡無碰撞為止。在存在移動(dòng)障礙物的密集和動(dòng)態(tài)環(huán)境中，軌跡可能需要許多附加的航路點(diǎn)來修復(fù)碰撞，因此需要多次重新求解優(yōu)化問題直到找到可行的解決方案。附加路徑點(diǎn)可能增加QP的計(jì)算復(fù)雜度，需要在每次迭代中求解。

為了評估障礙如何影響到整個(gè)算法復(fù)雜度的增加，本文在各種情況下進(jìn)行試驗(yàn)，例如區(qū)分固定的障礙和運(yùn)動(dòng)障礙以及它們的不同分布等情形。

通過增加控制路徑點(diǎn)，保持原始最小總拍照約束是否會帶來不同的解決方案？如果我們想保持這個(gè)約束，當(dāng)發(fā)現(xiàn)障礙物時(shí)，試圖通過添加路徑點(diǎn)后修改原始軌跡來解決它。基于最小拍照軌跡的完整性生成，以保持最小的差分推力。解決方案是通過搜索算法增加路徑點(diǎn)，尋找可行的無碰撞軌跡。如果我們考慮保持時(shí)間約束以及不花時(shí)間分配，它將花費(fèi)大量的時(shí)間來獲得可行的解決方案，甚至導(dǎo)致無效解或無解。

如圖2所示，再生軌跡進(jìn)入第三障礙物的區(qū)域，這意味著該軌跡不是最佳的解決方案。在仿真部分中，軌跡被視為多個(gè)點(diǎn)。在決策算法中，這些軌跡點(diǎn)不在障礙物區(qū)域內(nèi)，因?yàn)殡x該障礙物最近的兩點(diǎn)不在障礙物區(qū)域內(nèi)。即使通過改進(jìn)軌跡點(diǎn)的分布解和更新決策算法，即所生成的軌跡是否在障礙物的影響區(qū)域內(nèi)，也無法解決這個(gè)問題，因?yàn)榧词惯@樣做也會發(fā)生其他無效解，而且這意味著計(jì)算復(fù)雜度的增加從而帶來更大的問題。

圖2 通過添加航路點(diǎn)生成全最小快照軌跡示意圖

假設(shè)障礙物與位于路點(diǎn)1和路點(diǎn)2之間的一個(gè)參數(shù)相對應(yīng)，則該部分放棄了最小抓拍軌跡要求的完整性。機(jī)器人檢測到機(jī)器人到達(dá)A點(diǎn)時(shí)的障礙物，該點(diǎn)受到傳感器檢測范圍的限制。然后保持軌跡的其他部分不變，通過增加控制點(diǎn)，重新計(jì)算點(diǎn)A與最近路點(diǎn)之間的無碰撞軌跡。修改后的路徑仍將在有效約束中生成，如圖3所示。

圖3 通過添加航路點(diǎn)避開區(qū)間障礙物軌跡示意圖

通過在幾個(gè)遞歸步驟之后來比較結(jié)果的成本函數(shù)，可以搜索最優(yōu)解。在具體應(yīng)用中，避開障礙物時(shí)會有一些具體的要求。然而，本文選擇采用在點(diǎn)A和點(diǎn)B之間新軌跡長度最短的軌跡作為該問題的目標(biāo)解。

在該算法中，傳感器的檢測范圍應(yīng)大于安全范圍，并事先做出重新規(guī)劃軌跡的決定。由于區(qū)間最小抓拍軌跡很容易進(jìn)入障礙物影響范圍，增加遠(yuǎn)離原軌跡的控制點(diǎn)會產(chǎn)生較多的計(jì)算時(shí)間和奇異形狀的軌跡。在檢測障礙物時(shí)，避障是主要考慮的問題，因?yàn)榛诋?dāng)前傳感器信息生成整個(gè)軌跡，所以由其他算法生成的后續(xù)軌跡可能是無效的。在確定障礙物不會發(fā)生之前規(guī)劃整個(gè)下一個(gè)軌跡可能會導(dǎo)致一些無意義的計(jì)算工作發(fā)生，特別是在存在移動(dòng)障礙物的快速變化的環(huán)境中。

最后，立足于實(shí)驗(yàn)室現(xiàn)有條件，對所設(shè)計(jì)算法進(jìn)行實(shí)景在線實(shí)驗(yàn)驗(yàn)證。圖4為課題研究的模擬實(shí)驗(yàn)室場景圖，圖5所示為自身攜帶的BUMBLEBEE2傳感器的移動(dòng)機(jī)器人。具有全局快門曝光功能的雙目相機(jī)自帶BUMBLEBEE2，圖像分辨率以及幀率分別設(shè)定為：640*480ppi與30fps，基線設(shè)定為12 cm。從圖6可以清晰看出，基于最小快照軌跡生成算法，移動(dòng)機(jī)器人攜帶BUMBLEBEE2型雙目相機(jī)在實(shí)驗(yàn)室中進(jìn)行路徑尋優(yōu)規(guī)劃，其軌跡為路線圖，總共進(jìn)行了3圈實(shí)景在線路徑尋優(yōu)運(yùn)動(dòng)，能夠順利無碰撞地有效完成路徑規(guī)劃任務(wù)并成功地進(jìn)行了在線定位。

圖4 模擬實(shí)驗(yàn)室場景圖

圖5 攜帶BUMBLEBEE2傳感器的移動(dòng)機(jī)器人實(shí)物圖

圖6 在各種情況下最小快照軌跡生成示意圖

圖7為移動(dòng)機(jī)器人實(shí)景在線實(shí)驗(yàn)效果圖，其中，圖7(a)是移動(dòng)機(jī)器人路徑規(guī)劃及在線地圖定位實(shí)景圖，其采用的觀察視角與圖5模擬實(shí)驗(yàn)室場景圖中一致，圖7(b)為采用MATLAB Spacedyn工具箱仿真得到的移動(dòng)機(jī)器人軌跡優(yōu)化的俯視圖。由于實(shí)驗(yàn)室的實(shí)際模擬環(huán)境中存在大面積白墻、玻璃等，因而對BUMBLEBEE2型雙目相機(jī)的采集環(huán)境信息造成相當(dāng)大的壓力。

但是從圖7可以明顯看出，移動(dòng)機(jī)器人的三圈尋優(yōu)軌跡之間并沒有出現(xiàn)非常大的錯(cuò)位現(xiàn)象，并且都能性能俱佳無碰撞地順利完成路徑規(guī)劃尋優(yōu)任務(wù)，從一個(gè)側(cè)面成功地反映了所提算法的有效性與優(yōu)越性。

圖7 移動(dòng)機(jī)器人實(shí)景在線實(shí)驗(yàn)驗(yàn)證效果圖

5 結(jié)論

為了解決增加路徑點(diǎn)會產(chǎn)生大量的不確定性，使計(jì)算復(fù)雜度提高，且在特定區(qū)間內(nèi)局部增加控制點(diǎn)生成的路徑性能不好，提出一種全新的移動(dòng)機(jī)器人路徑規(guī)劃尋優(yōu)方法，獲得最小快照軌跡生成。首先建立模型，提出最小導(dǎo)數(shù)的代價(jià)函數(shù)并給出約束條件；其次把時(shí)間分配問題應(yīng)用到路徑規(guī)劃中，與動(dòng)態(tài)軌跡規(guī)劃相結(jié)合，在優(yōu)先考慮避障的同時(shí)，獲得了保證100%輸入平滑度的完整最小快照軌跡生成。