基于TSACO及動(dòng)態(tài)避障策略的無人機(jī)路徑規(guī)劃

摘 要：為了提高無人機(jī)路徑規(guī)劃中的避障效率，首先針對全局規(guī)劃，提出一種改進(jìn)蟻群算法TSACO（turning-sensitive ant colony optimization）。該算法利用A算法進(jìn)行非均勻分配初始信息素，通過在概率函數(shù)中引入轉(zhuǎn)向啟發(fā)函數(shù)，以及采用精英螞蟻系統(tǒng)等方法，來提高算法的收斂速度，減少路徑的轉(zhuǎn)角次數(shù)。其次，針對局部規(guī)劃提出一種改進(jìn)速度障礙算法，加入了無人機(jī)動(dòng)力學(xué)方程，考慮障礙物自適應(yīng)碰撞半徑和緊急碰撞錐，以及最優(yōu)速度選擇法等，來改善無人機(jī)局部避障的實(shí)時(shí)性與安全性。仿真實(shí)驗(yàn)表明，該算法相較于其他算法，在路徑長度、轉(zhuǎn)向次數(shù)及動(dòng)態(tài)避障等方面，均具有更好的有效性。

關(guān)鍵詞：無人機(jī)；路徑規(guī)劃；蟻群算法；速度障礙法

中圖分類號：V279 文獻(xiàn)標(biāo)志碼：A 文章編號：1001-3695（2024）10-019-3015-06

doi：10.19734/j.issn.1001-3695.2024.01.0028

UAV path planning based on TSACO and dynamic obstacle avoidance strategy

Jiang Nan，Xu Haiqin，Xing Haoxiang

（College of Information Science & Technology，Donghua University，Shanghai 201620，China）

Abstract：This paper proposed an improved ant colony optimization named TSACO and an enhanced velocity obstacle（VO）to solve the UAV path planning problem.The TSACO incorporated non-uniform initial pheromone distribution，a turning heuristic function，and an elite ant system to improve the convergence speed and reduce the number of path corners during global planning.The enhanced VO integrated the UAV’s dynamic equation，adaptive collision radius and emergency collision cone，along with an optimal velocity selection approach to increase real-time and safe local obstacle avoidance during local planning.Simulation experiments demonstrate that the proposed algorithms have better effectiveness in terms of path length，number of turns，and dynamic obstacle avoidance compared to other algorithms.

Key words：unmanned aerial vehicle（UAV）;path planning;ant colony optimization;velocity obstacle

0 引言

近年來，無人機(jī)（UAV）因?yàn)槠潴w積小、運(yùn)動(dòng)速度快、成本低、配置靈活等優(yōu)點(diǎn)被廣泛運(yùn)用于搜索巡邏［1］、偵察監(jiān)視［2］、電力巡檢［3］、農(nóng)林植保［4］等軍事和民用領(lǐng)域，而無人機(jī)路徑規(guī)劃是該領(lǐng)域的一個(gè)熱門研究方向，其主要由全局規(guī)劃和局部規(guī)劃兩部分組成［5］。無人機(jī)在全局路徑規(guī)劃中根據(jù)環(huán)境信息避開靜態(tài)障礙物，再在局部規(guī)劃中通過傳感器等裝置獲取即時(shí)信息避開動(dòng)態(tài)障礙物，實(shí)現(xiàn)完整的路徑規(guī)劃［6］。當(dāng)前對于全局路徑規(guī)劃的研究方法可分為兩大類：a）傳統(tǒng)搜索算法如A算法、D算法和圖搜索法等;b）智能優(yōu)化算法如粒子群算法、蟻群算法、模擬退火算法等［7］。常見的局部規(guī)劃算法有動(dòng)態(tài)窗口法、速度障礙法、人工勢場法等［8］。

蟻群算法在全局路徑規(guī)劃中運(yùn)用十分廣泛［9，10］，其正反饋并行機(jī)制具有良好的魯棒性及較強(qiáng)的全局搜索能力，且易與其他算法相融合，但也存在收斂速度慢、易陷入局部最優(yōu)、早熟收斂等問題。目前已有許多學(xué)者針對蟻群算法存在的不足進(jìn)行相應(yīng)的改進(jìn)。王曉燕等人［11］采用人工勢場法計(jì)算初始路徑與下一節(jié)點(diǎn)之間距離綜合構(gòu)造啟發(fā)信息，避免算法陷入局部最優(yōu)；Wang等人［12］在蟻群系統(tǒng)基礎(chǔ)上運(yùn)用精英螞蟻和最大最小蟻群算法改善算法收斂速度；申鉉京等人［13］采用雙向蟻群算法，減小了蟻群算法的計(jì)算規(guī)模，改善最優(yōu)解與收斂速度。但上述算法普遍存在搜索范圍較小、轉(zhuǎn)向次數(shù)較多等問題。

速度障礙法（VO）是一種局部避障算法，主要思想是在避障過程中考慮無人機(jī)和障礙物的速度，以確定無人機(jī)能夠安全通過的速度空間，但其存在避障速度和避障時(shí)間難以選擇等問題。眾多學(xué)者已對其提出了一些改進(jìn)方案，許文瑤等人［14］將動(dòng)態(tài)障礙物在速度空間中的運(yùn)動(dòng)不確定性轉(zhuǎn)換為位置的不確定性，改善了算法的安全性；Ueda等人［15］對碰撞錐劃分不同的碰撞概率區(qū)域來選擇速度，改善了路徑的抖動(dòng)和轉(zhuǎn)彎角度過大的問題；楊秀霞等人［16］提出基于時(shí)間的速度障礙法，為無人機(jī)提供了更大范圍的可選避障速度向量范圍，改善了避障的實(shí)時(shí)性。但上述算法存在避障時(shí)間過早，未考慮無人機(jī)運(yùn)動(dòng)學(xué)方程等問題。

