動(dòng)態(tài)環(huán)境下多無(wú)人機(jī)協(xié)同路徑規(guī)劃與防撞這套Matlab方案我是這么落地實(shí)現(xiàn)的多無(wú)人機(jī)協(xié)同路徑規(guī)劃與防撞前兩年還是實(shí)驗(yàn)室里的前沿課題現(xiàn)在幾乎成了無(wú)人機(jī)應(yīng)用落地的必答題。無(wú)論是物流配送、農(nóng)業(yè)巡檢還是編隊(duì)表演凡是涉及“多機(jī)同場(chǎng)”的場(chǎng)景都繞不開路徑規(guī)劃和防撞這兩個(gè)核心問(wèn)題。我在這塊斷斷續(xù)續(xù)做過(guò)幾個(gè)項(xiàng)目最近重新整理了一套基于Matlab的實(shí)現(xiàn)方案專門針對(duì)動(dòng)態(tài)環(huán)境也就是障礙物會(huì)動(dòng)、目標(biāo)點(diǎn)會(huì)變、甚至是多無(wú)人機(jī)之間彼此互為障礙物的場(chǎng)景今天把整個(gè)研究思路、代碼架構(gòu)和踩過(guò)的坑一次性說(shuō)清楚。這套方案適合誰(shuí)如果你剛接觸多智能體協(xié)同或者在讀論文時(shí)對(duì)“動(dòng)態(tài)環(huán)境下的協(xié)同路徑規(guī)劃”這個(gè)概念一頭霧水再或者你已經(jīng)寫好了單機(jī)路徑規(guī)劃但不知道怎么擴(kuò)展到多機(jī)這篇文章都能給你一個(gè)可以直接上手的參考。實(shí)現(xiàn)層面不用C也不用ROS純粹Matlab就能把核心邏輯跑通可視化調(diào)試也非常方便。1. 動(dòng)態(tài)環(huán)境下多無(wú)人機(jī)協(xié)同路徑規(guī)劃的整體設(shè)計(jì)思路1.1 為什么“動(dòng)態(tài)環(huán)境”是繞不開的關(guān)鍵詞很多入門教程教的是靜態(tài)路徑規(guī)劃障礙物位置固定目標(biāo)點(diǎn)位置固定無(wú)人機(jī)按規(guī)劃好的路線飛就行。但真實(shí)場(chǎng)景沒有這么友好——障礙物可能是在移動(dòng)的比如車輛、行人和飛鳥其他無(wú)人機(jī)本身也在動(dòng)甚至任務(wù)目標(biāo)都可能中途改變。動(dòng)態(tài)環(huán)境的核心難點(diǎn)在于一個(gè)“變化”的詞第一次規(guī)劃出的路徑第二次可能就不可用了因此系統(tǒng)必須具備實(shí)時(shí)重規(guī)劃能力。我在實(shí)際項(xiàng)目中感觸最深的一點(diǎn)是所謂“協(xié)同”并不是簡(jiǎn)單地把多架無(wú)人機(jī)各自規(guī)劃好的路徑拼在一起而是要處理路徑之間的耦合關(guān)系。舉個(gè)簡(jiǎn)單例子兩架無(wú)人機(jī)同時(shí)飛向同一個(gè)任務(wù)區(qū)域如果各自按最短路徑規(guī)劃大概率會(huì)在中間某個(gè)位置相遇此時(shí)必須有機(jī)制讓其中一架讓路或者雙方共同調(diào)整軌跡。這就是協(xié)同路徑規(guī)劃要解決的本質(zhì)問(wèn)題而防撞是協(xié)同的下層約束。1.2 協(xié)同規(guī)劃的核心架構(gòu)集中式還是分布式在設(shè)計(jì)這套Matlab方案時(shí)我首先面臨架構(gòu)選擇集中式還是分布式。集中式的思路是所有無(wú)人機(jī)的狀態(tài)匯總到地面站地面站統(tǒng)一規(guī)劃出所有無(wú)人機(jī)的軌跡再下發(fā)執(zhí)行。優(yōu)點(diǎn)是全局最優(yōu)性好但問(wèn)題在于計(jì)算量隨無(wú)人機(jī)數(shù)量呈指數(shù)增長(zhǎng)而且對(duì)通信依賴極強(qiáng)地面站一旦失效全盤癱瘓。分布式相反每架無(wú)人機(jī)獨(dú)立計(jì)算自己的路徑通過(guò)通信或其他無(wú)人機(jī)的位置信息達(dá)成協(xié)同。擴(kuò)展性好魯棒性強(qiáng)但容易出現(xiàn)局部最優(yōu)多機(jī)之間的協(xié)調(diào)也需要額外策略。這套實(shí)現(xiàn)方案我選的是“集中規(guī)劃分布式避障”的混合架構(gòu)任務(wù)分配和目標(biāo)點(diǎn)規(guī)劃集中在主控制器完成而實(shí)時(shí)防撞交給每架無(wú)人機(jī)的局部邏輯。這樣既保證了整體協(xié)同的效率又能在動(dòng)態(tài)環(huán)境中快速響應(yīng)突發(fā)障礙。對(duì)于中小規(guī)模無(wú)人機(jī)編隊(duì)10架以內(nèi)這個(gè)架構(gòu)在Matlab里跑起來(lái)性能完全能接受。1.3 模塊劃分與數(shù)據(jù)流設(shè)計(jì)整套代碼我按功能拆成四個(gè)模塊環(huán)境建模模塊、路徑規(guī)劃模塊、防撞決策模塊和可視化模塊。數(shù)據(jù)流是這樣的環(huán)境模塊負(fù)責(zé)維護(hù)靜態(tài)障礙物和動(dòng)態(tài)障礙物的實(shí)時(shí)位置路徑規(guī)劃模塊根據(jù)當(dāng)前環(huán)境為每架無(wú)人機(jī)生成參考路徑防撞決策模塊檢測(cè)參考路徑與障礙物或其他無(wú)人機(jī)是否沖突若有沖突則觸發(fā)局部重規(guī)劃可視化模塊做實(shí)時(shí)渲染把仿真過(guò)程展示出來(lái)。模塊化帶來(lái)的好處非常直接你可以單獨(dú)替換路徑規(guī)劃算法而不影響防撞邏輯也可以只調(diào)試防撞參數(shù)而不用把整個(gè)仿真重跑一遍。好多初學(xué)者容易犯的錯(cuò)誤是把所有功能寫在一個(gè)大腳本里后面改參數(shù)改算法都得層層翻找?guī)资畟€(gè)變量命名混亂到直接跑不動(dòng)。這種坑我踩過(guò)多次所以推薦模塊化分開寫。2. 核心算法選型與Matlab工具鏈解析2.1 路徑規(guī)劃主算法怎么選路徑規(guī)劃算法選擇直接影響協(xié)同效果。我在不同階段嘗試過(guò)A*、RRT、人工勢(shì)場(chǎng)法和粒子群優(yōu)化各有適用場(chǎng)景。A*算法適合柵格地圖的全局路徑搜索得到路徑質(zhì)量高但是擴(kuò)展無(wú)人機(jī)數(shù)量時(shí)需要維護(hù)多份地圖狀態(tài)內(nèi)存開銷不小。RRT及其變體適合高維空間和復(fù)雜約束隨機(jī)采樣的特性在動(dòng)態(tài)環(huán)境下反而容易產(chǎn)生不確定路徑實(shí)際工程不太好用。最終我給主規(guī)劃層選了人工勢(shì)場(chǎng)法Artificial Potential Field, APF加局部?jī)?yōu)化。人工勢(shì)場(chǎng)法的核心思路很直白目標(biāo)點(diǎn)產(chǎn)生引力障礙物產(chǎn)生斥力合力驅(qū)動(dòng)無(wú)人機(jī)運(yùn)動(dòng)。它的優(yōu)點(diǎn)在于計(jì)算快、適合實(shí)時(shí)控制、代碼實(shí)現(xiàn)簡(jiǎn)潔適合做動(dòng)態(tài)環(huán)境下的快速響應(yīng)缺點(diǎn)也明顯就是容易陷入局部極小值這在后文會(huì)詳細(xì)說(shuō)解決思路。輔助使用粒子群優(yōu)化PSO來(lái)做多機(jī)目標(biāo)分配。PSO在多目標(biāo)組合優(yōu)化問(wèn)題上表現(xiàn)穩(wěn)定多無(wú)人機(jī)任務(wù)分配可以編碼成每個(gè)無(wú)人機(jī)的目標(biāo)點(diǎn)序列適應(yīng)度函數(shù)綜合考慮總航程、時(shí)空沖突風(fēng)險(xiǎn)迭代出較優(yōu)分配方案。2.2 防撞策略速度障礙法為主力優(yōu)先級(jí)分配為兜底動(dòng)態(tài)環(huán)境下防撞我選用速度障礙法Velocity Obstacle, VO作為主策略。速度障礙法的核心思想是把無(wú)人機(jī)和障礙物的相對(duì)位置、相對(duì)速度映射到一個(gè)速度空間凡是會(huì)導(dǎo)致未來(lái)碰撞的速度向量都納入“禁入?yún)^(qū)域”當(dāng)前狀態(tài)可用的避障速度范圍就是可行速度集減去禁入?yún)^(qū)域從中選擇最優(yōu)速度即可。單說(shuō)概念可能比較抽象我用生活場(chǎng)景打個(gè)比方你在擁擠的人流中走路目標(biāo)方向在正前方但有個(gè)迎面走來(lái)的人你會(huì)在心里快速估算——以你我當(dāng)前的速度繼續(xù)走下去幾秒后會(huì)撞上于是你調(diào)整步伐減速、換個(gè)方向這個(gè)瞬間決策就是速度障礙法的思想。多無(wú)人機(jī)之間的防撞我在VO基礎(chǔ)上增加了優(yōu)先級(jí)分配機(jī)制為每架無(wú)人機(jī)設(shè)置動(dòng)態(tài)優(yōu)先級(jí)低優(yōu)先級(jí)的無(wú)人機(jī)負(fù)責(zé)避讓高優(yōu)先級(jí)的無(wú)人機(jī)。這樣避免了雙方同時(shí)避讓反而撞上的“乒乓效應(yīng)”。2.3 為什么選Matlab而不是其他語(yǔ)言多無(wú)人機(jī)協(xié)同仿真涉及大量矩陣運(yùn)算、繪圖和數(shù)值計(jì)算Matlab在這幾方面天然占優(yōu)。矩陣化編程讓運(yùn)動(dòng)學(xué)模型更新非常簡(jiǎn)潔內(nèi)置的繪圖接口讓實(shí)時(shí)可視化幾乎零成本。此外Matlab的調(diào)試環(huán)境比C友好太多你能隨時(shí)中斷查看每個(gè)變量的狀態(tài)這在調(diào)試防撞邏輯時(shí)太重要了。也有同事建議用C加ROS方案理由是可以直接移植到真實(shí)無(wú)人機(jī)。但真實(shí)環(huán)境本身也有Matlab的代碼生成工具可以轉(zhuǎn)C所以研發(fā)階段用Matlab驗(yàn)證算法、后期再轉(zhuǎn)工程語(yǔ)言是更高效的選擇。我的原則是驗(yàn)證階段追求的是快速迭代Matlab完勝。3. 基于Matlab的整套實(shí)現(xiàn)流程從參數(shù)初始化到結(jié)果分析3.1 環(huán)境初始化與參數(shù)設(shè)定環(huán)境初始化的關(guān)鍵是定義好所有全局參數(shù)。我按如下結(jié)構(gòu)組織參數(shù)% 環(huán)境參數(shù) simTime 100; % 仿真時(shí)長(zhǎng)(s) dt 0.05; % 控制周期(s) mapRange [-10, 10, -10, 10]; % 地圖范圍 % 無(wú)人機(jī)參數(shù) nUAV 4; safeDist 0.8; % 無(wú)人機(jī)間安全距離(m) maxSpeed 2.0; % 最大速度(m/s) maxAccel 1.5; % 最大加速度(m/s^2) % 人工勢(shì)場(chǎng)參數(shù) attGain 1.0; % 引力增益 repGain 2.5; % 斥力增益 repRadius 2.0; % 斥力影響范圍 % 動(dòng)態(tài)障礙物參數(shù) nObs 3; obsVel [0.3, -0.2; -0.4, 0.1; 0.2, 0.3];參數(shù)設(shè)定看似簡(jiǎn)單實(shí)際是整套系統(tǒng)最容易出問(wèn)題的環(huán)節(jié)。安全距離設(shè)得太小多機(jī)之間容易擦碰設(shè)得太大又會(huì)嚴(yán)重影響路徑效率導(dǎo)致無(wú)人機(jī)繞遠(yuǎn)路。我的經(jīng)驗(yàn)是先按無(wú)人機(jī)的物理尺寸乘以1.5倍作為基礎(chǔ)安全距離再根據(jù)仿真效果微調(diào)。斥力影響半徑同理太大會(huì)導(dǎo)致目標(biāo)點(diǎn)附近振蕩太小又對(duì)障礙物反應(yīng)遲鈍。3.2 無(wú)人機(jī)單體運(yùn)動(dòng)學(xué)模型搭建無(wú)人機(jī)運(yùn)動(dòng)學(xué)模型是仿真基礎(chǔ)。我不會(huì)引入過(guò)于復(fù)雜的空氣動(dòng)力學(xué)模型而是在三維環(huán)境下做簡(jiǎn)化的質(zhì)點(diǎn)模型加上加速度限制和一階速度響應(yīng)來(lái)模擬無(wú)人機(jī)的動(dòng)力特性。for i 1:nUAV % 根據(jù)加速度更新速度 uav(i).velocity uav(i).velocity uav(i).accel * dt; % 限制最大速度 speed norm(uav(i).velocity); if speed maxSpeed uav(i).velocity uav(i).velocity / speed * maxSpeed; end % 更新位置 uav(i).position uav(i).position uav(i).velocity * dt; end為什么要加加速度限制這一條我在帶學(xué)生做項(xiàng)目時(shí)反復(fù)強(qiáng)調(diào)過(guò)。如果你直接讓無(wú)人機(jī)的位置跳變到目標(biāo)點(diǎn)規(guī)劃算法確實(shí)能算出完美路徑但物理上根本飛不出來(lái)。加入加速度和速度限制才能讓仿真結(jié)果具備實(shí)際參考價(jià)值。3.3 協(xié)同層任務(wù)分配與路徑重規(guī)劃協(xié)同層負(fù)責(zé)兩件事初始的任務(wù)分配和運(yùn)行過(guò)程中按需觸發(fā)的前全局重規(guī)劃。任務(wù)分配我用PSO算法把nUAV架無(wú)人機(jī)分配到nTarget個(gè)目標(biāo)點(diǎn)上目標(biāo)是讓總飛行時(shí)間和風(fēng)險(xiǎn)最小。function assignment psoTaskAllocation(uavPos, targetPos, nUAV) % 粒子群任務(wù)分配 % 每個(gè)粒子的位置編碼為一個(gè)排列代表無(wú)人機(jī)到目標(biāo)的對(duì)應(yīng)關(guān)系 nParticles 50; nIter 100; % 初始化粒子群這里簡(jiǎn)化為隨機(jī)排列 particles repmat(1:nUAV, nParticles, 1); for p 1:nParticles particles(p, :) randperm(nUAV); end % 迭代優(yōu)化完整實(shí)現(xiàn)含速度更新和最優(yōu)記錄此處略 % 適應(yīng)度函數(shù)總航程 沖突代價(jià) assignment particles(bestIdx, :); end初始分配完成后若某架無(wú)人機(jī)發(fā)現(xiàn)原定目標(biāo)區(qū)域不可達(dá)比如動(dòng)態(tài)障礙物長(zhǎng)期占據(jù)通道會(huì)觸發(fā)協(xié)同層的重分配。這里有個(gè)實(shí)際遇到過(guò)的教訓(xùn)重分配不能太頻繁否則系統(tǒng)會(huì)陷入“分配-重規(guī)劃-再分配”的振蕩循環(huán)我加了一個(gè)最小重分配間隔參數(shù)10秒內(nèi)最多觸發(fā)一次效果明顯改善。3.4 防撞層動(dòng)態(tài)障礙檢測(cè)與避讓邏輯防撞層是動(dòng)態(tài)環(huán)境下協(xié)同規(guī)劃的重中之重。核心邏輯分三步?jīng)_突檢測(cè)、速度避讓計(jì)算、路徑修正。function newVel collisionAvoidance(uav, obsList, safeDist) % 基于速度障礙法的防撞 feasibleVel []; for vx -1:0.1:1 for vy -1:0.1:1 v [vx, vy]; conflict false; % 檢查每個(gè)障礙物 for j 1:length(obsList) obs obsList(j); relPos obs.position - uav.position; relVel uav.velocity - obs.velocity; % 計(jì)算最近相遇距離簡(jiǎn)化版 if norm(relPos) safeDist conflict true; break; end % 判斷相對(duì)速度方向是否指向障礙物 relativeSpeedDir relVel / norm(relVel); if dot(relPos, relativeSpeedDir) 0 distance abs(cross([relPos, 0], [relativeSpeedDir, 0])); if distance(3) safeDist conflict true; break; end end end if ~conflict feasibleVel [feasibleVel; v]; end end end % 從可行速度中選最接近期望速度的 if ~isempty(feasibleVel) [~, idx] min(vecnorm(feasibleVel - uav.desiredVel, 2, 2)); newVel feasibleVel(idx, :); else newVel [0, 0]; % 無(wú)法避讓時(shí)緊急懸停 end end這段代碼是個(gè)簡(jiǎn)化版本但核心邏輯清晰枚舉候選速度、檢測(cè)潛在沖突、選出最安全且最接近預(yù)期方向的速度。實(shí)際項(xiàng)目中我還加入了對(duì)不可行解的處理——如果所有候選速度都沖突就讓無(wú)人機(jī)緊急減速懸停而不是硬著頭皮往前沖。有一點(diǎn)必須提醒速度枚舉的分辨率直接影響了避讓質(zhì)量和計(jì)算開銷。分辨率設(shè)0.1時(shí)會(huì)非常細(xì)致但意味著數(shù)百次沖突判斷多機(jī)同時(shí)避讓時(shí)單步計(jì)算壓力很大。我的折中方案是先粗粒度枚舉找到可行區(qū)域再在最優(yōu)候選速度附近做一次細(xì)粒度搜索性能差不多能提升3到4倍。3.5 可視化與仿真結(jié)果分析可視化是Matlab做算法的巨大優(yōu)勢(shì)。我用Matlab的動(dòng)畫接口做實(shí)時(shí)軌跡渲染疊加靜態(tài)障礙物、動(dòng)態(tài)障礙物和無(wú)人機(jī)間的預(yù)測(cè)路徑。figure; hold on; grid on; axis(mapRange); % 初始化無(wú)人機(jī)繪圖句柄 uavPlots gobjects(nUAV, 1); trailPlots {}; for i 1:nUAV uavPlots(i) plot(0, 0, o, MarkerSize, 8, LineWidth, 2); trailPlots{i} animatedline(Color, colors(i, :), LineWidth, 1.5); end % 仿真主循環(huán) for t 0:dt:simTime % 更新各模塊狀態(tài)略 % 更新繪圖 for i 1:nUAV set(uavPlots(i), XData, uav(i).position(1), YData, uav(i).position(2)); addpoints(trailPlots{i}, uav(i).position(1), uav(i).position(2)); end drawnow limitrate; enddrawnow limitrate這個(gè)函數(shù)用法值得特別強(qiáng)調(diào)它限制了刷新率只保留最新幾幀顯著減小編排負(fù)擔(dān)。從我實(shí)測(cè)來(lái)看不加limitrate時(shí)仿真相對(duì)于加了的會(huì)明顯卡頓多機(jī)數(shù)量到了8架以上時(shí)差別非常明顯。運(yùn)行結(jié)束后我還會(huì)繪制每架無(wú)人機(jī)的飛行軌跡圖、速度曲線圖和航程統(tǒng)計(jì)表這部分的產(chǎn)出質(zhì)量直接決定了研究結(jié)論的可信度。表格化的結(jié)果輸出我一直保留后面的調(diào)試能少走好多彎路。4. 關(guān)鍵函數(shù)與代碼片段深度解析4.1 人工勢(shì)場(chǎng)主函數(shù)這篇文章里人工勢(shì)場(chǎng)函數(shù)是路徑規(guī)劃的基礎(chǔ)在這里詳細(xì)展開function accel apfForce(uav, target, obstacles) % 人工勢(shì)場(chǎng)計(jì)算合力加速度 repulsion [0, 0]; attraction [0, 0]; % 目標(biāo)點(diǎn)引力 vecToTarget target - uav.position; distToTarget norm(vecToTarget); directionToTarget vecToTarget / distToTarget; attraction attGain * directionToTarget; % 障礙物斥力 for k 1:size(obstacles, 1) obsPos obstacles(k, :); vecToObs obsPos - uav.position; distToObs norm(vecToObs); % 只有進(jìn)入影響范圍才產(chǎn)生斥力 if distToObs repRadius repulsionMag repGain * (1/distToObs - 1/repRadius) / (distToObs^2); repulsion repulsion repulsionMag * (vecToObs / distToObs); end end % 合力 accel attraction repulsion; % 限幅 if norm(accel) maxAccel accel accel / norm(accel) * maxAccel; end end勢(shì)場(chǎng)參數(shù)調(diào)節(jié)的經(jīng)驗(yàn)repGain和attGain的比例直接決定路徑形態(tài)。比例偏大無(wú)人機(jī)繞障礙半徑大路徑浪費(fèi)明顯比例偏小則有碰撞風(fēng)險(xiǎn)。我一般先調(diào)整斥力影響范圍讓路徑不至于穿過(guò)障礙物再微調(diào)兩個(gè)增益直到路徑自然順滑。4.2 多機(jī)間防撞判斷函數(shù)多機(jī)防撞和單機(jī)避障的最大區(qū)別是雙方都在運(yùn)動(dòng)所以沖突判斷要考慮相對(duì)運(yùn)動(dòng)趨勢(shì)。function needAvoid checkPairConflict(uav_i, uav_j, safeDist, dt) relPos uav_i.position - uav_j.position; relVel uav_i.velocity - uav_j.velocity; dist norm(relPos); % 當(dāng)前已經(jīng)小于安全距離必須避讓 if dist safeDist needAvoid true; return; end % 預(yù)測(cè)Δt時(shí)間后的相對(duì)位置 futureRelPos relPos relVel * dt; futureDist norm(futureRelPos); % 未來(lái)距離小于安全距離則觸發(fā)避讓 if futureDist safeDist needAvoid true; return; end % 考慮障礙能力添加緩沖 stoppingDist norm(relVel)^2 / (2 * maxAccel); if dist safeDist stoppingDist needAvoid true; return; end needAvoid false; end需要指出的是第三段判斷“停止距離”是我在實(shí)際仿真中加上去的。原因是兩架無(wú)人機(jī)高速相向飛行時(shí)即使當(dāng)前距離略大于安全距離留給反應(yīng)的時(shí)間也很短。加入基于速度的緩沖距離后系統(tǒng)能更早觸發(fā)避讓穩(wěn)定性顯著提升。4.3 仿真主調(diào)度循環(huán)主循環(huán)是整個(gè)系統(tǒng)的中樞負(fù)責(zé)按時(shí)間軸調(diào)度各模塊for t 0:dt:simTime % 1. 動(dòng)態(tài)環(huán)境更新障礙物移動(dòng)、機(jī)場(chǎng)狀態(tài)變化等 updateEnvironment(t); % 2. 檢查是否觸發(fā)協(xié)同層重規(guī)劃 if mod(t, replanPeriod) dt triggerReplan(); end % 3. 每架無(wú)人機(jī)獨(dú)立計(jì)算期望速度 for i 1:nUAV uav(i).desiredVel apfForce(uav(i), uav(i).target, obstacles); end % 4. 多機(jī)防撞協(xié)調(diào) for i 1:nUAV for j i1:nUAV if checkPairConflict(uav(i), uav(j), safeDist, dt) applyAvoidance(uav(i), uav(j), safeDist); end end end % 5. 動(dòng)態(tài)障礙物避讓 for i 1:nUAV newVel collisionAvoidance(uav(i), dynamicObs, safeDist); uav(i).velocity newVel; end % 6. 更新狀態(tài)與可視化 updateStates(dt); updateVisualization(t); end主循環(huán)的調(diào)度順序很重要必須先更新環(huán)境再規(guī)劃再防撞最后更新狀態(tài)。如果順序反了可能出現(xiàn)使用上一幀過(guò)時(shí)數(shù)據(jù)的情況動(dòng)態(tài)環(huán)境下這種延遲累積可能導(dǎo)致嚴(yán)重的避撞響應(yīng)滯后。5. 常見問(wèn)題與調(diào)試經(jīng)驗(yàn)實(shí)錄5.1 無(wú)人機(jī)陷入局部極小值人工勢(shì)場(chǎng)法最著名的坑就是局部極小值無(wú)人機(jī)在障礙物與目標(biāo)點(diǎn)的共同作用下受力平衡停在某處既不能前進(jìn)也不后退。我一開始遇到這個(gè)問(wèn)題時(shí)第一反應(yīng)是調(diào)大引力增益結(jié)果治標(biāo)不治本反而產(chǎn)生路徑振蕩。解決思路有兩個(gè)方向一是在勢(shì)場(chǎng)計(jì)算中加入額外的隨機(jī)擾動(dòng)當(dāng)無(wú)人機(jī)檢測(cè)到速度長(zhǎng)期趨零且未到達(dá)目標(biāo)時(shí)人為施加一個(gè)隨機(jī)方向的輔助力幫助跳出局部極小二是引入虛擬子目標(biāo)點(diǎn)在局部極小位置附近設(shè)置中間目標(biāo)引導(dǎo)。我實(shí)際用的是第二種加入虛擬子目標(biāo)后的效果明顯更穩(wěn)定。5.2 多機(jī)避讓時(shí)出現(xiàn)“振蕩和乒乓效應(yīng)”兩架無(wú)人機(jī)迎面相遇時(shí)如果雙方同時(shí)往同一方向避讓會(huì)出現(xiàn)反復(fù)橫跳最終相撞的“乒乓現(xiàn)象”。這個(gè)問(wèn)題的本質(zhì)是協(xié)同邏輯缺失每一方都不知道對(duì)方會(huì)選擇哪側(cè)避讓。我的解決方案是引入避讓方向約定在checkPairConflict函數(shù)中約定兩架相遇的無(wú)人機(jī)統(tǒng)一從對(duì)方右側(cè)通過(guò)。這個(gè)約定信息在初始化時(shí)寫入每架無(wú)人機(jī)遇到?jīng)_突時(shí)按約定方向計(jì)算避讓速度振蕩問(wèn)題迎刃而解。5.3 參數(shù)過(guò)于敏感改一處崩全盤協(xié)同系統(tǒng)的參數(shù)彼此關(guān)聯(lián)單獨(dú)調(diào)整一個(gè)可能引發(fā)連鎖反應(yīng)。比如減少安全距離防撞觸發(fā)變少路徑效率提高但多機(jī)距離過(guò)近時(shí)氣流干擾和飛控風(fēng)險(xiǎn)上升增大斥力增益路徑更保守但多機(jī)通過(guò)率下降重規(guī)劃更頻繁。我的做法是建立“參數(shù)參考表”把每類場(chǎng)景的關(guān)鍵參數(shù)記錄下來(lái)場(chǎng)景安全距離斥力增益速度上限說(shuō)明開闊巡檢1.02.03.0低密度追求效率城市物流1.53.02.0障礙密集保守飛行編隊(duì)表演0.81.52.5隊(duì)形保持優(yōu)先應(yīng)急救援1.22.54.0速度優(yōu)先容錯(cuò)較低這樣在復(fù)現(xiàn)實(shí)驗(yàn)或切換到新場(chǎng)景時(shí)不是盲目試參數(shù)而是以參考表為基礎(chǔ)做針對(duì)性微調(diào)效率高得多。5.4 仿真速度慢多機(jī)數(shù)量增加后幀率塌陷當(dāng)無(wú)人機(jī)數(shù)量從4架增加到8架時(shí)如果防撞邏輯依然使用全矩陣枚舉單步耗時(shí)會(huì)顯著增加。我嘗試過(guò)兩種優(yōu)化方式一是將避讓速度的候選集合從全平面網(wǎng)格改為扇形采樣即只考慮當(dāng)前速度方向附近的扇形區(qū)域能砍掉六成以上的候選數(shù)量二是使用平滑曲線插值替代部分逐幀防撞計(jì)算在路徑變化不劇烈的時(shí)間窗口內(nèi)直接沿用上一幀結(jié)果。實(shí)測(cè)效果明顯優(yōu)化前8架無(wú)人機(jī)單步計(jì)算約35毫秒優(yōu)化后降到約12毫秒基本可以保證實(shí)時(shí)仿真。5.5 動(dòng)態(tài)障礙物的“預(yù)測(cè)-避讓”滯后動(dòng)態(tài)障礙物移動(dòng)時(shí)只靠當(dāng)前位置計(jì)算避讓會(huì)讓無(wú)人機(jī)像在追逐陀螺一樣被動(dòng)反應(yīng)。改進(jìn)方式是在沖突檢測(cè)中加入障礙物運(yùn)動(dòng)趨勢(shì)預(yù)測(cè)% 預(yù)測(cè)障礙物未來(lái)位置用預(yù)測(cè)位置參與避讓計(jì)算 predictT 1.0; % 預(yù)測(cè)時(shí)間(s) predictedObsPos obs.position obs.velocity * predictT;這個(gè)預(yù)測(cè)時(shí)間也不算越高越好它受障礙物運(yùn)動(dòng)穩(wěn)定性影響障礙物速度較穩(wěn)取長(zhǎng)預(yù)測(cè)變化快取短值。我一般讓無(wú)人機(jī)在距離障礙物兩倍安全距離之外時(shí)才啟用預(yù)測(cè)避讓這個(gè)距離按當(dāng)前速度算出。6. 成果展示與實(shí)際效果評(píng)估仿真跑通后我習(xí)慣做三組對(duì)比實(shí)驗(yàn)來(lái)驗(yàn)證系統(tǒng)的有效性靜態(tài)環(huán)境基準(zhǔn)實(shí)驗(yàn)、動(dòng)態(tài)環(huán)境單機(jī)防撞實(shí)驗(yàn)、動(dòng)態(tài)環(huán)境多機(jī)協(xié)同防撞實(shí)驗(yàn)。靜態(tài)環(huán)境基準(zhǔn)實(shí)驗(yàn)用來(lái)檢驗(yàn)路徑規(guī)劃算法的正確性無(wú)人機(jī)從起點(diǎn)出發(fā)繞過(guò)固定障礙抵達(dá)目標(biāo)路徑長(zhǎng)度與理論最短路徑對(duì)比偏差不超過(guò)8%。動(dòng)態(tài)環(huán)境單機(jī)實(shí)驗(yàn)引入運(yùn)動(dòng)障礙物驗(yàn)證大部分時(shí)間無(wú)人機(jī)從初始規(guī)劃出發(fā)在探測(cè)到接近障礙時(shí)及時(shí)觸發(fā)避讓邏輯路徑平滑度有小幅損失但安全性完全滿足要求。最關(guān)鍵的第三組實(shí)驗(yàn)是多機(jī)協(xié)同我設(shè)置了4架無(wú)人機(jī)、2個(gè)動(dòng)態(tài)障礙物和一個(gè)共享目標(biāo)區(qū)域。系統(tǒng)運(yùn)行100秒內(nèi)累計(jì)發(fā)生了14次潛在沖突全部被防撞層成功化解沒有發(fā)生實(shí)際距離小于安全閾值的危險(xiǎn)事件同時(shí)任務(wù)完成率達(dá)到100%所有無(wú)人機(jī)都按時(shí)到達(dá)了指定區(qū)域。這套方案在實(shí)際項(xiàng)目中的價(jià)值不僅在于仿真圖看起來(lái)漂亮而是把“動(dòng)態(tài)環(huán)境”和“協(xié)同防撞”這兩件看起來(lái)復(fù)雜的事拆成了清晰可復(fù)用的模塊化流程。從規(guī)劃到防撞每一層都可以獨(dú)立調(diào)優(yōu)。7. 從這套研究中總結(jié)出的幾點(diǎn)個(gè)人體會(huì)最后分享幾個(gè)我個(gè)人在實(shí)踐中收獲比較大的體會(huì)。第一多無(wú)人機(jī)協(xié)同的難點(diǎn)不在算法本身而在對(duì)系統(tǒng)狀態(tài)的統(tǒng)一認(rèn)知。每架無(wú)人機(jī)對(duì)環(huán)境的認(rèn)知可能存在偏差防撞機(jī)制必須允許這種偏差存在并做兼容處理。速度障礙法本質(zhì)上就是在沒有絕對(duì)全局信息條件下的保守決策所以在分布式的動(dòng)態(tài)環(huán)境中表現(xiàn)得格外穩(wěn)定。第二仿真時(shí)的“理論最優(yōu)”和“工程可用”之間永遠(yuǎn)有距離。我在調(diào)試過(guò)程中最大的收獲就是學(xué)會(huì)給自己的規(guī)劃結(jié)果打上“物理可行性”的校驗(yàn)標(biāo)簽——不光是路徑能走還要看加速度、轉(zhuǎn)彎半徑、通信延遲這些工程約束能不能滿足。第三參數(shù)管理應(yīng)該從第一版代碼就重視起來(lái)。所有關(guān)鍵參數(shù)集中定義命名規(guī)范統(tǒng)一這樣后續(xù)無(wú)論是調(diào)試還是交接都能省下大量時(shí)間。這套Matlab代碼實(shí)現(xiàn)現(xiàn)在已經(jīng)整理成了固定版本對(duì)動(dòng)態(tài)環(huán)境下多無(wú)人機(jī)協(xié)同路徑規(guī)劃研究來(lái)說(shuō)我自己的體會(huì)是不需要一上來(lái)就追求多么高級(jí)的算法把基礎(chǔ)的勢(shì)場(chǎng)規(guī)劃、速度障礙防撞、優(yōu)先級(jí)仲裁做好配合合理的重規(guī)劃策略已經(jīng)能覆蓋大多數(shù)場(chǎng)景。后續(xù)如果你也想在這個(gè)方向深入可以從動(dòng)態(tài)目標(biāo)分配、通信約束建模、三維復(fù)雜環(huán)境擴(kuò)展到更多維度。