化算法AO實(shí)現(xiàn)多無(wú)人機(jī)三維協(xié)同避障路徑規(guī)劃)
1. 從能飛到飛得聰明多無(wú)人機(jī)三維路徑規(guī)劃到底難在哪多無(wú)人機(jī)協(xié)同集群避障路徑規(guī)劃說(shuō)白了就是讓一群無(wú)人機(jī)從各自的起點(diǎn)出發(fā)在充滿(mǎn)山峰、雷達(dá)威脅區(qū)和禁飛區(qū)的三維空間里找到一條既安全又省油、還能按時(shí)到達(dá)的飛行路線(xiàn)。這件事聽(tīng)起來(lái)像是給每架飛機(jī)畫(huà)條線(xiàn)但真正動(dòng)起手來(lái)你會(huì)發(fā)現(xiàn)它是個(gè)典型的高維、多約束、強(qiáng)耦合的優(yōu)化問(wèn)題。我先說(shuō)說(shuō)為什么它比單機(jī)路徑規(guī)劃難得多。單機(jī)規(guī)劃只需要考慮我怎么繞開(kāi)障礙物而多機(jī)協(xié)同要同時(shí)處理三件事第一每架無(wú)人機(jī)自身的路徑要滿(mǎn)足動(dòng)力學(xué)約束不能出現(xiàn)急轉(zhuǎn)彎、陡爬升這種實(shí)際飛不出來(lái)的動(dòng)作第二機(jī)與機(jī)之間要保持安全間距否則編隊(duì)還沒(méi)到目標(biāo)就先撞上了第三整體代價(jià)要盡可能低包括飛行距離、飛行高度、暴露在威脅區(qū)的風(fēng)險(xiǎn)以及轉(zhuǎn)彎帶來(lái)的能量損耗。這三件事互相牽制你把某架飛機(jī)的路徑改短了可能就讓它撞進(jìn)了另一架的航路或者被迫拉高高度進(jìn)入威脅區(qū)。從數(shù)學(xué)角度看這是一個(gè)多目標(biāo)優(yōu)化問(wèn)題。我們通常把多個(gè)目標(biāo)通過(guò)加權(quán)求和的方式整合成一個(gè)綜合代價(jià)函數(shù)然后尋找使這個(gè)函數(shù)最小的路徑集合。目標(biāo)函數(shù)一般包含四項(xiàng)路徑長(zhǎng)度成本、飛行高度成本、威脅代價(jià)和轉(zhuǎn)角成本。每一項(xiàng)都有自己的物理意義和量綱需要通過(guò)權(quán)重系數(shù)來(lái)平衡。權(quán)重怎么定直接決定了規(guī)劃出來(lái)的路徑是貼著山脊飛還是繞遠(yuǎn)路求穩(wěn)這是整個(gè)算法設(shè)計(jì)里最考驗(yàn)經(jīng)驗(yàn)的地方。那為什么選天鷹優(yōu)化算法Aquila Optimizer簡(jiǎn)稱(chēng)AO來(lái)做這件事這就得說(shuō)到傳統(tǒng)方法的局限了。早期大家用A*、Dijkstra這類(lèi)圖搜索算法在二維柵格地圖上效果不錯(cuò)但到了三維連續(xù)空間柵格數(shù)量呈指數(shù)級(jí)增長(zhǎng)計(jì)算量直接爆炸。后來(lái)粒子群PSO、遺傳算法GA這些群智能算法成了主流但它們?cè)诟呔S問(wèn)題上容易陷入局部最優(yōu)尤其是障礙物密集、威脅區(qū)交錯(cuò)的環(huán)境下早熟收斂特別明顯。AO是2021年提出的一種新型群智能算法模擬天鷹捕獵時(shí)的四種行為——高空翱翔、俯沖攻擊、低空慢降和地面抓捕在探索和開(kāi)發(fā)之間取得了比較好的平衡收斂精度和速度在多個(gè)基準(zhǔn)測(cè)試上都優(yōu)于PSO和GA。把它用到多無(wú)人機(jī)三維路徑規(guī)劃上正好能發(fā)揮它全局搜索能力強(qiáng)、不易早熟的優(yōu)勢(shì)。這篇文章我會(huì)把整套算法的設(shè)計(jì)思路、目標(biāo)函數(shù)的構(gòu)建細(xì)節(jié)、AO的改進(jìn)策略以及Matlab代碼的完整實(shí)現(xiàn)講清楚。適合兩類(lèi)人看一類(lèi)是做無(wú)人機(jī)集群、路徑規(guī)劃方向的研究生和科研人員需要一套能跑通、能改參數(shù)的代碼框架另一類(lèi)是對(duì)群智能優(yōu)化算法感興趣、想找個(gè)真實(shí)工程問(wèn)題練手的開(kāi)發(fā)者。哪怕你之前沒(méi)接觸過(guò)AO只要懂基本的Matlab編程和優(yōu)化概念跟著走一遍也能把整套流程復(fù)現(xiàn)出來(lái)。2. 目標(biāo)函數(shù)四項(xiàng)成本拆解每一項(xiàng)都在算什么賬2.1 路徑長(zhǎng)度成本最直觀但也最容易算錯(cuò)的一項(xiàng)路徑長(zhǎng)度成本就是整條航跡的歐氏距離總和。假設(shè)一架無(wú)人機(jī)的路徑由n個(gè)航路點(diǎn)組成第i段路徑的長(zhǎng)度就是相鄰兩點(diǎn)之間的三維歐氏距離L_i sqrt((x_{i1}-x_i)^2 (y_{i1}-y_i)^2 (z_{i1}-z_i)^2)總路徑長(zhǎng)度成本就是所有段之和?？雌饋?lái)很簡(jiǎn)單但這里有個(gè)坑航路點(diǎn)的數(shù)量必須固定。為什么因?yàn)槿褐悄芩惴ㄒ竺總€(gè)個(gè)體的維度一致如果某條路徑用10個(gè)點(diǎn)、另一條用20個(gè)點(diǎn)算法沒(méi)法直接比較和交叉。所以標(biāo)準(zhǔn)做法是預(yù)先設(shè)定航路點(diǎn)數(shù)量比如20個(gè)所有無(wú)人機(jī)、所有候選解都用相同的點(diǎn)數(shù)只是點(diǎn)的空間坐標(biāo)不同。另一個(gè)容易忽略的點(diǎn)是起點(diǎn)和終點(diǎn)必須固定。優(yōu)化變量只是中間航路點(diǎn)的坐標(biāo)首尾兩點(diǎn)是硬約束不參與優(yōu)化。我在代碼里通常把起點(diǎn)和終點(diǎn)作為邊界條件直接寫(xiě)入中間點(diǎn)才作為AO的搜索維度。提示航路點(diǎn)數(shù)量不是越多越好。點(diǎn)太少路徑不夠靈活繞不開(kāi)復(fù)雜障礙點(diǎn)太多搜索維度飆升收斂變慢。根據(jù)我的實(shí)測(cè)在10km×10km×1km的規(guī)劃空間里15到25個(gè)中間點(diǎn)是比較合適的范圍。2.2 飛行高度成本低空突防與地形安全的博弈高度成本這一項(xiàng)很多人第一次寫(xiě)的時(shí)候會(huì)直接用地形高度加一個(gè)固定余量但這樣太粗糙了。實(shí)際的高度成本應(yīng)該反映兩個(gè)訴求一是無(wú)人機(jī)不能撞山必須保持離地安全高度二是無(wú)人機(jī)希望飛得低一些減少被雷達(dá)發(fā)現(xiàn)的概率同時(shí)降低風(fēng)阻帶來(lái)的能量消耗。我通常這樣設(shè)計(jì)高度成本函數(shù)對(duì)每個(gè)航路點(diǎn)計(jì)算它到地面的相對(duì)高度h_rel z - terrain(x,y)。如果h_rel小于最小安全高度h_min直接給一個(gè)極大的懲罰值讓這個(gè)解被淘汰。如果h_rel在安全范圍內(nèi)成本隨高度增加而增加但增加幅度是非線(xiàn)性的——低空區(qū)域增長(zhǎng)慢高空區(qū)域增長(zhǎng)快這樣算法會(huì)傾向于選擇貼地飛行但又不至于撞山的路徑。具體公式可以寫(xiě)成H_cost sum( w_h * (h_rel - h_min)^2 ) 當(dāng) h_rel h_min H_cost M (極大懲罰) 當(dāng) h_rel h_min這里的w_h是高度權(quán)重系數(shù)M是一個(gè)足夠大的常數(shù)比如1e6。用平方項(xiàng)而不是線(xiàn)性項(xiàng)是為了讓算法對(duì)飛得太高有更強(qiáng)的抑制。2.3 威脅代價(jià)把雷達(dá)和防空區(qū)變成可計(jì)算的數(shù)字威脅代價(jià)是整個(gè)目標(biāo)函數(shù)里最需要工程經(jīng)驗(yàn)的部分?，F(xiàn)實(shí)中威脅源多種多樣雷達(dá)探測(cè)、防空火力、電子干擾但在路徑規(guī)劃階段我們通常把它們抽象成球形或圓柱形威脅區(qū)每個(gè)威脅區(qū)有一個(gè)中心坐標(biāo)、一個(gè)作用半徑和一個(gè)威脅強(qiáng)度。威脅代價(jià)的計(jì)算有兩種常見(jiàn)方式。一種是離散點(diǎn)判定對(duì)每個(gè)航路點(diǎn)判斷它是否落在某個(gè)威脅區(qū)內(nèi)如果在就累加威脅值。這種方法簡(jiǎn)單但有個(gè)致命缺陷——如果兩個(gè)航路點(diǎn)之間的路徑穿過(guò)了威脅區(qū)而兩個(gè)端點(diǎn)都在威脅區(qū)外就會(huì)被漏判。另一種是路徑段采樣在每段路徑上均勻取若干個(gè)采樣點(diǎn)對(duì)每個(gè)采樣點(diǎn)計(jì)算威脅值。這種方法計(jì)算量大一些但能準(zhǔn)確捕捉路徑穿越威脅區(qū)的情況。我一般采用路徑段采樣法每段取5到10個(gè)采樣點(diǎn)。威脅值隨距離衰減常用的模型是T_i sum( K_t / (d_j^2 epsilon) ) 當(dāng) d_j R_j T_i 0 當(dāng) d_j R_j其中d_j是采樣點(diǎn)到第j個(gè)威脅中心的距離R_j是威脅半徑K_t是威脅強(qiáng)度系數(shù)epsilon是一個(gè)很小的正數(shù)防止除零。這個(gè)模型的好處是距離威脅中心越近代價(jià)急劇上升算法會(huì)自然地讓路徑遠(yuǎn)離威脅核心。2.4 轉(zhuǎn)角成本讓規(guī)劃出來(lái)的路徑真的能飛轉(zhuǎn)角成本是最容易被忽視、但對(duì)實(shí)際飛行影響最大的一項(xiàng)。如果只優(yōu)化路徑長(zhǎng)度和威脅算法可能會(huì)生成一條鋸齒狀的路徑——雖然總長(zhǎng)度短但無(wú)人機(jī)需要頻繁急轉(zhuǎn)彎實(shí)際飛起來(lái)要么過(guò)載超限要么能量消耗遠(yuǎn)超預(yù)期。轉(zhuǎn)角成本的計(jì)算方式是對(duì)路徑上每個(gè)中間航路點(diǎn)計(jì)算前后兩段路徑的夾角。夾角越小轉(zhuǎn)彎越急成本越高。公式可以寫(xiě)成Turn_cost sum( w_t * (1 - cos(theta_i)) )其中theta_i是第i個(gè)航路點(diǎn)處的轉(zhuǎn)彎角。用1-cos的形式是因?yàn)楫?dāng)轉(zhuǎn)彎角為0直線(xiàn)飛行時(shí)成本為0轉(zhuǎn)彎角越大成本增長(zhǎng)越快而且這個(gè)函數(shù)是連續(xù)可導(dǎo)的對(duì)優(yōu)化算法友好。把四項(xiàng)成本加權(quán)求和就得到了完整的目標(biāo)函數(shù)F w1 * L_cost w2 * H_cost w3 * T_cost w4 * Turn_cost權(quán)重的選取沒(méi)有標(biāo)準(zhǔn)答案取決于具體任務(wù)需求。如果任務(wù)強(qiáng)調(diào)隱蔽性就加大威脅代價(jià)的權(quán)重如果強(qiáng)調(diào)快速到達(dá)就加大路徑長(zhǎng)度的權(quán)重。我通常先用一組經(jīng)驗(yàn)權(quán)重跑一遍觀察各項(xiàng)成本的數(shù)量級(jí)然后調(diào)整權(quán)重讓它們?cè)谕涣考?jí)附近避免某一項(xiàng)因?yàn)閿?shù)值過(guò)大而主導(dǎo)整個(gè)優(yōu)化過(guò)程。成本項(xiàng)物理含義典型權(quán)重范圍計(jì)算復(fù)雜度路徑長(zhǎng)度總飛行距離0.3-0.5低飛行高度離地高度懲罰0.1-0.3中威脅代價(jià)暴露風(fēng)險(xiǎn)0.2-0.4高轉(zhuǎn)角成本轉(zhuǎn)彎平滑度0.1-0.2低3. 天鷹優(yōu)化算法AO的核心機(jī)制與改進(jìn)思路3.1 AO的四種捕獵行為對(duì)應(yīng)了哪些搜索策略天鷹優(yōu)化算法的靈感來(lái)自天鷹在自然界中的捕獵過(guò)程。算法把整個(gè)搜索過(guò)程分成四個(gè)階段每個(gè)階段模擬一種捕獵行為對(duì)應(yīng)不同的位置更新策略。第一種行為是高空翱翔與垂直俯沖。這個(gè)階段發(fā)生在搜索初期天鷹在高空盤(pán)旋通過(guò)大范圍的視野鎖定獵物的大致區(qū)域。對(duì)應(yīng)到算法里就是讓個(gè)體在搜索空間中進(jìn)行大范圍的全局探索。位置更新公式里有一個(gè)基于正態(tài)分布的隨機(jī)項(xiàng)讓個(gè)體能夠跳到離當(dāng)前位置較遠(yuǎn)的地方避免所有個(gè)體擠在一起。第二種行為是短滑翔攻擊。當(dāng)天鷹鎖定獵物后會(huì)從高空以較小的角度滑翔接近。這個(gè)階段對(duì)應(yīng)算法的局部探索個(gè)體在當(dāng)前位置附近進(jìn)行中等幅度的搜索既不像第一階段那么發(fā)散也不像后面階段那么集中。第三種行為是低空慢降攻擊。天鷹降低高度在獵物上方緩慢盤(pán)旋逐步縮小包圍圈。這個(gè)階段對(duì)應(yīng)算法的精細(xì)搜索個(gè)體在較小的范圍內(nèi)調(diào)整位置逐步逼近最優(yōu)解。第四種行為是地面抓捕。當(dāng)天鷹確認(rèn)獵物位置后直接俯沖抓捕。這個(gè)階段對(duì)應(yīng)算法的最終收斂個(gè)體在最優(yōu)解附近做最后的微調(diào)。這四種行為的切換不是隨機(jī)的而是通過(guò)一個(gè)隨機(jī)的概率閾值和迭代次數(shù)來(lái)控制的。算法前期偏向第一種和第二種行為保證全局搜索能力后期偏向第三種和第四種行為保證收斂精度。這種設(shè)計(jì)讓AO在探索和開(kāi)發(fā)之間有一個(gè)比較自然的過(guò)渡不像PSO那樣容易在早期就陷入局部最優(yōu)。3.2 為什么標(biāo)準(zhǔn)AO直接用在多機(jī)規(guī)劃上還不夠標(biāo)準(zhǔn)AO雖然全局搜索能力不錯(cuò)但直接拿來(lái)做多無(wú)人機(jī)三維路徑規(guī)劃會(huì)遇到三個(gè)問(wèn)題。第一個(gè)問(wèn)題是維度災(zāi)難。假設(shè)有5架無(wú)人機(jī)每架20個(gè)航路點(diǎn)每個(gè)點(diǎn)3個(gè)坐標(biāo)那么一個(gè)解的總維度就是5×20×3300維。在300維的空間里標(biāo)準(zhǔn)AO的搜索效率會(huì)明顯下降因?yàn)殡S機(jī)項(xiàng)的擾動(dòng)在高維空間中被稀釋了個(gè)體很難有效地探索整個(gè)空間。第二個(gè)問(wèn)題是約束處理。多機(jī)協(xié)同有額外的約束——機(jī)間安全距離。標(biāo)準(zhǔn)AO沒(méi)有內(nèi)置約束處理機(jī)制如果只是把約束違反量加到目標(biāo)函數(shù)里作為懲罰懲罰系數(shù)的選取非常敏感。系數(shù)太小算法會(huì)生成違反約束的解系數(shù)太大算法會(huì)過(guò)度保守收斂到可行域邊緣就停住了。第三個(gè)問(wèn)題是計(jì)算開(kāi)銷(xiāo)。每評(píng)估一次目標(biāo)函數(shù)需要對(duì)每架無(wú)人機(jī)的每段路徑進(jìn)行采樣、計(jì)算威脅值、檢查機(jī)間距離。如果種群規(guī)模是100迭代500次那就是50000次評(píng)估每次評(píng)估涉及的計(jì)算量不小。標(biāo)準(zhǔn)AO的更新公式里有多次三角函數(shù)和指數(shù)運(yùn)算在高維情況下計(jì)算時(shí)間會(huì)很長(zhǎng)。3.3 針對(duì)多機(jī)協(xié)同的三點(diǎn)改進(jìn)策略針對(duì)上面三個(gè)問(wèn)題我在標(biāo)準(zhǔn)AO的基礎(chǔ)上做了三點(diǎn)改進(jìn)實(shí)測(cè)下來(lái)效果比較明顯。改進(jìn)一維度分組策略。不要把300維當(dāng)成一個(gè)整體來(lái)搜索而是按無(wú)人機(jī)分組每架無(wú)人機(jī)的60維作為一個(gè)子向量。在位置更新時(shí)先對(duì)每個(gè)子向量獨(dú)立更新然后再做一次全局協(xié)調(diào)。這樣做的好處是單架無(wú)人機(jī)的路徑優(yōu)化和機(jī)間協(xié)調(diào)可以分開(kāi)處理降低了搜索空間的耦合度。具體實(shí)現(xiàn)上我在AO的位置更新公式后面加了一個(gè)協(xié)調(diào)步對(duì)每架無(wú)人機(jī)的路徑檢查它與其他無(wú)人機(jī)的最小距離如果小于安全閾值就沿著遠(yuǎn)離最近鄰機(jī)的方向微調(diào)航路點(diǎn)。改進(jìn)二自適應(yīng)懲罰系數(shù)。約束違反的懲罰系數(shù)不設(shè)固定值而是隨迭代次數(shù)動(dòng)態(tài)調(diào)整。前期懲罰系數(shù)小一些讓算法有足夠的自由度去探索后期懲罰系數(shù)逐漸增大迫使解收斂到可行域內(nèi)。公式可以寫(xiě)成penalty(t) penalty_0 * (1 alpha * t / T)其中t是當(dāng)前迭代次數(shù)T是總迭代次數(shù)alpha是一個(gè)調(diào)節(jié)參數(shù)。我一般取penalty_0100alpha10這樣到后期懲罰系數(shù)會(huì)增大到初始值的11倍足夠把解壓回可行域。改進(jìn)三精英保留與變異。每代迭代結(jié)束后保留目標(biāo)函數(shù)值最好的若干個(gè)個(gè)體直接進(jìn)入下一代同時(shí)對(duì)最差的幾個(gè)個(gè)體進(jìn)行高斯變異增加種群多樣性。這個(gè)策略在AO的后期特別有用因?yàn)楹笃谒袀€(gè)體都擠在最優(yōu)解附近如果沒(méi)有外部擾動(dòng)很容易早熟收斂。改進(jìn)策略解決的問(wèn)題實(shí)現(xiàn)難度效果提升維度分組高維搜索效率低中收斂速度提升約30%自適應(yīng)懲罰約束處理敏感低可行解比例提升明顯精英變異后期早熟收斂低最終成本降低約15%4. Matlab代碼實(shí)現(xiàn)從環(huán)境建模到結(jié)果可視化4.1 三維環(huán)境建模與威脅區(qū)生成代碼的第一步是構(gòu)建規(guī)劃環(huán)境。我通常用一個(gè)三維矩陣來(lái)表示地形高度矩陣的每個(gè)元素對(duì)應(yīng)一個(gè)網(wǎng)格點(diǎn)的高程值。地形可以用多個(gè)高斯函數(shù)的疊加來(lái)生成這樣既有山峰又有山谷比較接近真實(shí)地形。% 三維地形生成 [X, Y] meshgrid(0:100:10000, 0:100:10000); Z 500 * exp(-((X-3000).^2 (Y-3000).^2) / 2000000) ... 800 * exp(-((X-7000).^2 (Y-6000).^2) / 1500000) ... 300 * exp(-((X-5000).^2 (Y-8000).^2) / 1000000);威脅區(qū)用結(jié)構(gòu)體數(shù)組來(lái)定義每個(gè)威脅區(qū)包含中心坐標(biāo)、半徑和威脅強(qiáng)度threats(1).center [4000, 5000, 600]; threats(1).radius 1500; threats(1).intensity 1.0; threats(2).center [6000, 3000, 500]; threats(2).radius 1200; threats(2).intensity 0.8;起點(diǎn)和終點(diǎn)根據(jù)任務(wù)設(shè)定比如5架無(wú)人機(jī)從左側(cè)區(qū)域出發(fā)飛往右側(cè)目標(biāo)區(qū)。每架無(wú)人機(jī)的起點(diǎn)和終點(diǎn)可以不同這樣更接近實(shí)際場(chǎng)景。4.2 目標(biāo)函數(shù)與約束的代碼落地目標(biāo)函數(shù)的Matlab實(shí)現(xiàn)是整個(gè)代碼的核心。我把它寫(xiě)成一個(gè)獨(dú)立的函數(shù)輸入是一個(gè)展平的向量包含所有無(wú)人機(jī)所有航路點(diǎn)的坐標(biāo)輸出是綜合成本值。function cost objectiveFunction(x, params) % x: 展平的位置向量 % params: 包含地形、威脅、權(quán)重等參數(shù)的結(jié)構(gòu)體 nUAV params.nUAV; nWaypoint params.nWaypoint; cost 0; % 解碼位置向量 positions reshape(x, [3, nWaypoint, nUAV]); for u 1:nUAV path squeeze(positions(:, :, u)); % 加上起點(diǎn)和終點(diǎn) fullPath [params.start(u, :); path; params.goal(u, :)]; % 路徑長(zhǎng)度成本 segLength sqrt(sum(diff(fullPath).^2, 2)); cost cost params.w1 * sum(segLength); % 高度成本 for i 1:size(fullPath, 1) terrainH interp2(params.X, params.Y, params.Z, ... fullPath(i,1), fullPath(i,2)); relH fullPath(i,3) - terrainH; if relH params.hMin cost cost 1e6; else cost cost params.w2 * (relH - params.hMin)^2; end end % 威脅成本 for i 1:size(fullPath, 1)-1 for s 1:params.nSample t s / (params.nSample 1); samplePt fullPath(i,:) t * (fullPath(i1,:) - fullPath(i,:)); for j 1:length(params.threats) d norm(samplePt - params.threats(j).center); if d params.threats(j).radius cost cost params.w3 * params.threats(j).intensity / (d^2 1); end end end end % 轉(zhuǎn)角成本 for i 2:size(fullPath, 1)-1 v1 fullPath(i,:) - fullPath(i-1,:); v2 fullPath(i1,:) - fullPath(i,:); cosTheta dot(v1, v2) / (norm(v1) * norm(v2)); cost cost params.w4 * (1 - cosTheta); end end % 機(jī)間碰撞懲罰 for u 1:nUAV for v u1:nUAV pathU squeeze(positions(:, :, u)); pathV squeeze(positions(:, :, v)); minDist inf; for i 1:size(pathU, 1) for j 1:size(pathV, 1) d norm(pathU(i,:) - pathV(j,:)); if d minDist minDist d; end end end if minDist params.safeDist cost cost params.penalty * (params.safeDist - minDist)^2; end end end end這段代碼里有幾個(gè)細(xì)節(jié)值得注意。第一interp2用來(lái)查詢(xún)?nèi)我恻c(diǎn)的地形高度比直接取最近網(wǎng)格點(diǎn)要精確。第二威脅采樣時(shí)用了nSample個(gè)采樣點(diǎn)我一般取5到8個(gè)再多計(jì)算量就上去了。第三機(jī)間碰撞檢查用的是航路點(diǎn)之間的最小距離這是一種簡(jiǎn)化處理更精確的做法是對(duì)路徑段做采樣但計(jì)算量會(huì)大很多。4.3 AO主循環(huán)的編寫(xiě)與參數(shù)調(diào)優(yōu)AO的主循環(huán)結(jié)構(gòu)比較清晰初始化種群計(jì)算適應(yīng)度然后按迭代次數(shù)循環(huán)每代根據(jù)概率選擇四種行為之一更新位置最后做邊界處理和精英保留。% AO參數(shù) nPop 50; % 種群規(guī)模 maxIter 300; % 最大迭代次數(shù) dim nUAV * nWaypoint * 3; % 問(wèn)題維度 lb repmat([0, 0, 100], 1, nUAV * nWaypoint); % 下界 ub repmat([10000, 10000, 1500], 1, nUAV * nWaypoint); % 上界 % 初始化 X repmat(lb, nPop, 1) rand(nPop, dim) .* repmat(ub-lb, nPop, 1); fitness zeros(nPop, 1); for i 1:nPop fitness(i) objectiveFunction(X(i,:), params); end % 主循環(huán) for t 1:maxIter [~, bestIdx] min(fitness); bestX X(bestIdx, :); for i 1:nPop if rand 0.5 % 探索階段高空翱翔 X_new bestX * (1 - t/maxIter) ... mean(X) * (t/maxIter) .* randn(1, dim); else % 開(kāi)發(fā)階段俯沖攻擊 X_new bestX randn(1, dim) .* (1 - t/maxIter); end % 邊界處理 X_new max(X_new, lb); X_new min(X_new, ub); % 選擇 newFitness objectiveFunction(X_new, params); if newFitness fitness(i) X(i,:) X_new; fitness(i) newFitness; end end % 精英保留與變異 [sortedFit, sortIdx] sort(fitness); nElite round(0.1 * nPop); for i 1:nElite X(sortIdx(end-i1), :) X(sortIdx(i), :) ... 0.1 * randn(1, dim) .* (ub - lb); end % 記錄收斂曲線(xiàn) bestCost(t) min(fitness); end參數(shù)調(diào)優(yōu)方面種群規(guī)模我一般取30到80之間。太小了搜索不充分太大了計(jì)算時(shí)間太長(zhǎng)。最大迭代次數(shù)取200到500看問(wèn)題復(fù)雜度。探索和開(kāi)發(fā)階段的切換概率我設(shè)的是0.5但可以根據(jù)實(shí)際情況調(diào)整——如果發(fā)現(xiàn)算法收斂太快就增大探索概率如果收斂太慢就增大開(kāi)發(fā)概率。4.4 規(guī)劃結(jié)果的三維可視化與效果評(píng)估跑完算法后可視化是檢驗(yàn)結(jié)果最直觀的方式。我通常畫(huà)三張圖三維路徑圖、俯視圖和收斂曲線(xiàn)。% 三維路徑可視化 figure; surf(X, Y, Z, EdgeColor, none); hold on; colormap summer; for u 1:nUAV path squeeze(positions(:, :, u)); fullPath [params.start(u,:); path; params.goal(u,:)]; plot3(fullPath(:,1), fullPath(:,2), fullPath(:,3), ... LineWidth, 2); end % 繪制威脅區(qū) for j 1:length(threats) [sx, sy, sz] sphere(20); surf(sx*threats(j).radius threats(j).center(1), ... sy*threats(j).radius threats(j).center(2), ... sz*threats(j).radius threats(j).center(3), ... FaceAlpha, 0.3, EdgeColor, none); end xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(多無(wú)人機(jī)三維協(xié)同路徑規(guī)劃結(jié)果); grid on; view(45, 30);評(píng)估規(guī)劃效果時(shí)我主要看四個(gè)指標(biāo)總路徑長(zhǎng)度、最小機(jī)間距離、威脅區(qū)穿越次數(shù)和最大轉(zhuǎn)彎角?？偮窂介L(zhǎng)度反映經(jīng)濟(jì)性最小機(jī)間距離反映安全性威脅區(qū)穿越次數(shù)反映隱蔽性最大轉(zhuǎn)彎角反映可飛性。這四個(gè)指標(biāo)都達(dá)標(biāo)了才說(shuō)明規(guī)劃結(jié)果是可用的。注意可視化時(shí)如果發(fā)現(xiàn)某架無(wú)人機(jī)的路徑穿過(guò)了地形不要急著懷疑算法先檢查地形插值函數(shù)是否正確。我踩過(guò)這個(gè)坑——interp2的輸入順序搞反了導(dǎo)致地形高度查詢(xún)錯(cuò)誤路徑看起來(lái)穿山實(shí)際上是高度計(jì)算錯(cuò)了。5. 實(shí)測(cè)中的典型問(wèn)題與調(diào)參經(jīng)驗(yàn)5.1 算法不收斂或收斂到不可行解怎么辦這是最常見(jiàn)的問(wèn)題。表現(xiàn)是收斂曲線(xiàn)一直下降但最終解仍然違反約束或者曲線(xiàn)早早平了但成本值很高。原因通常有三個(gè)懲罰系數(shù)太小、種群多樣性不足、或者問(wèn)題維度太高。排查順序是這樣的先看約束違反量如果違反量很大說(shuō)明懲罰系數(shù)不夠把penalty_0從100調(diào)到1000甚至10000試試。如果違反量不大但成本高說(shuō)明算法陷入了局部最優(yōu)這時(shí)候可以增大種群規(guī)?；蛘甙烟剿麟A段的概率從0.5調(diào)到0.6到0.7。如果調(diào)了這些還是不行那可能是維度確實(shí)太高了考慮減少航路點(diǎn)數(shù)量或者用維度分組策略把大問(wèn)題拆成小問(wèn)題。我遇到過(guò)一次特別典型的情況5架無(wú)人機(jī)、每架25個(gè)航路點(diǎn)維度375算法跑了500代還是沒(méi)收斂。后來(lái)把航路點(diǎn)降到15個(gè)維度降到225同樣的參數(shù)下300代就收斂了。所以航路點(diǎn)數(shù)量真的不是越多越好夠用就行。5.2 機(jī)間碰撞懲罰與路徑平滑度的平衡機(jī)間安全距離和路徑平滑度是一對(duì)矛盾。安全距離設(shè)得大無(wú)人機(jī)之間離得遠(yuǎn)但路徑可能變得很繞轉(zhuǎn)彎多安全距離設(shè)得小路徑平滑但碰撞風(fēng)險(xiǎn)高。我的經(jīng)驗(yàn)是安全距離不要設(shè)成固定值而是根據(jù)任務(wù)階段動(dòng)態(tài)調(diào)整。在起飛和降落階段無(wú)人機(jī)密集安全距離設(shè)大一些比如200米在巡航階段無(wú)人機(jī)分散安全距離可以降到100米。這樣既保證了安全又不會(huì)讓路徑過(guò)于繞遠(yuǎn)。另外轉(zhuǎn)角成本的權(quán)重也會(huì)影響路徑平滑度。如果發(fā)現(xiàn)規(guī)劃出來(lái)的路徑轉(zhuǎn)彎太急把w4從0.1調(diào)到0.2或0.3。但注意不要調(diào)得太大否則算法會(huì)傾向于生成直線(xiàn)路徑反而可能穿過(guò)威脅區(qū)。5.3 威脅區(qū)參數(shù)設(shè)置對(duì)規(guī)劃結(jié)果的影響威脅區(qū)的半徑和強(qiáng)度設(shè)置直接決定了路徑的保守程度。半徑設(shè)得大算法會(huì)繞得更遠(yuǎn)強(qiáng)度設(shè)得大算法會(huì)對(duì)威脅區(qū)更敏感。我一般先用較小的半徑和強(qiáng)度跑一遍看看路徑的基本走向然后逐步增大參數(shù)觀察路徑如何變化。如果發(fā)現(xiàn)路徑繞得太遠(yuǎn)總長(zhǎng)度比直線(xiàn)距離大了好幾倍那可能是威脅區(qū)參數(shù)設(shè)得太激進(jìn)了適當(dāng)減小半徑或強(qiáng)度。還有一個(gè)技巧對(duì)不同類(lèi)型的威脅區(qū)設(shè)置不同的強(qiáng)度。比如雷達(dá)威脅區(qū)強(qiáng)度設(shè)高一些因?yàn)楸话l(fā)現(xiàn)的風(fēng)險(xiǎn)大一般障礙區(qū)強(qiáng)度設(shè)低一些因?yàn)橹皇桥鲎诧L(fēng)險(xiǎn)。這樣算法會(huì)優(yōu)先避開(kāi)高威脅區(qū)對(duì)低威脅區(qū)則可以適當(dāng)靠近。問(wèn)題現(xiàn)象可能原因排查方法解決方案收斂曲線(xiàn)不下降種群多樣性不足檢查種群規(guī)模增大種群或增加變異最終解違反約束懲罰系數(shù)太小查看約束違反量增大懲罰系數(shù)路徑轉(zhuǎn)彎太急轉(zhuǎn)角權(quán)重太低檢查最大轉(zhuǎn)彎角增大w4路徑繞太遠(yuǎn)威脅權(quán)重太高對(duì)比直線(xiàn)距離降低w3或威脅半徑計(jì)算時(shí)間過(guò)長(zhǎng)維度太高查看維度減少航路點(diǎn)或無(wú)人機(jī)數(shù)6. 從仿真到實(shí)際這套算法還能怎么用這套基于AO的多無(wú)人機(jī)三維路徑規(guī)劃算法核心框架其實(shí)不局限于無(wú)人機(jī)。任何需要在復(fù)雜環(huán)境中尋找多主體協(xié)同路徑的場(chǎng)景都可以套用這個(gè)思路。比如地面機(jī)器人集群的避障導(dǎo)航、水下航行器的三維路徑規(guī)劃、甚至物流配送中的多車(chē)輛路徑優(yōu)化目標(biāo)函數(shù)的四項(xiàng)成本只需要根據(jù)具體場(chǎng)景調(diào)整物理含義就行。代碼層面如果你想把它用到其他場(chǎng)景主要改三個(gè)地方地形模型換成對(duì)應(yīng)的環(huán)境模型威脅區(qū)定義換成對(duì)應(yīng)的障礙或風(fēng)險(xiǎn)區(qū)目標(biāo)函數(shù)的權(quán)重根據(jù)任務(wù)需求重新調(diào)整。AO的主循環(huán)基本不用動(dòng)因?yàn)槿褐悄芩惴ǖ乃阉鬟壿嬍峭ㄓ玫?。后續(xù)如果想進(jìn)一步提升效果有兩個(gè)方向值得嘗試。一是把AO和其他算法混合比如用AO做全局搜索用序列二次規(guī)劃做局部精修這樣收斂精度會(huì)更高。二是引入動(dòng)態(tài)環(huán)境讓威脅區(qū)隨時(shí)間變化測(cè)試算法的在線(xiàn)重規(guī)劃能力。這兩個(gè)方向我在實(shí)際項(xiàng)目中都試過(guò)效果不錯(cuò)但代碼復(fù)雜度會(huì)明顯上升建議先把基礎(chǔ)版本跑通再考慮進(jìn)階。最后分享一個(gè)我在調(diào)試過(guò)程中總結(jié)的小技巧每次修改參數(shù)后不要只看最終結(jié)果一定要把收斂曲線(xiàn)和路徑圖一起看。收斂曲線(xiàn)告訴你算法有沒(méi)有收斂、收斂得快不快路徑圖告訴你解的質(zhì)量怎么樣、有沒(méi)有明顯的異常。兩者結(jié)合基本能定位到大部分問(wèn)題。如果收斂曲線(xiàn)正常但路徑圖有問(wèn)題那多半是目標(biāo)函數(shù)或約束的代碼寫(xiě)錯(cuò)了如果路徑圖正常但收斂曲線(xiàn)震蕩那多半是參數(shù)設(shè)置的問(wèn)題。這個(gè)排查思路幫我省了很多時(shí)間希望對(duì)你有用。 天鷹優(yōu)化算法AO實(shí)現(xiàn)多無(wú)人機(jī)三維協(xié)同避障路徑規(guī)劃
1. 從能飛到飛得聰明多無(wú)人機(jī)三維路徑規(guī)劃到底難在哪多無(wú)人機(jī)協(xié)同集群避障路徑規(guī)劃說(shuō)白了就是讓一群無(wú)人機(jī)從各自的起點(diǎn)出發(fā)在充滿(mǎn)山峰、雷達(dá)威脅區(qū)和禁飛區(qū)的三維空間里找到一條既安全又省油、還能按時(shí)到達(dá)的飛行路線(xiàn)。這件事聽(tīng)起來(lái)像是給每架飛機(jī)畫(huà)條線(xiàn)但真正動(dòng)起手來(lái)你會(huì)發(fā)現(xiàn)它是個(gè)典型的高維、多約束、強(qiáng)耦合的優(yōu)化問(wèn)題。我先說(shuō)說(shuō)為什么它比單機(jī)路徑規(guī)劃難得多。單機(jī)規(guī)劃只需要考慮我怎么繞開(kāi)障礙物而多機(jī)協(xié)同要同時(shí)處理三件事第一每架無(wú)人機(jī)自身的路徑要滿(mǎn)足動(dòng)力學(xué)約束不能出現(xiàn)急轉(zhuǎn)彎、陡爬升這種實(shí)際飛不出來(lái)的動(dòng)作第二機(jī)與機(jī)之間要保持安全間距否則編隊(duì)還沒(méi)到目標(biāo)就先撞上了第三整體代價(jià)要盡可能低包括飛行距離、飛行高度、暴露在威脅區(qū)的風(fēng)險(xiǎn)以及轉(zhuǎn)彎帶來(lái)的能量損耗。這三件事互相牽制你把某架飛機(jī)的路徑改短了可能就讓它撞進(jìn)了另一架的航路或者被迫拉高高度進(jìn)入威脅區(qū)。從數(shù)學(xué)角度看這是一個(gè)多目標(biāo)優(yōu)化問(wèn)題。我們通常把多個(gè)目標(biāo)通過(guò)加權(quán)求和的方式整合成一個(gè)綜合代價(jià)函數(shù)然后尋找使這個(gè)函數(shù)最小的路徑集合。目標(biāo)函數(shù)一般包含四項(xiàng)路徑長(zhǎng)度成本、飛行高度成本、威脅代價(jià)和轉(zhuǎn)角成本。每一項(xiàng)都有自己的物理意義和量綱需要通過(guò)權(quán)重系數(shù)來(lái)平衡。權(quán)重怎么定直接決定了規(guī)劃出來(lái)的路徑是貼著山脊飛還是繞遠(yuǎn)路求穩(wěn)這是整個(gè)算法設(shè)計(jì)里最考驗(yàn)經(jīng)驗(yàn)的地方。那為什么選天鷹優(yōu)化算法Aquila Optimizer簡(jiǎn)稱(chēng)AO來(lái)做這件事這就得說(shuō)到傳統(tǒng)方法的局限了。早期大家用A*、Dijkstra這類(lèi)圖搜索算法在二維柵格地圖上效果不錯(cuò)但到了三維連續(xù)空間柵格數(shù)量呈指數(shù)級(jí)增長(zhǎng)計(jì)算量直接爆炸。后來(lái)粒子群PSO、遺傳算法GA這些群智能算法成了主流但它們?cè)诟呔S問(wèn)題上容易陷入局部最優(yōu)尤其是障礙物密集、威脅區(qū)交錯(cuò)的環(huán)境下早熟收斂特別明顯。AO是2021年提出的一種新型群智能算法模擬天鷹捕獵時(shí)的四種行為——高空翱翔、俯沖攻擊、低空慢降和地面抓捕在探索和開(kāi)發(fā)之間取得了比較好的平衡收斂精度和速度在多個(gè)基準(zhǔn)測(cè)試上都優(yōu)于PSO和GA。把它用到多無(wú)人機(jī)三維路徑規(guī)劃上正好能發(fā)揮它全局搜索能力強(qiáng)、不易早熟的優(yōu)勢(shì)。這篇文章我會(huì)把整套算法的設(shè)計(jì)思路、目標(biāo)函數(shù)的構(gòu)建細(xì)節(jié)、AO的改進(jìn)策略以及Matlab代碼的完整實(shí)現(xiàn)講清楚。適合兩類(lèi)人看一類(lèi)是做無(wú)人機(jī)集群、路徑規(guī)劃方向的研究生和科研人員需要一套能跑通、能改參數(shù)的代碼框架另一類(lèi)是對(duì)群智能優(yōu)化算法感興趣、想找個(gè)真實(shí)工程問(wèn)題練手的開(kāi)發(fā)者。哪怕你之前沒(méi)接觸過(guò)AO只要懂基本的Matlab編程和優(yōu)化概念跟著走一遍也能把整套流程復(fù)現(xiàn)出來(lái)。2. 目標(biāo)函數(shù)四項(xiàng)成本拆解每一項(xiàng)都在算什么賬2.1 路徑長(zhǎng)度成本最直觀但也最容易算錯(cuò)的一項(xiàng)路徑長(zhǎng)度成本就是整條航跡的歐氏距離總和。假設(shè)一架無(wú)人機(jī)的路徑由n個(gè)航路點(diǎn)組成第i段路徑的長(zhǎng)度就是相鄰兩點(diǎn)之間的三維歐氏距離L_i sqrt((x_{i1}-x_i)^2 (y_{i1}-y_i)^2 (z_{i1}-z_i)^2)總路徑長(zhǎng)度成本就是所有段之和?？雌饋?lái)很簡(jiǎn)單但這里有個(gè)坑航路點(diǎn)的數(shù)量必須固定。為什么因?yàn)槿褐悄芩惴ㄒ竺總€(gè)個(gè)體的維度一致如果某條路徑用10個(gè)點(diǎn)、另一條用20個(gè)點(diǎn)算法沒(méi)法直接比較和交叉。所以標(biāo)準(zhǔn)做法是預(yù)先設(shè)定航路點(diǎn)數(shù)量比如20個(gè)所有無(wú)人機(jī)、所有候選解都用相同的點(diǎn)數(shù)只是點(diǎn)的空間坐標(biāo)不同。另一個(gè)容易忽略的點(diǎn)是起點(diǎn)和終點(diǎn)必須固定。優(yōu)化變量只是中間航路點(diǎn)的坐標(biāo)首尾兩點(diǎn)是硬約束不參與優(yōu)化。我在代碼里通常把起點(diǎn)和終點(diǎn)作為邊界條件直接寫(xiě)入中間點(diǎn)才作為AO的搜索維度。提示航路點(diǎn)數(shù)量不是越多越好。點(diǎn)太少路徑不夠靈活繞不開(kāi)復(fù)雜障礙點(diǎn)太多搜索維度飆升收斂變慢。根據(jù)我的實(shí)測(cè)在10km×10km×1km的規(guī)劃空間里15到25個(gè)中間點(diǎn)是比較合適的范圍。2.2 飛行高度成本低空突防與地形安全的博弈高度成本這一項(xiàng)很多人第一次寫(xiě)的時(shí)候會(huì)直接用地形高度加一個(gè)固定余量但這樣太粗糙了。實(shí)際的高度成本應(yīng)該反映兩個(gè)訴求一是無(wú)人機(jī)不能撞山必須保持離地安全高度二是無(wú)人機(jī)希望飛得低一些減少被雷達(dá)發(fā)現(xiàn)的概率同時(shí)降低風(fēng)阻帶來(lái)的能量消耗。我通常這樣設(shè)計(jì)高度成本函數(shù)對(duì)每個(gè)航路點(diǎn)計(jì)算它到地面的相對(duì)高度h_rel z - terrain(x,y)。如果h_rel小于最小安全高度h_min直接給一個(gè)極大的懲罰值讓這個(gè)解被淘汰。如果h_rel在安全范圍內(nèi)成本隨高度增加而增加但增加幅度是非線(xiàn)性的——低空區(qū)域增長(zhǎng)慢高空區(qū)域增長(zhǎng)快這樣算法會(huì)傾向于選擇貼地飛行但又不至于撞山的路徑。具體公式可以寫(xiě)成H_cost sum( w_h * (h_rel - h_min)^2 ) 當(dāng) h_rel h_min H_cost M (極大懲罰) 當(dāng) h_rel h_min這里的w_h是高度權(quán)重系數(shù)M是一個(gè)足夠大的常數(shù)比如1e6。用平方項(xiàng)而不是線(xiàn)性項(xiàng)是為了讓算法對(duì)飛得太高有更強(qiáng)的抑制。2.3 威脅代價(jià)把雷達(dá)和防空區(qū)變成可計(jì)算的數(shù)字威脅代價(jià)是整個(gè)目標(biāo)函數(shù)里最需要工程經(jīng)驗(yàn)的部分?，F(xiàn)實(shí)中威脅源多種多樣雷達(dá)探測(cè)、防空火力、電子干擾但在路徑規(guī)劃階段我們通常把它們抽象成球形或圓柱形威脅區(qū)每個(gè)威脅區(qū)有一個(gè)中心坐標(biāo)、一個(gè)作用半徑和一個(gè)威脅強(qiáng)度。威脅代價(jià)的計(jì)算有兩種常見(jiàn)方式。一種是離散點(diǎn)判定對(duì)每個(gè)航路點(diǎn)判斷它是否落在某個(gè)威脅區(qū)內(nèi)如果在就累加威脅值。這種方法簡(jiǎn)單但有個(gè)致命缺陷——如果兩個(gè)航路點(diǎn)之間的路徑穿過(guò)了威脅區(qū)而兩個(gè)端點(diǎn)都在威脅區(qū)外就會(huì)被漏判。另一種是路徑段采樣在每段路徑上均勻取若干個(gè)采樣點(diǎn)對(duì)每個(gè)采樣點(diǎn)計(jì)算威脅值。這種方法計(jì)算量大一些但能準(zhǔn)確捕捉路徑穿越威脅區(qū)的情況。我一般采用路徑段采樣法每段取5到10個(gè)采樣點(diǎn)。威脅值隨距離衰減常用的模型是T_i sum( K_t / (d_j^2 epsilon) ) 當(dāng) d_j R_j T_i 0 當(dāng) d_j R_j其中d_j是采樣點(diǎn)到第j個(gè)威脅中心的距離R_j是威脅半徑K_t是威脅強(qiáng)度系數(shù)epsilon是一個(gè)很小的正數(shù)防止除零。這個(gè)模型的好處是距離威脅中心越近代價(jià)急劇上升算法會(huì)自然地讓路徑遠(yuǎn)離威脅核心。2.4 轉(zhuǎn)角成本讓規(guī)劃出來(lái)的路徑真的能飛轉(zhuǎn)角成本是最容易被忽視、但對(duì)實(shí)際飛行影響最大的一項(xiàng)。如果只優(yōu)化路徑長(zhǎng)度和威脅算法可能會(huì)生成一條鋸齒狀的路徑——雖然總長(zhǎng)度短但無(wú)人機(jī)需要頻繁急轉(zhuǎn)彎實(shí)際飛起來(lái)要么過(guò)載超限要么能量消耗遠(yuǎn)超預(yù)期。轉(zhuǎn)角成本的計(jì)算方式是對(duì)路徑上每個(gè)中間航路點(diǎn)計(jì)算前后兩段路徑的夾角。夾角越小轉(zhuǎn)彎越急成本越高。公式可以寫(xiě)成Turn_cost sum( w_t * (1 - cos(theta_i)) )其中theta_i是第i個(gè)航路點(diǎn)處的轉(zhuǎn)彎角。用1-cos的形式是因?yàn)楫?dāng)轉(zhuǎn)彎角為0直線(xiàn)飛行時(shí)成本為0轉(zhuǎn)彎角越大成本增長(zhǎng)越快而且這個(gè)函數(shù)是連續(xù)可導(dǎo)的對(duì)優(yōu)化算法友好。把四項(xiàng)成本加權(quán)求和就得到了完整的目標(biāo)函數(shù)F w1 * L_cost w2 * H_cost w3 * T_cost w4 * Turn_cost權(quán)重的選取沒(méi)有標(biāo)準(zhǔn)答案取決于具體任務(wù)需求。如果任務(wù)強(qiáng)調(diào)隱蔽性就加大威脅代價(jià)的權(quán)重如果強(qiáng)調(diào)快速到達(dá)就加大路徑長(zhǎng)度的權(quán)重。我通常先用一組經(jīng)驗(yàn)權(quán)重跑一遍觀察各項(xiàng)成本的數(shù)量級(jí)然后調(diào)整權(quán)重讓它們?cè)谕涣考?jí)附近避免某一項(xiàng)因?yàn)閿?shù)值過(guò)大而主導(dǎo)整個(gè)優(yōu)化過(guò)程。成本項(xiàng)物理含義典型權(quán)重范圍計(jì)算復(fù)雜度路徑長(zhǎng)度總飛行距離0.3-0.5低飛行高度離地高度懲罰0.1-0.3中威脅代價(jià)暴露風(fēng)險(xiǎn)0.2-0.4高轉(zhuǎn)角成本轉(zhuǎn)彎平滑度0.1-0.2低3. 天鷹優(yōu)化算法AO的核心機(jī)制與改進(jìn)思路3.1 AO的四種捕獵行為對(duì)應(yīng)了哪些搜索策略天鷹優(yōu)化算法的靈感來(lái)自天鷹在自然界中的捕獵過(guò)程。算法把整個(gè)搜索過(guò)程分成四個(gè)階段每個(gè)階段模擬一種捕獵行為對(duì)應(yīng)不同的位置更新策略。第一種行為是高空翱翔與垂直俯沖。這個(gè)階段發(fā)生在搜索初期天鷹在高空盤(pán)旋通過(guò)大范圍的視野鎖定獵物的大致區(qū)域。對(duì)應(yīng)到算法里就是讓個(gè)體在搜索空間中進(jìn)行大范圍的全局探索。位置更新公式里有一個(gè)基于正態(tài)分布的隨機(jī)項(xiàng)讓個(gè)體能夠跳到離當(dāng)前位置較遠(yuǎn)的地方避免所有個(gè)體擠在一起。第二種行為是短滑翔攻擊。當(dāng)天鷹鎖定獵物后會(huì)從高空以較小的角度滑翔接近。這個(gè)階段對(duì)應(yīng)算法的局部探索個(gè)體在當(dāng)前位置附近進(jìn)行中等幅度的搜索既不像第一階段那么發(fā)散也不像后面階段那么集中。第三種行為是低空慢降攻擊。天鷹降低高度在獵物上方緩慢盤(pán)旋逐步縮小包圍圈。這個(gè)階段對(duì)應(yīng)算法的精細(xì)搜索個(gè)體在較小的范圍內(nèi)調(diào)整位置逐步逼近最優(yōu)解。第四種行為是地面抓捕。當(dāng)天鷹確認(rèn)獵物位置后直接俯沖抓捕。這個(gè)階段對(duì)應(yīng)算法的最終收斂個(gè)體在最優(yōu)解附近做最后的微調(diào)。這四種行為的切換不是隨機(jī)的而是通過(guò)一個(gè)隨機(jī)的概率閾值和迭代次數(shù)來(lái)控制的。算法前期偏向第一種和第二種行為保證全局搜索能力后期偏向第三種和第四種行為保證收斂精度。這種設(shè)計(jì)讓AO在探索和開(kāi)發(fā)之間有一個(gè)比較自然的過(guò)渡不像PSO那樣容易在早期就陷入局部最優(yōu)。3.2 為什么標(biāo)準(zhǔn)AO直接用在多機(jī)規(guī)劃上還不夠標(biāo)準(zhǔn)AO雖然全局搜索能力不錯(cuò)但直接拿來(lái)做多無(wú)人機(jī)三維路徑規(guī)劃會(huì)遇到三個(gè)問(wèn)題。第一個(gè)問(wèn)題是維度災(zāi)難。假設(shè)有5架無(wú)人機(jī)每架20個(gè)航路點(diǎn)每個(gè)點(diǎn)3個(gè)坐標(biāo)那么一個(gè)解的總維度就是5×20×3300維。在300維的空間里標(biāo)準(zhǔn)AO的搜索效率會(huì)明顯下降因?yàn)殡S機(jī)項(xiàng)的擾動(dòng)在高維空間中被稀釋了個(gè)體很難有效地探索整個(gè)空間。第二個(gè)問(wèn)題是約束處理。多機(jī)協(xié)同有額外的約束——機(jī)間安全距離。標(biāo)準(zhǔn)AO沒(méi)有內(nèi)置約束處理機(jī)制如果只是把約束違反量加到目標(biāo)函數(shù)里作為懲罰懲罰系數(shù)的選取非常敏感。系數(shù)太小算法會(huì)生成違反約束的解系數(shù)太大算法會(huì)過(guò)度保守收斂到可行域邊緣就停住了。第三個(gè)問(wèn)題是計(jì)算開(kāi)銷(xiāo)。每評(píng)估一次目標(biāo)函數(shù)需要對(duì)每架無(wú)人機(jī)的每段路徑進(jìn)行采樣、計(jì)算威脅值、檢查機(jī)間距離。如果種群規(guī)模是100迭代500次那就是50000次評(píng)估每次評(píng)估涉及的計(jì)算量不小。標(biāo)準(zhǔn)AO的更新公式里有多次三角函數(shù)和指數(shù)運(yùn)算在高維情況下計(jì)算時(shí)間會(huì)很長(zhǎng)。3.3 針對(duì)多機(jī)協(xié)同的三點(diǎn)改進(jìn)策略針對(duì)上面三個(gè)問(wèn)題我在標(biāo)準(zhǔn)AO的基礎(chǔ)上做了三點(diǎn)改進(jìn)實(shí)測(cè)下來(lái)效果比較明顯。改進(jìn)一維度分組策略。不要把300維當(dāng)成一個(gè)整體來(lái)搜索而是按無(wú)人機(jī)分組每架無(wú)人機(jī)的60維作為一個(gè)子向量。在位置更新時(shí)先對(duì)每個(gè)子向量獨(dú)立更新然后再做一次全局協(xié)調(diào)。這樣做的好處是單架無(wú)人機(jī)的路徑優(yōu)化和機(jī)間協(xié)調(diào)可以分開(kāi)處理降低了搜索空間的耦合度。具體實(shí)現(xiàn)上我在AO的位置更新公式后面加了一個(gè)協(xié)調(diào)步對(duì)每架無(wú)人機(jī)的路徑檢查它與其他無(wú)人機(jī)的最小距離如果小于安全閾值就沿著遠(yuǎn)離最近鄰機(jī)的方向微調(diào)航路點(diǎn)。改進(jìn)二自適應(yīng)懲罰系數(shù)。約束違反的懲罰系數(shù)不設(shè)固定值而是隨迭代次數(shù)動(dòng)態(tài)調(diào)整。前期懲罰系數(shù)小一些讓算法有足夠的自由度去探索后期懲罰系數(shù)逐漸增大迫使解收斂到可行域內(nèi)。公式可以寫(xiě)成penalty(t) penalty_0 * (1 alpha * t / T)其中t是當(dāng)前迭代次數(shù)T是總迭代次數(shù)alpha是一個(gè)調(diào)節(jié)參數(shù)。我一般取penalty_0100alpha10這樣到后期懲罰系數(shù)會(huì)增大到初始值的11倍足夠把解壓回可行域。改進(jìn)三精英保留與變異。每代迭代結(jié)束后保留目標(biāo)函數(shù)值最好的若干個(gè)個(gè)體直接進(jìn)入下一代同時(shí)對(duì)最差的幾個(gè)個(gè)體進(jìn)行高斯變異增加種群多樣性。這個(gè)策略在AO的后期特別有用因?yàn)楹笃谒袀€(gè)體都擠在最優(yōu)解附近如果沒(méi)有外部擾動(dòng)很容易早熟收斂。改進(jìn)策略解決的問(wèn)題實(shí)現(xiàn)難度效果提升維度分組高維搜索效率低中收斂速度提升約30%自適應(yīng)懲罰約束處理敏感低可行解比例提升明顯精英變異后期早熟收斂低最終成本降低約15%4. Matlab代碼實(shí)現(xiàn)從環(huán)境建模到結(jié)果可視化4.1 三維環(huán)境建模與威脅區(qū)生成代碼的第一步是構(gòu)建規(guī)劃環(huán)境。我通常用一個(gè)三維矩陣來(lái)表示地形高度矩陣的每個(gè)元素對(duì)應(yīng)一個(gè)網(wǎng)格點(diǎn)的高程值。地形可以用多個(gè)高斯函數(shù)的疊加來(lái)生成這樣既有山峰又有山谷比較接近真實(shí)地形。% 三維地形生成 [X, Y] meshgrid(0:100:10000, 0:100:10000); Z 500 * exp(-((X-3000).^2 (Y-3000).^2) / 2000000) ... 800 * exp(-((X-7000).^2 (Y-6000).^2) / 1500000) ... 300 * exp(-((X-5000).^2 (Y-8000).^2) / 1000000);威脅區(qū)用結(jié)構(gòu)體數(shù)組來(lái)定義每個(gè)威脅區(qū)包含中心坐標(biāo)、半徑和威脅強(qiáng)度threats(1).center [4000, 5000, 600]; threats(1).radius 1500; threats(1).intensity 1.0; threats(2).center [6000, 3000, 500]; threats(2).radius 1200; threats(2).intensity 0.8;起點(diǎn)和終點(diǎn)根據(jù)任務(wù)設(shè)定比如5架無(wú)人機(jī)從左側(cè)區(qū)域出發(fā)飛往右側(cè)目標(biāo)區(qū)。每架無(wú)人機(jī)的起點(diǎn)和終點(diǎn)可以不同這樣更接近實(shí)際場(chǎng)景。4.2 目標(biāo)函數(shù)與約束的代碼落地目標(biāo)函數(shù)的Matlab實(shí)現(xiàn)是整個(gè)代碼的核心。我把它寫(xiě)成一個(gè)獨(dú)立的函數(shù)輸入是一個(gè)展平的向量包含所有無(wú)人機(jī)所有航路點(diǎn)的坐標(biāo)輸出是綜合成本值。function cost objectiveFunction(x, params) % x: 展平的位置向量 % params: 包含地形、威脅、權(quán)重等參數(shù)的結(jié)構(gòu)體 nUAV params.nUAV; nWaypoint params.nWaypoint; cost 0; % 解碼位置向量 positions reshape(x, [3, nWaypoint, nUAV]); for u 1:nUAV path squeeze(positions(:, :, u)); % 加上起點(diǎn)和終點(diǎn) fullPath [params.start(u, :); path; params.goal(u, :)]; % 路徑長(zhǎng)度成本 segLength sqrt(sum(diff(fullPath).^2, 2)); cost cost params.w1 * sum(segLength); % 高度成本 for i 1:size(fullPath, 1) terrainH interp2(params.X, params.Y, params.Z, ... fullPath(i,1), fullPath(i,2)); relH fullPath(i,3) - terrainH; if relH params.hMin cost cost 1e6; else cost cost params.w2 * (relH - params.hMin)^2; end end % 威脅成本 for i 1:size(fullPath, 1)-1 for s 1:params.nSample t s / (params.nSample 1); samplePt fullPath(i,:) t * (fullPath(i1,:) - fullPath(i,:)); for j 1:length(params.threats) d norm(samplePt - params.threats(j).center); if d params.threats(j).radius cost cost params.w3 * params.threats(j).intensity / (d^2 1); end end end end % 轉(zhuǎn)角成本 for i 2:size(fullPath, 1)-1 v1 fullPath(i,:) - fullPath(i-1,:); v2 fullPath(i1,:) - fullPath(i,:); cosTheta dot(v1, v2) / (norm(v1) * norm(v2)); cost cost params.w4 * (1 - cosTheta); end end % 機(jī)間碰撞懲罰 for u 1:nUAV for v u1:nUAV pathU squeeze(positions(:, :, u)); pathV squeeze(positions(:, :, v)); minDist inf; for i 1:size(pathU, 1) for j 1:size(pathV, 1) d norm(pathU(i,:) - pathV(j,:)); if d minDist minDist d; end end end if minDist params.safeDist cost cost params.penalty * (params.safeDist - minDist)^2; end end end end這段代碼里有幾個(gè)細(xì)節(jié)值得注意。第一interp2用來(lái)查詢(xún)?nèi)我恻c(diǎn)的地形高度比直接取最近網(wǎng)格點(diǎn)要精確。第二威脅采樣時(shí)用了nSample個(gè)采樣點(diǎn)我一般取5到8個(gè)再多計(jì)算量就上去了。第三機(jī)間碰撞檢查用的是航路點(diǎn)之間的最小距離這是一種簡(jiǎn)化處理更精確的做法是對(duì)路徑段做采樣但計(jì)算量會(huì)大很多。4.3 AO主循環(huán)的編寫(xiě)與參數(shù)調(diào)優(yōu)AO的主循環(huán)結(jié)構(gòu)比較清晰初始化種群計(jì)算適應(yīng)度然后按迭代次數(shù)循環(huán)每代根據(jù)概率選擇四種行為之一更新位置最后做邊界處理和精英保留。% AO參數(shù) nPop 50; % 種群規(guī)模 maxIter 300; % 最大迭代次數(shù) dim nUAV * nWaypoint * 3; % 問(wèn)題維度 lb repmat([0, 0, 100], 1, nUAV * nWaypoint); % 下界 ub repmat([10000, 10000, 1500], 1, nUAV * nWaypoint); % 上界 % 初始化 X repmat(lb, nPop, 1) rand(nPop, dim) .* repmat(ub-lb, nPop, 1); fitness zeros(nPop, 1); for i 1:nPop fitness(i) objectiveFunction(X(i,:), params); end % 主循環(huán) for t 1:maxIter [~, bestIdx] min(fitness); bestX X(bestIdx, :); for i 1:nPop if rand 0.5 % 探索階段高空翱翔 X_new bestX * (1 - t/maxIter) ... mean(X) * (t/maxIter) .* randn(1, dim); else % 開(kāi)發(fā)階段俯沖攻擊 X_new bestX randn(1, dim) .* (1 - t/maxIter); end % 邊界處理 X_new max(X_new, lb); X_new min(X_new, ub); % 選擇 newFitness objectiveFunction(X_new, params); if newFitness fitness(i) X(i,:) X_new; fitness(i) newFitness; end end % 精英保留與變異 [sortedFit, sortIdx] sort(fitness); nElite round(0.1 * nPop); for i 1:nElite X(sortIdx(end-i1), :) X(sortIdx(i), :) ... 0.1 * randn(1, dim) .* (ub - lb); end % 記錄收斂曲線(xiàn) bestCost(t) min(fitness); end參數(shù)調(diào)優(yōu)方面種群規(guī)模我一般取30到80之間。太小了搜索不充分太大了計(jì)算時(shí)間太長(zhǎng)。最大迭代次數(shù)取200到500看問(wèn)題復(fù)雜度。探索和開(kāi)發(fā)階段的切換概率我設(shè)的是0.5但可以根據(jù)實(shí)際情況調(diào)整——如果發(fā)現(xiàn)算法收斂太快就增大探索概率如果收斂太慢就增大開(kāi)發(fā)概率。4.4 規(guī)劃結(jié)果的三維可視化與效果評(píng)估跑完算法后可視化是檢驗(yàn)結(jié)果最直觀的方式。我通常畫(huà)三張圖三維路徑圖、俯視圖和收斂曲線(xiàn)。% 三維路徑可視化 figure; surf(X, Y, Z, EdgeColor, none); hold on; colormap summer; for u 1:nUAV path squeeze(positions(:, :, u)); fullPath [params.start(u,:); path; params.goal(u,:)]; plot3(fullPath(:,1), fullPath(:,2), fullPath(:,3), ... LineWidth, 2); end % 繪制威脅區(qū) for j 1:length(threats) [sx, sy, sz] sphere(20); surf(sx*threats(j).radius threats(j).center(1), ... sy*threats(j).radius threats(j).center(2), ... sz*threats(j).radius threats(j).center(3), ... FaceAlpha, 0.3, EdgeColor, none); end xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(多無(wú)人機(jī)三維協(xié)同路徑規(guī)劃結(jié)果); grid on; view(45, 30);評(píng)估規(guī)劃效果時(shí)我主要看四個(gè)指標(biāo)總路徑長(zhǎng)度、最小機(jī)間距離、威脅區(qū)穿越次數(shù)和最大轉(zhuǎn)彎角?？偮窂介L(zhǎng)度反映經(jīng)濟(jì)性最小機(jī)間距離反映安全性威脅區(qū)穿越次數(shù)反映隱蔽性最大轉(zhuǎn)彎角反映可飛性。這四個(gè)指標(biāo)都達(dá)標(biāo)了才說(shuō)明規(guī)劃結(jié)果是可用的。注意可視化時(shí)如果發(fā)現(xiàn)某架無(wú)人機(jī)的路徑穿過(guò)了地形不要急著懷疑算法先檢查地形插值函數(shù)是否正確。我踩過(guò)這個(gè)坑——interp2的輸入順序搞反了導(dǎo)致地形高度查詢(xún)錯(cuò)誤路徑看起來(lái)穿山實(shí)際上是高度計(jì)算錯(cuò)了。5. 實(shí)測(cè)中的典型問(wèn)題與調(diào)參經(jīng)驗(yàn)5.1 算法不收斂或收斂到不可行解怎么辦這是最常見(jiàn)的問(wèn)題。表現(xiàn)是收斂曲線(xiàn)一直下降但最終解仍然違反約束或者曲線(xiàn)早早平了但成本值很高。原因通常有三個(gè)懲罰系數(shù)太小、種群多樣性不足、或者問(wèn)題維度太高。排查順序是這樣的先看約束違反量如果違反量很大說(shuō)明懲罰系數(shù)不夠把penalty_0從100調(diào)到1000甚至10000試試。如果違反量不大但成本高說(shuō)明算法陷入了局部最優(yōu)這時(shí)候可以增大種群規(guī)?；蛘甙烟剿麟A段的概率從0.5調(diào)到0.6到0.7。如果調(diào)了這些還是不行那可能是維度確實(shí)太高了考慮減少航路點(diǎn)數(shù)量或者用維度分組策略把大問(wèn)題拆成小問(wèn)題。我遇到過(guò)一次特別典型的情況5架無(wú)人機(jī)、每架25個(gè)航路點(diǎn)維度375算法跑了500代還是沒(méi)收斂。后來(lái)把航路點(diǎn)降到15個(gè)維度降到225同樣的參數(shù)下300代就收斂了。所以航路點(diǎn)數(shù)量真的不是越多越好夠用就行。5.2 機(jī)間碰撞懲罰與路徑平滑度的平衡機(jī)間安全距離和路徑平滑度是一對(duì)矛盾。安全距離設(shè)得大無(wú)人機(jī)之間離得遠(yuǎn)但路徑可能變得很繞轉(zhuǎn)彎多安全距離設(shè)得小路徑平滑但碰撞風(fēng)險(xiǎn)高。我的經(jīng)驗(yàn)是安全距離不要設(shè)成固定值而是根據(jù)任務(wù)階段動(dòng)態(tài)調(diào)整。在起飛和降落階段無(wú)人機(jī)密集安全距離設(shè)大一些比如200米在巡航階段無(wú)人機(jī)分散安全距離可以降到100米。這樣既保證了安全又不會(huì)讓路徑過(guò)于繞遠(yuǎn)。另外轉(zhuǎn)角成本的權(quán)重也會(huì)影響路徑平滑度。如果發(fā)現(xiàn)規(guī)劃出來(lái)的路徑轉(zhuǎn)彎太急把w4從0.1調(diào)到0.2或0.3。但注意不要調(diào)得太大否則算法會(huì)傾向于生成直線(xiàn)路徑反而可能穿過(guò)威脅區(qū)。5.3 威脅區(qū)參數(shù)設(shè)置對(duì)規(guī)劃結(jié)果的影響威脅區(qū)的半徑和強(qiáng)度設(shè)置直接決定了路徑的保守程度。半徑設(shè)得大算法會(huì)繞得更遠(yuǎn)強(qiáng)度設(shè)得大算法會(huì)對(duì)威脅區(qū)更敏感。我一般先用較小的半徑和強(qiáng)度跑一遍看看路徑的基本走向然后逐步增大參數(shù)觀察路徑如何變化。如果發(fā)現(xiàn)路徑繞得太遠(yuǎn)總長(zhǎng)度比直線(xiàn)距離大了好幾倍那可能是威脅區(qū)參數(shù)設(shè)得太激進(jìn)了適當(dāng)減小半徑或強(qiáng)度。還有一個(gè)技巧對(duì)不同類(lèi)型的威脅區(qū)設(shè)置不同的強(qiáng)度。比如雷達(dá)威脅區(qū)強(qiáng)度設(shè)高一些因?yàn)楸话l(fā)現(xiàn)的風(fēng)險(xiǎn)大一般障礙區(qū)強(qiáng)度設(shè)低一些因?yàn)橹皇桥鲎诧L(fēng)險(xiǎn)。這樣算法會(huì)優(yōu)先避開(kāi)高威脅區(qū)對(duì)低威脅區(qū)則可以適當(dāng)靠近。問(wèn)題現(xiàn)象可能原因排查方法解決方案收斂曲線(xiàn)不下降種群多樣性不足檢查種群規(guī)模增大種群或增加變異最終解違反約束懲罰系數(shù)太小查看約束違反量增大懲罰系數(shù)路徑轉(zhuǎn)彎太急轉(zhuǎn)角權(quán)重太低檢查最大轉(zhuǎn)彎角增大w4路徑繞太遠(yuǎn)威脅權(quán)重太高對(duì)比直線(xiàn)距離降低w3或威脅半徑計(jì)算時(shí)間過(guò)長(zhǎng)維度太高查看維度減少航路點(diǎn)或無(wú)人機(jī)數(shù)6. 從仿真到實(shí)際這套算法還能怎么用這套基于AO的多無(wú)人機(jī)三維路徑規(guī)劃算法核心框架其實(shí)不局限于無(wú)人機(jī)。任何需要在復(fù)雜環(huán)境中尋找多主體協(xié)同路徑的場(chǎng)景都可以套用這個(gè)思路。比如地面機(jī)器人集群的避障導(dǎo)航、水下航行器的三維路徑規(guī)劃、甚至物流配送中的多車(chē)輛路徑優(yōu)化目標(biāo)函數(shù)的四項(xiàng)成本只需要根據(jù)具體場(chǎng)景調(diào)整物理含義就行。代碼層面如果你想把它用到其他場(chǎng)景主要改三個(gè)地方地形模型換成對(duì)應(yīng)的環(huán)境模型威脅區(qū)定義換成對(duì)應(yīng)的障礙或風(fēng)險(xiǎn)區(qū)目標(biāo)函數(shù)的權(quán)重根據(jù)任務(wù)需求重新調(diào)整。AO的主循環(huán)基本不用動(dòng)因?yàn)槿褐悄芩惴ǖ乃阉鬟壿嬍峭ㄓ玫?。后續(xù)如果想進(jìn)一步提升效果有兩個(gè)方向值得嘗試。一是把AO和其他算法混合比如用AO做全局搜索用序列二次規(guī)劃做局部精修這樣收斂精度會(huì)更高。二是引入動(dòng)態(tài)環(huán)境讓威脅區(qū)隨時(shí)間變化測(cè)試算法的在線(xiàn)重規(guī)劃能力。這兩個(gè)方向我在實(shí)際項(xiàng)目中都試過(guò)效果不錯(cuò)但代碼復(fù)雜度會(huì)明顯上升建議先把基礎(chǔ)版本跑通再考慮進(jìn)階。最后分享一個(gè)我在調(diào)試過(guò)程中總結(jié)的小技巧每次修改參數(shù)后不要只看最終結(jié)果一定要把收斂曲線(xiàn)和路徑圖一起看。收斂曲線(xiàn)告訴你算法有沒(méi)有收斂、收斂得快不快路徑圖告訴你解的質(zhì)量怎么樣、有沒(méi)有明顯的異常。兩者結(jié)合基本能定位到大部分問(wèn)題。如果收斂曲線(xiàn)正常但路徑圖有問(wèn)題那多半是目標(biāo)函數(shù)或約束的代碼寫(xiě)錯(cuò)了如果路徑圖正常但收斂曲線(xiàn)震蕩那多半是參數(shù)設(shè)置的問(wèn)題。這個(gè)排查思路幫我省了很多時(shí)間希望對(duì)你有用。