基于ROS四軸飛行器的路徑規(guī)劃

彭求志　田麗　程彬

摘 要：針對四軸飛行器很難通過運動控制來完成快速擴展隨機樹（Rapidly-exploring Random Tree， RRT）所生成運動軌跡的問題，提出動力學(xué)和運動學(xué)約束的RRT算法.采用固定最終狀態(tài)和固定最終時間控制器改進算法，通過動力學(xué)和運動學(xué)約束產(chǎn)生四軸飛行器可執(zhí)行的路徑，對比實際四軸飛行器行走的路徑與期望的路徑，對RRT重新布線，調(diào)節(jié)其行走路徑，保證算法的漸近最優(yōu)性.仿真實驗及實物測試結(jié)果表明，該算法可以運用到四軸飛行器運動規(guī)劃中，在實際應(yīng)用中具有一定的價值.

關(guān)鍵詞：快速擴展隨機樹;運動規(guī)劃;四軸飛行器;ROS

[中圖分類號]TP301.6?? [文獻標(biāo)志碼]A

Path Planning Based on ROS Quadcopter

PENG Qiuzhi，TIAN Li*，CHENG Bin

（Key Laboratory of Advanced Perception and Intelligent Control of High-end Equipment，Ministry ofEducation，Anhui University of Engineering，Wuhu 241000，China）

Abstract：Aiming at the problem that it is difficult for a quadcopter to complete the motion trajectory generated by the Rapidly-exploring Random Tree through motion control，an RRT algorithm with dynamics and kinematics constraints is proposed.The algorithm is improved by the method of fixed final state and fixed final time controller，and the executable path of the quadcopter is generated through the dynamics and kinematics constraint RRT algorithm，and the error between the actual quadcopter's path and the expected path is compared，and the wiring is rerouted，adjust its walking path to ensure the asymptotic optimality of the algorithm. Simulation experiments and physical test results show that the algorithm can be used in the motion planning of quadcopter and has certain value in practical applications.

Key words：rapidly-exploring random tree;path planning;quadcopter;ROS

四軸飛行器自主導(dǎo)航應(yīng)用廣泛，其導(dǎo)航包含感知、定位、路徑規(guī)劃以及運動控制[1]四部分，路徑規(guī)劃（Path Planning）[2-3]是研究的核心之一.路徑規(guī)劃大致分為四類：傳統(tǒng)算法、圖形學(xué)方法、智能算法和其他算法，研究成果豐碩.[4]S.Karaman等證明了基于采樣的路徑規(guī)劃算法RRT和PRM具有概率完備性和漸進最優(yōu)性.[5]Kontoudis George P.等提出了一種基于漸進最優(yōu)快速探索隨機樹（RRT），利用積分強化學(xué)習(xí)，用化直為曲的連續(xù)時間線性系統(tǒng)的最優(yōu)成本和最優(yōu)策略.[6]Tang Zhilin等提出了一種基于采樣的運動規(guī)劃器，在快速探索隨機樹（RRT）中，通過考慮初始狀態(tài)和最終狀態(tài)對，為每個對確定與系統(tǒng)動力學(xué)和約束條件兼容的最佳軌跡，使成本最小化.[7]Moon Chang-bae等提出了一種雙向快速探索隨機樹（DT-RRT），增加節(jié)點擴展的成功率，節(jié)省計算成本.[8]Chi Wenzheng等提出了一種基于風(fēng)險的快速探索隨機樹算法（Risk-DTRRT），該算法在啟發(fā)式軌跡的基礎(chǔ)上提供了最優(yōu)的軌跡、導(dǎo)航運行時間和計劃軌跡的長度，證明所提出算法的有效性.[9]Aoude Georges S.等提出了一種實時路徑規(guī)劃算法（RR-GP），該算法通過結(jié)合高斯過程（GP）的靈活性和基于采樣的可達性計算的效率構(gòu)建學(xué)習(xí)的運動模式模型，顯著降低與動態(tài)障礙物碰撞的風(fēng)險.

筆者針對四軸飛行器很難通過運動控制來完成RRT所生成運動軌跡的問題，提出一種算法，可以生成連續(xù)且平滑的路徑.算法通過對固定最終狀態(tài)和固定最終時間最優(yōu)控制問題，得出最優(yōu)的、開環(huán)的、固定的、最終狀態(tài)自由的最終時間控制策略，通過局部路徑規(guī)劃與全局路徑規(guī)劃相結(jié)合的方法，通過增多采樣點縮短路徑的長度，節(jié)約成本.

1 Kinodynamic RRT

