基于圖搜索的陸空兩棲平臺3D 路徑規劃算法

摘 要：為了解決陸空兩棲平臺路徑規劃能效與尋徑效率問題，提出了一種基于圖搜索算法的全局路徑規劃算法。考慮了空中飛行能耗與地面行駛能耗模型的不同，結合起飛閾值、地面移動成本系數、空中移動成本系數等參數決定模態切換機制，在遇到難以越過的障礙時使用逃脫算法脫困，當允許地面通過時優先采用地面行駛的方式到達目標點以提升尋徑效率與能效。為驗證該算法，分別建立了針對陸空2 種模式下的能耗模型，在現有交通規則約束下完成城市場景和虛擬迷宮測試。結果表明：本算法的搜索效率相較于對照組算法提升了30%以上，有效提升搜尋效率的同時降低了能量消耗。

關鍵詞： 陸空兩棲平臺；跨模態路徑規劃；圖搜索算法；模態切換策略

中圖分類號： TP 23 文獻標識碼： A DOI： 10.3969/j.issn.1674-8484.2024.02.014

近年來，具有機動性強、靈活性高的多旋翼飛行器（unmanned aerial vehicle，UAV） 在運輸、偵察、巡檢、消防等領域占據重要地位[1]，但由于其能耗高的特點面臨著續航時間不足、作業范圍有限的困難[2] ；地面無人車輛與旋翼無人機組合而成的陸空兩棲飛行器可以兼具無人機的高機動性能和地面無人車輛的高運輸效率特點，在軍事、救援等諸多領域嶄露頭角。目前，許多公司已經開始研發相關類型產品，如Terrafugia 公司的TF-X、億航公司的AAV 等。雖然該陸空兩棲平臺已經初步應用于偵察、后勤等領域，并取得了良好的效果，但廣泛應用于市場還缺少必備條件，比如兼容陸空2種運動模式的陸空兩棲飛行車輛的路徑規劃技術。

二維路徑規劃算法和三維路徑規劃算法已經經歷了長久的發展，具有較為成熟的技術積累，例如，基于采樣算法的RRT [3] 及其變體如RRT-connect [4] 和RRT* [5]，基于圖搜索A*及其派生算法LPA* [6] 和D* [7]算法，能夠在部分已知環境中動態搜索路徑，ARA* [8]可以通過調整啟發函數的權重快速生成次優路徑并進行迭代優化，Theta [9] 通過移除父節點的方式實現任意角度的路徑規劃。

然而，現有的三維路徑規劃算法和平面路徑規劃算法卻無法直接應用于陸空兩棲平臺。陸空兩棲平臺賦予了地面平臺飛行能力，使得二維規劃方法無法直接適用，同樣如果直接采用空中三維規劃方法將無法充分利用地面平臺高能效的特點。

在陸空兩棲平臺路徑規劃領域，已經有學者完成了初步研究。B. Araki 等提出了混合群機器人的SIPP和ILP 算法[10]，有效提高了陸空兩棲平臺跨模態作業的能量效率，并可在連續時間域中規劃多個機器人的路徑。A. Sharif 等提出了一種通過修改不同行為的啟發式成本計算的改進A* 算法，降低了路徑能耗[11]。H. J.Terry Suh 等在多模態機器人軌跡規劃中考慮了能效和動力學約束，提出了一種多模式運動系統的規劃方法[12]，規劃結果具有更好的適用性。這些工作都為陸空兩棲車輛的高能效規劃提供了可行的方法，但是陸空兩棲平臺路徑規劃在飛行、駕駛、模態切換中過程中都需要考慮能量消耗、時間的損失。

針對陸空兩棲平臺路徑規劃運行時間和能耗的優化問題，本文提出一種陸空兩棲飛行平臺并對其完整的動力學模型進行建模，包括底盤動力學模型、飛行模塊動力學。為探究陸空兩棲平臺的機動性問題，規劃了包含模態切換策略的軌跡以避免頻繁切換，同時加入了逃脫算法以指引車輛在遇到難以翻越障礙時激發平臺切換模態，對比對照組的算法[10] 證明算法的高效性。

