基于改進運動原語生成的陸空兩棲機器人Kinodynamic A＊算法

徐彬，孫恒飛，唐壽星，王雨桐，張旺旺，艾田付

（1.北京理工大學 機械與車輛學院，北京 100081；2.北京理工大學 重慶創新中心，重慶 401135）

陸空兩棲機器人具備空中機器人的強機動能力和地面機器人的長續航能力的綜合優勢，在搜救、巡檢、快遞等長距離作業任務的應用場景下備受關注.應用場景的復雜性對陸空兩棲機器人提出了無人化、智能化的要求，機器人需要完成包括感知、定位與建圖、路徑規劃、運動控制等模塊的協調工作.路徑規劃作為智能化的關鍵模塊，其主要作用在于找到一條從起點到終點的無碰撞安全路徑，并實現路徑時間、距離、能耗等代價項的優化.

在已落地于實際場景的無人車算法里[1-2]，常見路徑搜索算法分為基于圖搜索的路徑規劃、基于采樣的路徑規劃、人工勢場法等.軌跡優化算法有B 樣條優化、五次多項式優化、螺旋曲線優化等[3].由于移動機器人受到實際運動約束，大多算法需要在A*或RRT 算法的基礎上進行改進，以滿足實際運動需要.如PIVTORAIKO 提出state lattice planning[4]，通過局部狀態之間的連接生成可行路徑，在可行路徑基元組成的搜索空間上進行圖搜索，從而保證了規劃路徑的可行性；DOLGOV 提出Hybrid A*[5]，通過捕獲A*離散節點中車輛的連續狀態，使用車輛運動學生成的運動軌跡代替A*算法中的節點轉移，從而保證了路徑的運動學可行性；WEBB 提出 Kinodynamic RRT*[6], 基于狀態空間采樣并引入最優控制計算代價項，用符合動力學特性的曲線代替直線連接到最近節點，從而保證路徑更加滿足動力學約束.這些算法彌補了基礎搜索算法生成的路徑不符合運動約束的問題，但仍存在一些不足：State lattice planning 基于控制空間的采樣無法精準采樣到給定狀態，基于狀態空間的采樣需要求解OBVP 問題來求得軌跡；Hybrid A*具有分辨率最優性，但在大地圖下復雜度過高；Kinodynamic RRT*概率完備，漸進最優，但同樣需要求解OBVP 問題來求得軌跡.

相較于單模態的路徑規劃，陸空兩棲機器人路徑規劃更具挑戰且備受關注[7]，其需要考慮陸空兩種不同模態下的運動學、動力學約束及運動代價差異，并要求模態切換過程順滑過度[8].Kinodynamic A*[9]由于其在運動原語的生成過程和剪枝過程中便考慮了約束，能很好滿足以上需求.Kinodynamic A*使用離散加速度、時間作為狀態空間控制輸入來生成運動原語，通過給終點在空中節點的運動原語施加額外能量代價項表征飛行運動代價，通過一系列工程優化加快了算法速度實現快速規劃[10]，在被動輪式四旋翼陸空機器人上表現出優異性能，在地面行駛中實現了約7 倍的節能[11].但其算法仍然存在以下問題：一是算法使用統一的離散控制輸入導致生成的運動原語一致，未能體現陸、空模態運動差異；二是算法使用統一的速度上限進行剪枝，不合理地限制了空中模式的運動能力；三是軌跡優化時采用一致的速度、加速度代價，以地面模式運動能力為標準時限制了空中模式運動潛力.上述問題導致算法無法充分發揮陸空兩棲機器人的強機動能力.

為充分發揮陸空兩棲機器人的機動性優勢，文中采用改進運動原語生成的陸空兩棲機器人Kinodynamic A*算法.具體來說，基于動力邊界離散加速度輸入，生成差異化運動原語表征陸、空兩種模態的運動能力；基于速度邊界對運動原語進行剪枝以滿足運動約束；修改優化代價函數，滿足陸、空模態下運動的安全性及機動性.最后在仿真場景下驗證改進后的Kinodynamic A*算法的機動性及節能性.

1 路徑搜索