1.1 問題的定義

令X=Rn和U=Rm分別是四軸飛行器的狀態(tài)空間和控制輸入空間，并讓飛行器的動力學(xué)由以下線性系統(tǒng)定義為：

x·[t]=Ax[t]+Bu[t]+c.（1）

其中，x[t]∈C是飛行器的狀態(tài)，u[t]∈U是四軸飛行器的控制輸入，且A∈Rn×n，B∈Rn×m，給定常數(shù)c∈Rn×1.

四軸飛行器的軌跡由元組變量π=x[]，u[]，τ定義，其中τ是軌跡的到達時間或持續(xù)時間，給定初始狀態(tài)x0，在有限時間（到達時間或持續(xù)時間），存在控制u：[0，τ]→U，使得x：[0，τ]→X是沿著相應(yīng)的狀態(tài)軌跡運動.

軌跡π的成本函數(shù)定義為：

c（π）=∫τ01+u[t]TRu[t]dt.（2）

懲罰函數(shù)指軌跡的持續(xù)時間和控制工作量的能量損耗.其中，R∈Rm×m是正定的、恒定的、給定的，且權(quán)衡了相對于彼此以及軌跡持續(xù)時間的控制輸入成本.

設(shè)C為系統(tǒng)的狀態(tài)空間，Cfree∈X定義飛行器的自由狀態(tài)空間，由用戶定義范圍內(nèi)且相對于環(huán)境中障礙物無碰撞的狀態(tài)組成.Ufree∈U定義四軸飛行器的自由控制輸入空間，該空間由放置在其上的控制輸入組成.路徑規(guī)劃的定義：給定一個起始狀態(tài)xstart∈Cfree和一個目標(biāo)狀態(tài)xgoal∈Cfree，在xstart和xgoal目標(biāo)之間找到一個無碰撞軌跡π*free的成本最低：

π*free=argmin π|x[0]=xstart∧x[τ]=xstart∧t∈[0，τ]x（t）∈Cfree∧u（t）∈Ufree c[π].（3）

軌跡函數(shù)：設(shè)π*[x0，x1]=x[]，u[]，τ是x0∈C和x1∈C之間的最佳軌跡，與邊界和障礙無關(guān).成本函數(shù)：c*[x0，x1]是x0∈C和x1∈C之間軌跡的持續(xù)時間和控制工作量的能量損耗.

c*[x0，x1]=minπ|x[0]=x0∧x[τ]=x1c[π].（4）

π*[x0，x1]=argminπ|x[0]=x0∧x[τ]=x1π[π].（5）

對于所有0

1.2 動力學(xué)和運動學(xué)問題

解決飛行器動力學(xué)和運動學(xué)轉(zhuǎn)化為數(shù)學(xué)的問題即最優(yōu)邊界值問題.公式（3）能夠計算出任意兩個狀態(tài)x0∈X和x1∈X之間的最優(yōu)軌跡π*[x0，x1]及其成本c*[x0，x1]，分別在等式（4）和（5）中定義.在固定到達時間的情況下，實現(xiàn)最優(yōu)控制策略，最佳自由到達時間，相應(yīng)的最佳軌跡.

1.2.1 固定最終狀態(tài)和固定最終時間的最優(yōu)控制

固定最終狀態(tài)和固定最終時間的最優(yōu)控制是給定一個固定的到達時間τ和兩個狀態(tài)x0，x1，找到一個軌跡（x[]，u[]，τ）使得x[0]=x0，x[τ]=x1，x·[t]=Ax[t]+Bu[t]+c（對于所有0

設(shè)G[t]為能控標(biāo)準(zhǔn)型性公式：

G·[t]=∫τ0expA（t-t′）BR-1BTexpAT（t-t′）dt′.（6）

這是Lyapunov方程的解：

G[t]=AG[t]+G[t]AT+BR-1BT， G（0）=0.（7）

設(shè)該系統(tǒng)為動力學(xué)系統(tǒng)，則G[t]是t>0的正定矩陣且是可控的.x-[t]描述系統(tǒng)的零輸入響應(yīng)為：

x-[t]=exp[At]x0+∫t0expA（t-t′）cdt′.（8）

微分方程解為：

x=[t]=Ax-[t]+c.（9）

固定最終狀態(tài)、固定最終時間、最優(yōu)控制問題的最優(yōu)控制策略為：

u[t]=R-1BTexpAT（τ-t）G[τ]-1（x1-x-[τ].（10）

1.2.2 尋找最佳到達時間

