區域快速優化下的無人機在線軌跡規劃方法

唐嘉寧，和雪梅，陳云浩，彭志祥，周思達

（1.云南民族大學電氣信息工程學院，昆明 650000；2.云南民族大學無人自主系統研究院，昆明 650000）

0 引言

近年來，隨著無人機在地震廢墟、火災現場、崎嶇山林等未知復雜環境下進行救援救災等應用的需求急劇增大，對無人機的自主導航提出了更高的要求。為確保無人機在未知環境下高速飛行時快速應對不可預見的風險，自主導航中在線軌跡規劃模塊至關重要。

在線軌跡規劃問題已經被廣泛研究［1－3］，其中基于梯度的規劃方法具有成功率高、規劃速度快等突出優勢，逐漸成為無人機在線軌跡規劃的主流方法［4］。該方法通常依賴特定的環境預處理方法將空間中的障礙物信息引入非線性優化問題中，并在平滑度、安全性和動態可行性之間進行權衡［5］。Ratliff等［6］提出利用預先構建的ESDF［7］提供障礙物梯度信息，通過協變梯度下降算法來最小化初始軌跡的平滑度和碰撞成本，進而優化離散時間軌跡；Oleynikova等［8］將文獻［6］的工作擴展到連續時間軌跡優化，以避免微分誤差，并采用隨機軌跡擾動和優化重啟，以實現實時避碰和獲得更高的成功率，然而，該方法帶來了沉重的計算負擔；Usenko等［9］提出了實時局部軌跡重規劃的方法，在避開未建模障礙物的同時使重規劃的局部軌跡接近全局軌跡，并將該軌跡參數化為B樣條，充分利用其凸包性質提高了優化性能；Zhou等［10］進一步利用了B樣條的凸包性質，使軌跡優化效率和魯棒性得到提升，并通過提供一條無碰撞的高質量初始軌跡，顯著提高了在線規劃的成功率，同時引入幾何引導路徑緩解了局部極小值的問題［11］；Tang等［12］基于均勻B樣條曲線和非均勻動力學搜索算法，能夠在保證安全的前提下，實現高效、平滑和安全的軌跡規劃，但該方法在實時生成軌跡時需要有強大的計算資源和性能作為支撐。

上述工作中，在線或離線生成的ESDF，為機器人的軌跡規劃和避障提供精確的障礙物距離信息和梯度信息［13－16］，被廣泛應用于大多基于梯度的在線軌跡規劃方法中。然而，ESDF的非光滑特性及空間離散性，使得優化過程的收斂速率和迭代的計算速率明顯下降，且所構建地圖中僅有部分空間直接服務于軌跡優化，構建時間卻占據整體規劃時間的70%［17］。隨著環境規模及復雜性的增長，ESDF計算障礙物梯度信息需要消耗大量的計算資源和存儲空間［18］。對于機載資源受限的無人機來說，這會極大限制其規劃效率，甚至無法滿足實時軌跡規劃的需求。Zhou等［17］首次提出了一種無需構建ESDF地圖即可實現基于梯度的軌跡規劃方法，然而該方法在優化時需要進行多次迭代才能找到一條安全、平滑、動態可行的軌跡，耗費了寶貴的規劃時間。

針對這個問題，提出了區域快速優化下的無人機在線軌跡規劃方法，通過碰撞控制點替換，快速優化初始軌跡中的碰撞區域軌跡以避免多次迭代優化，并設計了一種輕量的方法計算軌跡與必要障礙物的距離和梯度信息，避免與軌跡規劃無關區域的計算。在此基礎上，建立多目標優化函數，進一步優化軌跡以獲得滿足安全、平滑、動態可行的高質量軌跡。

1 系統概述

無人機在線軌跡規劃一般建立在狀態估計和體素建圖模塊上，更新的地圖和無人機的位姿信息被饋送到軌跡生成模塊以生成初始軌跡；之后進入軌跡優化模塊生成最佳軌跡，并發送給軌跡服務器，由相應飛行控制器實現對無人機的控制。所提出的區域快速優化下的無人機在線軌跡規劃方法，通過設計更高效的軌跡優化策略，快速將初始軌跡優化為滿足安全性、平滑性、動態可行性的高質量軌跡，其框架如圖1所示。

