林思偉，席萬強(qiáng)，李青云，林俊志，李 鵬,

(1.南京信息工程大學(xué)，南京 210000； 2.無錫學(xué)院，江蘇 無錫 214000)

0 引言

無人機(jī)(UAV)作為移動(dòng)機(jī)器人的研究熱點(diǎn)，不局限于地面的工作空間，使得其擁有更廣闊的應(yīng)用場(chǎng)景，從高空作業(yè)到低空與地面移動(dòng)機(jī)器人進(jìn)行相互協(xié)作，甚至于控制小型無人機(jī)在室內(nèi)進(jìn)行作業(yè)。隨著海拔的降低，隨之而來的是周圍環(huán)境復(fù)雜性的提高[1]，路徑規(guī)劃的難度也相應(yīng)提高，如何設(shè)計(jì)出一條符合UAV運(yùn)動(dòng)約束的較短距離的安全路線成為了研究熱點(diǎn)[2]。

無人機(jī)路徑規(guī)劃算法包括圖搜索法、智能仿真算法、采樣法和基于人工智能算法等[3]。文獻(xiàn)[4]實(shí)現(xiàn)了lazy_Theta*算法在三維路徑規(guī)劃中的應(yīng)用，并且以分段變階貝塞爾曲線進(jìn)行路徑優(yōu)化；文獻(xiàn)[5]將智能算法用于路徑規(guī)劃，由改進(jìn)的遺傳算法生成路徑點(diǎn)，再由三次B樣條曲線進(jìn)行平滑；文獻(xiàn)[6]針對(duì)三維路徑設(shè)計(jì)，將原A*算法的鄰近節(jié)點(diǎn)擴(kuò)展的方式改成采用球形節(jié)點(diǎn)擴(kuò)展的方式，實(shí)現(xiàn)了三維空間的搜索，從而實(shí)現(xiàn)路徑規(guī)劃。但是上述算法都是針對(duì)地圖環(huán)境已知的情況進(jìn)行的全局靜態(tài)路徑規(guī)劃，對(duì)于未事先檢測(cè)到的障礙物信息則無法進(jìn)行避障。針對(duì)此，文獻(xiàn)[7]采用人工勢(shì)場(chǎng)算法，并利用碰撞錐進(jìn)行改進(jìn)，對(duì)于易陷入局部最優(yōu)的問題，則通過設(shè)計(jì)模糊控制以調(diào)節(jié)系數(shù)的方式進(jìn)行解決；文獻(xiàn)[8]將避障問題描述為部分可觀察馬爾可夫決策過程，并采用動(dòng)態(tài)規(guī)劃方法求解近似最優(yōu)狀態(tài)-動(dòng)作組合。

上述進(jìn)行UAV全局靜態(tài)路徑規(guī)劃的研究中，皆是考慮在能夠?qū)崿F(xiàn)靜態(tài)避障的情況下，如何得到較短距離的解，然后采用將路徑進(jìn)行曲線優(yōu)化的方式使之具有C1，C2連續(xù)性，使得規(guī)劃路徑在速度及加速度上不會(huì)發(fā)生突變，利于后面進(jìn)行UAV軌跡跟蹤時(shí)的控制。但進(jìn)行規(guī)劃時(shí)并未考慮到UAV的運(yùn)動(dòng)約束，僅是保證了其路徑的C1，C2連續(xù)性，并不能保證UAV可以正常順利地進(jìn)行軌跡跟蹤而不會(huì)發(fā)生側(cè)翻，后面的UAV動(dòng)態(tài)避障研究中多數(shù)也沒有考慮此問題。文獻(xiàn)[8]通過進(jìn)行動(dòng)作組合的設(shè)計(jì)避免了這種情況的發(fā)生，動(dòng)作組合在保證了控制穩(wěn)定性的優(yōu)勢(shì)下，也約束了更多運(yùn)動(dòng)模式的可能性。