1 模型建立與問題分析

1.1 動力學建模

陸空兩棲平臺包含地面行駛的底盤模塊與一個固定連接的飛行模塊，構成了一個具有多個自由度的強耦合系統。本節為陸空兩棲平臺的底盤模塊和飛行模塊建立了等效的動力學模型。其中，飛行模塊的四旋翼可以視為具有6 個自由度的耦合剛體，飛行模塊的旋轉方程為

其中： ωb表示物體在體坐標系中的角速度， Mb為系統中的總轉矩，Jb 為體坐標系中飛行模塊的轉動慣性矩陣。根據Newton-Euler 方程表明，其動力學模型可以表示為

Mb = Jbωb + ωbJbωb + Mg + Md. （2）

其中： Mg 為陀螺效應產生的扭矩； Md 為氣動摩擦產生的扭矩，兩者的關系可表示為：

其中： Jb = diag（dΦ dθ dψ） 為各轉子的慣性矩陣； dΦ、dθ、dψ 為相應的氣動阻尼系數。

Magic Formula 便于計算路面施加在輪胎上的力和力矩，適應可能的并發縱向、橫向和弧度滑移條件[13]。這種能力能夠精確地描述具有瞬態特性的輪胎行為。在地面行駛模態下，輪胎受到縱向力以保持速度，所受側向力近似如下：

Fyi = DL·sin{CL·arctan[BLαi-arctan（BLαi）]}. （5）

其中： i = （fl、fr、rl、rr），fl、fr、rl、rr 分別表示底盤的左前輪，右前輪，左后輪，右后輪。BL、CL、DL、EL是通過模型擬合得到的公式的參數，陸空兩棲平臺地面行駛底盤的縱向載荷FNi 可以表示為

FNi = mi zi + ci（zi - z'i ） + ki（zi - z'i ）. （6）

其中：z'i 表示位于車輪末端的彈簧的彈簧質量垂直位移；Zi 表示非簧載質量垂直位移。因此，世界坐標系Fyi 中車輪的y 方向分量受到約束。車輪的模型與底盤的模型相結合，底盤結構在給定的力和力矩下的動態響應。底盤的平移方程（在x 和y 方向上）和偏航角的旋轉方程如下：

底盤的懸架系統主要由底盤、懸架震動臂、減震器和車輪表示。其中z'i 表示垂直方向上的簧載質量位移，zi 為非簧載質量位移，qi 為地面擾動。Bi1、Bi2、Ki1、Ki2分別表示減振器和車輪的剛度。fa 和fb 分別表示執行器力和擾動力。底盤懸架方程如下：

1.2 能耗模型

作為具備飛行和地面行駛能力的綜合性平臺（ 如圖1 所示），陸空兩棲平臺整體運動路徑的能耗是評判路徑最優性的重要指標之一，因此本文在規劃運動路徑過程中為陸空兩棲平臺建立了能耗模型。

在能耗模型中，運動能量分為地面行駛克服地面摩擦力消耗的能量，以及起飛時克服重力和空氣阻力做功所消耗的能量，總能耗分為飛行功耗Eh 和行駛功耗Em [12]。

E = Eh （m） + Em（m）. （10）

其中： m 表示車輛的運動模態，當平臺處于飛行模態時，螺旋槳產生向下的總推力等于平臺總重量[14-15]。根據式（11） 和式（12） 分析，在相同時內，平臺處于飛行模態下的能耗高于行駛模態。

其中：式（11）、式（12） 使用的參數值如表1 所示。為了簡化空氣阻力的計算，陸空兩棲平臺在任何方向上的阻力系數恒定。

2 規劃算法

本節將詳細介紹改進后的A* 規劃算法，以及路徑規劃過程中平臺的模態切換策略。

由于地面行駛的能耗和效率相較于飛行模式更低，因此更傾向于在地面行駛。在遇到難以找到通行的道路如交通擁堵、臺階、狹窄通道時切換到飛行模式。當檢測到路面條件適合平臺行駛時，控制器規劃平臺切換至行駛模式。

