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