這個(gè)問題我在仿真平臺(tái)上來來回回折騰了快兩周時(shí)間期間踩過不少坑也總結(jié)出一些可以直接用的經(jīng)驗(yàn)。今天把這套“動(dòng)態(tài)環(huán)境下多無人機(jī)系統(tǒng)的協(xié)同路徑規(guī)劃與防撞”的Matlab仿真方案完整拆給你看包含思路、算法選型、代碼結(jié)構(gòu)和調(diào)參技巧希望對(duì)正在做相關(guān)課題或者入門多機(jī)協(xié)同的朋友有幫助。開頭先交代一下這個(gè)項(xiàng)目做了什么它要在三維空間里同時(shí)控制多架無人機(jī)讓它們從不同起點(diǎn)飛到各自目標(biāo)點(diǎn)過程中既要繞開靜態(tài)障礙物還要應(yīng)對(duì)動(dòng)態(tài)障礙物——包括其他無人機(jī)和隨機(jī)出現(xiàn)的移動(dòng)威脅同時(shí)保證機(jī)間不發(fā)生碰撞。整套邏輯基于Matlab實(shí)現(xiàn)核心包含全局路徑規(guī)劃、局部防撞、多機(jī)協(xié)同優(yōu)先級(jí)調(diào)度以及動(dòng)態(tài)環(huán)境建模。適合正在做無人機(jī)編隊(duì)、機(jī)器人路徑規(guī)劃、多智能體協(xié)同控制相關(guān)課題的研究生和工程師參考。1. 問題拆解與整體方案設(shè)計(jì)1.1 為什么多機(jī)協(xié)同路徑規(guī)劃比單機(jī)難那么多單機(jī)路徑規(guī)劃本質(zhì)上是“已知起點(diǎn)和目標(biāo)在約束下找一條無碰路徑”成熟的算法非常多A*、RRT、人工勢(shì)場(chǎng)都能解決。但一旦升級(jí)到多無人機(jī)系統(tǒng)問題的性質(zhì)就變了每架無人機(jī)的“最優(yōu)”航線可能相互沖突。舉個(gè)最簡(jiǎn)單的例子兩架無人機(jī)同時(shí)穿過同一片狹窄空域單獨(dú)規(guī)劃出來的最短路徑可能在中點(diǎn)交匯如果不協(xié)商它們就會(huì)在交匯處撞上。更麻煩的是動(dòng)態(tài)環(huán)境下這個(gè)問題變成了一個(gè)隨時(shí)間變化的博弈問題。無人機(jī)A每走一步無人機(jī)B的可行空間都在變反過來A的決策也壓縮了B的選擇。經(jīng)典的“先規(guī)劃后偏置”思路在低密度場(chǎng)景下夠用但稍微復(fù)雜點(diǎn)就會(huì)出現(xiàn)死鎖兩架無人機(jī)在通道內(nèi)迎面相遇你往左擠我往右躲誰都沒法通過。振蕩A避讓B的同時(shí)B避讓A導(dǎo)致兩者路徑持續(xù)小幅度擺動(dòng)永遠(yuǎn)無法收斂。資源競(jìng)爭(zhēng)多架無人機(jī)搶同一段安全走廊后面準(zhǔn)備的路徑被迫反復(fù)重規(guī)劃計(jì)算量爆炸。所以多機(jī)協(xié)同路徑規(guī)劃的核心并不是“把單機(jī)算法跑多遍”而是要解決機(jī)間的沖突消解和動(dòng)態(tài)環(huán)境的實(shí)時(shí)響應(yīng)。1.2 我最終選定的技術(shù)路線分層規(guī)劃加反應(yīng)式防撞我做的方案沒有追求一個(gè)萬能的規(guī)劃器一站到底而是走了工程上更穩(wěn)妥的分層路線全局規(guī)劃層基于靜態(tài)地圖信息為每架無人機(jī)離線規(guī)劃一條引導(dǎo)路徑采用改進(jìn)的A*算法在代價(jià)函數(shù)里加入了轉(zhuǎn)向代價(jià)和平滑度約束。局部防撞層無人機(jī)沿全局路徑飛行的同時(shí)實(shí)時(shí)感知周圍環(huán)境用人工勢(shì)場(chǎng)法做反應(yīng)式避障應(yīng)對(duì)規(guī)劃階段沒建模的動(dòng)態(tài)障礙物。協(xié)同調(diào)度層通過通信拓?fù)涔蚕砀鳈C(jī)狀態(tài)和飛行計(jì)劃用優(yōu)先級(jí)加空域分層的方式解決機(jī)間沖突。三層結(jié)構(gòu)各司其職全局層保證“大方向正確”局部層保證“隨時(shí)能躲開突發(fā)威脅”協(xié)同層保證“多機(jī)在一起不打架”。這套結(jié)構(gòu)的好處是每一層都能獨(dú)立測(cè)試單獨(dú)調(diào)參不容易一出問題就推倒重來。方案選型上我對(duì)比過幾類思路這里把結(jié)論也分享一下方案優(yōu)勢(shì)劣勢(shì)結(jié)論純集中式規(guī)劃全局最優(yōu)性強(qiáng)計(jì)算量隨無人機(jī)數(shù)量指數(shù)增長(zhǎng)實(shí)時(shí)性差不適合動(dòng)態(tài)環(huán)境分布式一致性控制可擴(kuò)展性好需要嚴(yán)格的通信同步假設(shè)可以作為研究選題不適合快速出效果全局引導(dǎo) 加 本地防撞本方案兼顧全局與實(shí)時(shí)性實(shí)現(xiàn)難度適中參數(shù)好調(diào)全局路徑不是嚴(yán)格最優(yōu)工程實(shí)現(xiàn)首選最終主程序流程拆成五個(gè)模塊環(huán)境生成器、無人機(jī)動(dòng)力學(xué)模型、全局路徑規(guī)劃器、局部防撞控制器、協(xié)同調(diào)度模塊。下面逐一展開。2. 動(dòng)態(tài)環(huán)境建模與核心算法解析2.1 動(dòng)態(tài)環(huán)境到底怎么建仿真的環(huán)境我用的是三維柵格地圖尺寸設(shè)置為200m x 200m x 50m柵格分辨率1m。地圖分為兩層靜態(tài)層隨機(jī)生成若干個(gè)長(zhǎng)方體塊模擬建筑物、雷達(dá)站等固定障礙物。這部分用矩陣直接存儲(chǔ)占用信息供全局規(guī)劃器預(yù)加載。動(dòng)態(tài)層加入兩個(gè)類型的動(dòng)態(tài)威脅。第一類是規(guī)劃中的其他無人機(jī)它們?cè)诿恳粠挛恢孟喈?dāng)于移動(dòng)礙障第二類是在環(huán)境中預(yù)設(shè)的一組“巡邏障礙物”按某種軌跡運(yùn)動(dòng)模擬非合作的入侵目標(biāo)。動(dòng)態(tài)障礙物的軌跡建模上我做了一個(gè)小設(shè)計(jì)讓每個(gè)動(dòng)態(tài)障礙物具備“速度矢量”屬性并且速度矢量在每一小段時(shí)間內(nèi)做隨機(jī)擾動(dòng)這樣比單純的勻速直線運(yùn)動(dòng)更接近真實(shí)場(chǎng)景。實(shí)驗(yàn)時(shí)你可以控制擾動(dòng)幅度用來模擬飛行器受氣流影響時(shí)的位置漂移。這里有一個(gè)很關(guān)鍵的建模選擇無人機(jī)自身的位置模型。我在仿真里用的是簡(jiǎn)化二階模型狀態(tài)包括位置向量、速度向量和加速度指令。因?yàn)檫@次重點(diǎn)驗(yàn)證的是規(guī)劃與防撞邏輯不是飛控氣動(dòng)細(xì)節(jié)二階模型在保真度和計(jì)算效率之間平衡得比較好。如果你的課題想精細(xì)到動(dòng)力學(xué)層面可以在狀態(tài)量里加上姿態(tài)角但那就需要配一個(gè)內(nèi)環(huán)控制器了。2.2 全局路徑規(guī)劃改進(jìn)A*的代價(jià)函數(shù)全局層用的A本身沒什么神秘關(guān)鍵在于代價(jià)函數(shù)的設(shè)計(jì)。標(biāo)準(zhǔn)的A只考慮路徑長(zhǎng)度但無人機(jī)航跡如果不平滑后續(xù)局部層會(huì)非常難處理——頻繁切換避障方向和速度指令軌跡會(huì)出現(xiàn)鋸齒狀。我在代價(jià)函數(shù)里加了兩個(gè)量function cost pathCost(node, parent, goal) % 基礎(chǔ)距離代價(jià) distanceCost norm(node.pos - parent.pos); % 轉(zhuǎn)向代價(jià)角度變化懲罰 angle computeTurnAngle(parent.pos, node.pos, parent.vel); angleCost wAngle * angle^2; % 安全代價(jià)離靜態(tài)障礙物越近代價(jià)越高 obstacleCost wObs * max(0, safeDist - minDistToObstacle(node.pos)); cost distanceCost angleCost obstacleCost; end三個(gè)權(quán)重的初始取值分別是wAngle 0.3 wObs 0.5 safeDist 3后面調(diào)參階段你會(huì)發(fā)現(xiàn)這兩個(gè)系數(shù)非常影響路徑風(fēng)格。轉(zhuǎn)向代價(jià)能讓路徑早早就偏向“少拐彎”的方向而安全代價(jià)避免全局路徑貼著障礙物邊沿走給局部防撞留出充足的反應(yīng)空間。還有一個(gè)實(shí)操細(xì)節(jié)值得說全局路徑不需要每幀重算。我設(shè)置了重規(guī)劃觸發(fā)機(jī)制只有當(dāng)檢測(cè)到動(dòng)態(tài)障礙物侵入全局路徑的安全走廊時(shí)才在局部地圖中標(biāo)記該位置并重新調(diào)用A*。實(shí)驗(yàn)測(cè)下來這種“按需重規(guī)劃”相比逐幀規(guī)劃能省下大約60%的計(jì)算負(fù)載。2.3 局部防撞人工勢(shì)場(chǎng)法加速度障礙法人工勢(shì)場(chǎng)法大家應(yīng)該不陌生目標(biāo)點(diǎn)產(chǎn)生引力障礙物產(chǎn)生斥力合力決定無人機(jī)的運(yùn)動(dòng)方向。它的優(yōu)點(diǎn)是反應(yīng)快、實(shí)現(xiàn)簡(jiǎn)單但有兩個(gè)致命缺陷容易陷入局部極小點(diǎn)以及在高速運(yùn)動(dòng)場(chǎng)景下只按距離避障不按速度避障容易和動(dòng)態(tài)障礙物迎面撞上。我解決這兩個(gè)問題的思路是分層處理對(duì)于靜態(tài)障礙物用傳統(tǒng)勢(shì)場(chǎng)法做位置摩擦保持全局路徑規(guī)劃的連續(xù)性。對(duì)于動(dòng)態(tài)障礙物含其他無人機(jī)在勢(shì)場(chǎng)基礎(chǔ)上引進(jìn)了速度障礙法思想不只判斷“你離我遠(yuǎn)不遠(yuǎn)”還要計(jì)算“按當(dāng)前相對(duì)速度我們會(huì)不會(huì)在未來幾秒內(nèi)接近到危險(xiǎn)距離”。具體做法是這樣的對(duì)其他無人機(jī)和動(dòng)態(tài)威脅取一個(gè)預(yù)測(cè)時(shí)間窗tau 3s計(jì)算無人機(jī)i在當(dāng)前速度向量下推算出的未來位置以及它和障礙物j的相對(duì)速度。如果相對(duì)位移和相對(duì)速度的交點(diǎn)在威脅半徑內(nèi)則判定存在潛在碰撞此時(shí)觸發(fā)速度調(diào)整在速度空間中尋找一個(gè)離原期望速度最近、但不會(huì)落入碰撞區(qū)域的新速度。這套混合策略比單用勢(shì)場(chǎng)法可靠得多。最直觀的表現(xiàn)是動(dòng)態(tài)障礙物從側(cè)面高速接近時(shí)純勢(shì)場(chǎng)法往往來不及反應(yīng)因?yàn)槌饬Ξa(chǎn)生得太晚而速度障礙法在數(shù)百毫秒前就能預(yù)判軌跡交叉提前減速或轉(zhuǎn)向。下面是我在Matlab中實(shí)現(xiàn)速度障礙判斷的骨架代碼function vNew avoidDynamicObstacles(vDes, pos, v, obstacles, tau, rSafe) vNew vDes; for j 1:length(obstacles) relPos obstacles(j).pos - pos; relVel v - obstacles(j).vel; % 計(jì)算相對(duì)運(yùn)動(dòng)接近于零的時(shí)刻 a dot(relVel, relVel); b 2 * dot(relPos, relVel); c dot(relPos, relPos) - (rSafe obstacles(j).radius)^2; disc b*b - 4*a*c; if disc 0 a 1e-6 t1 (-b - sqrt(disc)) / (2*a); t2 (-b sqrt(disc)) / (2*a); % 存在未來tau秒內(nèi)碰撞風(fēng)險(xiǎn) if (t2 0 t1 tau t1 0) % 從期望速度向量中調(diào)整朝向 vNew projectToSafeVelocity(vDes, relPos, relVel, rSafe); end end end end2.4 多機(jī)協(xié)同調(diào)度與防撞機(jī)間防撞是這次實(shí)驗(yàn)里最容易“翻車”的部分。我最初采用的思路是讓每架無人機(jī)獨(dú)立執(zhí)行局部避障結(jié)果上面提到的振蕩和死鎖全都出現(xiàn)了。后來我把協(xié)同調(diào)度拆成兩個(gè)策略來執(zhí)行策略一飛行優(yōu)先級(jí)調(diào)度。給每架無人機(jī)分配一個(gè)優(yōu)先級(jí)編號(hào)當(dāng)兩架無人機(jī)進(jìn)入彼此的通信范圍且同時(shí)檢測(cè)到?jīng)_突時(shí)優(yōu)先級(jí)低的那架負(fù)責(zé)主動(dòng)避讓優(yōu)先級(jí)高的保持當(dāng)前航向速度不變。這個(gè)策略模仿了現(xiàn)實(shí)的空中交通規(guī)則簡(jiǎn)單但在數(shù)量和密度都較低的場(chǎng)景下非常有效。策略二空域分層。利用無人機(jī)在三維空間自由飛行的優(yōu)勢(shì)給不同無人機(jī)的全局路徑分配不同的高度基準(zhǔn)層。比如高度層間隔設(shè)為12米各機(jī)原則上在不同高度走廊飛行。機(jī)間距離天然被拉開就算需要變高度也只在特定過渡段進(jìn)行。這個(gè)策略在實(shí)際實(shí)驗(yàn)里直接把相撞次數(shù)降低了90%以上。當(dāng)然兩個(gè)策略各有局限優(yōu)先級(jí)調(diào)度在遇到三架以上無人機(jī)協(xié)同沖突時(shí)避讓機(jī)可能同時(shí)收到來自兩架更高優(yōu)先級(jí)無人機(jī)的避讓指令這時(shí)候需要再設(shè)計(jì)一個(gè)次優(yōu)先級(jí)判斷規(guī)則空域分層則不適合超低空密集飛行場(chǎng)景。但對(duì)于絕大多數(shù)仿真項(xiàng)目?jī)蓚€(gè)策略已經(jīng)足夠穩(wěn)健了。3. Matlab代碼實(shí)現(xiàn)與仿真環(huán)境搭建3.1 為什么選Matlab做這套仿真很多做多智能體的人一上來就選C或Python但就這個(gè)項(xiàng)目而言Matlab的優(yōu)勢(shì)非常明顯矩陣運(yùn)算天然契合多機(jī)狀態(tài)存儲(chǔ)直接用 N x 3 的矩陣存所有無人機(jī)位置運(yùn)算時(shí)整矩陣操作不用寫循環(huán)?？梢暬奖闳S軌跡、障礙物、動(dòng)態(tài)威脅的繪制只需要幾行代碼調(diào)試時(shí)能直觀看到避撞邏輯是否生效。工具箱確實(shí)省時(shí)間路徑規(guī)劃、優(yōu)化引擎都有現(xiàn)成接口盡管這次我主要用的是自實(shí)現(xiàn)算法但需要對(duì)比驗(yàn)證時(shí)拉個(gè)工具箱函數(shù)很快。當(dāng)然它的缺點(diǎn)我也提一句大規(guī)模仿真比如30架以上無人機(jī)的性能下降明顯。一旦到了那個(gè)規(guī)模你就要考慮用代碼生成器轉(zhuǎn)到C或者用混合架構(gòu)。但前期邏輯驗(yàn)證階段Matlab還是最能讓你把注意力集中在算法本身上的環(huán)境。3.2 仿真主循環(huán)與模塊劃分代碼的組織結(jié)構(gòu)如下simulation/ |-- initSimulation.m # 初始化參數(shù)、地圖、無人機(jī)狀態(tài) |-- createWorldMap.m # 生成靜態(tài)障礙物與動(dòng)態(tài)障礙物軌跡 |-- aStarGlobalPlanner.m # 全局路徑規(guī)劃 |-- potentialFieldController.m # 局部靜態(tài)避障 |-- velocityObstacleAvoidance.m # 動(dòng)態(tài)避障 |-- cooperativeScheduler.m # 優(yōu)先級(jí)調(diào)度與沖突消解 |-- updateDynamics.m # 無人機(jī)運(yùn)動(dòng)模型更新 |-- visualizeSimulation.m # 實(shí)時(shí)繪制 |-- main.m # 主循環(huán)主循環(huán)的核心框架如下% main.m 核心循環(huán) for t 0:dt:T_total % 1. 更新動(dòng)態(tài)障礙物位置 world updateDynamicObstacles(world, t); % 2. 對(duì)每架無人機(jī)依次計(jì)算速度指令 for i 1:N % 全局路徑跟蹤 [goalRef, path] planner.getReference(agents(i), world.map3D); % 局部靜態(tài)防撞 vPotential potentialFieldController(agents(i).pos, goalRef, world.staticObs); % 動(dòng)態(tài)防撞含其他無人機(jī)與移動(dòng)威脅 dynamicObs [world.movingObstacles, agents(1:N ~ i)]; vSafe velocityObstacleAvoidance(vPotential, agents(i), dynamicObs, tau, rSafe); % 協(xié)同調(diào)度 vFinal cooperativeScheduler(agents(i), allAgents, vSafe, priority(i)); % 3. 更新狀態(tài) agents(i).vel smoothAccel(agents(i).vel, vFinal, maxAccel); agents(i).pos agents(i).pos agents(i).vel * dt; end % 4. 碰撞檢測(cè)與統(tǒng)計(jì) checkCollisions(agents, world); % 5. 可視化 if mod(t, plotInterval) dt visualizeSimulation(agents, world, paths, t); end end注意主循環(huán)里的執(zhí)行順序先算局部靜態(tài)避障再做動(dòng)態(tài)避障最后才是協(xié)同調(diào)度。順序反過來的話協(xié)同層的速度指令會(huì)被局部層覆蓋優(yōu)先級(jí)調(diào)度就形同虛設(shè)了。3.3 仿真場(chǎng)景配置與參數(shù)初始化初始化腳本里我定義了這樣一組基準(zhǔn)參數(shù)你可以直接抄去用無人機(jī)數(shù)量5架飛行區(qū)域200m x 200m x 50m無人機(jī)巡航速度10m/s最大加速度5m/s^2安全距離3m機(jī)間通信距離50m全局重規(guī)劃觸發(fā)半徑10m速度障礙預(yù)測(cè)時(shí)間窗3s無人機(jī)起點(diǎn)和目標(biāo)點(diǎn)我設(shè)置成交叉路線這樣可以逼出急需要協(xié)同的場(chǎng)景。比如2號(hào)機(jī)和4號(hào)機(jī)的連線在空間上近似垂直交叉如果不對(duì)它們做調(diào)度原路徑會(huì)在中點(diǎn)沖突。在俯瞰的平面圖上你能看到全局路徑基本會(huì)沿著障礙物邊緣的緩沖帶繞行這就是安全代價(jià)起的作用。如果沒有把安全代價(jià)加進(jìn)A*全局路線的某些段會(huì)緊貼障礙物邊界一旦遇上動(dòng)態(tài)障礙物局部避障緩沖區(qū)不足就會(huì)產(chǎn)生劇烈轉(zhuǎn)向。3.4 動(dòng)態(tài)障礙物仿真方法動(dòng)態(tài)障礙物的實(shí)現(xiàn)我給幾個(gè)常用模式你按需選擇固定軌跡巡邏型障礙物沿預(yù)設(shè)軌跡做往返或圓周運(yùn)動(dòng)。適合測(cè)試局部避障是否穩(wěn)定因?yàn)檐壽E可預(yù)測(cè)容易對(duì)照分析。隨機(jī)擾動(dòng)型在軌跡基準(zhǔn)上疊加隨機(jī)速度偏移模擬非合作物體的不可預(yù)測(cè)行為。這種更貼近真實(shí)戰(zhàn)場(chǎng)環(huán)境也讓全局重規(guī)劃觸發(fā)頻率變高。針對(duì)性突襲型在仿真進(jìn)行到設(shè)定時(shí)間時(shí)突然從側(cè)向高速切入無人機(jī)航線。我在最終展示版本里加了這類型用來體現(xiàn)動(dòng)態(tài)防撞的必要性。動(dòng)態(tài)障礙物的半徑我設(shè)置為1.5m速度和無人機(jī)巡航速度相當(dāng)這樣速度障礙法會(huì)在預(yù)測(cè)時(shí)間窗內(nèi)提前約兩秒觸發(fā)避讓路徑偏移幅度也比較容易觀察。4. 參數(shù)調(diào)優(yōu)與常見問題排查4.1 關(guān)鍵參數(shù)的影響曲線與調(diào)參順序代碼跑通只是第一步想把仿真效果調(diào)得穩(wěn)定參數(shù)比算法更花時(shí)間。我給出一張關(guān)鍵參數(shù)對(duì)照表這些都是實(shí)測(cè)中影響顯著的參數(shù)推薦初始值偏小的影響偏大的影響全局轉(zhuǎn)向代價(jià)權(quán)重0.3路徑鋸齒狀局部層壓力大路徑過于平滑繞路明顯全局安全代價(jià)權(quán)重0.5全局路徑貼障礙物過近全局路徑過度保守飛行距離變長(zhǎng)速度障礙預(yù)測(cè)時(shí)間窗3s避障反應(yīng)過晚易撞避障過早路徑抖動(dòng)頻繁局部斥力系數(shù)1.2障礙物附近減速不明顯無人機(jī)被“彈開”路徑震蕩機(jī)間安全距離3m機(jī)間距離過近氣流干擾明顯空域利用率降低通信距離50m超距離無人機(jī)無法協(xié)同對(duì)通信模型要求過高調(diào)參順序上我建議按“全局層 → 局部層 → 協(xié)同層”來。先不管動(dòng)態(tài)威脅只調(diào)A*的權(quán)重讓單機(jī)路徑足夠光滑然后加動(dòng)態(tài)障礙物調(diào)勢(shì)場(chǎng)系數(shù)和預(yù)測(cè)時(shí)間窗最后才把多機(jī)協(xié)同開起來調(diào)優(yōu)先級(jí)邏輯。如果你一上來就在多機(jī)動(dòng)態(tài)環(huán)境里調(diào)參現(xiàn)象很難歸因到具體模塊。有一點(diǎn)特別想強(qiáng)調(diào)勢(shì)場(chǎng)法的斥力系數(shù)極端影響穩(wěn)定性。斥力過小時(shí)無人機(jī)貼著障礙物飛速度障礙法介入頻繁軌跡整體呈鋸齒狀斥力過大時(shí)無人機(jī)會(huì)在距離障礙物還很遠(yuǎn)的地方就被彈開一旦多個(gè)障礙物斥力場(chǎng)疊加會(huì)出現(xiàn)繞路甚至震蕩。我調(diào)試時(shí)反復(fù)對(duì)比了幾組系數(shù)最終鎖定了讓軌跡“略微壓向障礙物但不進(jìn)入危險(xiǎn)圈”的中間值1.2。4.2 常見的五類問題與排查思路問題一兩架無人機(jī)在路徑中點(diǎn)持續(xù)振蕩誰也過不去。這是協(xié)同調(diào)度缺失的典型癥狀。第一反應(yīng)不要調(diào)避障參數(shù)而是先檢查優(yōu)先級(jí)是否真正生效。我遇到過一次優(yōu)先級(jí)編號(hào)已經(jīng)分配但協(xié)同調(diào)度模塊里漏了把低優(yōu)先級(jí)無人機(jī)從高優(yōu)先級(jí)無人機(jī)的障礙物列表中排除導(dǎo)致高優(yōu)先級(jí)無人機(jī)也在避讓低優(yōu)先級(jí)無人機(jī)兩邊互讓就振蕩了。修正后高優(yōu)先級(jí)無人機(jī)保持原計(jì)劃低優(yōu)先級(jí)無人機(jī)主動(dòng)繞行問題立刻解決。問題二動(dòng)態(tài)障礙物從側(cè)面快速經(jīng)過時(shí)無人機(jī)和大障礙物距離突然拉近避讓動(dòng)作太猛。原因通常是預(yù)測(cè)時(shí)間窗太短。速度障礙法起作用的前提是預(yù)測(cè)時(shí)間窗tau內(nèi)能看到足夠長(zhǎng)的軌跡延長(zhǎng)段tau太小等于臨時(shí)抱佛腳。把tau從3s調(diào)到5s后側(cè)向威脅的避讓明顯平緩。但tau也不能無限調(diào)大太長(zhǎng)會(huì)讓無人機(jī)對(duì)遠(yuǎn)處本不沖突的障礙物也提前避讓造成不必要的路徑偏移。問題三全局路徑重規(guī)劃之后局部路徑出現(xiàn)明顯的折線。多數(shù)情況是重規(guī)劃觸發(fā)時(shí)使用的路徑點(diǎn)沒做插值平滑。A*輸出的節(jié)點(diǎn)密度不高直接作為航跡參考會(huì)讓局部層對(duì)每個(gè)節(jié)點(diǎn)都做一次“拉直”動(dòng)作。我加了一步簡(jiǎn)單的三次樣條插值把相鄰路徑節(jié)點(diǎn)之間補(bǔ)成曲線后續(xù)局部控制平滑度明顯改善。問題四增加無人機(jī)數(shù)量后低優(yōu)先級(jí)無人機(jī)的避障指令頻繁切換方向。這說明協(xié)同調(diào)度中的“多機(jī)沖突”解析沒有處理好。當(dāng)一架低優(yōu)先級(jí)無人機(jī)同時(shí)面對(duì)兩架高優(yōu)先級(jí)無人機(jī)時(shí)它會(huì)在兩個(gè)避讓方向之間反復(fù)橫跳。我最終的處理方式是讓低優(yōu)先級(jí)無人機(jī)選擇“調(diào)整量最小的方向”一旦選定保持至少2s不變待兩架高優(yōu)先級(jí)無人機(jī)都通過后再恢復(fù)。加入“最小保持時(shí)間”后航線抖動(dòng)問題消失。問題五仿真畫面掉幀嚴(yán)重大量時(shí)間消耗在繪圖函數(shù)上。Matlab的繪圖確實(shí)比計(jì)算慢得多。我的做法是把可視化頻率降低不是每幀都繪制而是每10幀繪制一次同時(shí)把點(diǎn)云和障礙物圖形對(duì)象句柄在初始化時(shí)創(chuàng)建之后只更新xdata和ydata屬性而不是每幀重新plot。這樣調(diào)整后仿真速度提升了近4倍。如果你還需要更快可以考慮把仿真過程分成“無界面快速運(yùn)行動(dòng)態(tài)數(shù)據(jù)記錄”和“回放模式”兩段執(zhí)行。4.3 調(diào)試?yán)髋鲎踩罩九c軌跡回放調(diào)試多機(jī)系統(tǒng)最忌只盯實(shí)時(shí)畫面因?yàn)閯?dòng)態(tài)過程轉(zhuǎn)瞬即逝很難在發(fā)生的瞬間看清是哪一步出了問題。我寫了一個(gè)碰撞日志模塊每當(dāng)檢測(cè)到潛在碰撞風(fēng)險(xiǎn)時(shí)記錄當(dāng)前時(shí)間、兩機(jī)/機(jī)障距離、各自速度向量、正在執(zhí)行的規(guī)劃器狀態(tài)。這樣每輪仿真結(jié)束通過查看日志就能準(zhǔn)確還原出“在什么時(shí)間、哪兩架飛機(jī)、因?yàn)槭裁丛蚓嚯x最近”。另外建議把所有無人機(jī)位置向量按時(shí)間步存成矩陣仿真結(jié)束后做一次軌跡回放?；胤艜r(shí)用暫停鍵逐幀檢查你會(huì)發(fā)現(xiàn)很多實(shí)時(shí)觀察時(shí)注意不到的規(guī)律性沖突。我最終提交方案里的幾個(gè)復(fù)雜bug都是靠回放定位出來的。5. 心得總結(jié)與后續(xù)擴(kuò)展方向回看這輪實(shí)驗(yàn)我最大的體會(huì)是多無人機(jī)協(xié)同路徑規(guī)劃這個(gè)題目算法層面的創(chuàng)新點(diǎn)固然重要但真正決定一個(gè)方案能不能實(shí)際跑的往往是工程細(xì)節(jié)比如模塊間執(zhí)行順序、參數(shù)失效時(shí)的異?，F(xiàn)象特征、全局路徑和平滑軌跡之間的耦合關(guān)系。這些內(nèi)容論文里不會(huì)寫但恰恰是Lab和項(xiàng)目中最花時(shí)間的部分。再分享一個(gè)本人在實(shí)踐中驗(yàn)證過的小技巧在協(xié)同調(diào)度模塊中優(yōu)先級(jí)不要固定不變而是在任務(wù)執(zhí)行過程中做一次動(dòng)態(tài)調(diào)整。比如某架無人機(jī)因?yàn)槌跏嘉恢幂^差不得不大幅繞路長(zhǎng)期處于低優(yōu)先級(jí)會(huì)讓它頻繁處于被動(dòng)避讓狀態(tài)。這時(shí)候根據(jù)“剩余路徑長(zhǎng)度”加一個(gè)動(dòng)態(tài)優(yōu)先級(jí)補(bǔ)償讓繞路多的無人機(jī)適度提高優(yōu)先級(jí)整個(gè)機(jī)群的完成時(shí)間可以縮短10%-15%。當(dāng)然這樣的調(diào)整機(jī)制必須配合嚴(yán)格的安全約束否則高優(yōu)先級(jí)無人機(jī)數(shù)量一多低優(yōu)先級(jí)會(huì)陷入無限避讓。后續(xù)如果要繼續(xù)深入還有三個(gè)方向可以考慮一是把編隊(duì)構(gòu)型保持和路徑規(guī)劃結(jié)合起來讓無人機(jī)群以固定隊(duì)形飛行但遇到障礙物時(shí)自動(dòng)分散通過后再恢復(fù)隊(duì)形二是在速度障礙法基礎(chǔ)上引入更復(fù)雜的動(dòng)力學(xué)約束讓規(guī)劃指令直接貼近真實(shí)飛控需求三是加入隨機(jī)風(fēng)場(chǎng)模型在速度障礙的預(yù)測(cè)計(jì)算中疊加風(fēng)的擾動(dòng)項(xiàng)。這三個(gè)方向我都做過預(yù)研每個(gè)都可以再撐起一篇完整的方案。希望這套基于Matlab的實(shí)現(xiàn)思路能幫你在多無人機(jī)協(xié)同路徑規(guī)劃上少走點(diǎn)彎路。如果調(diào)試過程中遇到“詭異”的振蕩和死鎖先別急著堆算法按上面的排查順序逐層檢查多半能定位到一個(gè)很具體的小問題上。歡迎在實(shí)際運(yùn)行后回來交流參數(shù)表現(xiàn)。