圖2 中的流程圖表述了本算法的基本邏輯，給定起點和目標坐標，規劃器依據高效搜尋路徑、降低能耗原則，結合陸空兩棲車輛地面高能效、空中高機動特性交替運行2D 與3D 路徑規劃算法，得出最終路徑。

2.1 2D 尋徑階段

陸空兩棲平臺地面行駛時，規劃器根據改進的A* 代價函數在陸地平面內規劃路徑。在此改進A* 算法中加入模態切換策略算法，此算法記錄每次搜索時間，并與起飛閾值做對比，運行機制如表2 的偽代碼所示：

在加入模態切換策略算法后系統生成節點時，若新生成的節點的啟發函數代價Hnew 超過了預設最小值Hmin，則系統會增加一個計數值cout。一旦增加的計數值達到或超過了設定的閾值thre，系統將觸發模態切換機制，將操作模式切換至飛行模態，以應對節點代價的變化。

這其中，閾值函數thre = C·res-1 與地圖分辨率的倒數成正比，C 值是控制起飛閾值函數的重要參數。當兩棲平臺在地面執行搜索路徑任務次數過多或搜索成本高于起飛閾值時，規劃器將觸發逃脫算法引導平臺切換至飛行模態，從而使其從地面狀態迅速轉變為能夠在空中任務的狀態，并記錄規劃階段最小Hmin。

2.2 3D 尋徑階段

當陸空兩棲平臺遇到地面難以通過的障礙時，如交通擁堵或由于地面障礙無法有效駕駛穿越等情況，導致路面規劃的代價過大，規劃器會觸發逃脫算法，指引平臺切換至飛行模態逃離當前困境飛向目標點。

逃脫算法引入了一個關鍵變量H2D，即當前平臺位置到目標點的Manhattan 距離。類似于在2D 路徑規劃時的代價函數，H2D 用于判斷平臺是否正在接近目標。該算法主要的目標是解決在2D 規劃階段難以繼續靠近目標點的問題。

具體做法是，首先設定一個初始值H2D0，H2d0 等于2D 尋徑階段的Hmin。在每個執行循環中，計算ΔH2D = H2Do - H2D，表示每個步長移動后到目標的距離變化。表3 中的偽代碼從第1 行到第4 行表述了這個過程。

根據ΔH2D 的值，將整個3D 尋徑過程可以劃分為：起飛階段、避障階段、著陸階段。

對于前2 個階段，本文引入了1 個水平移動成本系數khori，改變khori 值來實現期望的性能。增大khori會導致地面行駛代價增加，使逃脫算法更趨向于選擇飛行路徑。比如，當ΔH2D lt; Cesca 時， 設khorilt;lt;1，讓目標（goal） 的高度比當前高度（curr） 高一值，以確保地面尋徑總成本H 較小，維持在地面搜索路徑中。當Cesca lt; ΔH2D lt; Cland 時，決策系統將進入逃脫階段，使目標高度與當前高度相同，保證平臺水平飛行。最后，當ΔH2D gt; Cland，引導飛行器著陸。表3 中第5 行到第11 行表述了這個過程。

經過不斷的迭代，逃脫算法生成一條通往目標點的路徑。規劃器最終生成一條高能效的全局路徑后，通過對已有全局路徑插值優化，達到路徑可被跟蹤的效果。

3 實驗環境與設定

為驗證本算法，搭建測試場景以及設置運行規則。

通過生成的障礙車輛模擬城市道路擁堵模型，為路網和空域節點建立通行機制，并設置4 個固定的起降區域測試本算法規劃能力。

1） 節點結構：在城市場景中，將路網的每個交叉路口點定義為1 個地面節點，每個地面節點對應1 個空中節點。相鄰的地面節點沿道路相連。空中節點和地面節點之間是否可通行取決于所處位置是否是起降區。空中節點高度為飛行允許高度。如圖3 是節點的示意圖，其中箭頭所指為節點可延申的方向。

