1. PX4飛控PID簡介

相關鏈接：PX4官方中文文檔、PID概念(不了解PID的可以看看這個或者翻自控書吧)

PX4使用了串級PID控制器，串級PID通過分而治之的策略，將復雜控制問題分解為多個簡單子問題，在響應速度、抗干擾性、穩定性和工程實現上均優于單級PID。比如姿態環處理的IMU噪聲不會影響到位置環，同時各環PID參數可獨立調諧，內環保證局部穩定（如機體角速率收斂），外環在此基礎上優化全局性能。避免單級PID因參數耦合導致的振蕩或發散。

1.1 位置控制器

在位置外環中只使用了一個P控制器和一個限幅函數。使用當前的位置估計和期望位置相減得到誤差，然后經過P控制器和限幅得到期望速度，輸出到速度內環。

為什么外環只需要一個P控制環呢？
那是因為外環的核心任務是“設定目標”，而非快速跟蹤。核心任務是將位置誤差線性轉化為速度指令，無需積分或微分，速度內環的PID會補償外環的不足，避免振蕩和噪聲放大。若外環加入I或D控制，積分會導致超調或延遲，微分會放大噪聲，反而破壞系統穩定性，因此簡化外環、強化內環是最優策略。

1.2 速度控制器

速度內環控制器就熟悉一點，PID大套餐上齊了。首先估計速度直接進入LPF(低通濾波器，防止高頻干擾微分結果)然后計算微分，進入D環節，這一部分是防止輸出突變以及提前控制防止震蕩，其實也不難理解，就是對當前的估計做了微分反映了數據的運動趨勢；然后估計速度與期望速度作差得到誤差進入積分環節，這部分是**消除穩態誤差（如抵抗恒定風阻導致的速度偏差），其實就是識別到誤差長時間沒有歸零就加大矯正力度；最后是誤差進入P環節快速響應速度誤差**。

為什么內環使用PID控制器？速度內環用PID控制，因為它需要快速精準地跟蹤速度指令，比例項響應誤差，積分項消除靜差，微分項抑制擾動，確保內環動態性能足夠支撐外環。

總結：外環P控制像“指揮官”設定宏觀目標，內環PID像“執行者”處理細節，二者分工明確，避免單級PID的復雜性和不穩定性。

1.3 加速度和yaw轉到姿態

由于無人機的yaw和速度加速度在動力學上天然解耦，yaw角在規劃器中是可以單獨規劃的，不過常見規劃器還是按照軌跡的切向方向來的。圖中的yaw角也是直接從規劃器來的，而不是像pitch、roll需要從加速度中提取。

1.4 姿態控制器

多旋翼姿態控制器基于四元數（quaternion）設計，通過誤差四元數計算期望角速率，最終輸出飽和的角速率命令。

1.誤差計算（通過四元數乘法實現）：

四元數乘法如下（實部虛部分開）：

這里分為兩路，提取了實部的符號乘以虛部保證最短旋轉路徑（具體可以參考四元數的幾何表示，q和-q表示一樣的終端狀態，但是旋轉方向有大小區別，q可以保證旋轉最短角度）：

2.P控制器構造

角速度 Ω 的本質是旋轉偏差隨時間的變化率。在控制系統中，我們希望角速度命令與姿態誤差成比例關系，即**角速度的方向應對準旋轉軸，大小應與旋轉角度偏差成正比**。

觀察四元數的幾何表示：

可以看到w是影響旋轉角度大小，虛部影響旋轉軸方向還有旋轉角度大小。當誤差特別小時，可以使用小角度假設，此時的虛部等于1/2θn，那么根據前面的構造，可以進一步構造一個有物理意義的P系數即2P。

這個時候P的物理意義是姿態誤差每增加一弧度，角速度增加P弧度每秒。最后再限幅保證安全。

這里為什么選擇四元數而不選擇歐拉角呢？
主要有下列幾點原因：1.歐拉角有萬向節死鎖問題。2.四元數是線性計算，歐拉角是三角函數計算，更加復雜。3.通過四元數插值（如SLERP）可平滑過渡姿態，且誤差四元數的實部符號能自動選擇最短旋轉路徑。

這一環可以自動調參：自動調參，但是感覺對大軸距的飛機效果不好。

1.5 角速率控制器

K-PID：比例項（K）獨立于積分（I）和微分（D）路徑，直接作用于誤差信號。無人機需要快速響應姿態誤差（比例項K直接作用），同時通過積分消除穩態誤差（如風力干擾），微分抑制振蕩。