陸空兩棲機器人路徑搜索基于Kinodynamic A*算法.該算法在障礙物占據柵格地圖中，搜索一條符合運動學、動力學約束的無碰光滑路徑.Kinodynamic A*搜索過程與A*算法相似，在A*的基礎上，依據狀態空間方程生成運動原語，剪枝滿足每個柵格僅保留一個節點，求解最優化問題以獲取啟發項.本節在Kinodynamic A*的基礎上，改進Kinodynamic A*算法，使其運用于陸空兩棲機器人路徑搜索，通過控制狀態空間方程加速度輸入，差異化生成陸、空模態運動原語.

1.1 算法流程

改進Kinodynamic A*算法流程圖如圖1 所示.

1.2 改進運動原語生成

文中使用時間參數化的五次多項式函數表征機器人x、y、z三個獨立維度的軌跡，即

設機器人的狀態量x=，設控制量u=，使用狀態空間方程來描述系統

狀態空間方程表明文中可以通過輸入控制量u=來控制機器人的狀態量x=[px py pz;vx vy vz]，即輸入控制量加速度a來控制狀態量位置p和速度v.在給定機器人的當前狀態x(t0) 和控制輸入u(τ)的情況下，考慮時間t的向前積分，由狀態方程的解

即

由此獲取下一個節點的運動狀態，從而生成運動原語.

在改進運動原語生成時，輸入為一系列離散加速度，在每個方向上等分為每個離散時間 τ下段生成(2k+1)3個運動原語.陸空兩棲機器人在地面模式下及空中模式下可提供的動力不同，在空中模式下可提供更大的加速度輸入，應具有以下形式：

改進前后的運動原語生成結果對比如圖2 所示，通過控制狀態空間方程加速度輸入，差異化生成了陸、空模態運動原語.其中綠色帶星號運動原語為在改進后空中模式的運動原語，黑色為未改進前陸、空模態使用一致加速度時所產生的運動原語.經過改進，空中模式下產生的運動原語將探索更廣的范圍，產生更積極的速度和加速度，使得陸空兩棲機器人能充分發揮空中模式下優越的機動能力.

圖2 改進前后的運動原語生成結果對比Fig.2 Comparison of generation results of motion primitives before and after improvement

1.3 改進剪枝

剪枝以保證搜索算法的效率，Kinodynamic A*在每個柵格點內僅保留一個終點狀態位于柵格內的運動原語，此外對于終點速度超過運動能力上限的運動原語進行剔除.Kinodynamic A*算法對于陸、空模態下的運動原語采用一致的終點速度上限進行剪枝，這在保證了陸空兩棲機器人規劃路徑安全的同時帶來了規劃路徑過于保守的問題.特別是空中模式下的剪枝，若采用與地面模式下相同的速度上限，將極大降低機器人的機動能力.改進后的剪枝方法依據機器人運動模式使用不同的終點速度上限，具有以下形式

由運動原語終點狀態的高度pz區分機器人的運動模式，文中保留終點狀態符合速度上限的運動原語，其中空中模式下擁有更大的速度上限，以表征空中模式下的強機動性.

1.4 邊界轉移代價和啟發項設置

構造邊界轉移代價及啟發項函數來表征陸空兩棲運動控制、時間及能耗代價.在計算邊界轉移代價時，對終點位置位于空中的運動原語乘以飛行能耗系數表示飛行較于地面行駛能量損耗的倍數，對高度爬升的運動原語，增加額外代價表示模態切換過程中的額外能量損耗.綜合考慮時間和控制的代價量，實際代價函數定義為

其中u(t) 表示控制量；g(u(t))表示控制代價； ρ用來控制時間的權重.g(u(t))定義如下：

genergy_cost為地面行駛能耗代價；kf為飛行能耗系數；fswitch_cost為模態切換飛行代價.當給出離散化控制量u(t)=ud，持續時間t=t，運動狀態完成轉移，終點狀態為pz=p，計算得g(u(t))=g，得到運動原語的代價表示為

由起始狀態到節點狀態，經歷了個運動原語，則

啟發項設置基于龐特里亞金最小值原理[12]：在控制向量u(t)受約束的情況下，求解從狀態xcurrent到終點狀態xgoal的最優控制,使得目標函數J(T)取極小值J*(T)，這里的J(T)文中定義為

求解為

此外，考慮模態切換過程，xcurrent的狀態量pz決定當前飛行狀態，對于最終目標xgoal為地面模式的目標函數，修改啟發項為