根據節點通行規則，城市化場景中路網與空域網組成如圖4 所示。綠色實線和白色點線分別代表地面節點和空中節點，相鄰地節點之間可雙向通行。規劃算法將在這些節點中通過本算法進行路徑規劃。

2） 交通擁堵：為了表示道路擁堵程度，本算法定義了擁堵系數kc，kc∈[1， 2]，表示從暢通到擁堵的程度。同一地點的不同方向對應的kc 值不同。此外，還引入了擁堵重要因子α，其中α∈[0， 1] 與道路長度（Lroad） 正相關。α 的作用是降低對擁堵的敏感性，以避免頻繁的短距離飛行。每個相鄰的地面節點通過kc_revi = αkc 的值描述擁堵等級。

3） 移動代價：本文分別定義了地面節點和空中節點的移動代價系數，分別用kmg 和kma 表示。基于修正后的kc_revi，使用閾值T∈[1， 2] 來區分擁堵程度：當kc_revigt;T 時使kmg→∞，否則kmg = kc_revi。

使用G = Lroad kmg 描述從地面節點移動到另1 節點的移動代價。同理，計算2 個空中節點之間的移動成本使用G = dist·kma，其中dist 為2 個節點之間的距離，kma與T 成正比。地面節點與空中節點之間的起降代價可設為0～1 之間的常數，其數值越小，起降頻率越高，不滿足起降條件時起降代價是無窮大的。最后，本算法在環境約束下執行路徑規劃。

4 實驗結果與分析

4.1 仿真平臺

實驗在ROS/Gazebo 仿真環境下進行。在構建仿真系統時，陸空兩棲平臺包括空中模塊（UAV） 和地面模塊（UGV）。空中模塊采用四旋翼飛行結構，借助旋翼模擬器完成其飛行和傳感器數據仿真[16]。地面部分采用Ackerman 底盤結構，驅動控制采用ROS 驅動控制[17]。

本文使用OctoMap 對LiDAR 點云進行處理，生成三維占用網格圖。圖6 顯示了Gazebo 中的1 個簡單的城市場景和RViz 中的地圖可視化。

4.2 迷宮測試

場景由1 個開放區域和1 個封閉區域組成，每個區域的尺寸為36 m × 27 m。三維占用網格圖的分辨率為0.5 m。如圖7 所示，在開放式區域中，平臺從綠色點代表出發，目標標記為紅色五星。起點和目標點分別處于地面和空中。

圖8 為路徑規劃器在仿真環境中的運行結果，目標點1，2，3 分別對應不同類型的目標點。對于目標點1 和2，控制器優先選擇地面行駛，當地面檢索成本高于起飛閾值時飛行至目標。值得注意的是， 當C 值越小， 算法對搜索速度越敏感，二維路徑占比也就越高。當C 值過小時，如圖9 所示，平臺降落在地面上，其實是陷入了另1 個狹窄空間，即降落在另1 個狹窄空間內。相比之下，適當的增加C 值后，如圖10 所示的規劃結果會更合理。

4.3 結果對比

如圖11 所示，對照算法3D-A* 運行結果，生成的路徑在路徑長度上最優，但是能量效率很低。在圖12中，對照組在一定程度上優化了能耗問題。但是在本文的例子中，使用原論文中給出的計算方程，飛行代價將大于1，這使得飛行權重高于移動代價的權重，規劃算法更偏向于地面行駛模態。

表4 給出了本文的方法與3D-A* 和文獻[10] 中的方法的定量比較。其中使用的能耗模型寫在第二節。

分析目標點1 ：對比對照組算法，本文提出的算法規劃時間為0.072 s，提升了33%；能量消耗降低了4%。對比3D-A* 算法，本算法在搜索速度上提升了88%，能耗提升94.7%。原因是即使在地面上，3D-A* 在每個循環中搜索最多要檢索17 個鄰居節點，而本文提出的規劃器最多只搜索8 個鄰居節點，因此本算法有更快的檢索速度。