本文在前人研究的基礎(chǔ)上，針對全局規(guī)劃提出改進(jìn)蟻群算法TSACO。該算法先通過A算法改進(jìn)初始信息素，后在蟻群算法的概率函數(shù)和目標(biāo)函數(shù)中加入轉(zhuǎn)向策略和轉(zhuǎn)角代價(jià)，并運(yùn)用精英轉(zhuǎn)角螞蟻等方法以改善規(guī)劃路徑的轉(zhuǎn)角數(shù)與長度；針對局部規(guī)劃提出一種結(jié)合動(dòng)力學(xué)的速度障礙法，考慮障礙物自適應(yīng)碰撞半徑和緊急碰撞錐來優(yōu)化避障時(shí)機(jī)，加入最優(yōu)避障速度選擇策略，兼顧速度連續(xù)性與避障有效性。通過對兩者的有效結(jié)合進(jìn)行仿真實(shí)驗(yàn)，模擬無人機(jī)路徑規(guī)劃和運(yùn)動(dòng)過程，仿真驗(yàn)證了改進(jìn)算法的優(yōu)越性以及無人機(jī)在運(yùn)動(dòng)中的局部避障效果。

1 問題描述

為解決無人機(jī)路徑規(guī)劃問題，本文將實(shí)際環(huán)境簡化為二維環(huán)境，并運(yùn)用柵格法建模，在該環(huán)境中，存在動(dòng)態(tài)障礙物和靜態(tài)障礙物，其中動(dòng)態(tài)障礙物可能會(huì)在規(guī)劃過程中發(fā)生移動(dòng)。路徑規(guī)劃的起點(diǎn)設(shè)定為環(huán)境中的第一個(gè)柵格，終點(diǎn)為最后一個(gè)柵格。全局路徑規(guī)劃旨在找到起點(diǎn)到終點(diǎn)的一條最優(yōu)路徑，而局部路徑規(guī)劃則負(fù)責(zé)在這條全局路徑的基礎(chǔ)上進(jìn)行微調(diào)，以應(yīng)對環(huán)境中的障礙物和動(dòng)態(tài)變化。為了簡化問題，本文作出以下合理的假設(shè)：a）全局規(guī)劃中將無人機(jī)視為質(zhì)點(diǎn)，忽略動(dòng)力學(xué)特性；b）局部規(guī)劃中忽略傳感器誤差和響應(yīng)延遲。

1.1 環(huán)境建模

本文采用柵格法來進(jìn)行環(huán)境建模，將環(huán)境劃分成Nx×Ny個(gè)等面積的方塊作為柵格。建立二維直角坐標(biāo)系時(shí)，對柵格依次進(jìn)行編號為1，2，…，Nx×Ny，其中第i個(gè)柵格的位置坐標(biāo)（xi，yi）與序號的映射關(guān)系如下：

xi=a×［Nx-0.5］ mod（i，Nx）=0a×［mod（i，Nx）-0.5］ 其他yi=a×［Ny+0.5-ceil（i/Nx）］（1）

其中：a為柵格的邊長；i為柵格序號；ceil為向上取整函數(shù)；mod為取余函數(shù)。

1.2 無人機(jī)動(dòng)力學(xué)模型與控制

局部規(guī)劃問題中，將動(dòng)態(tài)避障算法與無人機(jī)動(dòng)態(tài)特性相結(jié)合能更好地驗(yàn)證避障算法的有效性和實(shí)用性。本文使用四旋翼無人機(jī)的動(dòng)力學(xué)模型，其主要可分為位置動(dòng)力學(xué)模型與姿態(tài)動(dòng)力學(xué)模型，將兩者聯(lián)立即可得到無人機(jī)飛行控制剛體模型［17］，如式（2）所示。

=1m（cossinθcosψ+sinsinψ）×U=1m（cossinθsinψ-sinsinψ）×U=1m（coscosθ）×U-g=1Ixx［Ux+（Iyy-Izz）+JrΩ］=1Iyy［Uy+（Izz-Ixx）-JrΩ］=1Izz［Uz+（Ixx-Iyy）］（2）

其中：m表示無人機(jī)的質(zhì)量；、θ和ψ分別代表無人機(jī)機(jī)體坐標(biāo)系和大地坐標(biāo)系X、Y、Z軸之間的旋轉(zhuǎn)角度；、和表示無人機(jī)在大地坐標(biāo)系下沿X、Y、Z軸的加速度；U表示作用在無人機(jī)上的總升力；Ux、Uy和Uz分別表示作用在無人機(jī)三個(gè)坐標(biāo)軸上的力矩大??；g表示重力加速度；Ixx、Iyy和Izz是無人機(jī)對各旋轉(zhuǎn)軸的轉(zhuǎn)動(dòng)慣量；Jr表示無人機(jī)電機(jī)和螺旋槳的總轉(zhuǎn)動(dòng)慣量；Ω=ω1-ω2+ω3-ω4，其中ω1，ω2，ω3，ω4分別代表無人機(jī)四個(gè)電機(jī)的轉(zhuǎn)速。