圖1 無人機在線軌跡規劃系統框架圖

軌跡優化分2個階段：第1階段為碰撞區域的軌跡快速優化，對輸入的初始軌跡進行碰撞檢測，生成碰撞軌跡區域對應的安全引導路徑，設計碰撞控制點替換策略，快速優化該區域軌跡；第2階段為軌跡多目標優化，通過構建局部障礙物梯度及計算碰撞成本為進一步軌跡優化做準備，之后以軌跡的平滑性、安全性和動態可行性為目標進行優化，得到最終軌跡。

2 碰撞區域的軌跡快速優化

2.1 軌跡表示與碰撞區域軌跡檢測

在無人機軌跡規劃中，需要預先規劃從指定起點到目標點的初始軌跡。研究中采用直線化初始軌跡，確定起點和目標點的位置，通過計算它們之間的距離和方向來確定直線的方向和長度。用直線連接起點和目標點的過程可以被視為一種最簡單的初始軌跡規劃方法，具有計算簡單、實現容易、快速響應等優點。

由于存在不可預見的障礙物或其他風險因素，在未知環境下需要保證無人機能夠快速、高效地進行局部規劃以應對風險。為將局部規劃轉換為初始軌跡局部快速優化問題，使用均勻B樣條進一步參數化初始軌跡，根據當前無人機的運動狀態和環境信息，對初始軌跡上的控制點進行快速優化，使無人機能夠安全、高效地通過當前局部環境。因此，將初始軌跡參數化為1組均勻B樣條曲線的控制點：

式中：Qi為軌跡優化的決策變量，并記錄獨自的環境信息；Nc為控制點的數目。

為保證無人機飛行的安全性，在初始軌跡上不斷進行碰撞檢測，如圖2所示。使用1對控制點Qin和Qout來記錄每段碰撞區域（灰色區域）軌跡的首末位置，找到由碰撞控制點組成的“碰撞集”Qcol，對應為圖2中的黑點。之后使用A*路徑搜索算法來搜索最優路徑，即尋找1條從Qin到Qout的安全引導路徑，對應為圖2中的藍色曲線，并得到路徑點集A，對應為圖2中的藍點。

圖2 碰撞區域軌跡檢測

2.2 碰撞控制點替換策略

為將“碰撞集”Qcol中的每個碰撞控制點Qi以最快的速度、最短的距離推離當前障礙物，提出碰撞控制點替換策略，即尋找在路徑點集A中每個碰撞控制點對應的路徑點AQ，之后進行替換操作，并作為新的控制點Qinew，如圖3所示。進行替換的過程中，只調整了初始軌跡上的碰撞控制點的位置，最大限度減少了對整個軌跡的影響，能夠更加靈活地調整碰撞區域軌跡，實現快速軌跡優化。

圖3 碰撞區域軌跡快速優化

圖3中，灰色為障礙物，黑色曲線為均勻B樣條表示的初始軌跡，黑色虛線為初始軌跡與障礙物形成的碰撞區域軌跡，黑點為“碰撞集”Qcol中的控制點，藍點為碰撞控制點策略尋找到的新控制點Qinew，紅色曲線為碰撞區域軌跡快速優化后的軌跡。

尋找碰撞控制點對應的路徑點示意圖如圖4所示。其中，黑色曲線為初始軌跡，藍色曲線為A*算法生成的安全引導路徑，黑點和灰點為碰撞控制點，藍點為安全引導路徑的路徑點，紫點為安全引導路徑的路徑點總數目一半的位置，紅點為最終尋找到的碰撞控制點對應的路徑點，Ri為碰撞控制點Qi的切向量灰色為垂直于切向量的平面。

圖4 尋找碰撞控制點對應的路徑點示意圖