理論上，飛行需要克服自身重力做功，而行駛主要克服地面摩擦力做功，因此飛行能耗遠高于地面行駛能耗，飛行能耗系數kf約為15～20，對應不同載荷下的能耗比.這使得算法趨向于選擇一條地面的行駛路徑，除非當繞路通過障礙物的代價遠超模式切換后飛越障礙物的代價.

2 軌跡優化

由于路徑搜索未將離障礙物的距離納入考慮，啟發項為達到能量優化的目的，生成的路徑經常緊貼障礙物，軌跡優化的主要目的在于平滑路徑、使之遠離障礙物并充分發揮機器人的機動性能.由于陸、空模態下陸空兩棲機器人的運動約束不同，文中在構造優化代價函數時，在空中段路徑使用更大的障礙物距離安全閾值以保證安全性能；給予空中模式下更大的速度、加速度閾值以保證機動性能；在地面段路徑加入曲率約束以保證跟蹤性能.

2.1 模式切換決策

在不同模態下，路徑的軌跡優化代價函數形式相似但參數不同，故軌跡優化前需要對路徑進行分段.首先通過均勻離散時間采樣獲取路徑點，參數化路徑為具有控制點序列S={Q0,Q1,···,Qtotal} 的3 階均勻B 樣條曲線.根據控制點高度進行模式切換決策，將路徑分為地面行駛路徑、陸空切換-起飛路徑、空中飛行路徑和陸空切換-降落路徑，對應機器人模式為以下四種：地面、起飛、空中飛行、降落.

設置模式切換高度閾值為pthr，根據控制點高度pz大小分為地面行駛的控制點集合Sg與空中行駛控制點集合Sf，表述為：

根據路徑連續性劃分各段路徑控制點集合.設Sg有m段地面行駛路徑，Sgi為第i 段連續的地面路徑控制點集合，共有gi_count 個控制點，其首尾控制點連接起始、終止狀態或空中路徑控制點.設Sf有n段地面行駛路徑,Sfi為第i 段連續的空中路徑控制點集合，共有fi_count個控制點，其前后連接地面路徑控制點.表述為

機器人運動過程中有限狀態機如圖3 所示.

圖3 陸空兩棲機器人運動狀態機Fig.3 Movement state machine of hybrid aerial-ground robot

① 文中認定地面模式切換起飛模式的邏輯為：當前控制點Qi為地面控制點且該點后連續n個序列點為空中控制點時，此點為地面模式到起飛模式切換點，表述為

② 起飛模式完成后無人機自動切換為空中模式.

③ 文中認定空中模式切換降落模式的邏輯為：當前序列控制點為空中控制點且該點后連續n個序列點為地面控制點時，此點為空中模式到降落模式切換點，表述為

④ 降落模式完成后無人機自動停槳，并切換為地面模式繼續行駛，如圖4 所示.

圖4 模式切換決策示意圖Fig.4 Schematic diagram of mode switching decision

對于一段軌跡Sgi={Qgi0,Qgi1,···,Qgi gi_count-1}或Sfi={Qfi0,Qfi1,···,Qfi fi_count-1}，為方便后續軌跡優化的描述，其控制點統一表述為

2.2 軌跡優化

軌跡優化使用分段B 樣條曲線擬合軌跡，結合B 樣條控制點躍度、歐氏距離場、速度加速度約束及曲率約束的梯度信息，通過優化B 樣條曲線控制點來優化軌跡的平順性、安全性及機動性.

設置代價函數為

式中：fs為平滑度代價；fd為障礙物距離代價；fv、fa分別為速度、加速度代價；fc為曲率代價；λs、λd、λf、λc為各代價的權重.

①平滑度代價

平滑度可以通過躍度（加速度對時間的導數）來進行量化分析，表示為

②障礙物距離代價

障礙物距離代價表示為控制點離最近障礙物距離的代價，通過此項排斥控制點使之遠離障礙物：

這里d(Qsi j) 為控制點到最近障礙物距離，Fd(d(Qsi j))為單個控制點的障礙物距離代價函數，dsithr為障礙物距離安全閾值.對于空中軌跡段，考慮到壁面效應，文中給予相對地面軌跡段更大的障礙物距離安全閾值.