針對(duì)上述研究的不足，本文通過結(jié)合靜態(tài)規(guī)劃和動(dòng)態(tài)規(guī)劃，融合Theta*算法與改進(jìn)DWA(Dynamic Window Approach)算法，實(shí)現(xiàn)靜態(tài)上能夠有較短的路徑，動(dòng)態(tài)上能夠?qū)ξ粗系K物信息避障，并在DWA算法的運(yùn)動(dòng)約束中加入U(xiǎn)AV的動(dòng)力學(xué)模型，使得最終生成的路徑符合UAV的飛行性能，最終輸出結(jié)果可以得到UAV各電機(jī)的轉(zhuǎn)速變化以及UAV各狀態(tài)量的變化。

1 三維地圖建立

為了應(yīng)用和驗(yàn)證提出的算法，本文創(chuàng)建了一個(gè)表示已知復(fù)雜環(huán)境的80×80×20的節(jié)點(diǎn)網(wǎng)格表示無人機(jī)周圍環(huán)境信息。如圖1(a)所示，網(wǎng)格中填充了代表室內(nèi)空間的物體或城市建筑的障礙物。障礙物的位置和大小被隨機(jī)分配，它們的長(zhǎng)寬被設(shè)定在3×3和5×5大小之間。數(shù)據(jù)結(jié)構(gòu)是一個(gè)簡(jiǎn)單的80×80矩陣，元素的離散值為0～20。與數(shù)字高程模型(Digital Elevation Model，DEM)[9]類似使用數(shù)值表示各元素的地面標(biāo)高。圖1右側(cè)的顏色欄表示顏色所對(duì)應(yīng)的海拔高度。

圖1 標(biāo)準(zhǔn)網(wǎng)格和帶緩沖區(qū)網(wǎng)格Fig.1 Standard grid and buffered grid

考慮UAV本身具有一定的空間大小，為增加路徑的安全性，在障礙物周圍創(chuàng)建一個(gè)緩沖區(qū)。如圖1(b)所示，將障礙物高度值分配給它周圍的節(jié)點(diǎn)，同時(shí)將障礙物和周圍節(jié)點(diǎn)的高度值都提高1，以此在標(biāo)準(zhǔn)網(wǎng)格的基礎(chǔ)上構(gòu)建出帶緩沖區(qū)網(wǎng)格，在其上進(jìn)行路徑規(guī)劃，增大其安全性。

2 Theta*算法

Theta*算法是一種廣度搜索算法與貪婪算法的融合算法，從起始節(jié)點(diǎn)開始，將起始節(jié)點(diǎn)的鄰近節(jié)點(diǎn)加入探索隊(duì)列[10]，以式(1)進(jìn)行各個(gè)鄰近節(jié)點(diǎn)的代價(jià)評(píng)估。稱起始節(jié)點(diǎn)為這些鄰近節(jié)點(diǎn)的父節(jié)點(diǎn)，這些鄰近節(jié)點(diǎn)為起始節(jié)點(diǎn)的子節(jié)點(diǎn)。選取當(dāng)前列表代價(jià)最低的鄰近節(jié)點(diǎn)，再繼續(xù)將此節(jié)點(diǎn)的鄰居節(jié)點(diǎn)加入探索隊(duì)列中。重復(fù)此過程直至探索到終點(diǎn)節(jié)點(diǎn)。上述也是A*算法的大致原理。其具體算式為

fTheta*=kd*D+kh*H+ke*E

(1)

式中:D是從起始節(jié)點(diǎn)移動(dòng)到節(jié)點(diǎn)的成本(移動(dòng)距離)，每次通過計(jì)算當(dāng)前節(jié)點(diǎn)到父節(jié)點(diǎn)的歐氏距離加上父節(jié)點(diǎn)的D求得;H是從節(jié)點(diǎn)到目標(biāo)點(diǎn)的啟發(fā)式距離(移動(dòng)距離);E是通過節(jié)點(diǎn)時(shí)的交通風(fēng)險(xiǎn)成本(進(jìn)行避障)，路徑點(diǎn)與障礙點(diǎn)重合時(shí)置1,無重合則為0;參數(shù)kd,kh和ke是經(jīng)過適當(dāng)調(diào)整的權(quán)重系數(shù)，用于調(diào)整路徑特性并使其達(dá)到目標(biāo)點(diǎn)。

