移動機器人最小快照軌跡尋優方法研究

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

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

1 引言

移動機器人路徑規劃是集動態環境檢測、動態規劃與執行、行為決策與控制等數項功能于一體的綜合化智能系統[1]。近年來已經成功地應用到工農業生產的各個領域，在我們生活的周圍可以隨處找到它的影子，因而移動機器人路徑規劃問題便引起了國內外眾多學者的廣泛關注，獲得了一大批卓有成效的研究成果。

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

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

盡管近年來針對移動機器人路徑規劃問題已經取得了很多卓有成效的研究成果，但就目前作者關注情況而言，對于未知動態不確定環境下的移動機器人路徑規劃尋優問題還鮮有聽聞，眾所周知，未來的應用環境是瞬息萬變的，這就要求移動機器人的路徑規劃尋優必須能夠做到與時俱進地適應任何苛刻的動態環境。基于此，本文的研究重點就放在移動障礙物時實現最小快照軌跡的生成。大多數研究人員處理這一問題的常用方法是嘗試添加路徑點。基本過程如下：如果在優化后發現特定的軌跡段與障礙物相交，則在其兩端之間簡單地添加一個附加的路徑點，將該路段分成兩部分。這個中點被認為是無碰撞的，因為它位于由搜索算法返回的最優分段線性路徑上。用附加的路徑點重新優化多項式，必要時重復該過程，直到多項式軌跡無碰撞為止。顯然，附加的路徑點增加了計算復雜度，尤其是在室內密集環境中有移動障礙物存在時。因此，本文嘗試分析添加路徑點時的具體影響，并試圖找出在避障成功的前提下可以降低計算復雜度的有效途徑。

2 模型建立

針對持續時間段是已知的情況，路徑可以描述如下。需要說明的是平坦輸出空間中兩點之間的單個軌跡段由獨立多項式f(t)組成。

(1)

處理多項式f(t)導數平方的代價函數可以復寫成如下：

(2)

其中c0,c1，…,cN是這些不同導數的系數。為了對這個快照之和進行最小化，除了c4之外，代價函數中的所有導數懲罰將被設置為零。

于是，一個段的成本函數可以描述為如下：

(f(4)(t))2=

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

(4)

利用多項式的系數和端點導數之間的映射矩陣(Aj)，對第段軌跡的導數約束進行公式化：

(5)

其中dj代表給定的第j段的位置信息。第j階導數表示路徑點約束。可以描述為如下。

(6)

對于聯合優化問題，這些多段約束可以編譯為單個線性等式約束集。

(7)

兩段之間的連續性約束[16]：

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

雖然上述方法適用于單個分段和小的聯合優化問題，但是這個公式在數值上是不穩定的，并且對于多個分段、高階多項式以及涉及廣泛變化的分段時間時將會變得陷入病態。

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

(8)

對上述解的改進與處理是利用替換技術[17-18]將問題轉化為無約束QP，直接用端點導數作為決策變量來求解，而不是用多項式系數求解。

(9)

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

(10)

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

(11)

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

(12)

對J進行微分，將J等同于零，利用固定導數和代價矩陣給出自由導數的最優值向量。

(13)

現在可以從適當約束方程的個別評估中恢復多項式，將導數映射到系數空間。

3 時間分配

在前面的導數處理中，設置T固定，這意味著機器人通過每個軌跡段的時間相同。前期沒有考慮時間分配問題。這些分段時間限制了解決方案的質量，但是可以允許在成本函數方面改進以提高總體解決方案的質量。在一些實際應用中，調整時間分配可以優化軌跡。在本節中，將會考慮時間分配。通過仿真給出考慮時間分配的移動機器人最小拍照軌跡生成實驗結果。

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

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

4 通過添加航路點避免障礙物

對于最小拍照軌跡生成，避障也是實際應用中需要解決的問題。這個問題的共同解決方案是增加航路點[19-20]。如果在優化后發現特定的軌跡段與障礙物相交，則在其兩端之間簡單地添加一個附加的航路點，將該段分割成兩個。這個中點是已知的無碰撞的，因為它位于由搜索算法返回的最優分段線性路徑上。使用附加的航路點重新優化多項式，如果需要，重復該過程，直到多項式軌跡無碰撞為止。在存在移動障礙物的密集和動態環境中，軌跡可能需要許多附加的航路點來修復碰撞，因此需要多次重新求解優化問題直到找到可行的解決方案。附加路徑點可能增加QP的計算復雜度，需要在每次迭代中求解。

為了評估障礙如何影響到整個算法復雜度的增加，本文在各種情況下進行試驗，例如區分固定的障礙和運動障礙以及它們的不同分布等情形。

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

如圖2所示，再生軌跡進入第三障礙物的區域，這意味著該軌跡不是最佳的解決方案。在仿真部分中，軌跡被視為多個點。在決策算法中，這些軌跡點不在障礙物區域內，因為離該障礙物最近的兩點不在障礙物區域內。即使通過改進軌跡點的分布解和更新決策算法，即所生成的軌跡是否在障礙物的影響區域內，也無法解決這個問題，因為即使這樣做也會發生其他無效解，而且這意味著計算復雜度的增加從而帶來更大的問題。

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

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

圖3 通過添加航路點避開區間障礙物軌跡示意圖

通過在幾個遞歸步驟之后來比較結果的成本函數，可以搜索最優解。在具體應用中，避開障礙物時會有一些具體的要求。然而，本文選擇采用在點A和點B之間新軌跡長度最短的軌跡作為該問題的目標解。

在該算法中，傳感器的檢測范圍應大于安全范圍，并事先做出重新規劃軌跡的決定。由于區間最小抓拍軌跡很容易進入障礙物影響范圍，增加遠離原軌跡的控制點會產生較多的計算時間和奇異形狀的軌跡。在檢測障礙物時，避障是主要考慮的問題，因為基于當前傳感器信息生成整個軌跡，所以由其他算法生成的后續軌跡可能是無效的。在確定障礙物不會發生之前規劃整個下一個軌跡可能會導致一些無意義的計算工作發生，特別是在存在移動障礙物的快速變化的環境中。

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

圖4 模擬實驗室場景圖

圖5 攜帶BUMBLEBEE2傳感器的移動機器人實物圖

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

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

但是從圖7可以明顯看出，移動機器人的三圈尋優軌跡之間并沒有出現非常大的錯位現象，并且都能性能俱佳無碰撞地順利完成路徑規劃尋優任務，從一個側面成功地反映了所提算法的有效性與優越性。

圖7 移動機器人實景在線實驗驗證效果圖

5 結論

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