為了找到定義在x0和x1之間的最佳軌跡π*[x0，x1]，解決固定的最終狀態(tài)、自由的最終時間最優(yōu)控制問題，可以通過選擇到達時間τ來最小化方程的成本函數(shù).

為了找到最佳到達時間τ*，通過把（10）代入成本函數(shù)（2）進行積分，得到固定到達時間τ，x0和x1之間的最佳軌跡成本的封閉式解：

c[τ]=τ+x1-x-[τ]TG[τ]（x1-x-[τ].（11）

最佳到達時間τ*是此函數(shù)最小的τ值：

τ*=argmin{τ>0}c[τ].（12）

x0和x1之間的最佳軌跡成本由c*[x0，x1]=c[τ*]給出，最佳到達時間τ*通過將c[τ]相對于τ的導(dǎo)數(shù)c[τ]=0得到：

c·[τ]=τ-2（Ax1+c）Td（τ）-d（τ）TBR-1BTd（τ）.（13）

定義：

d（τ）=G[τ]-1x-1-x-[τ].（14）

c[τ]可以具有多個局部最小值.

計算具有動力學(xué)A=（0，1），（0，0），B=（0，1）T，c=0，R=1的四軸飛行器的函數(shù)c[τ]的圖在x0=（0，0）T和x1=（1，1）T之間，最佳到達時間是τ*=7-1≈1.65，這是函數(shù)c[τ]最小的地方.

1.2.3 計算最佳軌跡

給定最佳到達時間τ*，找到相應(yīng)的最佳軌跡π*[x0，x1]=x[]，u[]，τ*.

y[t]=expAT（τ*-t）d[τ*].（15）

最優(yōu)控制策略為：

u[t]=R-1BTy[t].（16）

最佳控制策略為：

x·[t]=Ax[t]+BR-1BTy[t]+c， x[τ*]=x1.（17）

等式（15）是微分方程的解：

y·[t]=-ATy[t]， y[τ*]=d[τ*].（18）

復(fù)合微分方程為：

x·[t] y·[t]=ABR-1BT0-ATx[t]y[t]+c0x[τ*]y[τ*]=x1d[τ*].（19）

微分方程的解為：

x[t]y[t]=expABR-1BT0-AT（t-τ*）x1d[τ*]+∫tτ*expABR-1BT0-AT（t-t′）c0dt′.（20）

對于所有給定的x[t]，在0

1.3 Kinodynamic RRT★算法

Kinodynamic RRT★由局部路徑規(guī)劃和全局路徑規(guī)劃組成，如圖1所示.為了找到最佳的無碰撞軌跡π*free，首先由全局路徑規(guī)劃的RRT★產(chǎn)生初始路徑，根據(jù)動力學(xué)和運動學(xué)約束產(chǎn)生四軸飛行器可執(zhí)行的路徑.然后對比四軸飛行器行走的路徑與期望的理想路徑的誤差，調(diào)節(jié)四軸飛行器的行走路徑.最后局部路徑規(guī)劃.考慮到RRT有能夠在任何一對狀態(tài)之間找到最佳軌跡的能力，通過對RRT★的改進，可以實現(xiàn)漸近最優(yōu)性.隨著算法迭代次數(shù)接近無窮大，找到最佳路徑的概率接近1.由于隨著RRT★算法節(jié)點的增多，Kinodynamic RRT★會重新布線，很大程度上減少了路徑的長度.

Kinodynamic RRT★：算法與標(biāo)準(zhǔn)RRT★算法略有不同（表1）.首先，筆者將問題定義為找到恰好到達目標(biāo)狀態(tài)的軌跡，而不是RRT★中常見的目標(biāo)區(qū)域.由于RRT★算法計算的部分軌跡是不平滑的，而動力學(xué)和運動學(xué)規(guī)劃的算法會平滑的連接到采樣狀態(tài)，并將采樣狀態(tài)本身作為節(jié)點添加到樹中，不會影響算法的漸近最優(yōu)性.

RRT★：算法以起始點為根節(jié)點，在自由狀態(tài)空間中構(gòu)建軌跡樹T（表2）.算法每次迭代，從自由狀態(tài)空間Cfree采樣狀態(tài)xi成為樹的新節(jié)點（第5行）.對于每個新節(jié)點，在樹中已有的相鄰節(jié)點中找到一個父節(jié)點，計算其成本.對于某個相鄰半徑r，c*[x，xi]

新軌跡的最佳軌跡π[x，xi]無碰撞的節(jié)點，并且找到xstart和xnew之間的最小路徑即當(dāng)前最小

的成本.