分析目標點2 ：目標點2 處于高臺之上，3 組算法中3D-A* 的搜索量最低。原因是在這種場景下本算法需要更多的時間來檢索地面，以確定飛行器是否需要起飛，以確保能耗優勢，而3D-A* 可以直接生成立即起飛的路徑，引導平臺到達目標點。這種必定起飛的任務場景下，本算法綜合考量搜索速度與能量效率，相較于對照組[10] 與3D-A* 有著更均衡的表現。能耗相較于3D-A* 降低了32.3% ；搜索速度相較于對照組[12] 提升了250 倍。

對于目標點3 ：目標點3 位于高臺之上，且距離起點最遠，相較于對照組算法[10] 與3D-A* 算法的結果，本算法選擇地面行駛至離目標點最近的圍墻再翻越迷宮圍墻到達目標點的方案。在搜索時間和能量效率上有著綜合的突出表現，也更符合陸空兩棲平臺的特性。能耗相較于3D-A* 降低了69%；搜索速度相較于對照組[10] 提升了3 倍。

總而言之，本文所提算法在不同目標點的測試中表現出更為均衡的性能。對比對照組算法，該算法在平均規劃時間上實現了85.33% 的提升，與3D-A 算法相比，所提算法在平均搜索速度上提升了47.1%，且平均能耗相對于3D-A降低了74.73%。這一綜合表現表明，所提算法在不同場景下都能夠平衡搜索速度和能耗效率，呈現出顯著的性能優勢。

4 城市場景測試

為了測試擴展方法，本文使用如圖13 所示的環境，其面積約為230 m × 230 m。如圖所示，平臺從一片島嶼出發，目標點用紅色點標記。所有的起點和目標都在地面上，道路的擁堵程度通過隨機生成的汽車的數量來反映。本文將道路的每個交點作為節點，總共有20個地面節點和20 個空中節點。另外，設置了4 個起降區域，靠近這些區域的節點在空中和地面之間切換模態的代價是常數值，其余區域模態切換代價是正無窮。

圖14為同一圖內的初步規劃路徑和最終優化路徑。在左上角的圖中，紅色深淺反映了道路擁堵程度，顏色越深表示擁堵越高。2 條路徑呈現了規劃算法在復雜城市環境中的適應能力。算法通過巧妙規避擁堵，實現了在規劃的道路和指定區域之間的靈活切換，最終達到了目標點。這表明了算法在處理城市場景下的路徑規劃問題時具有很好的實用性。

本實驗案例中，尤其是在島嶼這樣的非道路區域，相較于其他算法[10， 18-20]，本算法通過使用2D 路徑規劃算法，在非道路區域規劃初步路徑，并通過節點收縮生成可跟蹤路徑，成功規避了非平坦地形的挑戰。這種方法不僅保證了路徑的可行性，還提高了車輛平臺的穩定性，對于城市中的復雜地形具有很好的適應性。在起降區，可規劃陸空兩棲平臺完成模態切換; 2 個空中節點之間采用3D-A*。結果表明，該方法具有較高的能量效率和搜索效率，并具有規避道路擁堵的能力。

5 結 論

本文提出了一種新的陸空兩棲平臺路徑規劃方法，將陸地駕駛視為常規模態，遇到難以通行的障礙時使用逃脫算法翻越當前障礙。所提出的規劃算法通過在陸地平面上規劃路徑來減少能量消耗，并通過二維檢索來降低搜索量，必要時切換模態進行三維路徑檢索。相較于對照組算法，在規劃速度和能耗性能上均有明顯提升。此外，本文提出的城市場景實驗是將陸空兩棲平臺應用于城市交通系統的一種嘗試，它具有對先驗信息要求低的特點。獲取道路長度、擁擠狀況、起降區域等信息后，可計算出移動到目標點所需的移動成本，從而為陸空兩棲平臺指引最優運動路徑。

未來計劃將本文的規劃方法部署在真實的陸空兩棲平臺上進行更多場景實驗。針對實際應用實驗，希望規劃算法能夠在動態和未知環境下進行動態規劃的能力，增強平臺在2D 和3D 之間切換的性能。

參考文獻（References）

[1] 秦博， 王蕾. 無人機發展綜述[J].飛航導彈， 2002（8）： 4-10.