③速度、加速度代價

速度、加速度代價用于對超過約束的速度、加速度施加懲罰：

這里vsi j、asi j分別為控制點速度與加速度，Fv(vsi j)、Fa(asi j)分別為單個控制點的速度代價與加速度代價函數，vsithr、asithr分別為控制點速度與加速度閾值.考慮到飛行模式下機器人有更強的運動能力，為更好發揮其強機動性的優勢，文中給予相對地面模式更大的速度閾值.

④曲率代價

履帶車輛于地面運動時速度方向為偏航角方向，其正逆運動學模型約束了速度、角速度大小.若是軌跡曲率過大，履帶車輛執行轉彎時將產生較大跟蹤誤差.使用曲率代價項fc來限制地面軌跡的曲率:

其中Csi j為控制點Qsi j的曲率，定義為

文中給予空中模式下λc=0表征空中模式下飛行器可以規劃大角度的轉彎且能順利完成跟蹤而不需考慮額外代價.給予地面模式下λc＞0表征地面模式下履帶車輛需考慮曲率限制，減少控制時的轉彎跟蹤誤差.

3 試驗結果與分析

3.1 試驗平臺及試驗設置

使用涵道-履帶復合式陸空兩棲機器人進行試驗：硬件平臺選擇上，使用D435i 作為深度傳感器，使用Jetson Xavier NX 板載計算機作為中心計算單元，使用燒錄PX4 固件的雷迅創新V5+作為飛控.陸空兩棲機器人如圖5 所示，其中相機前置，飛控及計算單元已安置在中心碳纖維板蓋下，機器人尺寸為0.6 m×0.6 m×0.1 m.陸空兩棲機器人采用雙模式一體化設計，飛行模式采用四涵道推進方式，行駛模式采用履帶式推進方式.

圖5 陸空兩棲機器人Fig.5 Hybrid aerial-ground robot

軟件平臺布置在Ubuntu18.04 操作系統上，使用ROS 進行節點間通信.算法代碼使用C++11 實現，使用非線性優化庫nlopt 求解軌跡優化問題，軟件框架如圖6 所示.

圖6 軟件框架Fig.6 Framework of software

算法參數設置表1 所示.

表1 算法參數Tab.1 Simulation parameters

搭建仿真場景及對應實際場景，在確保算法可行的情況下再上實機進行試驗.其中場景設定如圖7 所示，場景寬6 m、長11.4 m，外圍設有安全網.場景內設有立方體狀1.2 m 高障礙物，其中在6.6 m處設有橫跨障礙物完全封死地面通行.在已知環境下的任務為：給定目標位置，機器人需規劃出一條從當前位置到目標位置的安全無碰撞軌跡，必要時實現陸空兩棲切換以跨越障礙物.

圖7 仿真及試驗場景Fig.7 Simulation and experiment scenarios

圖7(a)為機器人工作空間在rviz 中的表示，其中start 點為坐標原點.由于機器人有大小及形狀，為便于規劃中的避障判定，常將機器人轉換為質點并將障礙物按照機器人體積進行膨脹，從而生成配置空間.按照機器人的尺寸及其運動可能的姿態將橫縱向障礙物皆膨脹0.45 m，垂向膨脹0.2 m（飛機為扁平狀且無法做出大角度俯仰或滾轉姿態，其垂向最大間距認為不超過0.4 m），配置空間在rviz 中的表示如圖7(b)所示.

圖7(c)和(d)為實際搭建場景展示，(c)處場景下為避開前方障礙物機器人需做出左轉或右轉決策，(d)處場景下為越過障礙物，機器人需做出模式切換決策并飛躍障礙物，仿真場景與實際場景障礙物設定一一對應.

3.2 試驗結果分析與對比

與同樣使用Kinodynamic A*作為搜索算法的陸空兩棲運動規劃器TIE[11]做對比，其中除算法運動原語及軌跡優化函數改進部分，其余部分參數如柵格分辨率、離散時間分辨率等設置一致以在同等條件下做對比.設置障礙物柵格地圖及Kinodynamic A*算法柵格分辨率為0.2 m，設置運動原語時間 τ為0.5 s，加速度分辨率為amax/3，時間分辨率為τ/2 ，其余參數設置為表二中數據，得到的規劃路線對比如圖8所示.