相較于A*算法，Theta*算法在每次得到最小代價(jià)節(jié)點(diǎn)時(shí)，通過判斷節(jié)點(diǎn)與節(jié)點(diǎn)之間是否有可見線的方式進(jìn)行節(jié)點(diǎn)父節(jié)點(diǎn)的重鏈接，回溯父節(jié)點(diǎn)的D進(jìn)行當(dāng)前節(jié)點(diǎn)的D更新，實(shí)現(xiàn)以任意角度進(jìn)行規(guī)劃，而不會(huì)像A*算法般受限于僅能進(jìn)行45°角轉(zhuǎn)向。Theta*算法生成的全局路徑規(guī)劃，僅用返回路徑轉(zhuǎn)折點(diǎn)的節(jié)點(diǎn)坐標(biāo)。節(jié)點(diǎn)與節(jié)點(diǎn)之間通過直線插值法生成連續(xù)離散點(diǎn)，構(gòu)建的路徑相較A*算法路徑更自然、平滑。相比一些在用A*算法規(guī)劃后去除冗余節(jié)點(diǎn)的改進(jìn)做法，Theta*算法規(guī)劃過程中進(jìn)行父節(jié)點(diǎn)的重新鏈接，更新其移動(dòng)成本，實(shí)現(xiàn)更優(yōu)更短的路徑規(guī)劃。

圖2所示為在40×40×20的三維地圖中A*算法與Theta*算法的對(duì)比，圖2(b)為圖2(a)的俯視圖。

圖2 A*算法與Theta*算法對(duì)比

由圖2可以看出：Theta*算法規(guī)劃出來的路徑比A*算法規(guī)劃出來的路徑更平滑，轉(zhuǎn)折點(diǎn)更少；尤其是在高度方面，A*算法僅能以一種類似臺(tái)階的方式進(jìn)行高度的爬升與下降。在此空間大小下進(jìn)行1000次障礙隨機(jī)生成與路徑規(guī)劃，Theta*算法平均所需時(shí)間為A*算法的76.67%,規(guī)劃路徑平均長(zhǎng)度為A*算法的92.84%。

3 改進(jìn)DWA算法與Theta*算法融合

在詹京吳等[11]研究下，針對(duì)無人小車實(shí)現(xiàn)了在二維平面上A*算法與DWA算法的融合路徑規(guī)劃算法，其選取A*算法生成的部分路徑點(diǎn)作為DWA算法跟蹤的目標(biāo)節(jié)點(diǎn)。由于A*算法返回的路徑節(jié)點(diǎn)是連續(xù)離散點(diǎn)，其通常做法是：將路徑點(diǎn)進(jìn)行等分，取每等分的最后一個(gè)路徑點(diǎn)作為DWA算法目標(biāo)節(jié)點(diǎn)，令小車根據(jù)DWA算法跟隨著A*算法規(guī)劃出的路徑，最終到達(dá)終點(diǎn)節(jié)點(diǎn)。這導(dǎo)致目標(biāo)節(jié)點(diǎn)選擇無特征性，移動(dòng)機(jī)器人易偏移于連續(xù)轉(zhuǎn)折的路徑上，失去全局路徑規(guī)劃算法的指導(dǎo)作用。而Theta*算法僅返回轉(zhuǎn)折點(diǎn)處路徑節(jié)點(diǎn)，以轉(zhuǎn)折點(diǎn)作為目標(biāo)節(jié)點(diǎn)具有特征性，移動(dòng)機(jī)器不易過大偏移較優(yōu)路徑。