對應(yīng)動(dòng)力學(xué)方程需要進(jìn)行姿態(tài)控制和位置控制，位置控制分為懸停控制和軌跡跟蹤控制。在姿態(tài)控制器方面，本文基于模型預(yù)測生成無人機(jī)的參考姿態(tài)角速度并通過PD控制來調(diào)整無人機(jī)的姿態(tài)角速度，實(shí)現(xiàn)期望的姿態(tài)控制。在懸停控制器中，運(yùn)用PD控制器來計(jì)算無人機(jī)的位置誤差和速度誤差，并產(chǎn)生一個(gè)控制輸入，以調(diào)整無人機(jī)的位置和速度，使其保持在指定的懸停位置。對于軌跡跟蹤控制，LQR（linaer quadratic regulator）控制器根據(jù)改進(jìn)VO算法規(guī)劃節(jié)點(diǎn)結(jié)合無人機(jī)的動(dòng)力學(xué)模型和環(huán)境信息生成最優(yōu)軌跡，再通過線性二次調(diào)節(jié)來計(jì)算最優(yōu)控制器，以實(shí)現(xiàn)能量最小化和軌跡控制。最后使用PD控制器通過實(shí)時(shí)軌跡信息和期望軌跡的誤差來計(jì)算無人機(jī)的姿態(tài)角速度和推力，使其跟蹤期望軌跡。

2 算法原理

本章針對全局規(guī)劃和局部規(guī)劃分別采用TSACO和改進(jìn)VO算法，以找到一條能兼顧路徑長度、轉(zhuǎn)角次數(shù)并能合理動(dòng)態(tài)避障的路徑。在本章后續(xù)內(nèi)容中，將先后介紹兩種方法的算法流程與改進(jìn)內(nèi)容。

2.1 TSACO算法描述

2.1.1 改進(jìn)初始信息素

在傳統(tǒng)蟻群算法中，各節(jié)點(diǎn)之間的初始信息素都為同一常數(shù)值S，這使得蟻群算法初期尋路有很大的盲目性，算法效率較低，故文獻(xiàn)［18］在原始信息素基礎(chǔ)上，按照式（3）增加A所得路徑節(jié)點(diǎn)間的信息素濃度，其余路徑濃度仍保持S值不變。

τ（R）=QS（3）

其中：R為運(yùn)用A算法所得的路徑；Q為正實(shí)數(shù)。

2.1.2 改進(jìn)啟發(fā)函數(shù)

傳統(tǒng)蟻群算法中，啟發(fā)函數(shù)僅考慮當(dāng)前節(jié)點(diǎn)到下一待選節(jié)點(diǎn)歐氏距離，并未考慮其與終點(diǎn)之間的關(guān)系，這樣會(huì)帶來蟻群搜索過程中的盲目性，易陷入局部最優(yōu)、收斂速度過慢等問題，故本文修改啟發(fā)函數(shù)如式（4）所示。

ηij=1μdij+σdje（4）

其中：dje為下一節(jié)點(diǎn)到終點(diǎn)的歐氏距離；μ和σ是常數(shù)，可按需求改變。

無人機(jī)實(shí)際飛行中，大量的轉(zhuǎn)彎會(huì)導(dǎo)致路徑軌跡不平滑，能量消耗過多。因此為了限制轉(zhuǎn)彎次數(shù)，減少能量消耗，本文將在轉(zhuǎn)移概率中引入轉(zhuǎn)角啟發(fā)函數(shù)：

=e-g×θ（5）

其中：g是自適應(yīng)參數(shù)，根據(jù)迭代次數(shù)的不同自適應(yīng)改變，本文設(shè)定為［0，1］即在迭代前期設(shè)定為0，當(dāng)達(dá)到總迭代數(shù)的20%時(shí)設(shè)定為1；θ為待選節(jié)點(diǎn)和當(dāng)前節(jié)點(diǎn)連線與當(dāng)前節(jié)點(diǎn)和前一節(jié)點(diǎn)連線的夾角。則改進(jìn)后的蟻群轉(zhuǎn)移概率如式（6）所示。

pkij（t）=［τij（t）］α×［ηij（t）］β×∑s∈allowedk［τis（t）］α×［ηis（t）］β×j∈allowedk0jallowedk（6）

其中：pkij（t）為t時(shí)刻螞蟻k從節(jié)點(diǎn)i處移動(dòng)到下一個(gè)節(jié)點(diǎn)j的概率；τij（t）為t時(shí)刻從節(jié)點(diǎn)i到j(luò)路徑上的信息素濃度。

2.1.3 精英螞蟻體系

傳統(tǒng)精英螞蟻方法會(huì)針對路徑最短的螞蟻所走路徑進(jìn)行信息素的更新，增大其所帶有的信息素濃度，讓后續(xù)螞蟻更多地選擇較優(yōu)的路徑，以增強(qiáng)正反饋效果，因本文加入了轉(zhuǎn)角策略，故在傳統(tǒng)精英螞蟻基礎(chǔ)上加入精英轉(zhuǎn)角螞蟻與精英轉(zhuǎn)向螞蟻，以增強(qiáng)最優(yōu)路徑的轉(zhuǎn)角特性。相應(yīng)的信息素修改如式（7）所示。

Δτij=∑mk=1Δτk，ij+ePΔτbP，ij+eTΔτbT，ij+eθΔτbθ，ij（7）

