簡介本資源是一套面向高校自動(dòng)化、航空航天類專業(yè)本科生及研究生的MATLAB課程設(shè)計(jì)與畢業(yè)設(shè)計(jì)實(shí)踐材料聚焦垂直起飛與垂直降落VTVL運(yùn)載器姿態(tài)控制這一前沿工程問題系統(tǒng)解決飛行全過程的姿態(tài)穩(wěn)定性與精準(zhǔn)調(diào)控難題。壓縮包共32個(gè)文件涵蓋8個(gè)核心MATLAB腳本如遺傳算法優(yōu)化PID的optimize_pid_with_ga.m、火箭參數(shù)建模rocket_params.m、6個(gè).fig可視化結(jié)果、9張.png性能對(duì)比圖含俯仰姿態(tài)與質(zhì)心平動(dòng)聯(lián)合仿真圖、4個(gè).mat參數(shù)文件含差分進(jìn)化與自適應(yīng)DE優(yōu)化結(jié)果、2個(gè).asv仿真主程序及1段實(shí)機(jī)運(yùn)行視頻.mov總大小12.73MB。已有60人學(xué)習(xí)下載。用戶可直接復(fù)現(xiàn)完整VTVL姿態(tài)控制仿真流程從彈道建模、內(nèi)外環(huán)PID控制器設(shè)計(jì)到GA/DE雙算法參數(shù)尋優(yōu)再到含執(zhí)行機(jī)構(gòu)延遲的閉環(huán)響應(yīng)驗(yàn)證配套優(yōu)化歷史記錄、多組對(duì)比曲線及實(shí)時(shí)監(jiān)控圖像顯著降低算法調(diào)參門檻提升控制系統(tǒng)設(shè)計(jì)的工程落地性。 最近群里一個(gè)做火箭回收模擬的朋友發(fā)了個(gè)“基于matlab的VTVL姿態(tài)控制.zip”結(jié)果好多人在問怎么解壓、怎么跑、跑完怎么調(diào)。我看了一下這個(gè)壓縮包里的東西其實(shí)就是一個(gè)典型的垂直起降VTVL飛行器姿態(tài)控制仿真工程用MATLAB腳本加Simulink模型組合完成覆蓋了六自由度剛體建模、推力矢量執(zhí)行機(jī)構(gòu)、PID/LQR姿態(tài)控制律、批量打靶和可視化后處理。這篇文章不打算替你把代碼逐行注釋而是直接講清楚這個(gè)工程背后的控制邏輯、模型怎么搭、壓縮包拿到手后怎么正確運(yùn)行以及我在實(shí)際調(diào)參過程中踩過的那幾個(gè)坑。1. 先理解VTVL姿態(tài)控制到底在解決什么問題1.1 為什么垂直起降的姿態(tài)控制這么難VTVLVertical Take-off and Vertical Landing飛行器通俗講就是能豎著起飛、豎著降落的火箭或驗(yàn)證平臺(tái)最典型的代表就是一級(jí)火箭回收和各類亞軌道試驗(yàn)飛行器。這種構(gòu)型在飛行的大部分時(shí)間里都沒有固定翼面提供氣動(dòng)升力和穩(wěn)定力矩整個(gè)飛行器本質(zhì)上是靠發(fā)動(dòng)機(jī)推力“頂著”飛行的。這里有一個(gè)非常反直覺的事實(shí)從控制角度看垂直起飛和垂直著陸階段飛行器就像一個(gè)倒立擺。發(fā)動(dòng)機(jī)和推力矢量機(jī)構(gòu)在底部重心在中間偏上一旦箭體偏離垂直方向重力就會(huì)讓這個(gè)偏轉(zhuǎn)繼續(xù)增大而不是自動(dòng)恢復(fù)。所以姿態(tài)控制系統(tǒng)的首要任務(wù)不是“保持姿態(tài)角盡量小”而是“讓姿態(tài)角能回到期望值”并且在擾動(dòng)下不振蕩發(fā)散。此外VTVL飛行器的受力環(huán)境變化非常劇烈。上升段有稠密大氣著陸段經(jīng)歷速度從幾百米每秒降到接近零的過程氣動(dòng)力、動(dòng)壓、質(zhì)心位置、轉(zhuǎn)動(dòng)慣量都在實(shí)時(shí)變化。再加上發(fā)動(dòng)機(jī)推力大范圍節(jié)流、燃料消耗導(dǎo)致質(zhì)量變化、風(fēng)場擾動(dòng)等不確定性姿態(tài)控制系統(tǒng)的魯棒性要求遠(yuǎn)高于普通飛行器。這個(gè)工程包的核心就是圍繞這些問題在MATLAB環(huán)境中建立一套完整的姿態(tài)控制仿真鏈路剛體姿態(tài)動(dòng)力學(xué)、推力矢量執(zhí)行機(jī)構(gòu)、傳感器測量、控制律計(jì)算、數(shù)值積分和結(jié)果可視化。它解決的問題就是“在給定的推力、擺角約束和外界擾動(dòng)下如何讓飛行器保持期望姿態(tài)并且最終穩(wěn)穩(wěn)落在著陸點(diǎn)”。1.2 姿態(tài)控制的任務(wù)拆解與通道劃分拿到這個(gè)工程后我建議先不要急著跑代碼先想明白姿態(tài)控制要管哪幾個(gè)量。一個(gè)剛體在空中的姿態(tài)可以用三個(gè)角度描述偏航角航向、俯仰角、滾轉(zhuǎn)角。但實(shí)際控制并不是三個(gè)角度各管各的而是按通道劃分俯仰通道和偏航通道一般由發(fā)動(dòng)機(jī)擺動(dòng)推力矢量控制。發(fā)動(dòng)機(jī)噴管擺動(dòng)一個(gè)小角度推力方向隨之改變產(chǎn)生一個(gè)繞重心的力矩從而改變飛行器的俯仰和偏航姿態(tài)。滾轉(zhuǎn)通道通常不通過發(fā)動(dòng)機(jī)擺動(dòng)來控制而是依靠RCS反作用控制系統(tǒng)或冷氣噴管實(shí)現(xiàn)。因?yàn)閱闻_(tái)發(fā)動(dòng)機(jī)擺動(dòng)無法有效產(chǎn)生繞箭體縱軸的滾轉(zhuǎn)力矩除非有多臺(tái)發(fā)動(dòng)機(jī)差動(dòng)擺動(dòng)。這也就是為什么你在工程包里會(huì)看到兩個(gè)并行的控制回路一個(gè)用于俯仰/偏航的角度控制一個(gè)用于滾轉(zhuǎn)的角度控制。前者響應(yīng)快、力矩大后者精度高、但力矩小、工作占空比受氣源限制。姿態(tài)控制的任務(wù)階段也不一樣。上升段一般跟蹤程序角讓火箭按照預(yù)定的俯仰程序轉(zhuǎn)彎減速下降段需要讓飛行器保持接近垂直的姿態(tài)同時(shí)配合導(dǎo)航制導(dǎo)完成軌跡跟蹤最終著陸段對(duì)姿態(tài)角的誤差要求非常嚴(yán)格因?yàn)橹憰r(shí)箭體傾斜太大直接就翻倒了。一個(gè)好的VTVL姿態(tài)控制仿真工程必須能體現(xiàn)這些階段的差異并且能夠切換控制目標(biāo)。1.3 坐標(biāo)系、姿態(tài)表示和MATLAB里的第一個(gè)坑這個(gè)工程里最“勸退”一部分人的是坐標(biāo)系和姿態(tài)表示沒搞清楚。我在幫朋友調(diào)試時(shí)發(fā)現(xiàn)出現(xiàn)“控制量符號(hào)反了”“角度一直在跳”“仿真數(shù)值爆炸”這類問題十有八九是坐標(biāo)系約定不一致。VTVL建模中至少涉及兩個(gè)坐標(biāo)系地面慣性坐標(biāo)系通常取北東地NED或東北天ENU用于描述位置和速度也是重力方向的定義基準(zhǔn)。彈體坐標(biāo)系原點(diǎn)位于飛行器質(zhì)心x軸沿箭體縱軸指向頭部y、z軸分別指向右和下方構(gòu)成右手系。姿態(tài)表示一般有三種方式歐拉角、旋轉(zhuǎn)矩陣、四元數(shù)。歐拉角最直觀工程包里的可視化和日志打印基本都用它。但歐拉角有兩個(gè)坑一是存在萬向節(jié)鎖奇異點(diǎn)當(dāng)俯仰角接近90度時(shí)滾轉(zhuǎn)和偏航無法區(qū)分二是有回繞問題角度從179度跳到-179度時(shí)看起來變化很大控制器如果直接拿這個(gè)誤差做PID輸出會(huì)出現(xiàn)劇烈跳變。四元數(shù)沒有奇異問題適合在仿真內(nèi)部做姿態(tài)遞推和誤差計(jì)算但是不直觀。很多工程包的策略是內(nèi)部狀態(tài)用四元數(shù)進(jìn)入控制律之前換算成歐拉角或誤差四元數(shù)顯示和記錄再用歐拉角。在MATLAB中從慣性系到彈體系的旋轉(zhuǎn)矩陣一般用三個(gè)歐拉角連續(xù)旋轉(zhuǎn)得到但這個(gè)旋轉(zhuǎn)順序定義因工程而異。常見的是ZYX偏航-俯仰-滾轉(zhuǎn)順序也有喜歡用ZYZ的。如果不懂這個(gè)讀代碼時(shí)經(jīng)常會(huì)出現(xiàn)“為什么我的滾轉(zhuǎn)角控制器生效了偏航也跟著動(dòng)”這種迷惑。提示拿到工程包的第一件事是先找README或者在代碼開頭找坐標(biāo)系和歐拉角順序的定義。沒有這個(gè)定義的工程建議直接放棄因?yàn)樽藨B(tài)控制里的符號(hào)錯(cuò)誤排查起來極其痛苦。2. 模型搭建從剛體動(dòng)力學(xué)到推力矢量執(zhí)行機(jī)構(gòu)2.1 剛體姿態(tài)動(dòng)力學(xué)方程姿態(tài)控制的物理基礎(chǔ)是剛體動(dòng)量矩定理。在彈體坐標(biāo)系下姿態(tài)動(dòng)力學(xué)方程可以寫成[ I \dot{\boldsymbol{\omega}} \boldsymbol{\omega} \times (I \boldsymbol{\omega}) \mathbf{M} ]其中(\mathbf{I})是轉(zhuǎn)動(dòng)慣量矩陣(\boldsymbol{\omega} [p, q, r]^T)是彈體角速度在彈體系下的分量(\mathbf{M})是外力矩包括推力矢量力矩、氣動(dòng)力矩這里通常先忽略或簡化為阻力力矩、RCS力矩。展開成標(biāo)量形式后你會(huì)看到很多“交叉項(xiàng)”比如滾轉(zhuǎn)角速度p影響偏航力矩、俯仰角速度q影響滾轉(zhuǎn)力矩。這就是陀螺力矩耦合。在快速旋轉(zhuǎn)時(shí)這個(gè)耦合非常明顯單純的三通道獨(dú)立PID難以兼顧需要在控制律里做解耦或者用LQR這類狀態(tài)反饋控制直接處理。這個(gè)工程包里的動(dòng)力學(xué)模型通常用三種方式實(shí)現(xiàn)純腳本函數(shù)、S-function、Simulink里的6DOF模塊組。如果用的是腳本函數(shù)你會(huì)看到類似這樣的結(jié)構(gòu)function [omega_dot, quat_dot] rigid_body_dynamics(omega, quat, M) % 轉(zhuǎn)動(dòng)慣量矩陣典型VTVL飛行器 I diag([Ixx, Iyy, Izz]); I_inv inv(I); omega_skew [0, -omega(3), omega(2); omega(3), 0, -omega(1); -omega(2), omega(1), 0]; omega_dot I_inv * (M - omega_skew * (I * omega)); % 四元數(shù)運(yùn)動(dòng)學(xué) quat quat / norm(quat); % 保持單位四元數(shù) quat_skew [0, -quat(3), quat(2); quat(3), 0, -quat(1); -quat(2), quat(1), 0]; quat_dot 0.5 * [0, -omega; omega, -quat_skew] * quat; end注意一點(diǎn)轉(zhuǎn)動(dòng)慣量矩陣在你讀一下工程包時(shí)會(huì)被誤認(rèn)為是常數(shù)。實(shí)際上隨著燃料消耗飛行器質(zhì)量、質(zhì)心和轉(zhuǎn)動(dòng)慣量都在變。工程代碼里如果假設(shè)I不變那就是在模型層面限制了控制方法的實(shí)用性。我在實(shí)際仿真時(shí)一般會(huì)在每個(gè)解算周期實(shí)時(shí)更新I和質(zhì)心位置具體方式是把質(zhì)量變化率按燃料秒耗量折算。2.2 推力矢量執(zhí)行機(jī)構(gòu)建模VTVL飛行器沒有翼面姿態(tài)控制力矩主要來自發(fā)動(dòng)機(jī)擺動(dòng)。發(fā)動(dòng)機(jī)噴管相對(duì)箭體縱軸擺動(dòng)一個(gè)角度δ推力F不再沿箭體縱軸方向而是分解為軸向分量和側(cè)向分量。在小角度假設(shè)下俯仰/偏航力矩近似為[ M_{pitch} F \cdot \delta_z \cdot L_{cg} ]其中(L_{cg})是發(fā)動(dòng)機(jī)擺心到質(zhì)心的距離δ_z是噴管在俯仰平面內(nèi)的擺動(dòng)角。從這里可以看出發(fā)動(dòng)機(jī)擺動(dòng)產(chǎn)生的控制力矩與推力成正比這就是為什么推力大時(shí)姿態(tài)控制“很有勁”推力節(jié)流到很低時(shí)比如著陸前懸停段即使?jié)M偏擺角產(chǎn)生的力矩也可能不足。工程包里的執(zhí)行機(jī)構(gòu)模型通常包含兩個(gè)環(huán)節(jié)一是擺角伺服機(jī)構(gòu)的動(dòng)態(tài)特性一般簡化為二階環(huán)節(jié)或者帶延遲的一階環(huán)節(jié)二是擺角限幅和速率限幅比如最大擺角±10度最大擺角速率±30度/秒。% 發(fā)動(dòng)機(jī)擺動(dòng)執(zhí)行機(jī)構(gòu)簡化模型 delta_cmd saturate(delta_cmd, delta_max, -delta_max); % 位置限幅 delta_dot saturate((delta_cmd - delta_prev) / dt, delta_rate_max, -delta_rate_max); delta delta_prev delta_dot * dt;這里很容易被忽略的是速率限幅。姿態(tài)控制律算出的擺角指令如果變化太快執(zhí)行機(jī)構(gòu)跟不上就會(huì)出現(xiàn)“控制量發(fā)了實(shí)際動(dòng)作滯后”的情況。要是仿真里沒建模這個(gè)滯后控制器設(shè)計(jì)出來可能很激進(jìn)到了實(shí)物階段一測就發(fā)散。RCS滾轉(zhuǎn)通道的執(zhí)行機(jī)構(gòu)建模則不同通常用開關(guān)式噴氣模型輸出力矩按脈沖寬度調(diào)制或者簡單的Bang-Bang控制。為了簡化很多工程會(huì)用線性化模型把RCS力矩直接描述為滾轉(zhuǎn)力矩指令加上一階慣性環(huán)節(jié)。這個(gè)工程包里大概率也做了這樣的簡化。2.3 傳感器測量和風(fēng)擾模型姿態(tài)控制的反饋來自傳感器主要是慣性測量單元IMU里的角速度陀螺和加速度計(jì)。一個(gè)完整的仿真工程不會(huì)假設(shè)傳感器完美無噪聲而是會(huì)加入零偏、比例因子誤差、白噪聲和重力耦合。這部分對(duì)調(diào)參影響很大尤其是陀螺零偏它會(huì)在積分過程中產(chǎn)生不斷累積的姿態(tài)誤差。風(fēng)場擾動(dòng)建模也值得說一下。很多人以為VTVL在大氣層內(nèi)飛行時(shí)風(fēng)擾動(dòng)主要影響位置其實(shí)風(fēng)還會(huì)產(chǎn)生氣動(dòng)力矩。尤其是側(cè)風(fēng)作用在箭體表面上壓心與質(zhì)心不重合時(shí)會(huì)產(chǎn)生相當(dāng)大的干擾力矩。工程包里一般會(huì)用簡化的側(cè)風(fēng)模型或者直接給姿態(tài)動(dòng)力學(xué)方程加一個(gè)隨機(jī)擾動(dòng)力矩項(xiàng)。在MATLAB里噪聲和擾動(dòng)通常用randn生成高斯白噪聲然后通過濾波器成形。比如gyro_measure omega_true gyro_bias randn(3,1) * gyro_noise_std;但要注意仿真里加噪聲的幅度不能只靠“經(jīng)驗(yàn)拍腦袋”。如果噪聲設(shè)得太大整個(gè)控制性能看起來會(huì)很差設(shè)得太小又無法暴露真實(shí)飛行中的問題。合理的做法是先查你所模擬的IMU型號(hào)的指標(biāo)再把這些指標(biāo)換算成仿真噪聲參數(shù)。2.4 數(shù)值積分步長與離散化選擇最后是數(shù)值積分。這個(gè)工程包里可能同時(shí)出現(xiàn)定步長RK4、變步長ode45和離散遞推三種實(shí)現(xiàn)方式。我的建議是姿態(tài)控制的主仿真盡量用固定步長的RK4或者直接按離散周期遞推步長取控制周期的一半以下。為什么要固定步長因?yàn)榭刂坡墒请x散的在每個(gè)控制周期里計(jì)算一次指令。如果仿真積分用變步長ode45而控制律定時(shí)更新兩者之間的時(shí)間對(duì)齊會(huì)變得很麻煩步長策略也影響仿真結(jié)果的確定性。固定步長仿真配上控制器的定時(shí)中斷邏輯更接近機(jī)載軟件的真實(shí)運(yùn)行方式。步長怎么選如果控制頻率是100Hz也就是10ms一個(gè)控制周期那內(nèi)部物理模型的積分步長建議至少5ms最好1ms。積分步長太大會(huì)引入數(shù)值阻尼導(dǎo)致仿真結(jié)果看起來比實(shí)際情況更穩(wěn)定這會(huì)掩蓋控制器參數(shù)的缺陷。3. 控制律設(shè)計(jì)為什么從PID開始LQR又怎么加3.1 角速度阻尼是整個(gè)控制系統(tǒng)的地基在VTVL姿態(tài)控制里最容易犯的錯(cuò)誤是只做角度反饋不做角速度反饋。例如只用俯仰角誤差給出發(fā)動(dòng)機(jī)擺角指令系統(tǒng)一定會(huì)振蕩而且很可能單調(diào)發(fā)散。原因是只反饋角度時(shí)控制器相當(dāng)于一個(gè)“位置控制器”沒有阻尼系統(tǒng)的特征根落在虛軸附近任何一點(diǎn)擾動(dòng)都會(huì)讓飛行器持續(xù)振蕩。在工程上這個(gè)問題用一句話概括就是“先加阻尼再提響應(yīng)”。角速度反饋的物理本質(zhì)是阻尼它把系統(tǒng)的特征根拉向左半平面。所以工程包里的控制器即使是最簡版也是一套角速度阻尼器加角度穩(wěn)定器。角速度阻尼器的實(shí)現(xiàn)可以直接把滾轉(zhuǎn)角速度p、俯仰角速度q、偏航角速度r分別乘以阻尼增益疊加到對(duì)應(yīng)通道的擺角指令上。調(diào)試時(shí)只看一個(gè)現(xiàn)象給飛行器一個(gè)初始姿態(tài)偏差如果它在沒有外環(huán)的情況下角速度能快速衰減到零阻尼器就調(diào)好了。3.2 姿態(tài)PID的雙環(huán)結(jié)構(gòu)有什么物理意義這套工程包里最常見的控制結(jié)構(gòu)是雙環(huán)PID。外環(huán)是姿態(tài)角回路輸入是期望姿態(tài)角與實(shí)際姿態(tài)角的誤差輸出是角速度指令內(nèi)環(huán)是角速度回路輸入是角速度指令與實(shí)際角速度的誤差輸出是擺角指令或RCS指令。外環(huán)為什么輸出角速度指令而不是直接輸出擺角指令因?yàn)榻撬俣仁强刂谱藨B(tài)的一階導(dǎo)數(shù)通過外環(huán)P控制獲得角速度指令相當(dāng)于把姿態(tài)控制問題變成一個(gè)角速度跟蹤問題。這樣做的好處是只要內(nèi)環(huán)能可靠跟蹤角速度指令外環(huán)的參數(shù)設(shè)計(jì)就變得非常直觀% 外環(huán)姿態(tài)角誤差 - 角速度指令 omega_cmd Kp_att * (att_cmd - att) Kd_att * (att_cmd_dot - att_dot); % 內(nèi)環(huán)角速度誤差 - 力矩指令 Moment_cmd Kp_rate * (omega_cmd - omega) Ki_rate * integral(omega_cmd - omega); % 力矩指令 - 擺動(dòng)角指令 delta_cmd Moment_cmd / (F * L_cg);調(diào)參順序也很固定先調(diào)內(nèi)環(huán)的Kp和Ki讓角速度響應(yīng)快速且無超調(diào)再調(diào)外環(huán)的Kp讓姿態(tài)角在期望帶寬內(nèi)收斂。一般的準(zhǔn)則是最內(nèi)環(huán)帶寬是外環(huán)帶寬的5到10倍這樣內(nèi)外環(huán)動(dòng)態(tài)解耦。對(duì)于著陸階段的姿態(tài)控制外環(huán)積分項(xiàng)Ki要特別小心。因?yàn)樵谥懫陂g如果持續(xù)存在穩(wěn)態(tài)偏差積分項(xiàng)會(huì)不斷累積最終引起超調(diào)和振蕩。更嚴(yán)重的是如果執(zhí)行機(jī)構(gòu)飽和了積分還在累積退出飽和后就會(huì)產(chǎn)生大幅超調(diào)這就是積分飽和windup。工程包里一般會(huì)加抗飽和邏輯比如積分限幅、PID輸出限幅或者更高級(jí)的條件積分法。3.3 LQR狀態(tài)反饋與PID的等價(jià)關(guān)系如果你在工程包里看到lqr這個(gè)函數(shù)不要覺得它是完全不同的另一套思路。它其實(shí)是在更系統(tǒng)地處理同一個(gè)問題。LQR把姿態(tài)誤差、角速度誤差放到一個(gè)狀態(tài)向量里通過設(shè)計(jì)Q和R權(quán)重矩陣求出一個(gè)最優(yōu)狀態(tài)反饋矩陣K使得給定的二次性能指標(biāo)最小。[ u -K x ]Q和R的物理意義是Q增大意味著你更重視姿態(tài)角和角速度誤差的快速收斂系統(tǒng)響應(yīng)更快R增大意味著你更重視控制量擺角、推力不要太大系統(tǒng)響應(yīng)更慢、更柔。在實(shí)際VTVL工程中LQR的實(shí)用價(jià)值有三點(diǎn)。第一它自動(dòng)處理三軸之間的陀螺力矩耦合不會(huì)像三個(gè)獨(dú)立PID那樣橫滾和偏航打架第二它能直接考慮執(zhí)行機(jī)構(gòu)的位置限制通過R權(quán)重調(diào)整讓控制器輸出保持在合理范圍內(nèi)第三它的反饋矩陣K是線性狀態(tài)反饋計(jì)算量小適合在機(jī)載計(jì)算機(jī)上實(shí)時(shí)運(yùn)行。用MATLAB寫起來非常簡單A [zeros(3), eye(3); zeros(3), zeros(3)]; B [zeros(3); inv(I)]; K lqr(A, B, Q, R);不過要注意lqr需要線性時(shí)不變系統(tǒng)而VTVL飛行器的轉(zhuǎn)動(dòng)慣量、推力、質(zhì)心都在變化。工程上的做法是把它做成“增益調(diào)度LQR”把飛行過程分成若干工作點(diǎn)每個(gè)工作點(diǎn)離線算一組K在線根據(jù)當(dāng)前狀態(tài)插值選擇K。這個(gè)工程包里如果K是常數(shù)說明它只是給你看控制效果要用于真實(shí)場景還得做調(diào)度。3.4 前饋補(bǔ)償重力、推力變化和擺動(dòng)角關(guān)系純反饋控制在擾動(dòng)大的情況下會(huì)疲于奔命。一個(gè)提升性能的簡單手段是前饋補(bǔ)償。最典型的例子是重力前饋飛行器要保持垂直姿態(tài)發(fā)動(dòng)機(jī)推力不僅要提供側(cè)向控制力還必須平衡重力。如果控制器只看姿態(tài)誤差那么姿態(tài)角誤差為零時(shí)控制指令可能也為零但此時(shí)發(fā)動(dòng)機(jī)擺動(dòng)角的靜平衡值并不為零因?yàn)榘l(fā)動(dòng)機(jī)要抵消風(fēng)或重心偏移產(chǎn)生的靜力矩。前饋補(bǔ)償?shù)淖龇ㄊ窃诳刂坡奢敵龌A(chǔ)上疊加一個(gè)靜平衡擺角。比如風(fēng)擾模型計(jì)算出的氣動(dòng)力矩是M_wind則前饋擺角為[ \delta_{ff} \frac{M_{wind}}{F \cdot L_{cg}} ]這個(gè)量加到反饋擺角指令上反饋環(huán)節(jié)只需要處理動(dòng)態(tài)偏差控制負(fù)擔(dān)大幅減小。工程包里通常會(huì)把這部分寫成獨(dú)立函數(shù)因?yàn)楹罄m(xù)如果要升級(jí)成自適應(yīng)控制前饋項(xiàng)可以替換成在線辨識(shí)的擾動(dòng)補(bǔ)償。4. MATLAB工程化落地腳本、Simulink與批量調(diào)參4.1 用腳本實(shí)現(xiàn)還是用Simulink實(shí)現(xiàn)這個(gè)工程包里大概率同時(shí)包含腳本和Simulink模型。如果你問我姿態(tài)控制仿真到底該用哪種我的答案是快速驗(yàn)證算法、批量掃參用腳本完整系統(tǒng)集成、考慮時(shí)序和接口用Simulink。腳本方式的優(yōu)勢是邏輯透明、可批量運(yùn)行、方便用MATLAB的調(diào)試器逐行看。缺點(diǎn)是如果一個(gè)系統(tǒng)有多個(gè)子系統(tǒng)代碼組織不好會(huì)變成面條代碼。Simulink的優(yōu)勢是模塊化尤其是Aerospace Blockset里自帶六自由度運(yùn)動(dòng)方程模塊、重力模型、大氣模型、執(zhí)行機(jī)構(gòu)模塊可以快速搭出完整系統(tǒng)。缺點(diǎn)是版本兼容問題多換一個(gè)MATLAB版本模型可能打不開或布局錯(cuò)亂。實(shí)際調(diào)試時(shí)我的習(xí)慣是先用腳本把控制律驗(yàn)證通過然后把這個(gè)控制律原封不動(dòng)搬到Simulink的MATLAB Function模塊里。這樣即使Simulink模型跑飛了也能確定問題出在模型封裝而不是控制算法。4.2 蒙特卡洛批量仿真與parfor并行加速VTVL姿態(tài)控制是不能只跑一次仿真就下結(jié)論的因?yàn)檎鎸?shí)飛行中存在大量不確定性。工程包里一般會(huì)有一個(gè)批量仿真的入口腳本通過循環(huán)遍歷不同的初始偏差、質(zhì)量偏差、風(fēng)場強(qiáng)度、推力擾動(dòng)等參數(shù)統(tǒng)計(jì)姿態(tài)控制的成功率。這就是蒙特卡洛仿真。如果直接用for循環(huán)跑幾百次每個(gè)case又包含幾萬步仿真MATLAB會(huì)慢到讓你懷疑人生。這時(shí)候可以用parfor但要先滿足MATLAB并行計(jì)算工具箱的并行池條件。parfor i 1:N % 每組參數(shù)計(jì)算一個(gè)仿真 results(i) run_single_case(params(i)); endparfor的使用有幾個(gè)坑最大的坑是“變量分類”。循環(huán)里的臨時(shí)變量會(huì)被識(shí)別為臨時(shí)變量不會(huì)傳回主循環(huán)如果你需要把每個(gè)case的姿態(tài)曲線都保存下來就得多花點(diǎn)心思。最穩(wěn)妥的方式是每個(gè)case只返回一個(gè)結(jié)構(gòu)體后續(xù)再匯總。另外如果你在虛擬機(jī)里跑MATLAB會(huì)發(fā)現(xiàn)整體性能很差這不是代碼問題而是虛擬機(jī)缺乏對(duì)CPU指令集和內(nèi)存帶寬的原生支持。并行池在這種環(huán)境下更容易出問題所以調(diào)試階段先用單核跑通再上parfor。4.3 結(jié)果可視化到底畫哪幾張圖能說明問題仿真跑完可視化是判斷控制律優(yōu)劣的關(guān)鍵。這個(gè)工程包里應(yīng)該有成體系的繪圖腳本但很多人在自己復(fù)現(xiàn)時(shí)只畫姿態(tài)角曲線這是不夠的。我認(rèn)為至少要畫這五張圖姿態(tài)角時(shí)間曲線俯仰、偏航、滾轉(zhuǎn)看角度跟蹤誤差和收斂時(shí)間。角速度時(shí)間曲線看角速度是否有高頻振蕩和峰值。發(fā)動(dòng)機(jī)擺角指令與實(shí)際擺角曲線看執(zhí)行機(jī)構(gòu)是否頻繁飽和、是否出現(xiàn)抖動(dòng)。推力指令曲線看推力變化是否劇烈。三維軌跡圖看飛行器整體是否按預(yù)期路徑下降和著陸。用MATLAB畫的時(shí)候推薦把多張圖用subplot排在一起統(tǒng)一x軸時(shí)間范圍。調(diào)試時(shí)可以直接從一張總覽圖里快速判斷是哪個(gè)環(huán)節(jié)出了問題。如果姿態(tài)角曲線振蕩小而擺角曲線一直頂在限幅處說明控制器輸出飽和需要增大R權(quán)重或降低外環(huán)帶寬。4.4 參數(shù)整定步驟從內(nèi)到外從線性到非線性最后說說參數(shù)整定。面對(duì)一堆Kp、Ki、Kd、Q、R新手最容易做的事是盲調(diào)——每次改一個(gè)參數(shù)跑一次仿真看曲線繼續(xù)改。這種方法效率極低而且容易陷入“這次看起來還行換一組擾動(dòng)又不行”的循環(huán)。我建議的整定順序是鎖定執(zhí)行機(jī)構(gòu)模型只把舵機(jī)擺動(dòng)角和推力設(shè)為階躍指令驗(yàn)證執(zhí)行機(jī)構(gòu)響應(yīng)是否正常。斷開外環(huán)只閉環(huán)內(nèi)環(huán)角速度回路給定角速度階躍指令觀察跟蹤速度。先調(diào)內(nèi)環(huán)Kp直到角速度上升時(shí)間符合預(yù)期再加Ki消除穩(wěn)態(tài)誤差。閉環(huán)外環(huán)姿態(tài)角回路給定一個(gè)30度左右的姿態(tài)角階躍指令觀察超調(diào)量。超調(diào)大就降低外環(huán)Kp或者提高內(nèi)環(huán)帶寬。加入風(fēng)擾和噪聲觀察穩(wěn)態(tài)抖振量。抖振過大時(shí)優(yōu)先檢查傳感器噪聲是否被控制器高頻放大。最后做蒙特卡洛批量測試看參數(shù)在不同工況下的表現(xiàn)。在MATLAB里你可以用PID Tuner工具輔助初值計(jì)算但它給出的初值通常偏保守最后還是要手動(dòng)微調(diào)。我個(gè)人最喜歡的方式是把參數(shù)統(tǒng)一放到一個(gè)結(jié)構(gòu)體里配一個(gè)參數(shù)表格腳本這樣每次調(diào)參只改配置不用去翻控制律代碼。5. 拿到zip之后解壓、目錄解讀和運(yùn)行入口5.1 壓縮包的解壓姿勢與常見誤區(qū)這個(gè)工程以zip形式分發(fā)很多人第一時(shí)間雙擊zip在壓縮管理器里看到幾個(gè).mat和.m文件就直接雙擊打開這是最常見的錯(cuò)誤操作。MATLAB工程不能在壓縮文件內(nèi)直接運(yùn)行必須先把整個(gè)zip解壓到本地目錄然后通過MATLAB的路徑設(shè)置加載。解壓工具的選擇Windows下我用7-Zip或者系統(tǒng)自帶資源管理器Linux/macOS直接用unzip命令。Linux下最常用的命令是unzip 基于matlab的VTVL姿態(tài)控制.zip -d ~/projects/vtvl解壓后務(wù)必確認(rèn)目錄下的文件都完整特別是.mat數(shù)據(jù)文件和Simulink的.slx/.mdl文件。如果解壓過程出現(xiàn)錯(cuò)誤后面MATLAB里會(huì)報(bào)出一堆莫名其妙的“找不到文件”錯(cuò)誤。5.2 解壓報(bào)錯(cuò)的典型原因與排查鏈路我見過太多人卡在解壓這一步報(bào)錯(cuò)信息看著五花八門其實(shí)主要就是下面幾類報(bào)錯(cuò)信息根本原因處理辦法file is not a zip file下載不完整、文件頭被破壞或擴(kuò)展名錯(cuò)誤重新下載檢查文件大小是否與源文件一致invalid zip archive: could not find eocdzip壓縮包尾部損壞或文件被截?cái)嘤?-Zip打開測試確認(rèn)壓縮包完整性重新下載error opening zip file or jar manifest missing路徑包含中文/空格/權(quán)限問題或文件未被識(shí)別為合法zip把文件移到純英文路徑下用管理員權(quán)限解壓解壓后某些文件夾為空壓縮包內(nèi)容缺失或解壓工具過濾了部分文件名嘗試更換解壓工具比如7-Zip這里要特別提醒一下zip文件下載后最好先看大小。如果下載下來只有幾KB甚至幾十字節(jié)那基本不是真正的zip可能是一個(gè)網(wǎng)頁錯(cuò)誤頁面被瀏覽器存成了zip后綴。這種情況下不要在解壓工具上浪費(fèi)時(shí)間直接重新下載。還有一類情況是壓縮包設(shè)置了密碼。如果工程作者在共享時(shí)給zip加了密碼而你沒有密碼任何解壓工具都無計(jì)可施。這時(shí)候只能聯(lián)系作者不要花精力去研究所謂密碼移除工具既不安全也不可靠。5.3 目錄結(jié)構(gòu)里應(yīng)該有哪些文件運(yùn)行入口在哪里解壓完成后先不要急著在MATLAB里點(diǎn)運(yùn)行先看一眼目錄結(jié)構(gòu)。一個(gè)規(guī)范的VTVL姿態(tài)控制工程大致會(huì)有以下內(nèi)容項(xiàng)目根目錄/ ├── README.md ├── main.m ├── config/ │ ├── init_params.m │ └── trajectory_cmd.m ├── models/ │ ├── vtvl_6dof.m │ └── thrust_vector_model.m ├── controllers/ │ ├── controller_pid.m │ ├── controller_lqr.m │ └── observer_kalman.m ├── sim/ │ └── vtvl_sim.slx └── plots/ ├── plot_attitude.m └── animate_trajectory.m運(yùn)行入口一般是main.m或RUNME.m。打開main.m后你通常能看到清晰的分步注釋初始化參數(shù)、加載模型、跑仿真、畫圖。如果作者規(guī)范一點(diǎn)還會(huì)在README.md里寫明MATLAB版本要求、工具箱要求和運(yùn)行順序。在MATLAB中運(yùn)行時(shí)第一步是把工程目錄連同子目錄加入路徑addpath(genpath(項(xiàng)目根目錄));如果不加路徑直接運(yùn)行main.m大概率會(huì)報(bào)“未定義函數(shù)或變量”錯(cuò)誤。這個(gè)錯(cuò)誤在論壇里被問爛了但每次都會(huì)有人踩坑。5.4 運(yùn)行時(shí)的版本兼容和常見報(bào)錯(cuò)MATLAB版本差異帶來的問題比想象中多。老版本寫的腳本用了新版本才有的函數(shù)比如某些工具箱函數(shù)或類定義在新版本跑會(huì)報(bào)錯(cuò)反過來新版本提供的腳本在舊環(huán)境里也可能不兼容。如果工程包是用MATLAB R2022b及以后版本寫的而你用的是R2020a出現(xiàn)兼容性問題的概率很高。另一個(gè)常見報(bào)錯(cuò)是“無法打開Simulink模型”原因通常是缺少Simulink、Aerospace Blockset或Simscape工具箱。安裝MATLAB時(shí)默認(rèn)不會(huì)裝所有工具箱這一步需要檢查并補(bǔ)裝。運(yùn)行Simulink模型時(shí)如果出現(xiàn)“Invalid Simulink object name”這類錯(cuò)誤多半是模型名稱與文件名稱不一致或者模型文件有多個(gè)版本。直接在當(dāng)前目錄下輸入模型文件名看是否能打開先排除這個(gè)因素。如果仿真時(shí)間很長注意看是不是電腦啟用了CPU降頻又沒開并行池?？梢韵日{(diào)低仿真終止時(shí)間跑通流程后再做完整仿真。6. 調(diào)參避坑手記仿真發(fā)散、歐拉角奇點(diǎn)和推力飽和6.1 仿真第一步就數(shù)值爆炸從哪里排查最讓人崩潰的調(diào)試場景是按下運(yùn)行姿態(tài)角沒幾十步就直接飛到了幾萬度MATLAB畫圖窗口里全是NaN和Inf。這種“第一步就發(fā)散”的問題絕大多數(shù)情況下不是控制器參數(shù)問題而是模型或接口層的錯(cuò)誤。排查順序我一般是這樣的檢查單位。角速度是用度/秒還是弧度/秒擺角是用度還是弧度。PID增益對(duì)單位極其敏感一個(gè)度/弧度混用增益數(shù)值差57倍直接發(fā)散。檢查姿態(tài)更新方程。四元數(shù)更新后是否歸一化歐拉角速率到體軸角速度的轉(zhuǎn)換矩陣是否寫對(duì)稱了這里一個(gè)符號(hào)錯(cuò)姿態(tài)遞推就是發(fā)散的。檢查控制力矩符號(hào)。擺角正方向與姿態(tài)角正方向是否一致。工程里常見的是“正擺角產(chǎn)生負(fù)俯仰力矩”這種定義反饋符號(hào)反了整個(gè)系統(tǒng)就是正反饋必然發(fā)散。檢查執(zhí)行機(jī)構(gòu)限幅。如果限幅寫錯(cuò)了范圍比如把±10度寫成了±0.1度控制力矩不足以糾正小誤差系統(tǒng)跑飛也是順理成章。排查的技巧是“最小化復(fù)現(xiàn)”把控制器輸出改成一個(gè)常數(shù)力矩看一個(gè)周期內(nèi)姿態(tài)更新是否正確如果正確再換成簡單的比例控制逐環(huán)加上去。這個(gè)方法雖然慢但能精準(zhǔn)定位問題層級(jí)。6.2 歐拉角奇異性與角度回繞的隱藏坑前面說過歐拉角在俯仰角90度時(shí)會(huì)出現(xiàn)萬向節(jié)鎖。VTVL飛行器在正常著陸段俯仰角接近90度反而特別容易踩進(jìn)這個(gè)坑。你以為飛行器幾乎直立俯仰角就是90度附近在這個(gè)區(qū)間用歐拉角做控制誤差計(jì)算誤差會(huì)變得極不穩(wěn)定。一個(gè)容易忽略的點(diǎn)是角度回繞。如果你的控制器存儲(chǔ)的姿態(tài)誤差是弧度值而姿態(tài)角是不斷轉(zhuǎn)過的當(dāng)誤差從179度變到-179度時(shí)直接相減得到的誤差是358度控制器以為要轉(zhuǎn)一個(gè)大圈實(shí)際只需要轉(zhuǎn)2度。表現(xiàn)在仿真里就是擺角指令突然加倍甚至飽和。處理辦法有兩個(gè)一是內(nèi)部用四元數(shù)計(jì)算誤差顯示和記錄才轉(zhuǎn)成歐拉角二是對(duì)所有角度誤差做wrapToPi處理讓誤差始終落在[-pi, pi]區(qū)間。MATLAB自帶wrapToPi函數(shù)但很多工程里控制律代碼是手工寫的要檢查有沒有做這一步。6.3 推力矢量飽和控制器輸出頂死的處理策略VTVL姿態(tài)控制最容易忽視的一個(gè)約束是推力矢量擺角限幅。當(dāng)飛行器初始姿態(tài)偏差很大或者外界擾動(dòng)很強(qiáng)時(shí)擺角指令會(huì)長時(shí)間頂在±10度的限幅位置。如果這時(shí)PID里的積分項(xiàng)還在繼續(xù)累積等偏差減小后積分項(xiàng)已經(jīng)過頭會(huì)帶來一股錯(cuò)誤的控制力矩導(dǎo)致姿態(tài)超調(diào)甚至發(fā)散?？狗e分飽和的實(shí)現(xiàn)不復(fù)雜常見的有三種積分限幅只讓積分項(xiàng)輸出不超過一個(gè)固定邊界。條件積分當(dāng)控制器輸出飽和時(shí)停止積分累加。反饋抗飽和把實(shí)際執(zhí)行機(jī)構(gòu)輸出與指令的差值反饋回積分環(huán)節(jié)讓積分量自動(dòng)衰減。工程包里如果用了PID大概率至少有積分限幅。沒有的話建議加一個(gè)代碼量不大但效果顯著。另外如果擺角一直飽和說明該通道的控制能力已經(jīng)不夠單純調(diào)PID沒用得考慮增大推力、加長力臂或降低控制帶寬。6.4 仿真步長對(duì)結(jié)果穩(wěn)定性的影響很多人在調(diào)參時(shí)遇到一個(gè)詭異現(xiàn)象同一組參數(shù)步長0.01s時(shí)姿態(tài)振蕩但能收斂步長0.001s時(shí)反而發(fā)散。這種情況不是控制器的問題而是積分算法穩(wěn)定性被步長和控制器延時(shí)的組合打破。固定步長離散化后控制回路存在一拍延遲。如果步長太大這個(gè)延遲相對(duì)系統(tǒng)時(shí)間常數(shù)來說就不可忽略等于給回路額外引入了一個(gè)純滯后環(huán)節(jié)。純滯后會(huì)降低相位裕度導(dǎo)致穩(wěn)定邊界變化。所以遇到步長變化引起結(jié)果大變的情況不是“步長越小越好”這么簡單而是要理解步長與控制周期的關(guān)系。一般建議固定步長取控制周期的五分之一到十分之一?？刂祁l率100Hz時(shí)仿真步長5ms到10ms通常可用10ms往上是勉強(qiáng)超過20ms基本失真。如果你的模型里有剛性執(zhí)行機(jī)構(gòu)動(dòng)態(tài)比如很強(qiáng)的彈簧約束還需要更小的步長這時(shí)可以考慮換用ode15s之類的隱式求解器。6.5 我實(shí)際調(diào)參后的幾點(diǎn)個(gè)人體會(huì)最后說幾個(gè)我在實(shí)際調(diào)參過程中積累的小經(jīng)驗(yàn)這些經(jīng)驗(yàn)書上不一定寫但非常管用。第一姿態(tài)控制器的“柔”比“快”重要。VTVL著陸段對(duì)姿態(tài)角速度峰值有嚴(yán)格要求如果為追求收斂時(shí)間把增益調(diào)得過高擺角和推力會(huì)產(chǎn)生明顯抖動(dòng)干擾導(dǎo)航和觸發(fā)結(jié)構(gòu)振動(dòng)。我通常用“無超調(diào)”作為調(diào)參目標(biāo)而不是“最短收斂時(shí)間”。第二外環(huán)P增益和外環(huán)帶寬要匹配執(zhí)行機(jī)構(gòu)能力。一個(gè)有用的驗(yàn)證方法是做“大偏差測試”給定20度初始俯仰偏差看擺角指令最大峰值是否超過限幅值的70%。如果超過說明擾動(dòng)稍微變大就會(huì)飽和需要降低外環(huán)帶寬或增大阻尼。第三每次調(diào)參都要記錄。把參數(shù)、仿真時(shí)間、蒙特卡洛成功率、姿態(tài)曲線峰值存成一個(gè)mat文件。批量掃參后復(fù)盤時(shí)你會(huì)慶幸自己做了記錄否則過了兩天連“這一組參數(shù)為什么好”都想不起來。第四仿真能過的指標(biāo)到了實(shí)物里至少留三倍裕度。這個(gè)工程包就算跑得再漂亮也只代表仿真環(huán)境下的控制效果。真實(shí)飛行里有結(jié)構(gòu)彈性、推進(jìn)劑晃動(dòng)、執(zhí)行機(jī)構(gòu)非線性、傳感器延遲等一系列仿真里沒建模的東西。所以在仿真階段就保持保守別把增益推到極限。這個(gè)VTVL姿態(tài)控制工程往大了說是火箭回收和垂直起降驗(yàn)證機(jī)的核心算法分支往小了說就是一套MATLAB下的剛體姿態(tài)控制完整案例。你把它跑通了等于把姿態(tài)動(dòng)力學(xué)建模、執(zhí)行機(jī)構(gòu)建模、反饋控制設(shè)計(jì)、數(shù)值仿真和調(diào)參驗(yàn)證這條鏈路走了一遍。以后換到無人機(jī)、導(dǎo)彈、機(jī)器人平衡等場景核心思路都是通用的。本文還有配套的精品資源點(diǎn)擊獲取