圖8 規劃路徑對比Fig.8 Comparision of planned path

兩種算法都實現了陸空兩棲規劃，但在不同場景下及不同參數下兩種算法的性能表現不同.在上述場景下，路徑總時間、飛行平均速度、路徑總代價、算法運行時間在不同運動原語時間下呈現如下圖9.

圖9 各項性能隨運動原語的變化Fig.9 Changes of various performances with motion primitives

當運動原語時間為0.1 s、0.2 s、0.3 s 時，算法由于運動原語時間過短，未能跨越柵格從而未能找到路徑.運動原語時間設置為0.4 s 及0.5 s 時，本算法的路徑總時間、飛行平均速度、路徑總代價略微優于TIE，但算法運行時間略差.在超過0.5 s 的運動原語時間下，算法性能表現不如TIE.這與Kinodynamic A*本身性質有關：空中模式下較大的控制量加運動原語時間導致運動原語生成時非常容易跨越一些柵格，加上運動原語剪枝，導致了一些可能運動狀態的丟失，從而生成的路徑并非最優.

路徑規劃算法的性能依賴于地圖場景，本算法的最大優勢在于發揮陸空兩棲機器人在空中運動的機動性.故搭建圖10 所示場景，進行進一步分析.

圖10 飛過障礙物需求多的場景下規劃路徑對比Fig.10 Comparison of planned paths in scenarios with high demand for flying over obstacles

圖10 為在大場景下規劃路徑對比，其中參數為表2 中參數，設置運動原語時間 τ為0.5 s，時間分辨率為 τ以加快搜索速度.可以發現改進后的Kinodynamic A*算法在第一個轉角處決策與TIE 不同，由于發揮了機器人空中機動性優勢，算法選擇直接飛過障礙物.改進后的算法將路徑總時間從20.5 s 降低到了12 s.路徑總代價由524 mWh 降低到420 mWh.由于路徑遍歷了更少節點，算法運行時間也從0.224 s降低到0.166 s，在此場景下充分體現了算法優越性.

表2 軌跡優化前后結果Tab.2 Results before and after trajectory optimization

在此場景下，路徑總時間、飛行平均速度、路徑總代價、算法運行時間在不同運動原語時間下呈現如下圖11.

圖11 各項性能隨運動原語的變化Fig.11 Changes of various performances with motion primitives

當運動原語時間為0.1 s、0.2 s 時，兩種算法皆未能找到路徑.當運動原語時間為0.3 s 時，TIE 規劃失敗，改進后的Kinodynamic A*算法由于機動性更強而成功規劃出路徑.當運動原語時間設置為0.4 s 至1.0 s 時，本算法的路徑總時間、飛行平均速度、路徑總代價、算法運行時間皆優于TIE.其中路徑總時間平均降低6.7 s，飛行平均速度平均提升54.1%，路徑總代價平均降低22.00%，算法運行時間平均降低0.069 s.

B 樣條優化前后軌跡對比如圖12 所示.

圖12 B 樣條優化前后軌跡對比Fig.12 Comparison of trajectories before and after B-spline optimization

B 樣條優化后，軌跡離障礙物距離顯著提升，軌跡平滑度顯著提高.在圖12 中A 所指處，緊貼拐角的路徑優化后與障礙物保留一定的安全距離，在B所指處，路徑優化后模態平順切換.

軌跡優化前后后結果如表2 所示.

軌跡優化結果表明軌跡優化充分發揮了機器人空中模式下的強機動性，保留了空中相較于地面的高速度并約束了地面行駛速度，提升了障礙物距離從而保證安全性，并完成了地面模式下曲率優化的目標.

結 論

本文提出一種改進運動原語生成的Kinodynamic A*算法用于陸空兩棲機器人路徑規劃.通過增大空中模式下的加速度輸入改進運動原語生成，并在后續剪枝和軌跡優化上做相應適配，在仿真場景下驗證了算法的性能.算法應用于陸空兩棲機器人路徑規劃，仿真結果表明在飛躍障礙物需求較多的所設場景下，路徑總時間平均降低6.7 s，飛行平均速度平均提升54.1%，路徑總代價平均降低22.00%，算法運行時間平均降低0.069 s，算法充分發揮了陸空兩棲機器人上的強機動能力與高續航能力.