其中：eP、eT和eθ分別為最短路徑、總最小轉(zhuǎn)角次數(shù)、總最小轉(zhuǎn)角角度的精英螞蟻影響權(quán)重；而ΔτbP，ij、ΔτbT，ij和Δτbθ，ij由式（8）～（10）給出。

ΔτbP，ij=1/LbP（i，j）∈TbP0其他（8）

ΔτbT，ij=1/LbT（i，j）∈TbT0其他（9）

Δτbθ，ij=1/Lbθ（i，j）∈Tbθ0其他（10）

其中：TbP、TbT和Tbθ分別為一次迭代中路徑最短、總轉(zhuǎn)角次數(shù)最小和總轉(zhuǎn)角角度最小的路徑；LbP、LbT和Lbθ為其對應(yīng)的路徑長度值。

2.2 改進(jìn)VO算法

VO算法是一種廣泛運(yùn)用于智能體避障的算法，具有簡單直觀、能夠?qū)崟r(shí)避障等優(yōu)點(diǎn)，其最早由意大利學(xué)者Fiorini［19］在1998年提出。VO算法基本原理如圖1所示，無人機(jī)的位置、半徑和速度分別為PA、RA、VA，障礙物的位置、半徑和速度分別為PB、RB、VB。一般在VO中，將無人機(jī)簡化為粒子，將障礙物的威脅區(qū)半徑擴(kuò)大至RQ=RA+RB，過無人機(jī)位置對威脅區(qū)作兩條切線，則其形成的錐形區(qū)域?yàn)橄鄬ε鲎插F（RCC），將相對碰撞面積沿障礙物B的速度移動(dòng)，可得到絕對碰撞錐（ACC）。如果無人機(jī)速度VA與障礙物速度VB的相對速度VAB（VA-VB）在相對碰撞錐或VA在絕對碰撞錐內(nèi)，則在未來一段時(shí)間內(nèi)，無人機(jī)將與障礙物發(fā)生碰撞。在這種情況下，需要選擇新的速度矢量，以避開動(dòng)態(tài)障礙物。絕對碰撞區(qū)域定義為

VOA，B（VB）={VA|λ（PA，VA-VB）∩（B⊕-A）≠}（11）

λ（P，V）={P+Δt×V|Δt≥0}（12）

其中：Δt為時(shí)間間隔;⊕為閔可夫斯基和;B⊕-A為相對速度障礙區(qū)域。如圖1所示，此時(shí)無人機(jī)速度矢量的終點(diǎn)落在ACC以內(nèi)，說明一段時(shí)間后，無人機(jī)將與障礙物發(fā)生碰撞;反之則不會(huì)。

2.2.1 障礙物不確定性分析

當(dāng)無人機(jī)檢測到障礙物時(shí)，使用VO算法計(jì)算碰撞錐，若速度落在絕對碰撞區(qū)域內(nèi)，則認(rèn)為無人機(jī)將與障礙物發(fā)生碰撞，可通過選擇碰撞錐范圍外的速度來避開障礙物；若速度剛好落在碰撞錐的邊緣，則無人機(jī)避障過程所需的時(shí)間最短，且其與障礙物相切，如果此時(shí)障礙物的速度突然發(fā)生變化，或者傳感器的檢測精度受到限制，檢測延遲等情況出現(xiàn)時(shí)，就有可能發(fā)生碰撞；若速度遠(yuǎn)離碰撞錐，無人機(jī)雖然可以與障礙物保持一定的安全距離，但避障時(shí)間會(huì)變長，會(huì)降低避障的實(shí)時(shí)性［20］。故本文在VO算法計(jì)算碰撞錐的過程中，為障礙物添加不確定性半徑，設(shè)定了自適應(yīng)系數(shù)使不確定性半徑隨著無人機(jī)與障礙物的距離，以及無人機(jī)速度的改變而變化，以同時(shí)兼顧安全性與實(shí)時(shí)性。自適應(yīng)不確定半徑r定義為

r=d×（ΔRB+RA）（13）

其中：ΔRB為最大不確定半徑，具體取值如式（14）所示；RA為無人機(jī)半徑，作安全距離裕度；d為自適應(yīng)系數(shù)具體取值如式（15）所示。

ΔRB=Δt×V2B（t）+（VB（t）+ΔVB）2-2×VB（t）×（VB（t）+ΔVB）×cos（ΔθB）（14）

d=e10-ζ10<ζ≤201ζ≤100其他（15）

其中：Δt為無人機(jī)最小決策時(shí)間間隔；VB（t）為t時(shí)刻障礙物的速率；ΔVB為障礙物最大速率變化估計(jì)量；ΔθB為障礙物最大方向變化估計(jì)量；ζ=|nAB|2/（nAB·VAB）為預(yù)測碰撞時(shí)間系數(shù)，nAB為無人機(jī)與障礙物的相對位置向量，VAB為無人機(jī)和障礙物的相對速度。故本文障礙物膨脹圓半徑為RQ+r。

2.2.2 緊迫碰nu52wDx61L8me35Zq9cqX3ia51kyes622DJkyHZmwO0=撞錐