生成的安全引導路徑的路徑點集A中的路徑點用Ai表示，QAi為Qi與Ai構成的向量，見圖4中的紫線，即：

當QAi與Qi的切向量Ri的內積為0時即可得到Qi對應的路徑點AQ，相當于尋找垂直于Ri方向上的路徑點AQ，如圖4中的紅線所示。為了避免Qi在路徑點集A中一一尋找AQ而消耗大量的算力，每個碰撞控制點Qi從路徑點集A中路徑點總數目一半的位置索引am開始進行尋找，即第1次尋找路徑點AQ時，路徑點Ai的位置索引ai＝am。

判斷Ri·QAi的值后，根據式（3）來決定下一個尋找AQ路徑點的位置索引ai＋1，當Ri·QAi的值大于0時，下一個尋找的路徑點的方向如圖4（a）所示；當Ri·QAi的值小于0時，下一個尋找的路徑點的方向如圖4（b）所示；當Ri·QAi的值等于0時，就可得到碰撞控制點Qi對應的路徑點AQ。

3 軌跡多目標優化

3.1 局部障礙物梯度及碰撞成本

在未預先構建ESDF的情況下，障礙物由占用柵格地圖上的體素表示，不能夠直接獲得軌跡優化的碰撞成本和梯度，因此利用碰撞區域快速優化后的軌跡上的控制點Qinew和其對應在障礙物表面的基點來估計軌跡與局部障礙物碰撞的成本和梯度。

采用A*算法生成安全引導路徑具有貼近障礙物表面的特點，且研究只對相對短的局部障礙物進行梯度信息構建，因此能夠快速完成所有Qinew對應在障礙物表面的基點的搜索。通過以Qinew為初始位置，朝原始位置Qi方向以占用柵格地圖的分辨率為步長快速搜索到第1個處于占用狀態的柵格，如圖5（a）所示，即為軌跡上控制點Qinew對應在障礙物表面的基點pi，i為控制點的索引。以基點pi與碰撞區域優化后軌跡上的控制點Qinew間的排斥方向向量vi來構建梯度信息，該方向為梯度的方向，如圖5（b）所示，pi與vi是一一對應的關系，簡稱｛p，v｝對。

圖5 局部障礙物梯度信息的構建

軌跡與局部障礙物的碰撞成本為控制點Qinew的碰撞成本累加，即：

式中：ccollision（Qinew）為控制點Qinew對應的｛p，v｝對碰撞成本函數。

式中：dthr為無人機飛行的安全距離閾值，若距離小于該閾值則碰撞成本將開始快速升高；di為控制點Qinew到對應基點pi的距離。通過Ccollision對Qinew進行求導，即可獲取梯度值［19］：

由于只在軌跡遇到新障礙時進行障礙物碰撞成本與梯度的估計，因此整個規劃過程只提取了必要的障礙物信息，減少地圖信息冗余的同時使規劃更加精確、高效。

3.2 多目標優化函數設計

無人機最終的飛行軌跡必須同時滿足安全性、平滑性、動態可行性，因此優化如下綜合三者的目標優化函數：

式中：fsafe為安全目標函數對應為軌跡的碰撞成本Ccollision；fsmooth為軌跡平滑目標函數；fdynamic為動態可行性目標函數；αsafe、αsmooth、αdynamic分別為對應目標函數的權重系數。

平滑目標函數的設置是為了優化軌跡的平滑性以達到節省無人機飛行時能量的目的。根據無人機動力學的微分平坦性［20］，可通過對無人機的加速度加以約束來實現，軌跡平滑目標函數可構造成如下形式［10］：

其中無人機的加速度為軌跡上控制點的速度，可通過求B樣條曲線的高階導數獲得，即：

動態可行性目標函數是為保證無人機的速度、加速度在可行的范圍內。受益于B樣條的凸包性質，如果控制點的速度、加速度在控制點處不超過極限值，那么該B樣條曲線上的任意一點的速度、加速度將小于極限值，從而滿足動態可行性。因此，在給定最大速度vmax、最大加速度amax的情況下，該函數可以定義為：

