如果你正在復(fù)現(xiàn)一篇無人船軌跡跟蹤方向的論文大概率會(huì)遇到和我類似的情況參考軌跡畫得干干凈凈控制器卻把船帶進(jìn)障礙物里或者NMPC求解器哼哧哼哧算半天結(jié)果第一步就數(shù)值發(fā)散再或者明明把論文里的公式抄下來了代價(jià)函數(shù)也寫了代碼一跑就是不符合預(yù)期明明有避碰項(xiàng)船還是硬生生穿過了障礙物。這篇博文就把這套基于NMPC的無人船USV軌跡跟蹤與避碰Matlab程序徹底拆開講透——模型怎么建、代價(jià)函數(shù)怎么配權(quán)重、避碰約束怎么寫才不會(huì)把優(yōu)化問題搞死、求解器選fmincon還是CasADi、以及我從發(fā)散到收斂的完整調(diào)試路線。適合正在做無人船方向的研究生或者想把非線性模型預(yù)測(cè)控制用于軌跡規(guī)劃和避障的工程師。先說一句總結(jié)性的判斷NMPC在無人船上的應(yīng)用價(jià)值不是因?yàn)樗萈ID“高級(jí)”而是因?yàn)樗衍壽E跟蹤和避碰統(tǒng)一到了一個(gè)帶約束的優(yōu)化框架里這是傳統(tǒng)控制方法很難做到的事。下面從建模、控制器設(shè)計(jì)、避碰實(shí)現(xiàn)、代碼架構(gòu)到調(diào)參經(jīng)驗(yàn)一層層拆開講。1. 項(xiàng)目概覽與設(shè)計(jì)思路1.1 為什么軌跡跟蹤必須考慮約束無人船USV的軌跡跟蹤任務(wù)表面上看是“船沿著參考路徑走”但實(shí)際工程里難點(diǎn)在三個(gè)地方第一船是典型的欠驅(qū)動(dòng)系統(tǒng)——沒有側(cè)向推進(jìn)器只能控制縱向推力和艏搖力矩橫向運(yùn)動(dòng)是被動(dòng)的耦合運(yùn)動(dòng)第二船體模型有非線性項(xiàng)包括阻尼、科氏力、舵力飽和等第三任務(wù)里同時(shí)存在跟蹤精度、控制量平滑度、避碰安全距離等多個(gè)目標(biāo)這些目標(biāo)之間本身就是相互沖突的。傳統(tǒng)方案里L(fēng)OS導(dǎo)引法加PID是經(jīng)典組合但LOS有一個(gè)致命問題它本質(zhì)上只做了視線角跟蹤避障必須單獨(dú)設(shè)計(jì)一個(gè)“導(dǎo)引重規(guī)劃層”兩層之間一旦切換邏輯不嚴(yán)密船在障礙物附近會(huì)出現(xiàn)劇烈轉(zhuǎn)向或者反復(fù)橫擺。而NMPC的優(yōu)勢(shì)在于它直接在當(dāng)前時(shí)刻預(yù)測(cè)未來一段時(shí)域內(nèi)船的運(yùn)動(dòng)在預(yù)測(cè)里同時(shí)考慮“跟蹤參考軌跡”和“避開障礙物”兩個(gè)目標(biāo)滾動(dòng)求解、滾動(dòng)施加。也就是說障礙物信息不是事后介入的而是從一開始就參與控制的實(shí)時(shí)決策。我在復(fù)現(xiàn)時(shí)采用的思路是按照論文里的整體框架用三自由度船體模型做被控對(duì)象用參考軌跡生成器提供直線/曲線/正弦等路徑NMPC控制器在每個(gè)采樣周期內(nèi)求解一個(gè)有限時(shí)域最優(yōu)控制問題再把第一個(gè)控制量施加給被控對(duì)象到下一拍重新滾動(dòng)。這個(gè)框架非常經(jīng)典幾乎是所有NMPC類USV控制論文的公共底盤。1.2 系統(tǒng)層級(jí)與仿真閉環(huán)整個(gè)仿真程序的閉環(huán)流程大概是這樣的先初始化船位、船速、障礙物位置和參考軌跡然后進(jìn)入主循環(huán)每個(gè)循環(huán)內(nèi)采集當(dāng)前狀態(tài)量把參考軌跡在預(yù)測(cè)時(shí)域內(nèi)的目標(biāo)狀態(tài)截取出來連同障礙物信息一起送入NMPC求解器求解器輸出未來Np步的控制序列取第一項(xiàng)施加到船上用動(dòng)力學(xué)模型積分更新狀態(tài)最后記錄數(shù)據(jù)并在結(jié)束時(shí)繪制軌跡、跟蹤誤差、控制量等曲線。這個(gè)結(jié)構(gòu)里有一個(gè)容易被忽略的點(diǎn)參考軌跡在每一步需要“向前看”一段。如果參考軌跡是一條全局路徑那么控制器在每個(gè)時(shí)刻要取出預(yù)測(cè)時(shí)域內(nèi)對(duì)應(yīng)的子序列也就是從k1到kNp的參考狀態(tài)。這一步做錯(cuò)了會(huì)出現(xiàn)一種非常典型的現(xiàn)象船明明跟著軌跡走得好好的但在轉(zhuǎn)彎的時(shí)候總是“抄近路”或者“沖過頭”因?yàn)閰⒖夹盘?hào)一直取的是當(dāng)前時(shí)刻最近點(diǎn)控制器壓根看不到未來的彎道。大家拿到代碼后第一件事建議先把參考軌跡截取邏輯看明白。1.3 與原論文方法的關(guān)系這篇程序復(fù)現(xiàn)的核心方法與近年來無人船NMPC方向的主流做法是一致的采用非線性三自由度船體模型代價(jià)函數(shù)包含軌跡跟蹤誤差、控制量懲罰、控制增量懲罰并把避碰作為軟約束或硬約束加入優(yōu)化問題。跟某些簡(jiǎn)化方法比原方法保留了模型的非線性動(dòng)力學(xué)特征沒有把模型線性化后再用線性MPC這是NMPC的優(yōu)勢(shì)所在也是復(fù)現(xiàn)時(shí)最容易出問題的地方——非線性優(yōu)化每一步都要調(diào)用求解器實(shí)時(shí)性比線性MPC差很多但換來的控制效果和約束滿足能力是值得的。2. 被控對(duì)象建模與參考軌跡設(shè)計(jì)2.1 三自由度運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型無人船建模最常用的是三自由度模型包括縱向速度u、橫向速度v、艏搖角速度r位置由大地坐標(biāo)系下的x、y和艏搖角ψ描述。運(yùn)動(dòng)學(xué)部分寫成$\dot{x} u\cos\psi - v\sin\psi$$\dot{y} u\sin\psi v\cos\psi$$\dot{\psi} r$這個(gè)式子本身不難但它是整個(gè)仿真的基礎(chǔ)因?yàn)榭刂屏ψ罱K輸出的是船體坐標(biāo)系下的推力和力矩而軌跡誤差必須在地球坐標(biāo)系下計(jì)算兩者之間的坐標(biāo)轉(zhuǎn)換全靠運(yùn)動(dòng)學(xué)。動(dòng)力學(xué)部分我用的是典型的剛體模型加阻尼項(xiàng)形式如下$m_{11}\dot{u} \tau_u - d_{11}u - c_{13}vr$$m_{22}\dot{v} - d_{22}v - c_{23}ur$$m_{33}\dot{r} \tau_r - d_{33}r$注意橫向方程里沒有直接控制力這體現(xiàn)了前面說的欠驅(qū)動(dòng)特性。在實(shí)際復(fù)現(xiàn)中我用的模型參數(shù)來自原論文的仿真部分船長(zhǎng)約1米質(zhì)量約20公斤慣性矩約4kg·m2阻尼系數(shù)根據(jù)水動(dòng)力經(jīng)驗(yàn)公式估算得到。這里有一個(gè)重要提醒模型參數(shù)直接影響NMPC的預(yù)測(cè)精度如果你自己有一條實(shí)船需要通過參數(shù)辨識(shí)獲取實(shí)際的水動(dòng)力系數(shù)不能隨便往代碼里套數(shù)字。仿真的目的只是驗(yàn)證算法可行性參數(shù)差別不是致命的但如果想移植到實(shí)船這一步必須重做。2.2 動(dòng)力學(xué)模型離散化與仿真步長(zhǎng)NMPC的預(yù)測(cè)模型和仿真模型實(shí)際上可以不是同一個(gè)離散化方式。仿真模型通常用四階龍格庫塔積分步長(zhǎng)選擇0.01到0.02秒精度高而NMPC內(nèi)部的預(yù)測(cè)模型為了兼顧求解速度可以用歐拉法離散預(yù)測(cè)步長(zhǎng)用0.05到0.2秒。兩者的一致性分析是個(gè)關(guān)鍵話題預(yù)測(cè)模型越精確控制器對(duì)系統(tǒng)行為的預(yù)判就越準(zhǔn)但求解越慢預(yù)測(cè)模型過于粗糙控制器看到的“未來”和實(shí)際發(fā)生的“未來”對(duì)不上就會(huì)出現(xiàn)模型失配導(dǎo)致控制性能退化。在我的程序里仿真步長(zhǎng)0.01秒NMPC預(yù)測(cè)步長(zhǎng)0.1秒預(yù)測(cè)時(shí)域20步即總共向前看2秒。這個(gè)配置是我跑了很多組參數(shù)之后的一個(gè)權(quán)衡方案Np過大求解太慢實(shí)時(shí)性不夠Np過小控制器看不到足夠遠(yuǎn)的未來遇到障礙物時(shí)來不及規(guī)劃繞行路徑。實(shí)測(cè)下來2秒的預(yù)測(cè)時(shí)域配合1m/s左右的船速給船預(yù)留了約2米的決策空間對(duì)于小型USV的避碰需求足夠了。2.3 參考軌跡的類型與生成參考軌跡我提供了三種方便驗(yàn)證控制器的不同能力。直線軌跡最簡(jiǎn)單用于驗(yàn)證基礎(chǔ)的跟蹤性能圓形軌跡檢驗(yàn)穩(wěn)態(tài)跟蹤精度S形曲線軌跡則能很好地檢驗(yàn)控制器在連續(xù)大角度機(jī)動(dòng)時(shí)的響應(yīng)能力。軌跡生成的核心是給出一組帶時(shí)間參數(shù)的參考狀態(tài)包括參考位置和參考航向角。位置直接由曲線方程給出速度可以取恒定值航向角可以由位置路徑的切線方向計(jì)算得到。有一個(gè)細(xì)節(jié)值得注意參考軌跡的光滑度直接影響控制器跟蹤效果。如果參考軌跡本身有突變比如折線拐角處航向角突變NMPC的預(yù)測(cè)時(shí)域有限它無法做到“瞬間轉(zhuǎn)過彎”就會(huì)表現(xiàn)為跟蹤滯后甚至出現(xiàn)大超調(diào)。所以在生成折線軌跡時(shí)我特意用圓弧過渡或者在軌跡方程里加入平滑濾波讓航向角連續(xù)變化。這個(gè)操作在論文里往往一句帶過但實(shí)際復(fù)現(xiàn)時(shí)它決定了對(duì)比實(shí)驗(yàn)的第一張圖好不好看。3. NMPC控制器核心設(shè)計(jì)與代價(jià)函數(shù)調(diào)參3.1 滾動(dòng)時(shí)域優(yōu)化的三個(gè)核心要素NMPC的本質(zhì)是在每個(gè)控制周期內(nèi)求解一個(gè)以當(dāng)前狀態(tài)為初值、以模型遞推為約束的有限時(shí)域最優(yōu)控制問題。它的核心要素有三個(gè)預(yù)測(cè)模型、代價(jià)函數(shù)、時(shí)域長(zhǎng)度。預(yù)測(cè)模型解決“系統(tǒng)未來怎么演化”的問題代價(jià)函數(shù)解決“什么樣的控制行為算好”的問題時(shí)域長(zhǎng)度解決“控制器能看多遠(yuǎn)、能提前多少步規(guī)劃”的問題。三者缺一不可。把這個(gè)思路用公式寫出來每個(gè)周期的優(yōu)化問題是$\min_{u_0,\dots,u_{N_p-1}} ; J \sum_{k0}^{N_p-1} \left[ e_k^\top Q e_k \Delta u_k^\top R \Delta u_k P_{obs,k} \right]$約束條件包括$x_{k1} f(x_k, u_k)$$u_{\min} \le u_k \le u_{\max}$$d_{safe} - |p_k - p_{obs}| \le s_k$最后一條里的s_k是松弛變量后面避碰部分會(huì)詳細(xì)講。這里先記住代價(jià)函數(shù)里每一項(xiàng)都對(duì)應(yīng)一個(gè)控制目標(biāo)權(quán)重矩陣Q和R決定了這些目標(biāo)之間的優(yōu)先級(jí)。3.2 誤差項(xiàng)與控制項(xiàng)的權(quán)重博弈代價(jià)函數(shù)里的誤差項(xiàng)Q、控制量懲罰R是我調(diào)試時(shí)花時(shí)間最多的地方。第一次復(fù)現(xiàn)時(shí)我直接把論文里寫的權(quán)重抄過來結(jié)果控制器要么太“軟”船慢吞吞地靠向參考軌跡完全沒有機(jī)動(dòng)性要么太“猛”控制量頻繁飽和船體在軌跡附近抖動(dòng)。后來我才意識(shí)到論文里的權(quán)重是為那個(gè)特定的船體參數(shù)和速度范圍設(shè)計(jì)的換了自己的模型參數(shù)必須重新標(biāo)定。直觀地說位置誤差權(quán)重大船會(huì)“拼命”靠向參考軌跡但代價(jià)是控制動(dòng)作猛烈、容易激發(fā)船體高頻運(yùn)動(dòng)控制量權(quán)重R大控制器傾向于“少出力、穩(wěn)著來”但跟蹤響應(yīng)會(huì)變慢。我最終采用的權(quán)重是位置誤差權(quán)重40航向角權(quán)重10控制量權(quán)重2。這個(gè)配置下船的機(jī)動(dòng)性和平滑性相對(duì)平衡。調(diào)權(quán)重的建議是先把Q里的位置權(quán)重設(shè)為1其他權(quán)重都先暫時(shí)設(shè)為0跑一個(gè)最簡(jiǎn)單直線軌跡觀察船能否基本貼合軌跡再把航向角權(quán)重逐步加上去直到轉(zhuǎn)向不出現(xiàn)明顯滯后最后再加R一點(diǎn)點(diǎn)增大直到控制曲線平滑。這個(gè)過程順序很重要跳著調(diào)往往會(huì)浪費(fèi)大量時(shí)間在一堆參數(shù)之間反復(fù)橫跳。3.3 非線性約束與求解器選型NMPC的“非線性”體現(xiàn)在兩個(gè)層面一是預(yù)測(cè)模型是非線性的二是代價(jià)函數(shù)里的避碰項(xiàng)是非線性的。這決定了不能直接用線性二次調(diào)節(jié)器那套解析解法而必須依賴數(shù)值優(yōu)化算法。Matlab里最常見的兩個(gè)選擇是fmincon和CasADi。fmincon是Matlab自帶的非線性優(yōu)化求解器不需要額外安裝環(huán)境用起來最方便CasADi是專門做最優(yōu)控制的工具箱支持自動(dòng)微分配合IPOPT求解器性能上比fmincon好不少但需要額外安裝配置。我最初的版本用fmincon跑通全流程求解耗時(shí)大約每步80毫秒對(duì)于仿真驗(yàn)證完全夠用后面為了展示更接近實(shí)時(shí)的效果換成了CasADi每步降到20毫秒左右。說實(shí)話如果你只是做算法驗(yàn)證和論文復(fù)現(xiàn)fmincon完全夠用沒必要一上來就折騰CasADi的環(huán)境配置。但如果你后續(xù)打算做硬件在環(huán)、實(shí)時(shí)控制那CasADi幾乎是必須的選擇。這里記錄一個(gè)我用fmincon時(shí)踩過的坑fmincon的默認(rèn)終止條件和步長(zhǎng)設(shè)置經(jīng)常導(dǎo)致NMPC在狀態(tài)更新后計(jì)算出“原地踏步”的控制量表現(xiàn)為船不動(dòng)或者控制力異常小。解決辦法是在fmincon的options里把OptimalityTolerance調(diào)大到1e-3把MaxFunctionEvaluations調(diào)大同時(shí)把上一步的最優(yōu)解作為本次求解的初始值也就是熱啟動(dòng)。熱啟動(dòng)不僅能加速收斂還能避免優(yōu)化器在每步都從頭搜索引起的控制量突變。4. 障礙物避碰的實(shí)現(xiàn)路徑4.1 靜態(tài)障礙物建模與懲罰函數(shù)避碰的基礎(chǔ)是障礙物的數(shù)學(xué)描述。最簡(jiǎn)單的做法是把障礙物視為一個(gè)圓形區(qū)域圓心坐標(biāo)和半徑代表障礙物位置和影響范圍。在NMPC里避碰可以通過硬約束或軟約束兩種方式實(shí)現(xiàn)。硬約束的寫法是在整個(gè)預(yù)測(cè)時(shí)域內(nèi)每個(gè)預(yù)測(cè)點(diǎn)的位置必須位于障礙物圓半徑之外。這個(gè)約束是可行但有風(fēng)險(xiǎn)的如果船當(dāng)前已經(jīng)進(jìn)入障礙物安全半徑內(nèi)或者前方障礙物分布復(fù)雜導(dǎo)致可行域非凸硬約束會(huì)讓整個(gè)優(yōu)化問題直接不可行求解器報(bào)錯(cuò)退出。我在調(diào)試時(shí)遇到過多次“當(dāng)前位置就在障礙物邊緣導(dǎo)致求解失敗”的情況非常頭疼。所以我在最終代碼里用的是軟約束加懲罰函數(shù)也就是在代價(jià)函數(shù)里加入避碰懲罰項(xiàng)$P_{obs}$。一種常見的寫法是$P_{obs,k} \rho \cdot \sum_{j} \max\left(0, ; d_{safe} - |p_k - p_{obs,j}|\right)^2$這個(gè)懲罰函數(shù)是一個(gè)外點(diǎn)罰函數(shù)只要預(yù)測(cè)點(diǎn)到障礙物中心的距離小于安全半徑就產(chǎn)生一個(gè)正懲罰值距離越近懲罰越大反之如果距離足夠遠(yuǎn)懲罰為零。它的好處是即使初始狀態(tài)就在障礙物附近優(yōu)化問題也不會(huì)立刻不可行控制器會(huì)“感覺到”危險(xiǎn)并嘗試把船推離障礙物而不是直接拒絕給出解。4.2 動(dòng)態(tài)障礙物的預(yù)測(cè)處理真實(shí)場(chǎng)景里障礙物往往是移動(dòng)的。移動(dòng)障礙物的處理方式比靜態(tài)障礙物多一個(gè)維度控制器不僅要考慮船本身未來的位置還要考慮障礙物未來時(shí)刻的位置。我的做法是假設(shè)障礙物在短時(shí)間內(nèi)做勻速直線運(yùn)動(dòng)根據(jù)障礙物的速度和方向在預(yù)測(cè)時(shí)域的每個(gè)時(shí)間步上估算出它未來時(shí)刻的位置。這樣懲罰函數(shù)在每個(gè)預(yù)測(cè)時(shí)刻都利用該時(shí)刻障礙物的預(yù)測(cè)位置來計(jì)算距離。這個(gè)近似在船載傳感器更新率較高的情況下是足夠準(zhǔn)確的因?yàn)轭A(yù)測(cè)時(shí)域通常只有兩三秒障礙物位置變化有限。如果障礙物運(yùn)動(dòng)軌跡本身是未知的那問題就變成不確定性下的避碰問題通常需要引入障礙物軌跡預(yù)測(cè)器或者魯棒MPC這里先不展開講。對(duì)于復(fù)現(xiàn)IEEE論文來說勻速運(yùn)動(dòng)假設(shè)已經(jīng)是一個(gè)相當(dāng)標(biāo)準(zhǔn)的設(shè)定了。4.3 避碰項(xiàng)與跟蹤項(xiàng)的權(quán)重平衡避碰項(xiàng)權(quán)重ρ的選擇是另一個(gè)大坑。ρ設(shè)得太大船會(huì)在離障礙物還很遠(yuǎn)的地方就跑開軌跡繞得非常大跟蹤性能大打折扣ρ設(shè)得太小船會(huì)“無視”障礙物直接穿過去避碰形同虛設(shè)。我實(shí)測(cè)下來避碰罰的權(quán)重通常要比跟蹤權(quán)重高一個(gè)數(shù)量級(jí)才能保證基本不撞障礙物但又不能無限大——當(dāng)ρ太大時(shí)船一旦進(jìn)入罰函數(shù)顯著的區(qū)域內(nèi)控制器會(huì)為了逃離罰函數(shù)區(qū)而輸出猛烈的轉(zhuǎn)向指令導(dǎo)致船體劇烈搖擺。一個(gè)穩(wěn)妥的策略是使用距離相關(guān)權(quán)重當(dāng)船距離障礙物較遠(yuǎn)時(shí)避碰項(xiàng)權(quán)重較小主要精力放在跟蹤上當(dāng)船接近障礙物安全半徑時(shí)避碰項(xiàng)權(quán)重自動(dòng)增大控制優(yōu)先級(jí)切換到安全通道。這種加權(quán)方式在工程上看著比固定權(quán)重自然而且能有效緩解“繞大彎”的問題。我的最終程序里固定權(quán)重取ρ600但代碼里保留了分段加權(quán)的接口方便做改進(jìn)實(shí)驗(yàn)。5. Matlab代碼架構(gòu)與核心函數(shù)實(shí)現(xiàn)5.1 程序文件組織與數(shù)據(jù)流整套程序我按功能模塊劃分共六個(gè)主要文件主腳本負(fù)責(zé)仿真流程控制模型文件負(fù)責(zé)運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)更新參考軌跡生成器負(fù)責(zé)曲線數(shù)據(jù)和參考狀態(tài)計(jì)算代價(jià)函數(shù)文件負(fù)責(zé)計(jì)算NMPC目標(biāo)值約束函數(shù)文件負(fù)責(zé)處理控制量和安全約束繪圖腳本負(fù)責(zé)結(jié)果可視化。代碼不復(fù)雜但每一個(gè)文件都只有一個(gè)明確職責(zé)這樣后續(xù)改參數(shù)、加障礙物、換軌跡都非常方便。在我的代碼里主循環(huán)的核心結(jié)構(gòu)大致如下% 主仿真循環(huán) for k 1 : N_sim % 獲取當(dāng)前狀態(tài) x_now x_history(:, k); % 截取預(yù)測(cè)時(shí)域內(nèi)的參考軌跡 ref_seq ref_gen(x_now, k, Np, dt_ref); % 調(diào)用NMPC求解器 u_opt nmpc_solve(x_now, ref_seq, obstacles, param); % 施加第一個(gè)控制量并積分 u_now u_opt(:, 1); x_next usv_dynamics_rk4(x_now, u_now, dt_sim); % 數(shù)據(jù)記錄 x_history(:, k1) x_next; u_history(:, k) u_now; end數(shù)據(jù)流的邏輯很簡(jiǎn)單狀態(tài)從歷史數(shù)組流出經(jīng)過參考軌跡截取和求解器變成控制量控制量積分出下一時(shí)刻狀態(tài)再寫回歷史。只要不在這條流水線上插入額外邏輯整個(gè)仿真就是確定性可復(fù)現(xiàn)的。5.2 關(guān)鍵代碼片段講解第一個(gè)關(guān)鍵函數(shù)是參考軌跡截取。這里最容易出錯(cuò)的是索引偏移因?yàn)槲矣玫念A(yù)測(cè)步長(zhǎng)與仿真步長(zhǎng)不一定相等所以參考軌跡需要按時(shí)間對(duì)齊。我的做法是仿真運(yùn)行到第k步時(shí)實(shí)際時(shí)間為k乘以仿真步長(zhǎng)NMPC預(yù)測(cè)的第j步對(duì)應(yīng)時(shí)間偏移為j乘以預(yù)測(cè)步長(zhǎng)所以參考軌跡索引需要做時(shí)間轉(zhuǎn)換而不能簡(jiǎn)單寫成ref_seq ref_traj(k1:kNp)。第二個(gè)關(guān)鍵函數(shù)是代價(jià)函數(shù)。代價(jià)函數(shù)要輸入預(yù)測(cè)狀態(tài)序列和控制序列輸出標(biāo)量代價(jià)值。我按照前面提到的公式逐項(xiàng)求和其中避碰項(xiàng)需要循環(huán)遍歷每一個(gè)障礙物和每一個(gè)預(yù)測(cè)步。為了提高效率我提前把所有障礙物的位置和半徑整理成結(jié)構(gòu)化數(shù)組在代價(jià)函數(shù)中直接訪問。第三個(gè)關(guān)鍵函數(shù)是約束函數(shù)?？刂屏考s束被寫成線性不等式控制增量約束需要利用上一步的控制值做差分。這里有個(gè)小細(xì)節(jié)控制增量的計(jì)算需要把上一步的實(shí)際控制量傳入約束函數(shù)如果遺漏很容易導(dǎo)致控制量雖然限制在上下限之內(nèi)但變化率過大的問題。5.3 仿真參數(shù)配置與可視化仿真參數(shù)我用一個(gè)struct集中管理包括仿真總時(shí)間、采樣周期、預(yù)測(cè)時(shí)域、控制時(shí)域、權(quán)重矩陣、安全半徑、船體參數(shù)等。這樣修改參數(shù)時(shí)不需要在多個(gè)腳本之間來回找寫論文實(shí)驗(yàn)記錄也方便。可視化部分我畫四張圖第一張是全局軌跡圖同時(shí)繪制參考軌跡、實(shí)際軌跡和障礙物區(qū)域第二張是位置跟蹤誤差曲線第三張是控制量推力和艏搖力矩變化曲線第四張是船與障礙物的最小距離隨時(shí)間的變化曲線。這四張圖基本覆蓋了論文里需要的仿真驗(yàn)證內(nèi)容。其中最小距離曲線是審稿人很愛看的圖它能直觀證明“控制器確實(shí)沒有穿越障礙物”。我在畫這張圖的時(shí)候?qū)ｉT計(jì)算了每個(gè)時(shí)刻船體到每個(gè)障礙物的幾何距離取最小值畫成曲線同時(shí)把安全半徑畫成一條水平虛線兩條線之間的距離一眼就能看出安全裕度。6. 調(diào)參流程與常見問題實(shí)錄6.1 從發(fā)散到收斂的調(diào)試路線我拿到一套新NMPC程序時(shí)的習(xí)慣是第一步先把避碰相關(guān)的項(xiàng)全部關(guān)掉只保留軌跡跟蹤把跟蹤效果調(diào)好第二步加入靜態(tài)障礙物調(diào)整避碰參數(shù)第三步再加入動(dòng)態(tài)障礙物最后才考慮加擾動(dòng)、模型失配等魯棒性問題。這個(gè)分階段推進(jìn)的好處是每一步的參數(shù)問題都能被獨(dú)立識(shí)別不至于出現(xiàn)“軌跡和避碰混在一起拐不清是誰的問題”。在第一步里如果軌跡跟蹤都發(fā)散優(yōu)先檢查三個(gè)地方初始狀態(tài)是否合理、權(quán)重的量級(jí)是否夸張、離散化是否穩(wěn)定。我遇到過一次發(fā)散是因?yàn)榘盐恢谜`差權(quán)重設(shè)置成1000控制器在第一步就輸出了超出物理極限的控制量船直接沖出了仿真邊界。調(diào)整思路是把所有量綱歸一化位置誤差以米為單位航向誤差以弧度為單位控制量以牛頓和牛米為單位權(quán)重設(shè)置要大致平衡這些量綱的量級(jí)差異。在第二步里如果船會(huì)撞障礙物大概率是避碰權(quán)重太小或者安全半徑設(shè)得過小。如果船在障礙物遠(yuǎn)處就大幅度繞行那說明避碰權(quán)重偏大或者障礙物影響范圍太大。這個(gè)階段要有點(diǎn)耐心繞行軌跡的形狀是權(quán)重比例的直觀體現(xiàn)。6.2 常見問題速查表我把調(diào)試過程中遇到的典型問題整理成了表格遇到類似情況可以直接對(duì)照排查?，F(xiàn)象可能原因解決對(duì)策仿真初期就發(fā)散初始狀態(tài)不合理或權(quán)重量級(jí)過大檢查初始船位船速降低Q權(quán)重提高R權(quán)重船在參考軌跡附近抖動(dòng)控制量懲罰R過小或預(yù)測(cè)時(shí)域過短增大R增大Np檢查控制增量約束船轉(zhuǎn)彎時(shí)大幅超調(diào)參考軌跡不光滑或航向權(quán)重低平滑參考軌跡增加航向角誤差權(quán)重船直接穿過障礙物避碰權(quán)重小或安全半徑小增大ρ增大安全距離檢查松弛變量船在障礙物附近反復(fù)擺動(dòng)避碰權(quán)重過大降低ρ或改用距離相關(guān)權(quán)重fmincon求解超時(shí)決策變量過多或初值不當(dāng)縮短預(yù)測(cè)時(shí)域啟用熱啟動(dòng)控制量每次跳變劇烈缺少控制增量約束在約束中加入Δu限制船長(zhǎng)時(shí)間偏離軌跡但不遠(yuǎn)離參考軌跡截取索引錯(cuò)誤檢查時(shí)間對(duì)齊邏輯6.3 實(shí)時(shí)性優(yōu)化與工程擴(kuò)展如果仿真里的每一步求解耗時(shí)太長(zhǎng)可以考慮的方向包括縮短預(yù)測(cè)時(shí)域、減少控制時(shí)域、使用CasADi自動(dòng)微分替代手寫雅可比、簡(jiǎn)化模型例如忽略部分非線性阻尼項(xiàng)、用并行計(jì)算加速多個(gè)障礙物的距離計(jì)算。需要說明的是這些優(yōu)化方向往往是在控制性能和求解速度之間做取舍沒有免費(fèi)的午餐。我實(shí)測(cè)過把預(yù)測(cè)時(shí)域從30步縮到15步求解時(shí)間大約減少一半但避碰路徑會(huì)顯得更急躁因?yàn)榭刂破鳌翱础钡酶?。往船端移植時(shí)還需要考慮幾個(gè)工程問題。第一控制量輸出往往要經(jīng)過推力分配環(huán)節(jié)把NMPC解出的縱向推力和艏搖力矩轉(zhuǎn)換為實(shí)際執(zhí)行器的轉(zhuǎn)速指令第二船體狀態(tài)反饋通常來自傳感器融合存在噪聲和延遲NMPC的性能會(huì)受到影響需要加入狀態(tài)估計(jì)器第三真實(shí)環(huán)境的障礙物信息來自雷達(dá)或視覺帶有不確定性和量測(cè)噪聲這時(shí)的避碰問題就升級(jí)為概率約束下的魯棒NMPC屬于更前沿的研究方向了。6.4 參數(shù)整定的一點(diǎn)個(gè)人經(jīng)驗(yàn)最后分享一個(gè)我自己的調(diào)參習(xí)慣。NMPC的權(quán)重矩陣Q和R我不會(huì)一次性精確到某組數(shù)值而是先用數(shù)量級(jí)粗調(diào)再用細(xì)調(diào)。粗調(diào)階段把Q的位置權(quán)重從1開始每次乘10遞增跑一次仿真看效果R同理。找到“大方向沒問題”的組合后再在這個(gè)組合附近做小范圍微調(diào)比如把位置權(quán)重從40調(diào)到50觀察跟蹤誤差均方根值的變化是否明顯。如果變化不大就說明當(dāng)前參數(shù)組合已經(jīng)處于一個(gè)性能平坦區(qū)不需要再糾結(jié)更細(xì)的數(shù)值。避碰參數(shù)ρ的調(diào)整也有類似規(guī)律。我通常會(huì)先用一個(gè)比較大的ρ值讓船無論如何都不敢撞障礙物然后再逐漸減小直到船在安全的前提下路徑盡量貼近理想軌跡。這個(gè)過程本質(zhì)上是在安全性邊界上反復(fù)試探找到“剛好不出事”的臨界值再把權(quán)重稍微取大一點(diǎn)作為安全冗余。整套程序跑通之后你會(huì)發(fā)現(xiàn)在無人船這樣一個(gè)涉及建模、優(yōu)化、約束處理的綜合問題里NMPC的潛力遠(yuǎn)遠(yuǎn)不止于“能跟著軌跡走”。它把復(fù)雜的多目標(biāo)控制問題變成了一個(gè)可調(diào)整、可分析、可復(fù)現(xiàn)的數(shù)值優(yōu)化問題這也是它能成為近年來無人船控制研究主線的根本原因。如果你正在復(fù)現(xiàn)類似的算法希望這份從建模到調(diào)參的記錄能幫你少走幾條彎路。