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