當速度、加速度超過限制后，使用二范數和單位歸一化因子λvel、λacc作為動態可行性懲罰。通過上述目標函數的優化，獲得了1條更安全、光滑、滿足動態可行性的更適合無人機飛行的軌跡。

4 實驗與結果分析

快速區域優化下的無人機在線軌跡規劃仿真實驗在Ubuntu操作系統下進行，基于ROS（robot operating system）框架和C＋＋語言實現規劃設計，并通過ROS的Rviz（the robot visualization tool）軟件完成規劃方法在三維環境下的可視化演示。實驗平臺的相關配置如下：處理器為Intel i9－12900KF 4.9 GHz，內存大小為32 GB，系統版本為Ubuntu18.04 LTS，ROS版本為Melodic。

仿真操作流程如圖6所示，在生成的障礙物隨機分布的三維柵格地圖中確定飛行目標點后，通過在線軌跡規劃算法生成最佳軌跡，并發送給軌跡服務器，由相應的飛行控制器實現對無人機的控制。

圖6 仿真操作流程圖

實驗采用表1所示的基礎參數配置，在簡單場景下短距離規劃和復雜場景下長距離規劃2種情況下對所提方法進行測試。其測試結果的數據為30次測試的平均值，其中起點與目標點的距離為D，障礙物數量為No，總規劃時間為Ta（包括了建圖、軌跡初始化和優化的時間），規劃初始化時間和軌跡優化的時間分別為Tinit和Topt，Topt用于衡量軌跡優化的收斂速度。

表1 基礎參數配置

4.1 簡單場景下短距離規劃的性能測試

當設置的目標點在無人機的傳感器感知范圍內時，可以認為是在已知環境下的軌跡規劃，無人機從起點到目標點一般只需進行1次規劃。同時由于無人機規劃距離較短，無人機一般進行定高飛行，可視為是二維平面上的簡單規劃。將所提方法與前沿的未構建ESDF地圖進行基于梯度的規劃方法EGO＿planner（an ESDF-free gradientbased local planner for quadrotors）［17］進行比較，該方法提出近似梯度場計算方法，將初始軌跡進行迭代優化推離障礙物。2種方法設置相同的仿真參數，分別在無人機起點距離目標點4、6、8 m的位置進行3組規劃實驗，地圖的障礙物數目為300。

所提方法在無人機起點距離目標點8 m位置的規劃過程如圖7所示。其中，藍色柱狀為生成的三維柵格地圖中隨機分布的障礙物。圖7（a）為初始化過程，通過無人機起點與目標點直線化來生成初始軌徑（深綠色直線），通過將初始軌跡參數化為B樣條的一系列控制點后，進行碰撞區域軌跡的檢測，之后在碰撞區域軌跡處由A*算法生成安全引導路徑（淡綠色曲線）；圖7（b）為所提方法的軌跡優化過程，紅色曲線為初始軌跡中碰撞區域快速優化后的軌跡，避免多次迭代情況下通過碰撞控制點替換直接將碰撞區域軌跡快速優化至安全區域，藍色曲線為考慮無人機飛行時的安全性、平滑性及動態可行性，設計的多目標函數進一步優化后得到的軌跡。最終生成的無人機飛行軌跡如圖7（c）粉色曲線所示，該軌跡足夠安全、平滑。

圖7 所提方法規劃過程

表2記錄了2種方法在規劃時間上的比較結果，包括總規劃時間、軌跡初始化時間和軌跡優化時間。

表2 規劃時間測試結果

由表2 可知，相比于前沿方法 EGO＿planner［17］，所提方法在軌跡規劃時間上有較大提升，具體如下：在規劃距離為4 m時，總規劃時間、軌跡初始化時間、軌跡優化時間分別縮短了38.3%、31.7%、39.1%；在規劃距離為6 m時，總規劃時間、軌跡初始化時間、軌跡優化時間分別縮短了36.1%、30.7%、38.5%；在規劃距離為8 m時，總規劃時間、軌跡初始化時間、軌跡優化時間分別縮短了34%、36.9%、35.5%。所提方法提出的碰撞控制點替換策略能夠實現碰撞區域軌跡的快速優化，避免多次迭代，同時設計的目標函數加速了軌跡優化收斂的過程，因此所提方法的規劃時間小于EGO＿planner方法，規劃效率得到了較大提升。