VO算法中碰撞錐是針對障礙物的速度做線性模擬得出的，故當(dāng)距離無人機(jī)較遠(yuǎn)的障礙物做非線性運(yùn)動(dòng)時(shí)，這樣的估計(jì)就會(huì)出現(xiàn)偏差，容易使無人機(jī)造成誤避障的操作；此外在障礙物較多時(shí)，距離較遠(yuǎn)處的障礙物在一定時(shí)間內(nèi)都不會(huì)存在威脅，遠(yuǎn)近障礙物同時(shí)考慮可能會(huì)導(dǎo)致避障困難［19］。故本文定義緊迫碰撞錐VOh：

VOh={VA|VA∈VO，‖VAB‖>dm/Th}（16）

其中：Th為緊迫時(shí)間，表征了無人機(jī)避障的時(shí)間條件；dm為障礙物和無人機(jī)之間的相對距離；VO為絕對碰撞錐；VOh表示了在Th時(shí)間之內(nèi)無人機(jī)和障礙物會(huì)發(fā)生碰撞的速度集合區(qū)域，具體區(qū)域如圖2所示。

2.2.3 最優(yōu)避障速度選擇

在傳統(tǒng)VO算法中，避障速度一般選取與指向目標(biāo)點(diǎn)速度向量差模值最小的速度向量［14］，這樣的選取方式會(huì)造成避障結(jié)束后速度方向的突變，雖在理論算法中影響不大，但在真實(shí)的無人機(jī)跟蹤中因?yàn)閯?dòng)力學(xué)約束，將很難跟蹤該軌跡，可能導(dǎo)致無人機(jī)運(yùn)動(dòng)失衡的情況。故本文設(shè)置了避障速度選擇的目標(biāo)函數(shù)：

D（VA（t））=ωV‖VA，pref-VA（t）‖+ωc‖VA（t）-VA（t-1）‖（17）

其中：ωV是考慮趨向目標(biāo)的權(quán)重;ωc是考慮速度變化量的權(quán)重；VA，pref=VAmaxPg-PA‖Pg-PA‖表示無人機(jī)相對目標(biāo)點(diǎn)的速度矢量，其中Pg為目標(biāo)點(diǎn)坐標(biāo)，VAmax為無人機(jī)速度最大值。此外，為了避免無人機(jī)間避障的速度選擇沖突，本文設(shè)定無人機(jī)優(yōu)先向右側(cè)避讓。

2.3 算法融合與分析

2.3.1 TSACO評價(jià)函數(shù)

鑒于TSACO考慮了路徑長度、轉(zhuǎn)角次數(shù)和總轉(zhuǎn)角角度。為了更好地體現(xiàn)三者的改善效果，本文以A算法的路徑數(shù)據(jù)作為衡量標(biāo)準(zhǔn)，定義了如式（18）所示的評價(jià)函數(shù)G。

G=a1LALLLA，ALL+a2TALLTA，ALL+a3θALLθA，ALL（18）

其中：LALL、TALL、θALL分別為仿真算法所得路徑的總長度、總轉(zhuǎn)向次數(shù)與總轉(zhuǎn)角度；LA，ALL、TA，ALL、θA，ALL則分別為A算法所得仿真路徑的總長度、總轉(zhuǎn)向次數(shù)與總轉(zhuǎn)角度；a1、a2和a3為權(quán)重，滿足a1+a2+a3=1。

2.3.2 融合算法流程

本文融合算法實(shí)現(xiàn)流程如圖3所示。

2.3.3 算法復(fù)雜度分析

算法復(fù)雜度可分為時(shí)間復(fù)雜度與空間復(fù)雜度，設(shè)參數(shù)：iter為迭代次數(shù)，M為螞蟻數(shù)目，n是問題規(guī)模，N為障礙物的數(shù)量。則TSACO算法和改進(jìn)VO算法的時(shí)間復(fù)雜度分別為式（19）和（20），空間復(fù)雜度分別為式（21）和（22）。

T1=O（iter×M×n2）（19）

T2=O（N2）（20）

S1=O（n2）（21）

S2=O（N）（22）

由式（19）～（22）可知，本文算法與傳統(tǒng)算法的運(yùn)算復(fù)雜度一致，改進(jìn)方法對傳統(tǒng)算法的性能和邏輯結(jié)構(gòu)產(chǎn)生積極影響，同時(shí)對整體復(fù)雜度的增加可忽略。

3 仿真實(shí)驗(yàn)與結(jié)果分析

本章通過仿真實(shí)驗(yàn)來驗(yàn)證前述提出的TSACO和改進(jìn)VO算法在無人機(jī)路徑規(guī)劃問題中的可行性和有效性，仿真實(shí)驗(yàn)工具為MATLAB 2019a，運(yùn)行環(huán)境為Windows 10，Intel i7-8750H CPU 2.20 GHz，16 GB。實(shí)驗(yàn)主要分為兩個(gè)部分：a）將TSACO算法與現(xiàn)有算法，包括文獻(xiàn)［21］（IACO，融合回退機(jī)制Xfj+1H8Brez8nTWsxOxpmnIpW/Riq3nAN8EfeP39XXY=與轉(zhuǎn)角策略），文獻(xiàn)［22］（EACO，融合航路選擇權(quán)重和轉(zhuǎn)向啟發(fā)函數(shù)），傳統(tǒng)蟻群算法（ACO）相比較，并按2.3.1節(jié)所述的評價(jià)函數(shù)進(jìn)行評估；b）基于改進(jìn)VO算法，設(shè)計(jì)包含多個(gè)動(dòng)態(tài)障礙物的環(huán)境作為測試案例，對其動(dòng)態(tài)避障特性進(jìn)行仿真實(shí)驗(yàn)。

