區(qū)域快速優(yōu)化下的無人機在線軌跡規(guī)劃方法

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

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

0 引言

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

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

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

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

1 系統(tǒng)概述

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

圖1 無人機在線軌跡規(guī)劃系統(tǒng)框架圖

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

2 碰撞區(qū)域的軌跡快速優(yōu)化

2.1 軌跡表示與碰撞區(qū)域軌跡檢測

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

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

式中：Qi為軌跡優(yōu)化的決策變量，并記錄獨自的環(huán)境信息；Nc為控制點的數(shù)目。

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

圖2 碰撞區(qū)域軌跡檢測

2.2 碰撞控制點替換策略

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

圖3 碰撞區(qū)域軌跡快速優(yōu)化

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

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

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

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

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

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

3 軌跡多目標優(yōu)化

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

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

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

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

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

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

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

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

3.2 多目標優(yōu)化函數(shù)設計

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

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

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

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

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

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

4 實驗與結果分析

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

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

圖6 仿真操作流程圖

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

表1 基礎參數(shù)配置

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

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

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

圖7 所提方法規(guī)劃過程

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

表2 規(guī)劃時間測試結果

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

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

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

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

表3 算法對比結果

圖8 軌跡規(guī)劃效果圖

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

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

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

5 結論

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

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