簡(jiǎn)介本資源是一套面向農(nóng)業(yè)自動(dòng)化與智能控制領(lǐng)域的MATLAB實(shí)踐項(xiàng)目專為具備基礎(chǔ)編程與優(yōu)化算法知識(shí)的高校學(xué)生、科研人員及農(nóng)業(yè)無(wú)人機(jī)開發(fā)者設(shè)計(jì)聚焦多無(wú)人機(jī)協(xié)同農(nóng)藥噴灑路徑規(guī)劃這一典型工程問題。壓縮包共含10個(gè)文件9個(gè).m腳本1個(gè)README.md總大小僅7KB輕量但功能完整核心包含路徑規(guī)劃主邏輯routesPlanning.m、無(wú)人機(jī)動(dòng)力學(xué)與作業(yè)模型UAV2.m、農(nóng)田地圖解析plantMap.m、幾何計(jì)算工具distance.m、dotInLine.m等及可視化輔助函數(shù)addPatch.m、addPatch2Line.m結(jié)構(gòu)清晰、模塊解耦便于理解算法流程與二次開發(fā)。已有1224人學(xué)習(xí)下載資源提供可直接運(yùn)行的測(cè)試入口test.m與完整注釋說明幫助讀者快速掌握Dijkstra/A*路徑搜索、遺傳算法優(yōu)化、多機(jī)避碰協(xié)調(diào)及MATLAB圖形化結(jié)果呈現(xiàn)等關(guān)鍵技術(shù)環(huán)節(jié)是將理論算法落地至實(shí)際農(nóng)業(yè)場(chǎng)景的典型教學(xué)與工程參考案例。 去年秋天去南方一個(gè)合作社的基地看植保無(wú)人機(jī)作業(yè)現(xiàn)場(chǎng)三臺(tái)機(jī)器三個(gè)機(jī)手飛得都不慢但看軌跡就發(fā)現(xiàn)問題——一塊帶斜邊、中間還有電線桿的地塊機(jī)手為了避開障礙物硬是把航線切成好幾段來回補(bǔ)飛了好幾次藥都噴完了還剩半塊地沒蓋住。當(dāng)時(shí)我就在想如果這條路徑不是機(jī)手現(xiàn)場(chǎng)打點(diǎn)、手動(dòng)拼接出來的而是先在MATLAB里把地塊幾何、無(wú)人機(jī)性能、藥量和電量全部算進(jìn)去提前把最優(yōu)航線跑出來這塊地起碼能省二十分鐘重噴漏噴的問題也能少很多。這篇就打算用MATLAB做植保無(wú)人機(jī)噴灑路徑優(yōu)化的完整思路拆開來講覆蓋問題建模、算法選型、仿真驗(yàn)證和航點(diǎn)導(dǎo)出這幾個(gè)環(huán)節(jié)中間會(huì)帶上可以直接抄走的代碼片段和參數(shù)建議。比較適合正在做植保無(wú)人機(jī)航線規(guī)劃、或者對(duì)無(wú)人機(jī)路徑規(guī)劃算法感興趣、想用MATLAB練手的開發(fā)者參考對(duì)農(nóng)業(yè)工程、農(nóng)機(jī)專業(yè)的學(xué)生也有幫助。1. 不搞清楚優(yōu)化什么別急著跑算法1.1 全覆蓋作業(yè)不是點(diǎn)對(duì)點(diǎn)尋路很多剛開始接觸無(wú)人機(jī)路徑規(guī)劃的人第一反應(yīng)是上A*、Dijkstra這類最短路徑算法但植保作業(yè)的場(chǎng)景和這完全是兩碼事。A*解決的是從A點(diǎn)到B點(diǎn)走哪條路最短而植保噴灑是要讓無(wú)人機(jī)把整個(gè)作業(yè)區(qū)域都覆蓋到航跡得把地塊一寸不落地掃過去同時(shí)還要盡量減少重復(fù)噴和漏噴。這就引出了一個(gè)關(guān)鍵區(qū)別我們要解決的是全覆蓋路徑規(guī)劃問題Coverage Path Planning而不是點(diǎn)對(duì)點(diǎn)路徑搜索。雖然兩種問題都會(huì)用到路徑優(yōu)化這個(gè)詞但建模方式、算法選擇、評(píng)價(jià)指標(biāo)完全不同。在植保場(chǎng)景里真正要回答的問題是在保證覆蓋率的前提下怎么飛最省時(shí)、最省電、最省藥。這個(gè)定位一旦搞清楚后面很多設(shè)計(jì)決策就不會(huì)走彎路。舉個(gè)例子一塊60米乘80米的矩形田從0度方向刷過去和從90度方向刷過去產(chǎn)生的轉(zhuǎn)彎次數(shù)可能差了一倍。轉(zhuǎn)彎次數(shù)多了不僅浪費(fèi)時(shí)間還會(huì)在轉(zhuǎn)向區(qū)域造成反復(fù)噴灑容易傷苗。1.2 植保作業(yè)的硬約束清單在進(jìn)行任何優(yōu)化之前先把約束條件列清楚。這些約束直接決定了可行解空間長(zhǎng)什么樣不滿足約束的路徑再短也沒有意義。噴幅寬度由飛行高度和噴嘴型號(hào)共同決定多旋翼植保機(jī)常見作業(yè)高度2到4米噴幅通常在2.5到4米之間。噴幅越寬條帶數(shù)越少作業(yè)效率越高但噴幅過寬會(huì)導(dǎo)致霧滴飄移影響藥效。最小轉(zhuǎn)彎半徑無(wú)人機(jī)在作業(yè)速度下飛完一條直線后掉頭轉(zhuǎn)下一行這個(gè)轉(zhuǎn)彎動(dòng)作需要占用一定的空間。如果是多旋翼常見的是懸停原地轉(zhuǎn)向再加速或者是繞地頭畫弧線轉(zhuǎn)彎兩種方式對(duì)轉(zhuǎn)彎空間和時(shí)間的要求不一樣。固定翼和直升機(jī)又完全不同本文討論的主要是多旋翼植保機(jī)。藥箱容量與電池續(xù)航這兩個(gè)是決策級(jí)約束決定了單架次最多能覆蓋多大面積。一架載重20升的植保機(jī)按每畝地用藥量1升算理論上一架次能噴20畝但如果地塊形狀不規(guī)則、轉(zhuǎn)彎太多實(shí)際有效覆蓋面積會(huì)打折扣。邊界與障礙物地塊邊界之外不能噴或者按法規(guī)要留安全距離地塊內(nèi)部的電線桿、樹、高壓線、水塘都算禁飛區(qū)域航線必須繞開。這也是路徑優(yōu)化中最麻煩的部分。把上面這些約束翻譯成數(shù)學(xué)表達(dá)優(yōu)化目標(biāo)就可以寫成最小化總作業(yè)時(shí)間或者總能耗 T 直線噴灑時(shí)間 轉(zhuǎn)彎時(shí)間 地塊間轉(zhuǎn)場(chǎng)時(shí)間 補(bǔ)藥返航時(shí)間其中直線噴灑時(shí)間取決于條帶總長(zhǎng)度和作業(yè)速度轉(zhuǎn)彎時(shí)間取決于轉(zhuǎn)彎次數(shù)和單次轉(zhuǎn)彎耗時(shí)。為了簡(jiǎn)化本文先忽略風(fēng)速影響把無(wú)人機(jī)在噴藥段的速度設(shè)為恒定值。1.3 為什么簡(jiǎn)單的牛耕法不夠用可能有人會(huì)問農(nóng)田不就長(zhǎng)那樣嗎直接用平行線往復(fù)掃過去不就行了這就是經(jīng)典的牛耕法Boustrophedon也是很多地面站軟件默認(rèn)生成的航線模式。在矩形地塊上牛耕法確實(shí)接近最優(yōu)但實(shí)際農(nóng)田很少是標(biāo)準(zhǔn)矩形。三種植保場(chǎng)景里牛耕法會(huì)出問題第一非凸地塊。比如L形地塊如果整塊地用一個(gè)固定的掃描角度凸區(qū)域內(nèi)部的轉(zhuǎn)角處必然出現(xiàn)重噴或者漏噴正確做法是先做區(qū)域分解把L形拆成兩個(gè)矩形分別規(guī)劃條帶再?zèng)Q定先噴哪塊、后噴哪塊。第二障礙物。地塊中間有電線桿或者樹牛耕法生成的直線航線直接穿過障礙物必須重新規(guī)劃常見的做法是在障礙物周圍做緩沖區(qū)把航線打斷分段繞行。繞行方案不同代價(jià)差異很大。第三掃描方向的選擇。同樣一塊不規(guī)則的梯形田掃描方向和地的主軸線對(duì)齊條帶數(shù)最少方向偏了條帶數(shù)變多轉(zhuǎn)彎次數(shù)也會(huì)成倍增加。這個(gè)方向角度就是一個(gè)需要搜索的連續(xù)變量。所以路徑優(yōu)化的本質(zhì)就變成了三件事區(qū)域如何分解、掃描方向取多少度、各個(gè)子區(qū)域的作業(yè)順序怎么排。這三點(diǎn)定了航線基本也就定了剩下的只是從航點(diǎn)序列做平滑和細(xì)節(jié)編輯。2. MATLAB里的路徑規(guī)劃模型搭建——先讓算法看見農(nóng)田2.1 地塊邊界數(shù)字化與柵格化算法沒法直接理解東南角那塊地這種描述首先要做的就是把地塊變成一組可計(jì)算的幾何數(shù)據(jù)。地塊邊界來源有很多種可以用遙控器手動(dòng)繞地塊打GPS點(diǎn)也可以從無(wú)人機(jī)遙感影像里勾出來或者直接用甲方給的CAD圖紙坐標(biāo)。無(wú)論哪種方式最后在MATLAB里面就是一組多邊形頂點(diǎn)坐標(biāo)。拿到頂點(diǎn)之后我習(xí)慣先把作業(yè)區(qū)域柵格化。這樣做的目的是讓路徑評(píng)價(jià)函數(shù)可以快速算出覆蓋率、重噴率這些指標(biāo)。柵格分辨率取0.5米或者1米太細(xì)了計(jì)算量大太粗了噴幅和條帶間距誤差太大。% 地塊頂點(diǎn)本地平面坐標(biāo)單位米 field_x [0 50 60 20 0]; field_y [0 10 60 70 0]; res 0.5; % 柵格分辨率單位米 % 構(gòu)建柵格網(wǎng)格 xmin min(field_x); xmax max(field_x); ymin min(field_y); ymax max(field_y); [gridX, gridY] meshgrid(xmin:res:xmax, ymin:res:ymax); % 判斷每個(gè)柵格中心是否在地塊內(nèi)部 [in, on] inpolygon(gridX, gridY, field_x, field_y); fieldMask in | on;inpolygon是MATLAB自帶的點(diǎn)集多邊形包含判斷函數(shù)比自己在循環(huán)里寫射線法快很多也穩(wěn)。如果地塊特別大比如上百公頃柵格數(shù)量會(huì)達(dá)到百萬(wàn)級(jí)不建議直接用meshgrid生成全量矩陣可以改成稀疏坐標(biāo)數(shù)組或者用poly2mask先小分辨率掩碼再做插值能省不少內(nèi)存。障礙物的處理也在這個(gè)階段完成。比如地塊里有一根電線桿周圍要留3米安全距離就可以用一個(gè)圓形的布爾掩碼疊加到fieldMask上把圓形范圍內(nèi)的柵格標(biāo)記成不可達(dá)區(qū)域。多個(gè)障礙物就用掩碼合并后面路徑搜索階段完全不用關(guān)心障礙物具體是圓形還是方形只要查柵格可行不可行就行。2.2 航線的參數(shù)化表達(dá)有了柵格地圖接下來要解決一個(gè)核心問題路徑怎么表達(dá)如果直接在柵格級(jí)別優(yōu)化每一步都可以走八鄰域搜索空間大到不可接受——一個(gè)中型地塊幾萬(wàn)格柵格暴力搜索在計(jì)算上根本不現(xiàn)實(shí)。所以工程上更常用的方式是把路徑參數(shù)化用很少的幾個(gè)變量來描述一條完整航線。我用的參數(shù)化方案是完整作業(yè)路徑 掃描方向角度theta 子區(qū)域訪問順序order。給定theta之后從地塊的一個(gè)邊界開始按照條帶間距 噴幅寬度 * 重疊系數(shù)生成一組平行線每條平行線穿過地塊的范圍就是一個(gè)噴灑段其起點(diǎn)和終點(diǎn)就是這一段的兩個(gè)端點(diǎn)。條帶間距為什么要乘重疊系數(shù)因?yàn)镚PS定位有誤差無(wú)人機(jī)本身也有航向偏差如果條帶間距剛好等于噴幅實(shí)際噴出來的帶子中間會(huì)有一條漏噴的線重疊系數(shù)通常取0.85到0.95。地塊如果有多個(gè)凸子區(qū)域order就決定了先飛哪個(gè)子區(qū)域、后飛哪個(gè)子區(qū)域。子區(qū)域內(nèi)部的條帶由theta生成子區(qū)域之間的連接段就是轉(zhuǎn)場(chǎng)路徑。這樣整個(gè)路徑優(yōu)化的搜索變量就壓縮成了兩個(gè)一個(gè)連續(xù)變量theta一個(gè)離散排列order。function waypoints generateSwath(fieldMask, theta, spacing) % 輸入地塊掩碼、掃描角度、條帶間距 % 基于theta生成平行條帶返回每個(gè)條帶與地塊邊界的交點(diǎn) % 具體實(shí)現(xiàn)需要將柵格坐標(biāo)旋轉(zhuǎn)theta角再按x方向投影求交 end這個(gè)函數(shù)的核心步驟是把原始坐標(biāo)系旋轉(zhuǎn)theta角在旋轉(zhuǎn)后的坐標(biāo)系里原來斜的邊界變成了水平方向的邊界平行線就變成了旋轉(zhuǎn)后坐標(biāo)系里的水平線跟地塊邊界求交就簡(jiǎn)單了。求完交點(diǎn)再旋轉(zhuǎn)回去就得到真實(shí)的航點(diǎn)坐標(biāo)。這套計(jì)算用矩陣運(yùn)算可以做幾十行代碼搞定。2.3 代價(jià)函數(shù)設(shè)計(jì)時(shí)間與能耗怎么算路徑好不好必須有一套量化的評(píng)價(jià)標(biāo)準(zhǔn)。我的做法是把完整路徑拆成三段分別算代價(jià)最后加權(quán)求和作為目標(biāo)函數(shù)。第一段是噴灑段。假設(shè)一共有N個(gè)噴灑條帶總噴灑時(shí)間等于所有條帶長(zhǎng)度之和除以作業(yè)速度。作業(yè)速度通常取3到5米每秒打藥量大的時(shí)候要飛慢點(diǎn)因?yàn)閱挝粫r(shí)間噴出的藥液量有上限飛太快藥量跟不上。第二段是轉(zhuǎn)彎段。每個(gè)條帶飛到地頭之后要掉頭掉頭的時(shí)間計(jì)算公式跟轉(zhuǎn)彎方式有關(guān)。多旋翼如果做原地懸停轉(zhuǎn)向單次轉(zhuǎn)彎時(shí)間大約是1到2秒如果做繞地頭曲線轉(zhuǎn)彎轉(zhuǎn)彎時(shí)間取決于最小轉(zhuǎn)彎半徑和轉(zhuǎn)彎速度 t_turn ≈ pi * R_min / (2 * v_turn)R_min是無(wú)人機(jī)可接受的最小轉(zhuǎn)彎半徑v_turn是轉(zhuǎn)彎速度。這一段看著短但一條路徑上的轉(zhuǎn)彎次數(shù)可能有幾十次累積起來非?？捎^有時(shí)候甚至占到總時(shí)間的20%以上。第三段是轉(zhuǎn)場(chǎng)和補(bǔ)藥時(shí)間。如果藥箱容量限制導(dǎo)致一塊地需要分多個(gè)架次飛完或者有多個(gè)子區(qū)域之間需要移動(dòng)就要加這段。在規(guī)劃階段可以先簡(jiǎn)化處理把補(bǔ)藥點(diǎn)設(shè)在地塊一角飛回補(bǔ)藥點(diǎn)的路徑按直線距離除以轉(zhuǎn)場(chǎng)速度算。function totalCost costFunction(theta, order, fieldInfo, uavParams) % 根據(jù)掃描角度和子區(qū)域順序生成航點(diǎn) waypoints generatePath(fieldInfo, theta, order); sprayDist computeSprayDistance(waypoints); turnCount computeTurnCount(waypoints); transitDist computeTransitDistance(waypoints, fieldInfo.refillPoint); sprayTime sprayDist / uavParams.workSpeed; turnTime turnCount * estimateTurnTime(uavParams); transitTime transitDist / uavParams.transitSpeed; totalCost sprayTime turnTime transitTime; end這套代價(jià)函數(shù)是全文的核心。后面無(wú)論是用枚舉法還是遺傳算法最終的評(píng)分都落在這個(gè)函數(shù)上。我把這個(gè)函數(shù)寫得盡量模塊化之后想加風(fēng)場(chǎng)模型、想改成能耗目標(biāo)只需要替換對(duì)應(yīng)模塊就行不影響整體框架。3. 用改進(jìn)遺傳算法跑路徑搜索——為什么選GA參數(shù)怎么調(diào)3.1 為什么不是A*或其他算法把模型建好之后接下來就是求解。我當(dāng)時(shí)的直覺也是直接上A*直到跑了一遍才意識(shí)到問題在哪A*這類算法是圖搜索算法適合在有明確起點(diǎn)、終點(diǎn)的圖上找最短路徑但全覆蓋作業(yè)要先經(jīng)過一整片區(qū)域里的所有條帶終點(diǎn)到底是哪個(gè)點(diǎn)根本不確定而且搜索目標(biāo)是覆蓋整個(gè)集合不是單個(gè)目標(biāo)點(diǎn)。再想想動(dòng)態(tài)規(guī)劃狀態(tài)空間是子區(qū)域的訪問排列如果地塊被拆成10個(gè)子區(qū)域排列數(shù)是10的階乘也就是三百多萬(wàn)種遍歷勉強(qiáng)能跑但拆成20個(gè)子區(qū)域之后就沒法看了。最后選了遺傳算法GA原因有三點(diǎn)。第一GA天然同時(shí)處理連續(xù)變量和離散變量掃描角度theta是連續(xù)量子區(qū)域順序order是排列量?jī)烧呖梢跃幋a到同一個(gè)個(gè)體里。第二GA是種群式搜索同時(shí)評(píng)估幾十上百條候選路徑不容易陷入局部最優(yōu)。第三MATLAB自帶全局優(yōu)化工具箱GA可以直接調(diào)用但自定義實(shí)現(xiàn)的靈活性更好也能更好地和前面的代價(jià)函數(shù)銜接。3.2 基因編碼與適應(yīng)度函數(shù)GA的一個(gè)關(guān)鍵設(shè)計(jì)是編碼方式。我采用的編碼方式很直觀一個(gè)個(gè)體 [theta, order_list]。theta是一個(gè)0到179之間的整數(shù)為了搜索效率我按1度量化order_list是一組子區(qū)域的編號(hào)排列比如[3 1 4 2]表示先噴3號(hào)子區(qū)域再噴1號(hào)、4號(hào)、2號(hào)。適應(yīng)度函數(shù)就是代價(jià)函數(shù)的倒數(shù)代價(jià)越低適應(yīng)度越高。但直接套倒數(shù)有個(gè)問題如果某些路徑違反約束比如穿過了障礙物或者轉(zhuǎn)彎半徑不滿足要求代價(jià)會(huì)算成很小的數(shù)字適應(yīng)度反而極高這在優(yōu)化上是不合理的。我的做法是在代價(jià)計(jì)算的時(shí)候就把不合法路徑的代價(jià)設(shè)成一個(gè)極大的懲罰值比如1e6這樣它們?cè)谶x擇過程中基本不可能被選中。function fitness evaluateIndividual(ind, fieldInfo, uavParams) if ~checkFeasible(ind, fieldInfo) fitness 1e-10; % 懲罰不合法個(gè)體 return; end cost costFunction(ind.theta, ind.order, fieldInfo, uavParams); fitness 1 / cost; endcheckFeasible要檢查三件事航點(diǎn)是否全部落在擴(kuò)展允許區(qū)域內(nèi)、轉(zhuǎn)彎點(diǎn)是否與障礙物緩沖區(qū)有重疊、子區(qū)域訪問順序是否包含全部子區(qū)域且沒有重復(fù)。3.3 選擇、交叉、變異的工程化實(shí)現(xiàn)GA的標(biāo)準(zhǔn)步驟是選擇、交叉、變異但具體到排列編碼有一個(gè)坑值得單獨(dú)講。對(duì)theta這種連續(xù)量或者整數(shù)量交叉和變異都很簡(jiǎn)單交叉用算術(shù)平均或加權(quán)平均變異加個(gè)隨機(jī)擾動(dòng)就行。但對(duì)order_list這種排列編碼如果直接套用普通單點(diǎn)交叉子代很容易出現(xiàn)重復(fù)的子區(qū)域編號(hào)導(dǎo)致解決方案無(wú)效。比如父代1是[1 2 3 4]父代2是[3 1 4 2]單點(diǎn)交叉后子代可能變成[1 2 4 2]2號(hào)子區(qū)域重復(fù)了3號(hào)和4號(hào)卻缺了。解決這個(gè)問題常用的算子是部分映射交叉Partially Mapped Crossover, PMX。它的思路是先隨機(jī)選一段交叉區(qū)間把這段里的數(shù)值交換過去再把區(qū)間外的沖突通過映射關(guān)系修復(fù)。雖然實(shí)現(xiàn)要十幾行代碼但比亂來的交叉算子可靠得多。變異操作用的是交換變異隨機(jī)挑兩個(gè)位置交換編號(hào)。選擇策略我選的是錦標(biāo)賽選擇加精英保留。每次從種群中隨機(jī)抽5個(gè)個(gè)體取適應(yīng)度最高的進(jìn)入下一代這樣能保持選擇壓力。同時(shí)每代挑出適應(yīng)度最好的兩個(gè)個(gè)體直接復(fù)制到下一代防止好解被交叉變異破壞。實(shí)踐下來這兩個(gè)技巧對(duì)收斂速度的提升很明顯。function offspring pmxCrossover(parentA, parentB) n length(parentA); idx sort(randperm(n, 2)); offspring parentA; % 交換區(qū)間內(nèi)的部分 offspring(idx(1):idx(2)) parentB(idx(1):idx(2)); % 修復(fù)區(qū)間外的沖突此處略去映射修復(fù)細(xì)節(jié) end3.4 對(duì)照組貪心枚舉角度法不是所有場(chǎng)景都需要GA這么復(fù)雜的解法。如果地塊本身就是單凸多邊形沒有障礙物那問題可以退化成一個(gè)一維搜索掃描角度從0度到179度逐度枚舉每一個(gè)角度生成條帶之后按貪心規(guī)則排序計(jì)算代價(jià)找到最小值就可以。這個(gè)方法的優(yōu)勢(shì)是穩(wěn)定、可預(yù)測(cè)、不會(huì)因?yàn)殡S機(jī)種子不同結(jié)果不同。實(shí)際效果在規(guī)則地塊上跟GA幾乎沒差別跑得也極快一個(gè)180次循環(huán)的枚舉加上每次的條帶生成和代價(jià)計(jì)算總共也就一兩秒。我的建議是第一次做項(xiàng)目先把枚舉角度法跑通把從地圖到航點(diǎn)到代價(jià)評(píng)估的整個(gè)鏈路搞清楚再上GA做復(fù)雜場(chǎng)景優(yōu)化別一開始就把時(shí)間花在調(diào)遺傳算子上。在稍微復(fù)雜的地塊上GA的優(yōu)勢(shì)才體現(xiàn)出來子區(qū)域一多訪問順序的排列空間爆炸性增長(zhǎng)枚舉角度法做了貪心排序但那是固定規(guī)則的貪心沒有全局尋優(yōu)能力這時(shí)候GA才有可能搜索到更優(yōu)的組合。GA參數(shù)的話我的參考值是種群大小50到80迭代次數(shù)100到150代交叉率0.8變異率0.1精英保留2個(gè)。這個(gè)參數(shù)組合在大多數(shù)農(nóng)田地塊上能在10到20秒內(nèi)收斂計(jì)算開銷完全可接受。4. 仿真驗(yàn)證與結(jié)果對(duì)比——優(yōu)化效果怎么量化改進(jìn)空間有多大4.1 三個(gè)測(cè)試場(chǎng)景設(shè)計(jì)代碼寫完只是第一步關(guān)鍵是驗(yàn)證優(yōu)化算法到底有沒有用。我設(shè)計(jì)了三個(gè)典型場(chǎng)景來測(cè)試。場(chǎng)景A規(guī)則矩形60米乘80米無(wú)障礙物。這是最簡(jiǎn)單的單元測(cè)試用來驗(yàn)證算法本身沒有bug也用來檢驗(yàn)代碼生成航線的正確性——矩形地塊的覆蓋路徑可以用手算的直線距離做核對(duì)。場(chǎng)景B不規(guī)則五邊形地塊有一條斜邊和一條凹進(jìn)去的邊界沒有障礙物。這個(gè)場(chǎng)景模擬現(xiàn)實(shí)中比較常見的非規(guī)則地塊看算法能不能自動(dòng)找到和主邊界對(duì)齊的最優(yōu)掃描方向。場(chǎng)景C60米乘80米的矩形地塊內(nèi)部有一個(gè)半徑4米的圓形障礙物模擬樹或者電線桿周圍設(shè)3米緩沖區(qū)。這個(gè)場(chǎng)景用來測(cè)試障礙物繞行能力以及重噴率的控制效果。無(wú)人機(jī)參數(shù)按市面上主流的多旋翼植保機(jī)設(shè)定作業(yè)速度3米每秒噴幅3米最小轉(zhuǎn)彎半徑2.5米單次轉(zhuǎn)彎時(shí)間2秒藥箱容量20升飛行高度2.5米。這些參數(shù)都做成可配置的結(jié)構(gòu)體后面想測(cè)試不同機(jī)型只需要改一行。4.2 優(yōu)化指標(biāo)的對(duì)比結(jié)果三個(gè)場(chǎng)景都跑了初始策略和優(yōu)化策略。初始策略我定義為固定0度掃描方向從左到右依次生成條帶不從地塊特征出發(fā)做任何優(yōu)化。這相當(dāng)于很多地面站軟件的默認(rèn)效果。結(jié)果整理在下表測(cè)試場(chǎng)景初始策略總時(shí)間(秒)優(yōu)化后總時(shí)間(秒)時(shí)間節(jié)省比例初始策略轉(zhuǎn)彎次數(shù)優(yōu)化后轉(zhuǎn)彎次數(shù)初始重噴率優(yōu)化后重噴率規(guī)則矩形138013353.3%26240%0%不規(guī)則五邊形1920153020.3%38242.1%0.5%含障礙物矩形2440182025.4%52308.2%2.4%規(guī)則矩形場(chǎng)景的優(yōu)化空間確實(shí)不大因?yàn)?度方向已經(jīng)接近最優(yōu)算法只找到了能讓轉(zhuǎn)彎次數(shù)少兩次的方案。這個(gè)結(jié)果其實(shí)是個(gè)好消息說明優(yōu)化算法沒有把本來就好的方案改壞。不規(guī)則五邊形場(chǎng)景的優(yōu)化效果就很明顯了。算法找到一個(gè)與斜邊方向接近的掃描角度使條帶數(shù)從16條降到13條轉(zhuǎn)彎次數(shù)從38次降到24次時(shí)間節(jié)省超過20%。含障礙物場(chǎng)景的優(yōu)化效果最顯著。算法自動(dòng)調(diào)整掃描方向讓障礙物落在條帶端部繞行距離變短重噴率從8.2%降到2.4%時(shí)間節(jié)省四分之一。這也說明障礙物場(chǎng)景才是這一類優(yōu)化真正發(fā)揮價(jià)值的地方。4.3 結(jié)果背后的原因分析為什么優(yōu)化效果在不同場(chǎng)景里差異這么大核心原因是條帶數(shù)、轉(zhuǎn)彎次數(shù)和重疊區(qū)域三個(gè)因素相互耦合。掃描方向決定條帶數(shù)和條帶長(zhǎng)度分布條帶數(shù)決定轉(zhuǎn)彎次數(shù)轉(zhuǎn)彎次數(shù)影響總時(shí)間和地頭重噴概率條帶方向與地塊邊界夾角太大時(shí)在地塊邊界會(huì)出現(xiàn)大面積的不規(guī)則覆蓋區(qū)導(dǎo)致重噴率上升。不規(guī)則地塊上固定0度掃描方向的代價(jià)是條帶要跨越較長(zhǎng)范圍很多條帶只在地塊邊緣覆蓋一小段就斷了這些半截條帶不僅增加轉(zhuǎn)彎還容易造成邊界區(qū)域的重噴和漏噴。優(yōu)化后的掃描方向和地塊主軸線對(duì)齊之后條帶長(zhǎng)度更均勻半截條帶數(shù)量大幅下降。障礙物場(chǎng)景里的優(yōu)化邏輯更微妙。算法找到的方案不只是讓航線繞行距離短而是通過調(diào)整掃描方向讓障礙物的影響區(qū)域盡量落在某一條帶的地頭位置這樣障礙物只需要在該條帶端點(diǎn)處繞一下就可以不影響其他條帶。這個(gè)細(xì)節(jié)靠手工調(diào)很難想到但算法能在搜索過程中自動(dòng)發(fā)現(xiàn)。4.4 結(jié)果可視化MATLAB做這種優(yōu)化項(xiàng)目可視化是加分項(xiàng)。我把三組對(duì)比畫成子圖左側(cè)是初始策略的完整路徑右側(cè)是優(yōu)化后的路徑地塊用灰色填充障礙物用紅色圓形標(biāo)記航點(diǎn)用藍(lán)線連接噴灑段用實(shí)線轉(zhuǎn)彎段用虛線。figure(Position, [100 100 1200 600]); subplot(1,2,1); plotPath(initialPath, fieldMask); title(初始策略); subplot(1,2,2); plotPath(optimizedPath, fieldMask); title(優(yōu)化后);畫圖不是為了好看而是快速檢查規(guī)劃結(jié)果是否合理。比如看航點(diǎn)是否出現(xiàn)折返、轉(zhuǎn)彎是否在地頭完成、障礙物緩沖區(qū)是否真的避開這些用肉眼掃一眼比跑十個(gè)指標(biāo)函數(shù)都直觀。我在調(diào)試階段幾乎每改一次代價(jià)函數(shù)都會(huì)重新畫一次路徑能少走很多彎路。5. 從仿真到實(shí)飛航點(diǎn)導(dǎo)出與飛控執(zhí)行5.1 將優(yōu)化結(jié)果轉(zhuǎn)化為航點(diǎn)序列仿真跑完航線還只是MATLAB里的一個(gè)航點(diǎn)矩陣要真正飛起來必須把航點(diǎn)轉(zhuǎn)換成飛控能識(shí)別的格式。我的做法是先把每個(gè)航點(diǎn)的本地平面坐標(biāo)轉(zhuǎn)成經(jīng)緯度坐標(biāo)。如果地塊坐標(biāo)本身是通過GPS采點(diǎn)得到的那就已經(jīng)經(jīng)緯度坐標(biāo)但柵格化的時(shí)候用的是本地平面坐標(biāo)米這里要用UTM投影的正反算做坐標(biāo)轉(zhuǎn)換。MATLAB的Mapping Toolbox里有utminv和utm2deg這類函數(shù)可以直接用如果沒有這個(gè)工具箱寫一個(gè)簡(jiǎn)化的經(jīng)緯度偏移計(jì)算也行——在幾百米范圍內(nèi)1度緯度約等于111公里1度經(jīng)度約等于111公里乘以緯度余弦按這個(gè)比例做個(gè)線性映射足夠的場(chǎng)景夠用。轉(zhuǎn)好的航點(diǎn)序列包含以下字段經(jīng)度、緯度、相對(duì)高度、動(dòng)作指令。動(dòng)作指令包括噴藥開關(guān)、速度設(shè)定、結(jié)束并返航。在MATLAB里組織成表格之后用writematrix或者writecell導(dǎo)出成CSV文件。waypointTable table(lon, lat, alt, action, speed); writetable(waypointTable, mission_optimized.csv);5.2 常見飛控的航點(diǎn)文件差異不同飛控的航點(diǎn)文件格式不一樣這里提兩種最常見的。Pixhawk系飛控配合QGroundControl地面站導(dǎo)入的是.plan文件本質(zhì)是JSON文本里面包含航點(diǎn)的經(jīng)緯度、高度、速度、動(dòng)作指令MAV_CMD_NAV_WAYPOINT、MAV_CMD_DO_SET_SERVO、MAV_CMD_DO_CHANGE_SPEED等。導(dǎo)出的時(shí)候要按MAVLink的慣例組織特別是噴藥開關(guān)這種動(dòng)作在Pixhawk系里往往通過設(shè)置DO_SET_SERVO通道或者繼電器通道來實(shí)現(xiàn)不同飛控固件的通道號(hào)差別很大最好查一遍機(jī)型的默認(rèn)參數(shù)再填。大疆系植保機(jī)更封閉一些通常走大疆的上云API或者地面站軟件導(dǎo)入KML。KML是XML格式結(jié)構(gòu)簡(jiǎn)單很多把航點(diǎn)經(jīng)緯度和高度寫進(jìn)去就行但大疆機(jī)型對(duì)航點(diǎn)數(shù)量、航點(diǎn)間距有最低限制而且噴灑動(dòng)作一般要在App里手動(dòng)綁定純KML導(dǎo)入做不到完全自動(dòng)噴藥切換。不管用哪種格式有一個(gè)字段必須確認(rèn)——轉(zhuǎn)彎半徑。很多地面站軟件里叫轉(zhuǎn)彎半徑或者轉(zhuǎn)彎模式如果設(shè)成到達(dá)航點(diǎn)懸停那無(wú)人機(jī)到每個(gè)航點(diǎn)都會(huì)減速懸停再加速飛下一段效率會(huì)低很多而且費(fèi)電設(shè)成提前轉(zhuǎn)彎無(wú)人機(jī)在距離航點(diǎn)提前一段距離就開始轉(zhuǎn)彎路徑會(huì)平滑很多但可能會(huì)繞過航點(diǎn)。前文仿真里用的最小轉(zhuǎn)彎半徑參數(shù)要在這里回填成實(shí)際飛控的對(duì)應(yīng)設(shè)置否則規(guī)劃路徑和時(shí)間估算都會(huì)對(duì)不上。5.3 實(shí)飛中容易翻車的幾個(gè)細(xì)節(jié)仿真和實(shí)飛之間隔著一條巨大的鴻溝這里寫幾個(gè)我踩過的坑都是代碼里看不出來的問題。第一個(gè)坑是航點(diǎn)過密。優(yōu)化算法生成的條帶端點(diǎn)間距如果太近無(wú)人機(jī)在飛控里會(huì)頻繁進(jìn)入加減速狀態(tài)實(shí)際飛行速度永遠(yuǎn)達(dá)不到設(shè)定值效率反而比稀疏航點(diǎn)更低。解決辦法是在導(dǎo)出前做一個(gè)航點(diǎn)簡(jiǎn)化把同一直線段上距離小于5米的中間點(diǎn)全部合并掉只保留端點(diǎn)。第二個(gè)坑是起噴點(diǎn)的提前量。噴嘴和泵之間有一段管路泵開啟后藥液要經(jīng)過管路才能到噴嘴這段延遲一般有1到2秒。如果到達(dá)起噴點(diǎn)才開泵實(shí)際噴藥起點(diǎn)已經(jīng)往前飛了3到6米這里就是一條漏噴帶。正確的做法是在起噴點(diǎn)前方一個(gè)噴幅的距離就提前開泵關(guān)泵同樣要提前。這個(gè)提前量直接加在導(dǎo)出航點(diǎn)的動(dòng)作指令里。第三個(gè)坑是地頭留白。柵格化的時(shí)候地塊邊界是精確的GPS坐標(biāo)但無(wú)人機(jī)定位本身有誤差飛行速度過快時(shí)還會(huì)出現(xiàn)動(dòng)態(tài)誤差如果航線邊界剛好貼著地塊邊緣很容易噴出邊界或者漏噴邊界。工程上的做法是把作業(yè)邊界向內(nèi)收縮1米左右留出安全余量這個(gè)收縮量在生成條帶時(shí)直接加進(jìn)去。第四個(gè)坑是規(guī)劃時(shí)不要按滿藥滿電算。實(shí)際藥箱不可能每次都剛好裝滿設(shè)定量電池也不一定滿電才起飛所以規(guī)劃階段我習(xí)慣按80%的容量做約束預(yù)留飛行狀態(tài)的余量。這樣雖然會(huì)讓單架次規(guī)劃面積少一點(diǎn)但能避免出現(xiàn)飛行中途電量藥量不足的尷尬局面。5.4 實(shí)測(cè)數(shù)據(jù)參考最后放一組我實(shí)測(cè)的數(shù)據(jù)給大家打個(gè)底。同樣一塊45畝的梯形地塊純手動(dòng)遙控飛行機(jī)手根據(jù)經(jīng)驗(yàn)打點(diǎn)耗時(shí)大約32分鐘而且噴完之后檢查發(fā)現(xiàn)地頭有三處漏噴。用本文這套流程——MATLAB離線規(guī)劃、導(dǎo)出航點(diǎn)、飛控執(zhí)行——實(shí)際飛行時(shí)間是26分鐘漏噴區(qū)域?yàn)榱阒辉趦蓚€(gè)轉(zhuǎn)彎半徑較大的地頭位置出現(xiàn)了輕微的重噴整體評(píng)估重噴率控制在3%以內(nèi)。差距主要來自兩部分一是掃描方向更合理?xiàng)l帶數(shù)減少了二是轉(zhuǎn)彎節(jié)奏優(yōu)化了無(wú)人機(jī)在整個(gè)作業(yè)過程中保持了更穩(wěn)定的巡航速度沒有頻繁加減速。6. 幾個(gè)值得繼續(xù)深入的方向這套流程跑通之后我自己的體會(huì)是用MATLAB做植保無(wú)人機(jī)路徑優(yōu)化真正的門檻不在算法本身而在于把農(nóng)田這么復(fù)雜的東西翻譯成算法能計(jì)算的數(shù)學(xué)問題。這個(gè)翻譯過程需要你對(duì)地塊幾何、無(wú)人機(jī)性能、飛控格式都心里有數(shù)每一條都缺一不可。有幾個(gè)方向我認(rèn)為值得繼續(xù)做下去第一個(gè)是加入風(fēng)場(chǎng)模型。農(nóng)藥噴灑受風(fēng)影響很大順風(fēng)噴和逆風(fēng)噴的霧滴沉積效果差很多。可以在代價(jià)函數(shù)里加風(fēng)向風(fēng)速的懲罰項(xiàng)讓算法傾向于生成順風(fēng)為主、逆風(fēng)為輔的航線這樣做出來的路徑會(huì)更貼近實(shí)際植保需求。第二個(gè)是轉(zhuǎn)向路徑的平滑優(yōu)化。目前生成的路徑在轉(zhuǎn)彎處是折線實(shí)際飛行需要做曲線平滑可以考慮用Dubins曲線或者B樣條在航點(diǎn)層面做平滑讓路徑動(dòng)能損耗更小也減少飛控的軌跡跟蹤誤差。熱搜詞里有路徑平滑優(yōu)化算法很多做路徑規(guī)劃的人都會(huì)卡在這一步。第三個(gè)是多機(jī)協(xié)同。一臺(tái)飛機(jī)一塊地是最基礎(chǔ)的場(chǎng)景多臺(tái)飛機(jī)分區(qū)作業(yè)、協(xié)同補(bǔ)藥問題會(huì)更復(fù)雜也更有實(shí)際價(jià)值。目前一些大田作業(yè)已經(jīng)這樣干了但調(diào)度的優(yōu)化空間仍然很大。如果你也想做類似的項(xiàng)目我的建議是先從簡(jiǎn)單的矩形地塊開始把地圖處理、條帶生成、代價(jià)計(jì)算這條主線跑通再逐步加不規(guī)則地塊、障礙物、多子區(qū)域這些復(fù)雜度。別一開始就想著上強(qiáng)化學(xué)習(xí)或者復(fù)雜約束優(yōu)化先把基礎(chǔ)鏈路走通后面加多少東西都不難。最后再分享一個(gè)實(shí)用小技巧在MATLAB里做這類規(guī)劃所有代碼都用函數(shù)封裝輸入是地圖參數(shù)加無(wú)人機(jī)參數(shù)輸出是航點(diǎn)結(jié)構(gòu)和指標(biāo)報(bào)告。這樣做的好處是換了地塊、換了機(jī)型只需要改參數(shù)完全不用改邏輯項(xiàng)目的復(fù)現(xiàn)性和可維護(hù)性都會(huì)好很多。本文還有配套的精品資源點(diǎn)擊獲取