3.1 TSACO實(shí)驗(yàn)分析

為檢驗(yàn)TSACO的尋優(yōu)性能，本文分別在20×20和40×40的柵格環(huán)境下對ACO、IACO、EACO與TSACO進(jìn)行仿真對比，分別進(jìn)行10次獨(dú)立實(shí)驗(yàn)，評價(jià)函數(shù)的參數(shù)設(shè)定為a1=0.5，a2=0.3，a3=0.2，實(shí)驗(yàn)參數(shù)如表1所示，具體路徑參數(shù)對比如表2、3所示，其中40×40的尋優(yōu)結(jié)果如圖4所示。

從表2、3和圖4可看出，在兩種環(huán)境中，TSACO在參數(shù)相同的情況下，相較于ACO、IACO和EACO搜索到了更短的全局最優(yōu)路徑，且平均轉(zhuǎn)角和平均轉(zhuǎn)向次數(shù)相較于其他兩種算法也更小。在40×40的柵格環(huán)境中，TSACO、IACO、EACO和ACO得到的最短路徑長度分別為61.012、64.472、63.921和163.414，平均轉(zhuǎn)角次數(shù)分別為17.5、22.4、20.4和112.3，平均轉(zhuǎn)彎角分別為945.0、1 350.0、1 188.0和18 553.5，TSACO相較于IACO、EACO和ACO得到的路徑長度分別縮短了5.4%、4.6%和62.6%，平均轉(zhuǎn)角次數(shù)減少了21.9%、14.2%和84.4%，平均轉(zhuǎn)彎角減少了30.0%、20.5%和94.9%。然而，相較于文獻(xiàn)算法，TSACO的收斂速度略慢，這是為了獲得更優(yōu)的路徑，TSACO在蟻群的初始迭代過程中未加入轉(zhuǎn)角啟發(fā)函數(shù)，而是在迭代次數(shù)達(dá)到總數(shù)的20%后才引入。這樣的設(shè)計(jì)可以避免過早地收斂于轉(zhuǎn)角較優(yōu)但路徑長度較長的局部最優(yōu)路徑，因此需要更長的時(shí)間來搜索路徑，但這種策略能使TSACO最終收斂到評價(jià)函數(shù)值更優(yōu)的路徑，且在全局規(guī)劃對實(shí)時(shí)性要求不高的情況下，略低的收斂速度對算法的整體影響并不顯著。

3.2 多動(dòng)態(tài)障礙物環(huán)境中的避障實(shí)驗(yàn)

本文結(jié)合無人機(jī)動(dòng)力學(xué)模型的改進(jìn)VO算法，在20×20的柵格環(huán)境下進(jìn)行仿真實(shí)驗(yàn)，障礙物的參數(shù)信息如表4所示，其中無人機(jī)最大速度設(shè)定為10 m/s，半徑為0.5 m，對環(huán)境信息進(jìn)行采樣的時(shí)間周期為0.1 s。局部避障效果如圖5、6所示，無人機(jī)控制參數(shù)反饋如圖7所示。

圖5中，藍(lán)色點(diǎn)畫線為參考線（部分被紅色圈線覆蓋），是使用TSACO規(guī)劃的全局路徑；紅色圈所構(gòu)成的路徑為無人機(jī)的運(yùn)動(dòng)軌跡；OB1為做局部規(guī)劃時(shí)出現(xiàn)的靜態(tài)障礙物；OB2和OB3分別為與無人機(jī)運(yùn)動(dòng)方向夾角呈鈍角和銳角的勻速動(dòng)態(tài)障礙物；AG為同樣運(yùn)用VO算法和無人機(jī)相向而行的智能體，箭頭分別為各個(gè)動(dòng)態(tài)障礙物的運(yùn)動(dòng)方向。從圖5可以看出無人機(jī)在遇到障礙物前會(huì)沿著全局規(guī)劃路徑前進(jìn)，而當(dāng)判斷前方出現(xiàn)障礙物時(shí)便啟用改進(jìn)VO算法，根據(jù)碰撞錐和最優(yōu)速度選擇原則選定速度來避開障礙物。值得注意的是在發(fā)現(xiàn)障礙物后無人機(jī)都會(huì)進(jìn)行減速（圈更密集），而在傳統(tǒng)VO算法避障時(shí)無人機(jī)則會(huì)不減速直接轉(zhuǎn)向，這是因?yàn)榧尤肓藷o人機(jī)動(dòng)力學(xué)方程后，無人機(jī)的運(yùn)動(dòng)速度不能像理想算法那樣突變，故會(huì)更加保守地選擇改變速度來避開障礙物。圖6為無人機(jī)動(dòng)態(tài)避障的瞬時(shí)圖，從圖中可更清晰地看出無人機(jī)在檢測到障礙物后先減速，再改變速度方向避障的過程（紅色圈部分）。此外，因本文設(shè)定無人機(jī)優(yōu)先向右側(cè)避障，故圖6（a）～（d）中無人機(jī)均按全局規(guī)劃路線的右側(cè)方向分別避開障礙物。在本次避障實(shí)驗(yàn)中，共進(jìn)行了43次避障變速操作，平均解算時(shí)間為0.024 4 s，最大解算時(shí)間0.034 1 s，均在單次采樣周期0.1 s以內(nèi)。