這一環可以自動調參：自動調參，但是感覺對大軸距的飛機效果不好。

計算完期望的體速率和推力之后，使用旋翼的模型計算每個電機的轉速：

由電機轉速得到力矩和推力公式：

對上式求逆得到每個電機轉速：

2. 線性二次型優化（LQR）控制

無人機的底層控制算法中，LQR（Linear Quadratic Regulator，線性二次調節器）是一種基于狀態空間模型的優化控制方法，廣泛應用于姿態、位置等底層控制環節。LQR是一種最優控制算法，通過最小化一個“代價函數”來求解最優控制輸入。其核心思想是：

• 線性系統：無人機動力學模型在平衡點附近可線性化（如懸停狀態附近）。
• 二次代價函數：平衡狀態誤差和控制輸入的能耗，實現“高性能+低能耗”的折中。

由于限制為一個線性系統，所以這個狀態方程就不會太復雜，以無人機懸停為例。

無人機的狀態方程（此時當做質點）：

設x為無人機的位置、速度，A為無人機的狀態轉移矩陣，B為輸入矩陣（描述控制輸入對狀態的影響），u為無人機的輸入，這里應該是加速度。

代價函數為：

最小化跟蹤誤差以及輸入加速度（節省能量）。Q：增大對角元素會強制對應狀態快速收斂。R：增大對角元素會限制控制輸入的幅值。

這個目標函數是一個標準的二次型優化問題（可以類比于二次函數），是有解析解的。下面是推導過程：

1.先構建一個關于狀態向量的值函數(其中P 是對稱正定矩陣，需要求解)：

2.帶入HJB方程（哈密頓-雅各比-貝爾曼方程）

這個方程表達了 從當前狀態出發，若采取最優控制策略，所能達到的最小代價應滿足某種微分方程。

再把值函數帶入得到：

然后對這個HJB方程對輸入u求導：

得到

再構造一個反饋增益矩陣K：

再將結果帶入HJB方程得到：

最終求得

**總結：**LQR通過數學模型和優化理論，為無人機提供了一種高效、穩定的底層控制方案。其核心是狀態反饋和代價函數最小化，適合需要精確建模和性能優化的場景。

3. 模型預測控制MPC/NMPC

MPC（Model Predictive Control，模型預測控制）是現代控制中非常重要的一個方法，特別適用于多變量系統、有約束系統或需要提前規劃行為的場景，比如無人車路徑跟蹤、無人機避障、工業過程控制等。MPC 是一種滾動優化控制策略。它的基本思想是：

• 使用系統模型預測未來狀態變化（例如未來幾秒或幾個采樣周期）；
• 基于預測結果最小化一個代價函數（類似 LQR 的目標函數）；
• 求解一個有限時域的優化問題，得到一組未來控制序列；
• 只執行第一個控制輸入，然后重新預測、重新優化，循環進行。

這就是 MPC 的“預測-優化-執行-滾動”機制。

如果系統方程是線性的就是MPC，如果非線性就是NMPC。

3.1 MPC

還是以無人機懸停為例，且暫時考慮一維模型，狀態變量：

控制輸入為加速度：

系統模型（離散）為：

代價函數為：

求解過程：

1.預測：

給定當前狀態𝑥0，可以展開出整個時域的狀態預測：

定義控制向量：

將整個狀態預測寫成矩陣形式：

2.優化求解
將預測狀態代入代價函數：

代入狀態展開：

展開為標準形式：

同時考慮一些必要的硬件約束，調用優化庫進行求解得到數值解。

3.執行

執行求解后的u_0，即控制向量的第一個元素。

總流程

每個控制周期：

1. 讀取當前狀態𝑥0建預測矩陣 𝑆𝑥,𝑆𝑢 構造代價函數和約束
2. 解二次規劃問題，得到控制向量 𝑢?
3. 僅執行第一個控制輸入 𝑢_0?
4. 系統前進一步，滾動窗口，重復上述步驟

3.2 NMPC

仍然是無人機懸停為例，使用NMPC直接控制無人機的電機轉速。

狀態變量（12維）：

控制變量（4維）：

系統模型：

這個M就是1.5節的M矩陣。

代價函數：

然后再寫出擴展過的系統模型，帶入代價函數進行優化求解。

**總結：**LQR就是MPC無約束、無時域限制的特殊形式，而且由于形式簡單，可以準確計算解析解，計算快、準。