DWA算法是一種常用于動(dòng)態(tài)避障的算法[12]，其本質(zhì)是通過采樣移動(dòng)機(jī)器人的速度變化范圍，將每一個(gè)采樣速度代入當(dāng)前移動(dòng)機(jī)器人位置進(jìn)行模擬預(yù)測(cè)，返回預(yù)測(cè)出來的位置及速度信息。通過評(píng)價(jià)函數(shù)

G=α*e+β*dE+γ*v

(2)

求得每次預(yù)測(cè)位置及速度的得分G，以貪婪算法的思想，每次選擇最優(yōu)解，在每次的最優(yōu)解下進(jìn)行下一輪的速度采樣模擬預(yù)測(cè)，直至終點(diǎn)。式(2)中:α，β，γ是權(quán)重系數(shù)；e為航向角評(píng)價(jià)值，表示當(dāng)前前進(jìn)方向與終點(diǎn)方向角度偏差；dE是與障礙物之間的避障距離得分,即UAV與最近障礙物之間的歐氏距離；v是當(dāng)前速度得分。其中:

(3)

(4)

式中:Pgoal為當(dāng)前跟蹤的目標(biāo)節(jié)點(diǎn)位置;Pt為預(yù)測(cè)位置；vx,vy和vz為UAV在三軸上的分速度大小。

計(jì)算dE時(shí)，通過如圖3(a)到圖3(b)所示切片的方式，并通過如圖3(c)所示隨著距離由近及遠(yuǎn)地旋轉(zhuǎn)擴(kuò)散搜索障礙物。圖3(c)所示為以五角星為中心擴(kuò)散搜索周圍2圈障礙物：每次搜索的4個(gè)點(diǎn)，都是由一個(gè)點(diǎn)以繞中心每次旋轉(zhuǎn)90°的方式生成，這4點(diǎn)距離中心點(diǎn)的距離都是相同的，從而做到由近及遠(yuǎn)地搜索。每一圈的搜索點(diǎn)數(shù)是(n+1)×4，n代表第幾圈。如果以中心建立直角坐標(biāo)系，就是將第一象限及其x正半軸上的搜索點(diǎn)位，按照距離中心點(diǎn)的距離，從小到大依次以繞中心點(diǎn)每次旋轉(zhuǎn)90°進(jìn)行旋轉(zhuǎn)擴(kuò)散式搜索。當(dāng)碰到障礙物或者達(dá)到搜索圈數(shù)時(shí)，跳出搜索循環(huán)?；谡系K物為不浮空矩形，高層障礙物信息總是少于底層障礙物信息，僅搜索水平四周及UAV正下方一層的3×3的搜索空間。

圖3 障礙物搜索Fig.3 Obstacle search

圖4所示為四旋翼簡(jiǎn)易結(jié)構(gòu)圖。

圖4 四旋翼結(jié)構(gòu)示意圖Fig.4 Diagram of quadrotor structure

由Newton-Euler方程推導(dǎo)可得四旋翼動(dòng)力學(xué)模型[13](本文變量只考慮數(shù)值大小)

(5)

(6)

其中:w1,w2,w3和w4分別表示4個(gè)電機(jī)的轉(zhuǎn)速;x,y,z分別表示在世界坐標(biāo)系三軸上的分量;φ,θ和ψ分別表示滾轉(zhuǎn)角、俯仰角和偏航角。

圖5 DWA算法流程圖Fig.5 Flow chart of DWA algorithm

4 仿真驗(yàn)證與結(jié)果分析

為了驗(yàn)證融合算法的有效性，基于Matlab2021a仿真環(huán)境，生成如圖6所示80×80×20地圖模型，其中，高于5的障礙物占地圖空間的12.61%，高于10的障礙物占地圖空間的6.54%，高于12的障礙物占地圖空間的5%。電機(jī)范圍取0～7000 r/min，采樣間隔取100 r/min。采樣時(shí)間為0.1 s。選取文獻(xiàn)[13]中的無人機(jī)參數(shù)作為仿真參數(shù)，如表1所示。起點(diǎn)坐標(biāo)為(5,7,12),以綠色矩形進(jìn)行標(biāo)注;終點(diǎn)坐標(biāo)為(48,61,8),以紅色菱形進(jìn)行標(biāo)注。