QIN Bo， WANG Lei. Overview of UAV development [J].Wing Missi J， 2002（8）： 4-10. （in Chinese）

[2] Chan C W ， Kam T Y. A procedure for power consumptionestimation of multi-rotor unmanned aerial vehicle [C]// JPhys： Conf Seri. IOP Publishing， 2020， 1509（1）： 012015.

[3] Lavalle S M. Rapidly-exploring random trees： A new toolfor path planning [J]. Comp Sci， 1998， 98： 11.

[4] Jr J J K， Lavalle S M. Rrt-connect： An efficient approachto single-query path planning [C]// Proceed 2000 IEEEInt’l Conf Robot Auto， ICRA 2000. San Francisco， CA，USA， 2000.

[5] Karaman S， Frazzoli E. Sampling-based algorithms foroptimal motion planning [J]. Int’l J Robot Res， 2011，30（7）： 846-894.

[6] Koenig S， Likhachev M， Furcy D. Lifelong planning A*[J].Artif Intel， 2004， 155（1-2）： 93-146.

[7] Koenig S， Likhachev M. D* lite [C]// 18th National ConfArtif Intel. Edmonton Alberta Canada. 2002： 476-483.

[8] Likhachev M， Ferguson D， Gordon G， et al. Anytime dynamicA*： An anytime， replanning algorithm [C]// Int’l ConfAutom Plan Schedul. Monterey （USA）， 2005， 5： 262-271.

[9] Daniel K， Nash A， Koenig S， et al. Theta*： Any-angle pathplanning on grids [J]. J Artif Intel Res， 2010， 39： 533-579.

[10] Araki B， Strang J， Pohorecky S， et al. Multi-robot pathplanning for a swarm of robots that can both fly and drive[C]// 2017 IEEE In’l Conf Robot Auto （ICRA）. Marina BaySands， Singapore， 2017： 5575-5582.

[11] Sharif A， Lahiru H M， Herath S， et al. Energy efficientpath planning of hybrid fly-drive robot （hyfdr） using A*algorithm [C]// 15th Int’l Conf Info-Atics Contr， AutoRobot. Porto， Portugal. 2018.

[12] Suh H J T， Xiong X， Singletary A， et al. Energy-efficientmotion planning for multi-modal hybrid locomotion [J].arXiv e-prints， p. arXiv： 1909.10209， 2019.

[13] Pacejka H B， Bakker E. The magic formula tyre model [J].Vehi Syst Dyna， 1992， 21（S1）： 1-18.

[14] Sergiu Baluta， Starlino Electronics. How much power isneeded to hover ？ [/OL]. 2015. （2015-10-28）. http：//www.starlino.com/power2thrust.html， 2015.

[15] Leishman G J. Principles of Helicopter Aerodynamics withCD Extra [M]. Cambridge university press， 2006： 142-144.

[16] Furrer F， Burri M， Achtelik M， et al. Rotorsa： A modulargazebo mav simulator framework [J]. Robot Operat Syst（ROS）， 2016（1）： 595-625.

[17] Chitta S， Marder-Eppstein E， Meeussen W， et al. ros_control： A generic and simple control framework for ROS[J]. J Open Sour Software， 2017， 2（20）： 456-456.

[18] CHEN Yang， CHEN Mengqing， CHEN Zhuichuan， et al.Delivery path planning of heterogeneous robot systemunder road network constraints [J]. Comp Elect Engi，2021， 92： No 107197.

[19] LI Jianqiang， SUN Tao， HUANG Xiaopeng， et al. Amemetic path planning algorithm for unmanned air/groundvehicle cooperative detection systems [J]. IEEE TransAuto Sci Engi， 2021， 19（4）： 2724-2737.

[20] NIU Guanchong， WU Lan， GAO Yunfan， et al. Unmannedaerial vehicle （UAV）-assisted path planning for unmannedground vehicles （UGVs） via disciplined convex-concaveprogramming [J]. IEEE Trans Vehi Tech， 2022， 71（7）：6996-7007.

基金項目 / Supported by ： 國家自然科學基金資助項目（62273198）。