圖7（a）為無人機(jī)運(yùn)動(dòng)過程中橫縱坐標(biāo)的變化曲線；圖7（b）為橫縱方向上的速度分量變化曲線；藍(lán)色虛線為LQR控制器所生成的期望軌跡參數(shù)；而紅色曲線為PD控制的跟蹤軌跡參數(shù)。從圖7可以看出跟蹤軌跡的參數(shù)幾乎和期望軌跡參數(shù)一致，可見控制效果較好，但在起步階段因?yàn)閂O算法設(shè)定的初始速度為10 m/s，所以跟蹤軌跡在速度參數(shù)方面有一段0～10 m/s的起步階段。如圖7（c）所示，跟蹤軌跡相較期望軌跡更為平滑，這是由于LQR控制器使用線性化的系統(tǒng)模型計(jì)算最優(yōu)控制輸入，無法準(zhǔn)確地描述無人機(jī)系統(tǒng)的非線性特性，而在跟蹤軌跡中通過PD控制平衡了系統(tǒng)響應(yīng)速度和穩(wěn)定性，更好地適應(yīng)無人機(jī)系統(tǒng)的非線性特性。

為了更直觀地驗(yàn)證改進(jìn)VO算法的避障特性，本文計(jì)算了仿真過程中無人機(jī)與各障礙物的最小避讓距離（實(shí)時(shí)距離的最小值），如表5所示，其中半徑和為無人機(jī)半徑與障礙物半徑的代數(shù)和。

從表5可看出，使用改進(jìn)VO算法進(jìn)行避障時(shí)，無人機(jī)能夠在避開障礙物時(shí)保留足夠的空間。具體而言，對于障礙物OB1、OB2、OB3和AG，無人機(jī)分別保留了0.3 m、0.72 m、0.61 m和0.74 m的安全裕度。需要注意的是OB1最小避讓距離相對較小，這是因?yàn)镺B1是靜止的障礙物，其速度為0，無人機(jī)無須保留較大的避障距離，故2.2.1節(jié)中的不確定半徑計(jì)算結(jié)果較小，使得避讓距離的裕度較小，提高了避障效率。

4 結(jié)束語

本文從全局和局部兩方面完整地進(jìn)行了無人機(jī)路徑規(guī)劃研究。在全局規(guī)劃方面，以傳統(tǒng)蟻群算法為基礎(chǔ)，利用A非均勻分配初始信息素，解決初始搜索效率低的問題；在概率函數(shù)中對啟發(fā)函數(shù)進(jìn)行改進(jìn)，使路徑減少轉(zhuǎn)向，搜索時(shí)更有目標(biāo)性；采用精英螞蟻系統(tǒng)加快收斂速度。通過仿真比較驗(yàn)證了TSACO的優(yōu)越性。在局部避障規(guī)劃方面，在傳統(tǒng)VO算法基礎(chǔ)上，加入障礙物自適應(yīng)不確定半徑，增加了避障時(shí)的安全性；將碰撞錐改為緊急碰撞錐，避免過早避障；加入速度選擇函數(shù)，保證所選速度兼顧目標(biāo)性與連續(xù)性。最后在仿真實(shí)驗(yàn)中，加入無人機(jī)動(dòng)力學(xué)模型，驗(yàn)證了所用算法在模擬環(huán)境中都能有效地避開動(dòng)態(tài)障礙物。在今后的研究工作中，會(huì)側(cè)重于三維環(huán)境和真實(shí)環(huán)境下的無人機(jī)路徑規(guī)劃研究。

參考文獻(xiàn)：

［1］Lee K S，Ovinis M，Nagarajan T，et al.Autonomous patrol and surveillance system using unmanned aerial vehicles［C］//Proc of the 15th International Conference on Environment and Electrical Engineering.2015：1291-1297.

［2］杜云，賈慧敏，邵士凱，等.面向多目標(biāo)偵察任務(wù)的無人機(jī)航線規(guī)劃［J］.控制與決策，2021，36（5）：1191-1198.（Du Yun，Jia Huimin，Shao Shikai，et al.UAV trajectory planning for multi-target reconnaissance missions［J］.Control and Decision，2021，36（5）：1191-1198.）

［3］郭英萃，靳慧斌.基于穩(wěn)定風(fēng)場下的無人機(jī)電力巡檢路徑規(guī)劃研究［J］.綜合運(yùn)輸，2023，45（3）：110-117.（Guo Yingcui，Jin Hui-bin.Path planning of UAV electric power inspection based on stable wind field［J］.China Transportation Review，2023，45（3）：110-117.）

［4］王東.山地果園植保無人機(jī)自適應(yīng)導(dǎo)航關(guān)鍵技術(shù)研究［D］.楊凌：西北農(nóng)林科技大學(xué)，2019.（Wang Dong.Key technologies of adaptive navigation for plant protection UAV in mountain orchard［D］.Yangling：Northwest A&F University，2019.）

［5］王中玉，曾國輝，黃勃.基于改進(jìn)雙向A的移動(dòng)機(jī)器人路徑規(guī)劃算法［J］.傳感器與微系統(tǒng)，2020，39（11）：141-143，147.（Wang Zhongyu，Zeng Guohui，Huang Bo.Mobile robot path planning algorithm based on improved bidirectional A［J］.Transducer and Microsystem Technologies，2020，39（11）：141-143，147.）