表1 四旋翼物理參數(shù)Table 1 Quadcopter physical parameters

角度及角速度約束如下：-0.785 4 rad≤φ≤0.785 4 rad；-0.785 4 rad≤θ≤0.785 4 rad；-0.349 1 rad/s≤dφ≤0.349 1 rad/s；-0.349 1 rad/s≤dθ≤0.349 1 rad/s；-0.698 1 rad/s≤dψ≤0.698 1 rad/s。其中，dφ,dθ和dψ分別為滾轉(zhuǎn)角速度、俯仰角速度和偏航角速度。

如圖6所示，用Theta*算法對(duì)原始地圖進(jìn)行全局路徑規(guī)劃。當(dāng)新增障礙物時(shí)，原Theta*算法規(guī)劃的路徑被擋住，Theta*與DWA融合算法仍能繞過新增障礙物，且運(yùn)動(dòng)約束生成的路徑比Theta*算法規(guī)劃出來的路徑更平滑。

將融合算法生成的路徑關(guān)于時(shí)間求導(dǎo)，求得其三軸速度變化情況，如圖7所示。在三軸速度上皆無突變，路徑滿足C1連續(xù)性。在飛行過程中，UAV滾轉(zhuǎn)角φ、俯仰角θ和偏航角ψ的角度變化如圖8所示，電機(jī)轉(zhuǎn)速變化如圖9所示。在16 s附近發(fā)現(xiàn)電機(jī)轉(zhuǎn)速皆突變?yōu)? r/min，對(duì)應(yīng)位置在圖6(d)中用白色五角星標(biāo)記，恰是飛行拐點(diǎn)，UAV選擇以僅受重力的方式，通過慣性調(diào)整姿態(tài)和飛行航向。然后增大Y軸方向速度降低X軸方向速度，飛向目標(biāo)點(diǎn)。

圖7 三軸速度變化Fig.7 Three-axis speed change

圖8 三軸角度變化Fig.8 Three-axis angle change

圖9 電機(jī)轉(zhuǎn)速變化Fig.9 Motor speed change

結(jié)合上述信息發(fā)現(xiàn)混合算法路徑下的飛行速度連續(xù)，而Theta*算法的路徑轉(zhuǎn)折處是位移對(duì)于時(shí)間的不可導(dǎo)點(diǎn),會(huì)存在速度突變。并且所提方法規(guī)劃出的路徑，在無干擾的情況下進(jìn)行軌跡跟蹤僅需考慮跟蹤4個(gè)電機(jī)轉(zhuǎn)速，相較于以前需要對(duì)位置進(jìn)行跟蹤控制，其將控制對(duì)象系統(tǒng)簡(jiǎn)化到電壓輸入轉(zhuǎn)速輸出這一環(huán)節(jié)，降低控制器的設(shè)計(jì)難度。

5 結(jié)論

通過研究對(duì)比發(fā)現(xiàn),Theta*算法相較于A*算法更易與改進(jìn)DWA算法進(jìn)行融合，使得DWA算法的目標(biāo)節(jié)點(diǎn)選取更加明確，大大減小UAV偏離Theta*算法規(guī)劃出的路徑的可能性，更適用于室內(nèi)或者城市中復(fù)雜環(huán)境下的運(yùn)動(dòng)規(guī)劃。此外，不需要實(shí)時(shí)更新地圖或保持空間布局不變的使用要求，將使得UAV的應(yīng)用范圍進(jìn)一步擴(kuò)大。其中，以控制電機(jī)轉(zhuǎn)速進(jìn)行UAV軌跡跟蹤的方式，更適用于室內(nèi)少干擾的環(huán)境，相較于不考慮UAV動(dòng)力學(xué)模型進(jìn)行路徑規(guī)劃的方式，本文方法生成的路徑更利于實(shí)際UAV的軌跡跟蹤。