4.2 復雜場景下長距離規劃的性能測試

當設置的目標點較遠且處于未知環境時，無人機生成的軌跡遇見新障礙時將不斷進行重新規劃以確保軌跡安全，因此從起點位置到目標點需要進行多次規劃。同時障礙物數量是模擬真實場景復雜性的關鍵參數，通常障礙物數目越大，環境越復雜，對在線軌跡規劃算法的性能要求越高。為此，在隨機森林三維柵格地圖中，給定相同的起點（－15.5，0，1）和目標點（15.5，0，1），規劃距離為31 m，其他參數設置相同的情況下，依次設置障礙物數目為180、270、360，其中隨機生成圓環和柱狀障礙物的數量分別占據地圖中障礙物總數的1／3和2／3。由于無人機規劃距離較長，在三維空間內將進行較復雜的規劃，因此該部分的性能測試可以用于驗證所提方法在三維環境中的有效性。

將所提方法與前沿方法EGO＿planner和經典的基于梯度的規劃方法Fast＿planner［10］進行規劃比較，結果見表3，軌跡效果如圖8所示。

表3 算法對比結果

圖8 軌跡規劃效果圖

由表3可知，3種方法的規劃時間隨著地圖中障礙物數目的增大而增大。Fast＿planner利用ESDF來獲取地圖中的障礙物距離信息和梯度信息，而所提方法和EGO＿planner不進行ESDF的構建，只計算當前軌跡面臨的新障礙物的距離和梯度信息，因此相比Fast＿planner，所提方法極大減少了軌跡規劃的時間。在總規劃時間上，相比Fast＿planner，障礙物數目為180時，所提方法耗時縮短了87.64%；障礙物數目為270時，耗時縮短了86.63%；在障礙物數目為360時，耗時縮短了85.41%?？傊?，所提方法的規劃時間遠遠小于Fast＿planner方法，規劃效率得到了極大提升。

障礙物數目越大，所提方法較EGO＿planner的總規劃時間縮短越多，優勢越大。這是因為EGO＿planner在軌跡優化期間需要不斷迭代來獲得最終可執行的軌跡，但隨著地圖中障礙物數目的增加，環境規模及復雜性增長，迭代優化軌跡次數增加，影響了整體規劃效率；而所提方法能夠實現碰撞區域軌跡的快速優化，避免多次迭代，加速了軌跡優化的效率。

上述實驗結果表明，所提方法在不同復雜場景中都顯示了在線規劃的有效性，且相比于前沿方法EGO＿planner和經典的基于梯度的規劃方法Fast＿planner，在規劃時間上存在明顯優勢。隨著地圖中障礙物數目的增大，所提方法能夠快速高效實現碰撞區域軌跡的優化，避免在障礙物中進行多次迭代優化，極大降低了規劃的計算時間。

5 結論

提出了區域快速優化下的無人機在線軌跡規劃方法，在不構建ESDF情況下，通過快速優化碰撞區域的軌跡、有效構建局部障礙物梯度信息和設計多目標優化函數來高效優化軌跡，極大降低軌跡規劃時間。

實驗結果表明，該方法相比于現有算法在規劃時間上有明顯提升：在簡單場景下短距離規劃中，與前沿方法相比，總規劃時間、軌跡初始化時間、軌跡優化時間分別縮短了36.1%、33.1%、37.7%；在復雜場景下長距離規劃中，較經典的基于梯度的規劃方法，軌跡規劃時間平均縮短了86.56%，規劃效率極大提升，較前沿的基于梯度的規劃方法，隨著環境的復雜度變高，軌跡優化的效率進一步提高。同時，所提方法在不同復雜環境中能進行有效的在線規劃，具備較強的魯棒性和擴展性。