用數(shù)據(jù)把非線性無人機(jī)變成線性系統(tǒng)Koopman-MPC這套思路最近在控制圈討論度很高。這個項(xiàng)目把Koopman算子理論、數(shù)據(jù)驅(qū)動建模和模型預(yù)測控制結(jié)合在一起做了一套完整的四旋翼無人機(jī)控制方案全部基于Matlab實(shí)現(xiàn)。簡單說它的核心價(jià)值在于以前處理四旋翼這種強(qiáng)非線性系統(tǒng)要么得費(fèi)勁推導(dǎo)線性化模型要么得靠PID這種不太在意模型精度的辦法硬扛。而現(xiàn)在我們可以直接從飛行數(shù)據(jù)里學(xué)出一個高維線性模型再在這個模型上跑MPC既能保留MPC的前瞻優(yōu)化能力又不需要面對非線性優(yōu)化的求解負(fù)擔(dān)。這篇分享從原理到代碼實(shí)現(xiàn)、從參數(shù)調(diào)到坑實(shí)錄完整過一遍這套方案的落地過程適合研究無人機(jī)控制方向的研究生也適合想給傳統(tǒng)控制方案換一種新思路的工程師。1. 項(xiàng)目核心思路為什么非要用Koopman算子把系統(tǒng)“拉直”先說清楚這項(xiàng)目到底在折騰什么。四旋翼無人機(jī)在空中的運(yùn)動本質(zhì)上是剛體動力學(xué)加上空氣動力學(xué)的耦合姿態(tài)和位置之間還存在著強(qiáng)非線性關(guān)系。你油門推大一點(diǎn)機(jī)體傾斜升力方向跟著變水平方向就開始加速這中間還夾著重力、陀螺力矩、空氣阻力這些因素。傳統(tǒng)的做法是拿一個小擾動線性化模型去近似比如懸停點(diǎn)附近的線性化方程然后在這個近似模型上設(shè)計(jì)控制器。這個方法不是不能用但你想做大機(jī)動、大角度姿態(tài)變化時小擾動假設(shè)就撐不住了模型誤差一大控制器性能就明顯掉下來。那Koopman算子是干什么的呢。它提供了一種很巧妙的視角一個非線性動力系統(tǒng)可以在一個無限維的函數(shù)空間里被表示成一個線性系統(tǒng)。好比你在原來的狀態(tài)空間里看到的是各種彎彎繞繞的軌跡但如果找到一個合適的觀測函數(shù)lifting function把這些狀態(tài)映射到一個更高維的空間在這個新空間里動力學(xué)的演化竟然變成了線性映射。這就是把系統(tǒng)拉直的意思。實(shí)際的工程做法當(dāng)然不可能真用無限維空間但我們可以用數(shù)據(jù)驅(qū)動的方式在有限維空間里近似這個Koopman算子。最常用的方法是EDMDExtended Dynamic Mode Decomposition擴(kuò)展動態(tài)模態(tài)分解。你把飛行數(shù)據(jù)分成快照對每個快照對是一個狀態(tài)點(diǎn)和他下一步的狀態(tài)點(diǎn)然后在這些數(shù)據(jù)上做回歸擬合一個線性矩陣 K 使得這些快照對的演化關(guān)系在提升后的空間里盡量精確。K 矩陣就是Koopman算子在這個有限維子空間里的近似。有了這個線性模型之后MPC就變得特別好用了。標(biāo)準(zhǔn)的MPC是個在線優(yōu)化問題每一控制周期都要解一個帶約束的優(yōu)化問題。如果系統(tǒng)模型是線性的那這個優(yōu)化問題就變成了一個標(biāo)準(zhǔn)的二次規(guī)劃QP求解速度快還有成熟求解器可以用穩(wěn)定性和實(shí)時性都好處理。非線性 MPC 雖然也能做但每一步都要處理非線性優(yōu)化問題求解代價(jià)高而且非線性求解器的收斂性有時候讓你頭疼。Koopman-MPC 的本質(zhì)就是用數(shù)據(jù)把非線性系統(tǒng)偽裝成線性系統(tǒng)吃下MPC的優(yōu)化框架兩頭的好處都占到。這里要插一句我對這套思路的總體評價(jià)Koopman-MPC 這個方向很吸引人關(guān)鍵在于它代表了一種模型來源的范式變化。以前我們的模型要么靠物理推導(dǎo)要么靠系統(tǒng)辨識在固定模型結(jié)構(gòu)里擬合參數(shù)。Koopman 方法跳出了固定的模型結(jié)構(gòu)假設(shè)直接從數(shù)據(jù)里長出一個線性推進(jìn)器某種程度上是數(shù)據(jù)驅(qū)動和模型預(yù)測控制之間的一座很自然的橋梁。2. Koopman算子與EDMD算法核心原理怎么落地成代碼很多看這個項(xiàng)目的人卡在理解 Koopman 算子的數(shù)學(xué)概念上。這里我換個說法來試著說明白。你可以把動力系統(tǒng)想象成一臺復(fù)雜的機(jī)器從外面看這臺機(jī)器是個黑箱輸入一個狀態(tài)輸出下一個狀態(tài)非線性系統(tǒng)意味著輸入變化和輸出變化之間還存在著各種各樣的拐彎關(guān)系。Koopman 算子做的事情相當(dāng)于你把這臺機(jī)器放到一個更大的展廳里換了一組探針去觀測它而這些探針的讀數(shù)組合在一起后這臺機(jī)器的行為突然變成了線性的。關(guān)鍵問題來了這些探針也就是提升函數(shù)到底應(yīng)該怎么選EDMD 的做法是選一組基函數(shù)庫比如多項(xiàng)式函數(shù)、正弦余弦函數(shù)、徑向基函數(shù)等。把這些基函數(shù)作用到原始狀態(tài)上生成一個更高維的提升狀態(tài)。假設(shè)原始狀態(tài)是 x∈R^n選了 N 個基函數(shù)提升狀態(tài)就是 z Φ(x) ∈ R^N一般 N 遠(yuǎn)大于 n。我們可以把這個過程理解成原來12維的無人機(jī)狀態(tài)位置3維、速度3維、姿態(tài)四元數(shù)4維、角速度3維去掉冗余約束之后實(shí)際動力學(xué)用12維狀態(tài)提升到50維甚至100維的空間里去。EDMD 的具體計(jì)算過程是這樣走從仿真或者實(shí)驗(yàn)中采集數(shù)據(jù)得到一系列快照對 (x_k, x_{k1})。對每一個快照對計(jì)算提升狀態(tài) z_k Φ(x_k)z_{k1} Φ(x_{k1})。構(gòu)造兩個矩陣 Z1 和 Z2Z1 的每一列是 z_kZ2 的每一列是 z_{k1}。求解最小二乘問題K Z2 * Z1^這里的 Z1^ 是 Z1 的偽逆。這個過程在Matlab里實(shí)現(xiàn)起來非常直接核心代碼就幾行。我自己寫的時候會用 [U, S, V] svd(Z1, econ) 然后做截?cái)郤VD來求偽逆比直接用 pinv 更快也更穩(wěn)特別是在數(shù)據(jù)量大或者矩陣條件數(shù)不太好的時候。% 數(shù)據(jù)矩陣: snapshots 是原始狀態(tài)快照對 Z liftFunction(snapshots); % (N, numSnapshots) Z_next liftFunction(snapshotsNext); % (N, numSnapshots) % 截?cái)郤VD求偽逆 [U, S, V] svd(Z, econ); s diag(S); r sum(s 1e-8); % 截?cái)嚅撝蛋磳?shí)際數(shù)據(jù)調(diào)節(jié) K Z_next * V(:, 1:r) * diag(1 ./ s(1:r)) * U(:, 1:r);這個代碼里最重要的是 liftFunction 函數(shù)的設(shè)計(jì)它直接決定了Koopman模型的近似精度。我前幾次跑數(shù)據(jù)時隨便選了一組基函數(shù)結(jié)果模型驗(yàn)證階段誤差大得離譜后來才發(fā)現(xiàn)問題出在基函數(shù)選擇上。提升函數(shù)庫的設(shè)計(jì)有一些經(jīng)驗(yàn)性的原則基礎(chǔ)狀態(tài)本身必須包含在內(nèi)保證線性反饋的完整表達(dá)空間。加入狀態(tài)之間的耦合項(xiàng)比如位置乘速度、速度乘角速度這種交叉項(xiàng)能顯著提升對非線性氣動力項(xiàng)的逼近能力。正弦余弦項(xiàng)對姿態(tài)相關(guān)的周期變化有幫助四旋翼的升力和姿態(tài)角是三角函數(shù)關(guān)系這組基函數(shù)不能漏?；瘮?shù)數(shù)量要夠但也不是越多越好。數(shù)量太少擬合精度不夠數(shù)量太多會遇到維數(shù)災(zāi)難而且觀測矩陣會變得病態(tài)。我在這個項(xiàng)目里壓測過不同維度30維時誤差明顯50維時效果不錯到80維之后提升很有限反而不如50維穩(wěn)定。關(guān)于EDMD的收斂性還有一個值得提的點(diǎn)。EDMD 本質(zhì)上是在一個有限維子空間里對無限維Koopman算子做投影理論上隨著數(shù)據(jù)量增大和子空間維度提升這個近似是收斂的。但在實(shí)際使用中你要保證數(shù)據(jù)覆蓋了系統(tǒng)的工作區(qū)域。如果你訓(xùn)練數(shù)據(jù)只覆蓋了懸停附近的小范圍那學(xué)到得Koopman模型在大姿態(tài)角區(qū)域就是外推誤差會急劇增大。所有采集數(shù)據(jù)時要有意識地做激勵信號設(shè)計(jì)比如用不同幅度的掃頻信號讓無人機(jī)充分經(jīng)歷各個工作點(diǎn)。3. 四旋翼的Koopman-MPC控制器設(shè)計(jì)與Matlab實(shí)現(xiàn)細(xì)節(jié)3.1 整體框架和狀態(tài)定義在Matlab里落地這套方案我把系統(tǒng)拆成四個模塊數(shù)據(jù)生成模塊、Koopman模型學(xué)習(xí)模塊、MPC控制器模塊、仿真驗(yàn)證模塊。數(shù)據(jù)生成模塊負(fù)責(zé)跑一組開環(huán)激勵把飛行狀態(tài)記錄成數(shù)據(jù)集。Koopman模型學(xué)習(xí)模塊用EDMD從數(shù)據(jù)集里擬合出線性推進(jìn)器。MPC控制器模塊基于這個線性模型做滾動優(yōu)化輸出控制量。仿真驗(yàn)證模塊把控制器放回原始非線性仿真模型看看實(shí)際表現(xiàn)。狀態(tài)定義這部分我直接用的是四旋翼的標(biāo)準(zhǔn)12維狀態(tài)位置 (p_x, p_y, p_z)、速度 (v_x, v_y, v_z)、姿態(tài)角和角速度。姿態(tài)表示我用的是歐拉角而不是四元數(shù)原因是這個項(xiàng)目偏教學(xué)和演示性質(zhì)歐拉角更直觀好調(diào)試。如果你要飛大機(jī)動甚至倒飛那還是得換四元數(shù)避免萬向鎖問題。控制輸入是四個電機(jī)的轉(zhuǎn)速指令為了和MPC的線性框架匹配我把輸入定義成相對懸停點(diǎn)的轉(zhuǎn)速增量這樣線性模型更準(zhǔn)一些。具體而言懸停時四個電機(jī)有一個基準(zhǔn)轉(zhuǎn)速 ω0控制量 u 表示各個電機(jī)相對基準(zhǔn)值的偏移量。3.2 提升函數(shù)的實(shí)操選型提升函數(shù)這塊我試過好幾版方案最后用的是一個混合組合。原始狀態(tài)12維我在這個基礎(chǔ)上加了18個提升基函數(shù)總維度是30。先看一眼效果還不夠又加了角速度的二次項(xiàng)和速度-角速度交叉項(xiàng)總維度提到40多效果明顯改善。后面再想加更高階的項(xiàng)效果就不明顯了。一個實(shí)戰(zhàn)經(jīng)驗(yàn)是提升函數(shù)的構(gòu)造盡量利用你已經(jīng)知道的物理知識。完全當(dāng)黑箱來湊基函數(shù)雖然也可以但沒必要。比如四旋翼的升力和電機(jī)轉(zhuǎn)速的平方大致成正比總升力方向和姿態(tài)角之間有三角函數(shù)關(guān)系。你可以把這些認(rèn)識放到基函數(shù)設(shè)計(jì)里比純多項(xiàng)式擬合效率高很多。匹配物理直覺的基函數(shù)還有一個額外的好處數(shù)據(jù)需求會下降。我測試過如果基函數(shù)選得貼合物理結(jié)構(gòu)5000個訓(xùn)練快照對就夠用了而純通用基函數(shù)可能要好幾萬條數(shù)據(jù)才能達(dá)到類似精度這在真實(shí)平臺上意味著多飛好幾個架次成本差別很大。3.3 MPC問題的建立與標(biāo)準(zhǔn)化求解Koopman 模型學(xué)出來之后就可以把它嵌進(jìn)MPC框架。我用的MPC公式是標(biāo)準(zhǔn)的有限時域二次規(guī)劃預(yù)測模型z_{k1} K * z_k B * u_k狀態(tài)約束把原始狀態(tài)約束映射到提升空間控制量約束電機(jī)轉(zhuǎn)速限制、變化率限制目標(biāo)函數(shù)跟蹤誤差加權(quán)項(xiàng) 控制量懲罰項(xiàng)注意一下Koopman 算子模型通常給出的是 z_{k1} K * z_k 加上輸入項(xiàng)這里需要額外處理 B 矩陣。我通常的做法是在EDMD回歸時把輸入也納入快照對構(gòu)造把系統(tǒng)看成增廣狀態(tài)或者采用一種帶輸入EDMD的變體在回歸時顯式計(jì)算 B。這個細(xì)節(jié)容易踩坑后面在問題排查里會細(xì)講。MPC的目標(biāo)函數(shù)我寫成這樣% 二次規(guī)劃標(biāo)準(zhǔn)形式: min 1/2 * U * H * U f * U % H 矩陣包含預(yù)測模型推導(dǎo)出的罰矩陣, f 和當(dāng)前狀態(tài)相關(guān) H G * Q * G R; f (x_ref - Kp * z0) * Q * G; % 約束用線性不等式 A * U b 表示這里 G 是提升狀態(tài)對控制序列的脈沖響應(yīng)矩陣Q 和 R 是對應(yīng)的權(quán)重矩陣。在Matlab里我直接調(diào) quadprog 求解用 interior-point-convex 算法控制周期設(shè)在50毫秒。這個時間窗口在Matlab環(huán)境下夠用因?yàn)樘嵘臻g的維度不高幾十維預(yù)測時域取15步?jīng)Q策變量也就一百來個quadprog 求解非?？?。3.4 軌跡跟蹤的完整閉環(huán)流程把整個閉環(huán)串起來跑通我花了最長的時間。最開始我單獨(dú)驗(yàn)證Koopman模型時效果還行一到閉環(huán)就出問題無人機(jī)位置控制慢半拍姿態(tài)還有小幅震蕩。后來追查原因發(fā)現(xiàn)是預(yù)測模型在閉環(huán)工況下和開環(huán)辨識時的數(shù)據(jù)分布不一致典型的distribution shift問題。解決方案是在訓(xùn)練數(shù)據(jù)里加入一些閉環(huán)數(shù)據(jù)也就是先用一個保守的PID控制器讓無人機(jī)飛一段軌跡把PID閉環(huán)數(shù)據(jù)也放進(jìn)訓(xùn)練集里Koopman模型對閉環(huán)控制區(qū)域的覆蓋能力就強(qiáng)了很多。這個閉環(huán)迭代訓(xùn)練的流程在文獻(xiàn)里叫 iterative learning control 或者>