［6］馮建新，解爽，郭冠麟，等.POMDP-APF：一種基于POMDP模型的APF無人機(jī)路徑規(guī)劃策略［J］.計(jì)算機(jī)應(yīng)用研究，2023，40（7）：2124-2129，2145.（Feng Jianxin，Xie Shuang，Guo Guanlin，et al.POMDP-APF：UAV path planning strategy of APF based on POMDP model［J］.Application Research of Computers，2023，40（7）：2124-2129，2145.）

［7］趙暢，劉允剛，陳琳，等.面向元啟發(fā)式算法的多無人機(jī)路徑規(guī)劃現(xiàn)狀與展望［J］.控制與決策，2022，37（5）：1102-1115.（Zhao Chang，Liu Yungang，Chen Lin，et al.Research and development trend of multi-UAV path planning based on metaheuristic algorithm［J］.Control and Decision，2022，37（5）：1102-1115.）

［8］張艷菊，吳俊，程錦倩，等.多搬運(yùn)任務(wù)下考慮碰撞避免的AGV路徑規(guī)劃［J］.計(jì)算機(jī)應(yīng)用研究，2023，41（5）：1-9.（Zhang Yanju，Wu Jun，Cheng Jinqian，et al.AGV path planning considering collision avoidance of multiple handling tasks［J］.Application Research of Computers，2023，41（5）：1-9.）

［9］Dorigo M，Maniezzo V，Colorni A.ANT system：optimization by a colony of cooperating agents［J］.IEEE Trans on Systems，Man，and Cybernetics：Systems，1996，26（1）：29-41.

［10］Dorigo M，Gambardella L M.Ant colony system：a cooperative learning approach to the traveling salesman problem［J］.IEEE Trans on Evolutionary Computation，1997，1（1）：53-66

［11］王曉燕，楊樂，張宇，等.基于改進(jìn)勢場蟻群算法的機(jī)器人路徑規(guī)劃［J］.控制與決策，2018，33（10）：1775-1781.（Wang Xiaoyan，Yang Le，Zhang Yu，et al.Robot path planning based on improved ant colony algorithm with potential field heuristic［J］.Control and Decision，2018，33（10）：1775-1781.）

［12］Wang Qingyi，Sun Yongkui，Tang Qichao，et al.A dual-robot cooperative welding path planning algorithm based on improved ant colony optimization［J］.IFAC-Papers Online，2022，55（8）：7-12.

［13］申鉉京，施英杰，黃永平XZ8KAlzi5Nv9rbso0gKdsw==，等.基于雙向蟻群算法的路徑規(guī)劃研究［J］.哈爾濱工程大學(xué)學(xué)報(bào)，2023，44（5）：865-875.（Shen Xuanjing，Shi Yingjie，Huang Yongping，et al.Path planning optimization using the bidirectional ant colony algorithm［J］.Journal of Harbin Engineering University，2023，44（5）：865-875.）

［14］許文瑤，賀繼林.基于改進(jìn)速度障礙法的水下機(jī)器人動(dòng)態(tài)避障［J］.電光與控制，2021，28（12）：86-90.（Xu Wenyao，He Jilin.Dynamic obstacle avoidance for ROV based on improved velocity obstacle method［J］.Electronics Optics & Control，2021，28（12）：86-90.）

［15］Ueda Y，Motoi N.Local path planning based on velocity obstacle considering collision probability and kinematic constraint for mobile robot［C］//Proc of the 48th Annual Conference of the IEEE In dustrila Electronics Society.Piscataway，NJ：IEEE Press，2022：1-6.

［16］楊秀霞，華偉，孟啟源.基于有限時(shí)間速度障礙法的UAV避障研究［J］.彈箭與制導(dǎo)學(xué)報(bào)，2018，38（5）：19-22，26.（Yang Xiuxia，Hua Wei，Meng Qiyuan.Study on UAV obstacle avoidance based on finite time velocity obstruction method［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2018，38（5）：19-22，26.）

［17］Michael N，Mellinger D，Lindsey Q，et al.The GRASP multiple micro-UAV testbed［J］.IEEE Robotics & Automation Magazine，2010，17（3）：56-65.

［18］趙倩楠，黃宜慶.融合A蟻群和動(dòng)態(tài)窗口法的機(jī)器人路徑規(guī)劃［J］.電子測量與儀器學(xué)報(bào)，2023，37（2）：28-38.（Zhao Qiannan，Huang Yiqing.Robot path planning based on A ant colony and dynamic window algorithm［J］.Journal of Electronic Measurement and Instrumentation，2023，37（2）：28-38.）

［19］Fiorini P.Motion planning in dynamic environments using velocity obstacles［J］.The International Journal of Robotics Research，1998，7（17）：760-772.

［20］Fu Jun，Lyu Teng，Li Bao.Underwater submarine path planning based on artificial potential field ant colony algorithm and velocity obstacle method［J］.Sensors，2022，22（10）：3652.

［21］Song Baoye，Miao Huimin，Xu Lin.Path planning for coal mine robot via improved ant colony optimization algorithm［J］.Systems Science & Control Engineering，2021，9（1）：283-289.

［22］吳劍，江澤軍，朱效洲，等.基于改進(jìn)蟻群算法的隱身無人機(jī)快速突防航路規(guī)劃技術(shù)［J］.電光與控制，2023，30（12）：18-23.（Wu Jian，Jiang Zejun，Zhu Xiaozhou，et al.Rapid penetration path planning of stealth UAVs based on improved ant colony optimization［J］.Electronics Optics & Control，2023，30（12）：18-23.）