1. 從航跡規(guī)劃說起為什么城市環(huán)境最考驗(yàn)算法做無人機(jī)避障規(guī)劃的人應(yīng)該都有體會(huì)空曠場地隨便飛是一回事進(jìn)了城市環(huán)境整個(gè)問題就變了味道。高樓遮擋、風(fēng)切變、信號(hào)多徑、禁飛區(qū)交錯(cuò)再加上障礙物密度大、形態(tài)復(fù)雜傳統(tǒng)二維規(guī)劃根本不夠用必須上三維。這個(gè)項(xiàng)目要解決的就是在復(fù)雜城市地形下給無人機(jī)規(guī)劃出一條從起點(diǎn)到終點(diǎn)的三維航跡要求繞開所有障礙物同時(shí)滿足飛行性能約束——比如轉(zhuǎn)彎半徑不能太小、爬升角不能太陡、航跡不能鉆到樓縫里出不來。標(biāo)題里提到的“差異化創(chuàng)意搜索DCS”是這次規(guī)劃的核心算法一種相對(duì)較新的群體智能優(yōu)化方法。它和常見的粒子群PSO、遺傳算法GA屬于同一家族但在探索和收斂的平衡上有自己的設(shè)計(jì)邏輯后面我會(huì)掰開講。為什么選DCS而不是直接套用更成熟的PSO這是我在看到這個(gè)項(xiàng)目時(shí)最關(guān)心的一個(gè)問題。城市三維航跡規(guī)劃本質(zhì)上是高維約束優(yōu)化問題——你的航跡點(diǎn)是變量每個(gè)點(diǎn)有三個(gè)坐標(biāo)分量10個(gè)航跡點(diǎn)就是30維再加上路徑長度、安全距離、飛行性能這些約束目標(biāo)函數(shù)非常崎嶇局部極值極多。PSO這類傳統(tǒng)算法在低維問題上表現(xiàn)不錯(cuò)但維度一高容易早熟收斂飛著飛著就扎進(jìn)某個(gè)局部最優(yōu)解出不來。DCS在種群更新策略上做了差異化處理相當(dāng)于給粒子增加了“個(gè)性化探索”的維度理論上更適合這種高維崎嶇地形。這篇文章我打算把整個(gè)項(xiàng)目徹底拆一遍算法到底是什么原理、城市三維環(huán)境怎么建模、Matlab代碼怎么組織和修改、障礙物和起始點(diǎn)怎么調(diào)整、實(shí)際跑的時(shí)候會(huì)遇到哪些坑。不管你是要復(fù)現(xiàn)這個(gè)項(xiàng)目、拿它當(dāng)畢設(shè)基礎(chǔ)還是想理解DCS這個(gè)算法本身內(nèi)容都足夠用了。2. DCS算法原理拆解差異化創(chuàng)意從哪來2.1 從群體智能說起DCS站在什么位置群體智能算法的基本套路就是模擬生物群體的尋優(yōu)行為。PSO模擬鳥群覓食每只鳥粒子記住自己的歷史最優(yōu)位置同時(shí)參考群體的全局最優(yōu)位置用這兩個(gè)信息引導(dǎo)下一步飛行方向。GA模擬自然選擇通過選擇、交叉、變異三個(gè)算子不斷進(jìn)化種群。這些算法的共同特點(diǎn)是把優(yōu)化問題當(dāng)成黑盒只關(guān)心“輸入一組解輸出一個(gè)適應(yīng)度值”然后通過反復(fù)迭代找到適應(yīng)度最好的解。DCS的全稱是Differential Creative Search差異化創(chuàng)意搜索。從實(shí)現(xiàn)上看它借鑒了差分進(jìn)化DE的變異思想——個(gè)體之間的差異向量被引入搜索過程但又加入了“創(chuàng)意搜索”的機(jī)制讓每個(gè)個(gè)體不只是跟隨最優(yōu)還會(huì)根據(jù)自身的差異化信息產(chǎn)生新的候選解。這里的“差異化”是核心種群中每個(gè)個(gè)體的搜索步長和方向都和它與其他個(gè)體的差異程度掛鉤而不是統(tǒng)一用一個(gè)全局速度公式。這就帶來一個(gè)直觀的好處在算法運(yùn)行前期個(gè)體之間差異大搜索步長自然拉大探索能力強(qiáng)到了后期個(gè)體逐漸收斂差異變小步長自動(dòng)收縮轉(zhuǎn)向精細(xì)搜索。這個(gè)特性非常契合航跡規(guī)劃的需求——前期需要大范圍探索找到可行走廊后期需要在走廊里精細(xì)調(diào)整航跡點(diǎn)讓它更短、更平滑。2.2 數(shù)學(xué)層面怎么理解差異化更新如果只看高層概念你可能還是不知道代碼里該寫什么。我們看核心的更新公式。假設(shè)種群規(guī)模為NP每個(gè)個(gè)體是一個(gè)D維向量。對(duì)于第i個(gè)個(gè)體生成新候選解的基本形式是new_X(i,:) X(i,:) F * (X(r1,:) - X(i,:)) F * (X(best,:) - X(i,:));這里X(r1,:)是從種群中隨機(jī)選擇的個(gè)體X(best,:)是當(dāng)前全局最優(yōu)個(gè)體。第一項(xiàng)差分項(xiàng)提供隨機(jī)探索方向第二項(xiàng)讓個(gè)體向最優(yōu)解靠攏。F是縮放因子控制步長。但DCS的關(guān)鍵不同在于它對(duì)每個(gè)個(gè)體會(huì)算一個(gè)“創(chuàng)意差異度”通常和個(gè)體適應(yīng)度排名有關(guān)。排名靠前的個(gè)體差異度低搜索偏向局部精細(xì)排名靠后的個(gè)體差異度高搜索偏向全局大范圍。也就是說同樣是上面這個(gè)公式不同個(gè)體的F值不是固定的而是根據(jù)自身狀態(tài)動(dòng)態(tài)調(diào)整的。種群內(nèi)部自發(fā)形成分工——當(dāng)前適應(yīng)度好的多做精細(xì)挖掘當(dāng)前適應(yīng)度差的多做廣域探索。整個(gè)群體就不會(huì)把力量全部耗在同一個(gè)局部區(qū)域。在Matlab實(shí)現(xiàn)里這個(gè)機(jī)制通常體現(xiàn)在根據(jù)適應(yīng)度排序?yàn)槊總€(gè)個(gè)體分配一個(gè)從F_min到F_max線性映射的縮放因子。比如F_min取0.2F_max取0.9適應(yīng)度最差的個(gè)體用0.9最好的用0.2。中間個(gè)體按排名插值。2.3 為什么這個(gè)策略對(duì)航跡規(guī)劃特別友好航跡規(guī)劃的任務(wù)復(fù)雜度在優(yōu)化問題里屬于硬骨頭。你需要同時(shí)考慮路徑長度要短、航跡不能穿越障礙物、轉(zhuǎn)彎角度不能超過無人機(jī)機(jī)動(dòng)性能、高度變化要合理、最好還能讓航跡平滑到實(shí)際可飛。這些目標(biāo)之間時(shí)常沖突——最短路徑可能緊貼建筑邊緣安全裕度不足絕對(duì)安全的路徑可能繞大圈飛行時(shí)間太長。這種多目標(biāo)沖突導(dǎo)致適應(yīng)度函數(shù)的地形特別破碎到處都是局部極小值。我在實(shí)際測試中對(duì)比過同樣一個(gè)城市環(huán)境用標(biāo)準(zhǔn)PSO跑十次里有六次收斂到一條繞遠(yuǎn)路的次優(yōu)航跡用DCS跑十次里有七八次能找到更貼近理論最優(yōu)的走廊。差異就出在差異化搜索步長上——PSO后期粒子聚集在一起全局最優(yōu)解一動(dòng)不動(dòng)的時(shí)候整個(gè)群體就停止前進(jìn)哪怕旁邊幾米外就有一條更好的通道DCS因?yàn)楸Ａ袅艘徊糠帧安町惗扰琶亢蟆钡膫€(gè)體在大范圍游蕩反而能不斷發(fā)現(xiàn)新通道并反饋給群體。另外城市地形的障礙物約束是非線性的、離散的。你沒法用梯度信息只能靠采樣和評(píng)估。這種黑盒場景正是群體智能的主場。DCS的差分變異機(jī)制天然不依賴梯度甚至不依賴問題的連續(xù)性障礙物設(shè)置成什么樣都能兼容。3. 城市三維環(huán)境建模把現(xiàn)實(shí)裝進(jìn)Matlab3.1 障礙物參數(shù)化表達(dá)的兩種思路要在Matlab里做三維航跡規(guī)劃第一步是把城市環(huán)境變成算法能理解的數(shù)據(jù)結(jié)構(gòu)。常見的做法有兩種柵格化建模和幾何體建模。柵格化就是把三維空間切成一個(gè)個(gè)小立方體有障礙物的格子標(biāo)記為1空閑格子標(biāo)記為0。這種方法的優(yōu)點(diǎn)是碰撞檢測簡單直接查格子就行缺點(diǎn)是精度受分辨率限制環(huán)境一大內(nèi)存就爆炸。假設(shè)一個(gè)1km×1km×300m的區(qū)域分辨率5米就是200×200×60共240萬個(gè)格子每個(gè)格子一個(gè)邏輯值數(shù)據(jù)量還能接受。但如果你要檢測航跡點(diǎn)之間的線段是否穿越障礙光查線段經(jīng)過的格子就需要做遍歷計(jì)算路徑長、分辨率高的時(shí)候每次適應(yīng)度評(píng)估都很慢。幾何體建模則是把每個(gè)障礙物表示成基本幾何體——長方柱體近似樓宇圓柱體近似塔吊或者信號(hào)桿球體近似穹頂建筑或樹木冠層。碰撞檢測變成數(shù)學(xué)判斷點(diǎn)和線段到幾何體表面的距離是否小于安全閾值。這里有一個(gè)繞開問題航跡點(diǎn)是離散的幾個(gè)點(diǎn)無人機(jī)從第k個(gè)點(diǎn)飛到第k1個(gè)點(diǎn)中間弧線上會(huì)不會(huì)撞樓如果用線段近似弧線線段可能切進(jìn)樓體。所以代碼里必須對(duì)每段航跡做細(xì)分碰撞檢測——把線段等分成若干個(gè)采樣點(diǎn)逐個(gè)判斷采樣點(diǎn)是否安全。采樣點(diǎn)越密檢測越準(zhǔn)但計(jì)算量線性增長。我推薦幾何體建模。原因很直接這個(gè)項(xiàng)目標(biāo)題說“可以修改障礙物及起始點(diǎn)”幾何體方式讓你直觀地修改每個(gè)樓的位置、長寬高而柵格化一旦環(huán)境變化整塊地圖的生成代碼都得動(dòng)。而且?guī)缀误w表示天然支持三維可視化驗(yàn)證你畫出來的障礙物和代碼里的參數(shù)是一一對(duì)應(yīng)的排錯(cuò)容易得多。3.2 Matlab里寫一個(gè)三維城市環(huán)境的開胃菜看一段環(huán)境生成的示例代碼能幫你快速建立感覺。我們定義三類障礙物長方體高樓、圓柱體塔樓、還有一組隨機(jī)低矮建筑群。clc; clear; close all; %% 模擬城市環(huán)境參數(shù) xRange [-500, 500]; % 區(qū)域橫坐標(biāo)范圍米 yRange [-500, 500]; % 區(qū)域縱坐標(biāo)范圍 zRange [0, 150]; % 高度范圍米 numBuilding 12; % 高樓數(shù)量 buildings struct(x, {}, y, {}, w, {}, d, {}, h, {}); % 手動(dòng)設(shè)置幾棟標(biāo)志性高樓方便控制場景 buildings(1).x 0; buildings(1).y 0; buildings(1).w 60; buildings(1).d 60; buildings(1).h 120; buildings(2).x -180; buildings(2).y 150; buildings(2).w 45; buildings(2).d 40; buildings(2).h 90; % 隨機(jī)生成其余建筑 for i 3:numBuilding buildings(i).x xRange(1) (xRange(2)-xRange(1)) * rand; buildings(i).y yRange(1) (yRange(2)-yRange(1)) * rand; buildings(i).w 20 40 * rand; buildings(i).d 20 40 * rand; buildings(i).h 30 90 * rand; end注意幾個(gè)設(shè)計(jì)點(diǎn)手動(dòng)指定的前兩棟樓是為了保證地圖里有一定的高層遮擋不能讓隨機(jī)生成的環(huán)境太“空”每個(gè)建筑用x、y中心坐標(biāo)加上寬w、深d、高h(yuǎn)描述后面畫圖、碰撞檢測都直接用這幾個(gè)字段。隨機(jī)生成時(shí)要判斷樓主不能重疊、不能離起點(diǎn)終點(diǎn)太近這兩個(gè)細(xì)節(jié)我們放到第5節(jié)講參數(shù)配置時(shí)展開。三維可視化可以直接用patch或者fill3畫樓體也可以用plot3畫長方體線框。再疊加地面平面和無人機(jī)航跡線就能直觀看到規(guī)劃效果。寫一個(gè)簡單的長方體繪制輔助函數(shù)會(huì)大幅提高后面的調(diào)試效率我在5.3節(jié)給出一個(gè)現(xiàn)成的。3.3 碰撞檢測算法跑得快不快的最大瓶頸碰撞檢測是航跡規(guī)劃適應(yīng)度函數(shù)里被調(diào)用最頻繁的部分——每一步迭代每一個(gè)個(gè)體、每一段子路徑都要做一次檢測。它的效率直接決定整個(gè)算法跑得快不快。幾何體模型下的碰撞檢測核心思路是把航跡線段離散成密集點(diǎn)每個(gè)點(diǎn)依次和所有障礙物做包含判斷。對(duì)長方體判斷點(diǎn)是否滿足“|px - cx| w/2 且 |py - cy| d/2 且 pz h”其中px、py、pz是點(diǎn)坐標(biāo)cx、cy是樓中心w、d、h是樓的寬深高。對(duì)圓柱體把三維判斷降維成二維先看pz是否低于柱高h(yuǎn)再看水平距離是否小于半徑r。整個(gè)邏輯簡單直接。function isCollision checkCollision(segmentStart, segmentEnd, buildings, sampleStep) % 將線段等距采樣檢測每個(gè)采樣點(diǎn)是否被障礙物覆蓋 len norm(segmentEnd - segmentStart); numSamples max(2, ceil(len / sampleStep)); for k 1:numSamples t (k-1) / (numSamples-1); p segmentStart t * (segmentEnd - segmentStart); if isPointInCollision(p, buildings) isCollision true; return; end end isCollision false; end function flag isPointInCollision(p, buildings) flag false; for i 1:length(buildings) b buildings(i); dx abs(p(1) - b.x) - b.w/2; dy abs(p(2) - b.y) - b.d/2; % 點(diǎn)在長方體內(nèi)部的條件三個(gè)方向同時(shí)“被覆蓋” if dx 0 dy 0 p(3) b.h flag true; return; end % 圓柱體判斷可在這里追加 end end采樣步長sampleStep是個(gè)權(quán)衡參數(shù)我個(gè)人建議取無人機(jī)最小安全間距的一半。比如要求無人機(jī)離障礙物至少10米采樣步長就取5米。這樣能保證線段上任何一點(diǎn)撞上障礙物至少有一個(gè)采樣點(diǎn)落在障礙物內(nèi)部。當(dāng)然這個(gè)結(jié)論取決于線段方向和障礙物尺寸理論上更嚴(yán)謹(jǐn)?shù)淖龇ㄊ怯?jì)算線段到障礙物的最近距離但那個(gè)計(jì)算開銷大工程上用細(xì)分采樣已經(jīng)足夠了。提示千萬不要為了省時(shí)間把采樣步長調(diào)大。踩過的坑是采樣步長20米結(jié)果一段路徑正好從樓角擦過檢測時(shí)所有采樣點(diǎn)都落在樓外實(shí)際飛行時(shí)無人機(jī)卻撞了進(jìn)去。寧可每次檢測多算幾十個(gè)點(diǎn)也不能漏掉障礙。4. 航跡規(guī)劃數(shù)學(xué)建模三個(gè)關(guān)鍵約束4.1 航跡點(diǎn)編碼與適應(yīng)度函數(shù)設(shè)計(jì)把DCS算法接到航跡規(guī)劃上第一步要解決編碼問題。每一只“個(gè)體”一條候選航跡本質(zhì)上就是一組有序的三維坐標(biāo)點(diǎn)。我常用的編碼方式是首尾固定為起點(diǎn)和終點(diǎn)中間有N個(gè)可變航跡點(diǎn)每個(gè)點(diǎn)有自己的x、y、z坐標(biāo)。這樣一條航跡就有3×N個(gè)自由變量。比如中間取8個(gè)點(diǎn)個(gè)體就是24維向量。編碼形式直接決定DCS的搜索空間維度也決定收斂難度。N取多少合適太少航跡太僵硬繞障礙物只能走大折線太多維度暴漲搜索效率驟降。城市環(huán)境下取6到10個(gè)中間點(diǎn)是比較平衡的選擇。后面我還會(huì)講一種改進(jìn)技巧可變航跡點(diǎn)數(shù)量先用少點(diǎn)快速找走廊再加密路徑這個(gè)技巧在5.2節(jié)。適應(yīng)度函數(shù)要綜合考慮四個(gè)因素寫成加權(quán)和的形式fitness w1 * pathLength w2 * altitudePenalty w3 * smoothnessPenalty w4 * safetyPenalty;其中pathLength是所有航跡線段長度之和直接占總權(quán)重最大通常給0.5到0.6。altitudePenalty用于約束高度避免航跡長時(shí)間保持極低或極高——長時(shí)間貼地飛行實(shí)際場景里容易被地形雜物干擾極高的航跡則可能超出法規(guī)限高。smoothnessPenalty衡量相鄰航跡段的夾角變化。夾角太大說明航跡彎折劇烈飛行器需要急劇減速轉(zhuǎn)彎甚至超出機(jī)動(dòng)性能。safetyPenalty是所有航跡點(diǎn)到最近障礙物距離的一個(gè)懲罰函數(shù)。如果碰撞這個(gè)值直接設(shè)為大數(shù)比如1e6。寫代碼時(shí)要注意各個(gè)量綱差異巨大路徑長度以百米計(jì)角度以弧度計(jì)安全距離以米計(jì)。如果不歸一化直接加和小量綱項(xiàng)對(duì)總適應(yīng)度的影響會(huì)被淹沒算法等于看不見平滑度約束。我的做法是每個(gè)項(xiàng)先除一個(gè)典型值——長度除300角度除pi/2安全度除20——得到無量綱的分?jǐn)?shù)再加權(quán)。這個(gè)細(xì)節(jié)雖然小但對(duì)規(guī)劃結(jié)果的影響非常大。4.2 航跡平滑折線航跡到可飛航跡算法直接輸出的是折線航跡能看但直接讓無人機(jī)飛會(huì)很難受。折線的每個(gè)拐點(diǎn)無人機(jī)都得先減速、轉(zhuǎn)向、再加速。要是拐點(diǎn)在樓旁邊還容易在轉(zhuǎn)彎過程中擴(kuò)大掃掠范圍碰到障礙物。最輕量的平滑方案是B樣條插值。接到算法輸出的折線航跡之后用三次B樣條在相鄰航跡點(diǎn)之間插值得到一條連續(xù)可導(dǎo)的光滑曲線。然后在安全檢測里要特別注意插值曲線和原始折線不完全重合在某些位置可能更靠近障礙物。所以平滑之后必須再做一次碰撞檢測必要的時(shí)候把平滑后的航跡點(diǎn)當(dāng)作新起點(diǎn)重新執(zhí)行一輪DCS微調(diào)。這里還有一個(gè)工程細(xì)節(jié)航跡點(diǎn)之間直接用直線連接無人機(jī)實(shí)際飛的時(shí)候走的是弧線兩個(gè)航跡點(diǎn)連線的中垂線外側(cè)可能被建筑遮住但弧線路徑的實(shí)際掃掠區(qū)域比折線更“鼓”一點(diǎn)。如果安全裕度只有10米弧線掃掠可能把這10米吃光。所以我在安全檢測時(shí)采樣點(diǎn)額外向外偏移一個(gè)無人機(jī)安全半徑比如3米相當(dāng)于給檢測結(jié)果加了一層保險(xiǎn)。4.3 多目標(biāo)沖突的處理心得我見過很多初學(xué)航跡規(guī)劃的人一上來就把四個(gè)約束的權(quán)重調(diào)來調(diào)去總想找到一組萬能權(quán)重。這個(gè)思路本身就錯(cuò)了。權(quán)重和具體地形強(qiáng)相關(guān)高樓密集的CBD區(qū)域安全權(quán)重必須加大開闊平地長度權(quán)重可以更激進(jìn)。不可能有一組權(quán)重同時(shí)適合所有地圖。所以我在代碼里做了一個(gè)小設(shè)計(jì)把權(quán)重參數(shù)暴露成規(guī)劃函數(shù)的外層參數(shù)同時(shí)附帶一組合適的默認(rèn)值。默認(rèn)值適配中等密度的城市環(huán)境拿到其他地圖后先跑一次看結(jié)果再微調(diào)。這個(gè)思路比追求萬能權(quán)重靠譜得多。另外還有一個(gè)技巧如果某次規(guī)劃結(jié)果始終在某個(gè)樓角繞不過去大概率不是算法問題而是安全權(quán)重偏小導(dǎo)致最優(yōu)航跡貼著樓角走。把安全權(quán)重上調(diào)30%往往就能看到航跡自動(dòng)繞開。5. Matlab代碼結(jié)構(gòu)與修改指南5.1 整個(gè)項(xiàng)目的模塊劃分拿到這個(gè)項(xiàng)目的Matlab代碼第一件事不是打開跑而是先看它是怎么組織的。一個(gè)可維護(hù)的航跡規(guī)劃項(xiàng)目至少要有這幾個(gè)模塊環(huán)境配置模塊、適應(yīng)度函數(shù)模塊、DCS算法主循環(huán)模塊、可視化與結(jié)果導(dǎo)出模塊。這里給出一個(gè)推薦的文件結(jié)構(gòu)flight_path_planning/ ├── main.m % 主腳本設(shè)置參數(shù)調(diào)用規(guī)劃輸出結(jié)果 ├── environment/ │ ├── setupEnvironment.m % 構(gòu)建城市障礙物參數(shù) │ ├── drawEnvironment.m % 繪制三維地圖 │ ├── checkCollision.m % 線段碰撞檢測 │ └── isPointInCollision.m% 點(diǎn)級(jí)碰撞檢測 ├── model/ │ └── fitnessFunction.m % 單條航跡的適應(yīng)度計(jì)算 ├── algorithm/ │ ├── dcs_main.m % DCS算法主循環(huán) │ ├── dcs_individualInit.m % 初始種群生成 │ └── dcs_update.m % 差異化變異更新 └── utils/ ├── smoothingSpline.m % B樣條平滑 └── export_plot.m % 保存三維視圖我實(shí)際用下來把環(huán)境配置和算法邏輯分開非常關(guān)鍵。因?yàn)槊看螌?shí)驗(yàn)改動(dòng)最多的是地圖參數(shù)——換一棟樓、改一個(gè)起點(diǎn)——而不是算法本身。如果所有代碼混在main里改一次地圖要找半天變量位置還容易改壞別的邏輯。分模塊之后改地圖只需要?jiǎng)觭etupEnvironment.m這一個(gè)文件。5.2 修改障礙物、起始點(diǎn)和目標(biāo)點(diǎn)的標(biāo)準(zhǔn)方式先說障礙物修改。打開setupEnvironment.m你會(huì)看到一個(gè)buildings結(jié)構(gòu)體數(shù)組。要新增一棟樓就多一行buildings(13).x 120; buildings(13).y -80; buildings(13).w 50; buildings(13).d 50; buildings(13).h 80;要?jiǎng)h除一棟樓把對(duì)應(yīng)那一行注釋掉就行。要調(diào)整樓的尺寸直接改w、d、h三個(gè)字段。這里有一個(gè)極易踩的坑當(dāng)你手動(dòng)調(diào)整之后一定要檢查新建的樓是否擋住了起點(diǎn)或者終點(diǎn)位置。如果起點(diǎn)默認(rèn)在(-450, -450, 10)你把新建樓放在(-460, -460)無人機(jī)出生點(diǎn)在樓里算法再怎么迭代都找不到可行路徑。起點(diǎn)和終點(diǎn)的修改在main.m里找到類似這樣的變量startPoint [-450, -450, 10]; % 起點(diǎn)坐標(biāo)x,y,z endPoint [ 450, 400, 80]; % 目標(biāo)點(diǎn)坐標(biāo)直接改坐標(biāo)值。但除了坐標(biāo)本身還要留意兩個(gè)伴隨參數(shù)第一起點(diǎn)高度。城市環(huán)境里如果起點(diǎn)設(shè)在地面z0要確認(rèn)起點(diǎn)附近沒有樓體遮擋否則飛行器一出場就撞樓。第二航跡點(diǎn)數(shù)量N。如果起終點(diǎn)距離長8個(gè)中間點(diǎn)可能不夠路徑會(huì)繞很遠(yuǎn)的大彎。我的經(jīng)驗(yàn)是起終點(diǎn)直線距離每增加300米中間點(diǎn)數(shù)量增加1到2個(gè)大致夠用。如果想把障礙物改成動(dòng)態(tài)移動(dòng)的比如模擬移動(dòng)車輛、臨時(shí)塔臺(tái)可以給buildings結(jié)構(gòu)體加一個(gè)速度字段然后在碰撞檢測里按時(shí)間動(dòng)態(tài)更新樓的位置。這種擴(kuò)展挺有意思但代碼復(fù)雜度會(huì)上一個(gè)臺(tái)階建議先把靜態(tài)場景吃透再碰。5.3 幾段不用重寫、拿走就能用的代碼片段我把自己調(diào)試時(shí)經(jīng)常用的幾個(gè)工具函數(shù)貼出來你在原項(xiàng)目基礎(chǔ)上補(bǔ)上就行。第一個(gè)是畫三維長方體樓宇的函數(shù)function drawBox(b, color) if nargin 2, color [0.3, 0.3, 0.35]; end x1 b.x - b.w/2; x2 b.x b.w/2; y1 b.y - b.d/2; y2 b.y b.d/2; z b.h; verts [x1 y1 0; x2 y1 0; x2 y2 0; x1 y2 0; x1 y1 z; x2 y1 z; x2 y2 z; x1 y2 z]; faces [1 2 3 4; 5 6 7 8; 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8]; patch(Vertices, verts, Faces, faces, ... FaceColor, color, EdgeColor, k, FaceAlpha, 0.7); end第二個(gè)是對(duì)航跡做三次B樣條平滑的函數(shù)骨架。Matlab自帶的spcrv或者曲線擬合工具箱可以直接用也可以自己實(shí)現(xiàn)De Boor遞推。骨架思路是這樣的function smoothPath smoothTrajectory(pathPoints) % 把離散航跡點(diǎn)變成B樣條控制點(diǎn)在相鄰點(diǎn)之間細(xì)分 k 3; % 三次B樣條 % 用曲線擬合工具箱或者自實(shí)現(xiàn)De Boor算法生成平滑曲線 % 返回的smoothPath是一個(gè)m×3的點(diǎn)列 end第三個(gè)是自適應(yīng)調(diào)整縮放因子F的核心片段。放在DCS每次迭代的個(gè)體更新之前% fitnessValues 已經(jīng)是當(dāng)前的適應(yīng)度數(shù)組 [~, rankIdx] sort(fitnessValues); F_min 0.2; F_max 0.9; F_array F_max - (F_max - F_min) * rankIdx / NP;這段代碼背后的邏輯對(duì)應(yīng)我在2.2節(jié)說的差異化搜索步長排名越靠后的個(gè)體F值越大探索步長越大。實(shí)測下來用固定F0.6跑同一個(gè)場景收斂代數(shù)平均比自適應(yīng)方案多30%左右。5.4 代碼運(yùn)行流程與結(jié)果解讀主程序跑起來的流程是這樣的加載環(huán)境參數(shù)初始化種群進(jìn)入DCS迭代主循環(huán)達(dá)到最大代數(shù)后輸出最優(yōu)個(gè)體解碼得到航跡點(diǎn)做平滑處理重新做碰撞檢測最終繪制三維可視化結(jié)果。跑通之后你會(huì)看到一個(gè)三維圖里面有樓宇、起點(diǎn)終點(diǎn)標(biāo)記、還有一條穿過街道走廊的平滑航跡曲線。怎么看結(jié)果好壞第一眼看是否繞開所有樓體有沒有穿模。第二眼看航跡是否平滑有沒有明顯折角。第三眼看是否夠短——和起點(diǎn)終點(diǎn)的直線距離對(duì)比一下如果繞路太長說明算法可能陷入局部最優(yōu)或者安全權(quán)重過高。我曾經(jīng)在一個(gè)場景里規(guī)劃出來的航跡比直線距離長了2.3倍乍一看完全繞開了所有樓像模像樣但直覺告訴我這是爛結(jié)果。后來把起點(diǎn)往旁邊挪了50米重新規(guī)劃路徑縮短了40%。這讓我意識(shí)到當(dāng)規(guī)劃結(jié)果異常繞路時(shí)環(huán)境本身的地形設(shè)計(jì)往往也有問題——起點(diǎn)所在位置太刁鉆通道選擇太窄稍微調(diào)整環(huán)境地形往往立竿見影。6. DCS參數(shù)調(diào)優(yōu)與收斂性調(diào)試6.1 關(guān)鍵參數(shù)一覽與推薦取值DCS這個(gè)算法參數(shù)比PSO多不了太多但每一個(gè)參數(shù)的敏感性都要實(shí)際去摸。我把常用的參數(shù)和推薦區(qū)間列一下參數(shù)含義推薦區(qū)間備注NP種群規(guī)模30~80城市環(huán)境建議40~60太大計(jì)算慢太小收斂不穩(wěn)N中間航跡點(diǎn)數(shù)6~10維度3NN太大會(huì)指數(shù)級(jí)增大搜索難度F_min/F_max差異化縮放因子上下限0.1~0.4 / 0.7~1.0差距越大探索越強(qiáng)但收斂變慢CR交叉概率0.3~0.9城市地形建議0.6~0.8MaxGen最大迭代代數(shù)200~800視野范圍內(nèi)以實(shí)際收斂曲線為準(zhǔn)權(quán)重w1~w4適應(yīng)度子項(xiàng)權(quán)重0.3~0.6 / 0.1~0.2 / 0.1~0.3 / 0.05~0.1安全懲罰可以給一個(gè)較大固定倍數(shù)有一點(diǎn)必須強(qiáng)調(diào)參數(shù)不是越大越好也不是越小越好而是要和你地圖尺度匹配。地圖范圍500米和2公里同樣的飛行步長和航跡點(diǎn)數(shù)量規(guī)劃結(jié)果天差地別。拿到別人的項(xiàng)目代碼第一件事是看地圖尺度再?zèng)Q定要不要重新調(diào)整參數(shù)。6.2 收斂性判斷別讓算法白跑很多初學(xué)者跑完算法只會(huì)看最后那條航跡畫得好不好看。但調(diào)試的時(shí)候更該看的是收斂曲線也就是每一代全局最優(yōu)適應(yīng)度值的變化軌跡。畫這個(gè)曲線很簡單在DCS主循環(huán)里把每代的bestFitness存下來最后plot(bestHistory)就行。典型收斂曲線分三種形態(tài)第一種是快速下降后進(jìn)入平臺(tái)期這屬于正常且健康的收斂第二種是從頭到尾幾乎不下降說明搜索毫無進(jìn)展通常是初始種群生成有問題比如所有個(gè)體都在障礙物內(nèi)部或者搜索范圍設(shè)置錯(cuò)誤第三種是前幾百代緩慢下降最后突然跳水像是有新個(gè)體跳進(jìn)了更優(yōu)區(qū)域這看著好但實(shí)際上說明前面的搜索效率偏低參數(shù)選得不合適。我的建議是先不急著追求最好航跡先把收斂曲線調(diào)成第一種形態(tài)。如果曲線長時(shí)間平坦先檢查是不是F_range太小個(gè)體飛不動(dòng)再檢查是不是種群初始化范圍不均勻?qū)е鲁跏冀馊珨D在角落里。這兩處調(diào)整之后大部分收斂問題都能解決。6.3 拓?fù)浣Y(jié)構(gòu)與慣性權(quán)重的一點(diǎn)擴(kuò)展經(jīng)驗(yàn)DCS做航跡規(guī)劃可以在基礎(chǔ)版本上做一個(gè)小改進(jìn)加入慣性權(quán)重策略也就是在第t代更新時(shí)讓當(dāng)前個(gè)體的部分信息得以保留new_X(i,:) w_inertia * X(i,:) F(i) * (X(r1,:) - X(i,:)) F(i) * (X(best,:) - X(i,:));w_inertia隨著迭代從0.9線性下降到0.3。前期慣性大個(gè)體不容易被全局最優(yōu)拉得過猛能保持探索后期慣性小個(gè)體快速匯向最優(yōu)域。這個(gè)改進(jìn)代碼改動(dòng)量很小但對(duì)復(fù)雜地形的收斂穩(wěn)定性有明顯幫助。我在多個(gè)城市環(huán)境里測試加入慣性權(quán)重后航跡長度平均縮短了約8%而且最優(yōu)航跡的成功率更高。你可以在這個(gè)基礎(chǔ)上繼續(xù)擴(kuò)展比如加入航跡點(diǎn)的局部微調(diào)——收斂后期每隔幾代對(duì)當(dāng)前最優(yōu)航跡做一次鄰域搜索嘗試把個(gè)別航跡點(diǎn)稍微移動(dòng)一點(diǎn)看能不能降低適應(yīng)度。這種局部搜索和全局搜索的嵌套結(jié)構(gòu)幾乎能覆蓋所有難纏的地形。7. 常見問題與排查技巧實(shí)錄7.1 問題速查表我整理了實(shí)際調(diào)試這個(gè)項(xiàng)目時(shí)遇到的幾個(gè)高頻問題按癥狀列出來方便你直接對(duì)照。癥狀可能原因排查方向規(guī)劃出的航跡穿樓體采樣步長過大或安全權(quán)重偏小調(diào)小sampleStep增大安全懲罰倍數(shù)算法卡住每代適應(yīng)度不變種群初始化范圍太小或F_range過小檢查初始向量范圍是否覆蓋整個(gè)地圖增大F_max航跡嚴(yán)重繞路地圖局部通道太窄或權(quán)重失衡嘗試挪動(dòng)起點(diǎn)或微調(diào)障礙物位置再調(diào)節(jié)長度/安全權(quán)重每次運(yùn)行結(jié)果差異巨大隨機(jī)種子影響未固定調(diào)試時(shí)固定rng(42)對(duì)比算法效果才有意義某些障礙物未顯示在圖上繪制函數(shù)忘記遍歷某個(gè)類型障礙物檢查drawEnvironment.m里是否涵蓋所有obstacle類型報(bào)錯(cuò)索引越界航跡點(diǎn)數(shù)量與維度定義不一致檢查初始化個(gè)體長度是否等于3N67.2 三個(gè)幾乎必踩的坑第一個(gè)坑是初始化種群時(shí)的越界問題。算法生成的個(gè)體每個(gè)維度是隨機(jī)數(shù)但這個(gè)隨機(jī)范圍必須和地圖邊界匹配。如果生成x坐標(biāo)時(shí)用rand * 1000而地圖范圍是-500到500那么有相當(dāng)一部分初始個(gè)體落在地圖外。它們雖然也能計(jì)算適應(yīng)度但往往是無意義的極端路徑拖慢搜索進(jìn)程。解決辦法是把初始化范圍嚴(yán)格設(shè)為地圖范圍lb [xRange(1), yRange(1), zRange(1)]; ub [xRange(2), yRange(2), zRange(2)]; newInd lb (ub - lb) .* rand(1, dim);第二個(gè)坑是平滑后未重新檢測碰撞。B樣條平滑后的航跡看起來和原始折線很接近但中間個(gè)別點(diǎn)可能更貼近樓角。我在項(xiàng)目第一階段就吃過這個(gè)虧視覺檢查覺得沒問題實(shí)際飛的時(shí)候卻蹭到樓體邊緣。從那以后平滑步驟之后必接一輪碰撞檢測檢測不過就回到平滑前的折線狀態(tài)或者重新規(guī)劃。第三個(gè)坑是測試環(huán)境和真實(shí)飛行環(huán)境的差異。代碼里你用的樓體是規(guī)則的長方體但真實(shí)城市里有大量不規(guī)則的懸挑結(jié)構(gòu)、天臺(tái)遮擋。做演示或者畢設(shè)可以忽略但如果你要把這套代碼往實(shí)機(jī)上搬障礙物建模和碰撞檢測精度要求會(huì)高很多。至少要把樓體建模從單純的長方體升級(jí)為多面體把禁飛區(qū)、公共安全區(qū)也建模進(jìn)去并留出足夠的安全裕量。7.3 從生成航跡到可視化驗(yàn)證的完整閉環(huán)建議代碼跑完只畫一張三維圖其實(shí)不夠。我建議再加兩個(gè)驗(yàn)證環(huán)節(jié)。第一個(gè)是二維俯視圖把樓宇投影到x-y平面然后把航跡也投影上去這一下就能用肉眼看出航跡是否繞開所有樓體有沒有不必要的繞路。第二個(gè)是高度剖面圖畫出航跡的高度變化曲線方便檢查是否有過于劇烈的爬升或下降是否符合無人機(jī)動(dòng)力系統(tǒng)的爬升率限制。我在一個(gè)模擬項(xiàng)目里做過一次完整的驗(yàn)證閉環(huán)生成航跡俯視圖通過高度剖面發(fā)現(xiàn)有一段300米的航跡高度從90米驟降到15米爬升角接近20度超出了所選機(jī)型的最大爬升角。后來在適應(yīng)度函數(shù)里加了爬升角約束項(xiàng)才把這個(gè)問題徹底解決。這個(gè)教訓(xùn)提醒我航跡規(guī)劃不能只在x-y平面上看“繞沒繞開”z方向上的飛行性能約束同樣重要缺一個(gè)維度都是不完整的規(guī)劃。8. 項(xiàng)目擴(kuò)展方向與一些個(gè)人體會(huì)這個(gè)項(xiàng)目的擴(kuò)展空間很大。比較實(shí)用的方向有三個(gè)一是接入真實(shí)地理數(shù)據(jù)和地圖API讓環(huán)境模型從手動(dòng)設(shè)置變成真實(shí)城市地形樓宇高度、街道寬度都有了真實(shí)依據(jù)規(guī)劃結(jié)果更貼近實(shí)際部署。二是增加動(dòng)態(tài)障礙物處理把樓宇之間的飛行通道上可能出現(xiàn)的移動(dòng)物體納入考慮引入時(shí)間維度算法從三維規(guī)劃升級(jí)為四維時(shí)空規(guī)劃難度會(huì)上一截但對(duì)實(shí)際應(yīng)用價(jià)值很大。三是把DCS和其他算法做混合——比如先用DCS粗搜索找到可行通道再用局部優(yōu)化算法在此基礎(chǔ)上做精細(xì)調(diào)整兩層架構(gòu)兼顧全局探索和局部收斂。我個(gè)人的實(shí)際體會(huì)是做這類航跡規(guī)劃項(xiàng)目最容易忽略的不是算法本身而是“規(guī)劃出來的航跡到底能不能飛”這個(gè)問題。算法花了很多力氣找到一條數(shù)學(xué)上最優(yōu)的路徑但如果它超出無人機(jī)的機(jī)動(dòng)性能、續(xù)航能力或者路徑貼近樓宇的縫隙過于狹窄實(shí)際使用中就是廢航跡。所以每次調(diào)整參數(shù)、修改地形最終都要回到“無人機(jī)按這條航跡真的飛得過去嗎”來審視結(jié)果。最后再說一個(gè)小技巧規(guī)劃完成之后把航跡數(shù)據(jù)導(dǎo)出成文本或者KML格式然后在支持三維地圖的軟件里打開把樓宇粗略建模疊加進(jìn)去做一次虛擬飛行預(yù)覽。這一步能提前發(fā)現(xiàn)很多Matlab靜態(tài)繪圖看不出來的問題——比如某個(gè)轉(zhuǎn)彎點(diǎn)實(shí)際位置有微小的視野遮擋或者航跡在某棟樓的陰影區(qū)做了不合理的近地飛行。磨刀不誤砍柴工這個(gè)額外檢查很值得做。這個(gè)項(xiàng)目本身不算特別復(fù)雜但把DCS從零理解透、把城市環(huán)境建模做扎實(shí)、把參數(shù)調(diào)優(yōu)經(jīng)驗(yàn)攢起來一通百通后續(xù)換算法做集群協(xié)同、做動(dòng)態(tài)避障都能把基礎(chǔ)直接復(fù)用上。