Local Planning（表3）：通過局部路徑兩個最接近的狀態(tài)xlocstart，xlocgoal，以在中心Oloc和半徑rloc處構(gòu)造一個圓如圖1所示.在xaugobs產(chǎn)生一個新的節(jié)點xtest，根據(jù)節(jié)點產(chǎn)生如圖1所示的黑色路徑.

Parent：此操作通過將新節(jié)點連接到樹中的相鄰節(jié)點，即c*[xi，x]的節(jié)點x，降低從樹的起點到樹中其他節(jié)點的成本（表4）.對于每個狀態(tài)x來說，連接都是無碰撞的，并且從一開始就可以降低成本到x，則將新節(jié)點xi設(shè)為x的父級，該算法繼續(xù)進行新的迭代.如果無限期重復(fù)此操作，則樹中出現(xiàn)xstart與xgoal之間的最佳路徑.

? Replanning RRT★：對局部路徑進行碰撞檢測，如果無碰撞，根據(jù)Kinodynamic RRT★產(chǎn)生曲線路徑（表5）.

2 實驗結(jié)果

2.1 四軸飛行器模型

四軸飛行器是又稱四旋翼飛行器，可以通過控制其加速度沿任意方向移動.它的狀態(tài)空間是四維的，其線性動力學(xué)描述為[10-11]：

x=pvrw，u=uxuyuf，A=0I0000I0000I0000，B=00I0000LI.（21）

p∈[0，100][0，200]（m），v∈[-10，10]2（m/s），r∈[-1，1]2（rad），w∈[-5，5]2（rad）.其中，p描飛行器在平面中的位置，v為速度，r為角速度，w為角加速度，且uf∈[-4.545，9.935]（N），ux=uy∈[-3.62，3.62]（N）.設(shè)置控制懲罰r為0.5，即允許四軸飛行器達到最大速度，但不會經(jīng)常超過

它們.

通過在MATLAB進行仿真實驗，在RRT樹中添加5 000個節(jié)點后，規(guī)劃出四軸飛行器的漸近最優(yōu)軌跡（圖2）.

2.2 仿真實驗及實驗結(jié)果分析

通過Kinodynamic RRT★的算法進行四軸飛行器的運動規(guī)劃，MATLAB計算出其漸近最優(yōu)軌跡如圖2.可以看出，Kinodynamic RRT★規(guī)劃出來的路徑是連續(xù)且平滑的，適用于四軸飛行器.筆者構(gòu)建了ROS平臺開發(fā)環(huán)境，使用地圖構(gòu)建、四軸飛行器定位和導(dǎo)航三個軟件包[12-13]，在GAZEBO搭建仿真環(huán)境，通過SLAM構(gòu)建地圖.

Cartographer：可以實現(xiàn)基于激光雷達的即時定位與地圖構(gòu)建（SLAM）.

Localization：采用擴展卡爾曼濾波和自適應(yīng)蒙特卡洛定位的定位算法，融合各種傳感器的信息，獲得準(zhǔn)確的定位效果.

Navigation：基于Kinodynamic RRT★的路徑規(guī)劃，實現(xiàn)二維四軸飛行器的導(dǎo)航.

實物測試：采用PX4四軸飛行器.四軸飛行器自動駕駛技術(shù)包含環(huán)境感知、定位與導(dǎo)航、路徑規(guī)劃、控制.環(huán)境感知技術(shù)以“攝像頭+激光雷達”的多傳感器融合為主;定位技術(shù)以“激光雷達+攝像頭+IMU+GPS”的多傳感器融合定位為主.采用Kinodynamic RRT★的路徑規(guī)劃如圖3所示.

實物測試結(jié)果（圖3）表明，因滿足運動學(xué)和動力學(xué)的條件即最優(yōu)邊界問題，四軸飛行器可以按照Kinodynamic RRT★規(guī)劃出來的路徑行走，且與MATLAB仿真結(jié)果保持一致.該軌跡在RRT算法中不僅減少了飛行器急加速和急減速中的能量損耗，而且降低了飛行控制實現(xiàn)的難度，減少了飛行器運動中的成本.

表6的數(shù)據(jù)顯示，隨著節(jié)點的增加，路徑的長度會縮短，接近一個漸進最優(yōu)值，說明局部路徑規(guī)劃在Kinodynamic RRT★起著很重要的作用，體現(xiàn)了該算法的優(yōu)越性，保持了RRT★的最優(yōu)性.

3 結(jié)論

本文提出動力學(xué)和運動學(xué)約束的一種基于增量式采樣的RRT算法，可以生成連續(xù)且平滑的路徑，適用于具有四軸飛

行器的漸近最優(yōu)運動規(guī)劃.研究采用動力學(xué)和運動學(xué)方法改進RRT★算法，確保任何狀態(tài)空間中具有可控性動力學(xué)的任何系統(tǒng)的漸近最優(yōu)性.Kinodynamic RRT★算法通過動力學(xué)和運動學(xué)約束RRT算法產(chǎn)生四軸飛行器可執(zhí)行的路徑，對比四軸飛行器行走的路徑與期望的路徑誤差，對RRT★的重新布線，從而調(diào)節(jié)其行走路徑.ROS的仿真實驗以及實物測試結(jié)果表明，該算法可以運用到四軸飛行器運動規(guī)劃中.四軸飛行器實際與理想的路徑雖然存在一定偏差，但在允許的范圍之內(nèi)，說明該算法在實際應(yīng)用中具有良好的效果，可以應(yīng)用到四軸飛行器之中.

參考文獻

[1]C.Goerzen，Z.Kong，B.Mettler.A Survey of Motion Planning Algorithms from the Perspective of Autonomous UAV Guidance[J].Intelligent & Robotics Systems，2010，57（1-4）：65-100.

[2]E.Galceran，M.Carreras.A survey on coverage path planning for robotics[J].Robotics and Autonomous Systems，2013，61（12）：1258-1276.

[3]ThiThoa Mac，Copot Cosmin，Duc Trung Tran，Heuristic approaches in robot path planning：A survey[J].Robotics and Autonomous Systems，2016，86：13-28.

[4]S.Karaman，E.Frazzoli.Sampling-based algorithms for optimal motion planning[J].Robotics Research，2011，30（7）：846-894.

[5]Kontoudis，George P.，Vamvoudakis，Kyriakos G..Kinodynamic Motion Planning With Continuous-Time Q-Learning：An Online，Model-Free，and Safe Navigation Framework[J].IEEE Transactions on neural networks and learning systems，2019，30（12）：3803-3817.

[6]Tang，Zhiling，Chen，Bowei，Lan，Rushi.Vector Field Guided RRT★ Based on Motion Primitives for Quadrotor Kinodynamic Planning[J].Intelligent & Robotics Systems.2020.

[7]Moon Chang-bae，Chung Woojin.Kinodynamic Planner Dual-Tree RRT （DT-RRT） for Two-Wheeled Mobile Robots Using the Rapidly Exploring Random Tree[J].IEEE Transactions on industrial electronics，2015，62（2）：1080-1090.

[8]Chi Wenzheng，Wang，Chaoqun，Wang Jiankun.Risk-DTRRT-Based Optimal Motion Planning Algorithm for Mobile Robots[J].IEEE Transactions on Science and engineering，2019，16（3）：1271-1288.

[9]Aoude，Georges S.，Luders，Brandon D.，Joseph，Joshua M.Probabilistically safe motion planning to avoid dynamic obstacles with uncertain motion patterns[J].Autonomous Robots，2013，35（1）：51-76.

[10]白天.室內(nèi)環(huán)境中移動四軸飛行器的多目標(biāo)點路徑規(guī)劃方法研究[D].杭州：浙江大學(xué)，2019.

[11]閆曉東.基于視覺的四軸飛行器自主避障系統(tǒng)的研究與實現(xiàn)[D].成都：電子科技大學(xué)，2018.

[12]楊小育.中高速超車場景下基于模型預(yù)測的無人車運動規(guī)劃[D].合肥：中國科學(xué)院大學(xué)（中國科學(xué)院深圳先進技術(shù)研究院），2020.

[13]劉昊.基于ROS的移動操作四軸飛行器設(shè)計與開發(fā)[D].上海：上海交通大學(xué)，2018.

編輯：琳莉

收稿日期：2020-10-13

基金項目：安徽省自然科學(xué)基金面上項目（1908085MF179）;安徽高校自然科學(xué)研究重點項目（KJ2018A0121）;安徽省高校優(yōu)秀青年人才支持計劃項目（gxyq2017013）

作者簡介：彭求志（1995-），男，安徽安慶人.碩士研究生，主要從事移動機器人四軸飛行器路徑規(guī)劃研究;田麗（1962-），女，安徽蕪湖人.教授，碩士，碩士生導(dǎo)師，主要從事智能電網(wǎng)和四軸飛行器控制技術(shù)研究;程彬（1998-），男，安徽安慶人.本科生，主要從事四軸飛行器路徑規(guī)劃研究.