四旋翼飛行器的LQR優化控制

北京信息科技大學自動化學院，北京 100192

一、引言

隨著無人機地快速發展，無人機巡檢技術以其良好的特性成為各大公司的巡檢利器，并大大減少了維護成本。為了便于對巡檢位置上的故障分析診斷，無人機巡檢時要求拍攝高清晰度的照片，因此，巡檢時要求無人機要盡可能地靠近巡檢對象，但是距離越近，必然存在另一方面的安全隱患，若不對其速度和飛行姿態，飛行方向加以控制，就很有可能撞上巡檢對象，所以研究巡檢無人機的優化飛行控制具有重要的工程應用意義。

在巡檢無人機中，四旋翼飛行器具有結構簡單、成本低、輕小易攜帶、能垂直起降、可實現六自由度等優點，因而可以看作是一種良好的飛行控制算法驗證平臺。它除了具備一般無人機隱蔽性好、生命力強、造價低廉、操控簡單、起降穩定的優點以外，其特殊的結構使其還有以下優點[1-5]。

1、飛行器體積小、方便攜帶，適合多平臺、多空間的應用，可以在地面、軍艦上自由靈活地垂直降，無需發射架、彈射器等輔助裝置；

2、具備良好的機動性，能快速、靈活地在三維空間進行運動，可大大縮小旋回半徑，從而可以節省時間、提高效率；

3、結構簡單、便于操控，可執行諸如軍事偵察、目標鎖定、航拍、森林救火、重點區域安防巡邏等各種特殊、危險的任務。

本文以四旋翼飛行器作為研究對象，在原有的線性二次最優控制算法的基礎上，采用LQR優化控制算法，該算法保證了無人機在巡檢過程中其飛行控制系統的穩定性，以及面對不同干擾信號良好的抗干擾性能。

二、四旋翼飛行器的數學模型

圖1所示為四旋翼飛行器結構模型圖。從該結構模型可以看出，四旋翼飛行器是通過分布于四周的電機（M1，M2，M3，M4）的轉速調整來實現俯仰、滾轉、偏航運動的，并且四個電機承擔著提供升力的任務。通過改變4個電機的轉動速度，我們可以控制飛行器的垂直起降運動。其中，M1和M3將為飛行器的俯仰運動提供力矩；M2和M4為飛行器的滾轉運動提供力矩；M1、M2、M3、M4共同作用將實現飛行器的偏航運動。

下面研究如何根據飛行器的動力學特性建立其數學模型。

首先，建立空間坐標系A(OXYZ)和機體坐標系B(oxyz)如圖2所示。并做如下假設[1-5]：

1、四旋翼飛行器是完全均勻對稱的剛體結構；

2、四旋翼飛行器的幾何中心和重心重合；

3、四旋翼飛行器飛行時其飛行姿態角變化很小；

4、直流電機的輸入電壓和輸出力矩之間呈線性關系。

在這些假設條件下，可以把四旋翼飛行器的動力學模型等效成一個固定的飛行器在空中由電機轉子轉動而引起的空氣動力學特性。

利用牛頓第二定律，該四旋翼飛行器的動力學模型為[1]：

其中，L—表示四旋翼飛行器的質心到空間坐標系原地的距離；

m—表示四旋翼飛行器的總質量；

Fu—4個電機的合力；

λ—表示四旋翼飛行器相對于機體坐標系中的旋轉角速度，其具體表達式如下：

（C和S分別代表三角函數cos和sin）

在小角度情況下，λ可近似為即：

式中，θ—表示四旋翼飛行器的縱軸(oz)與水平面(OXY)的夾角，為俯仰角，若飛行器縱軸指向水平面上方，則θ為正，反之為負；

φ—表示B坐標系的坐標軸與包含四旋翼飛行器縱軸(oz)的鉛垂直平面的夾角，為滾轉角，由飛行器尾部順縱軸前視，若(oz)縱軸位于鉛垂面右側（即飛行器向右傾斜），則φ為正，反之為負；

Ψ—為四旋翼飛行器縱軸在水平面內投影與空間坐標系(OX)軸之間的夾角，為偏航角，前視Ψ角平面，若由(OX)軸轉至投影線為逆時針旋轉，則Ψ為正，反之為負。

I∈R3×3—表示為坐標系B中機體的轉動慣量：

式中，Ixx，Iyy，Izz—分別為機體繞三坐標軸的轉動慣量；

R—表示一個3×3階的方向余弦矩陣[1-2]，表示從空間坐標系到機體坐標系的轉換矩陣[3-4]：

（C和S分別代表三角函數cos和sin）

對于給定的列向量λ=[λ1λ2λ3]T，定義S(λ)為：

綜上所述，空間坐標系下的四旋翼飛行器的數學模型為：

（C和S分別代表三角函數cos和sin）

其中，l—機體幾何中心距電機安裝中心的距離；

Kp—升力系數，

ωi—旋轉角速度；

U1,U2,U3,U4——4個電機的角速度決定的系統的控制輸入量，具體如下：

式中，Kd—拖拉系數。

旋翼由無刷直流電機驅動，直流電機電樞回路的電壓平衡方程和電機轉動的力矩平衡方程為[1]：

式中，u—電機輸入電壓；

i—電機輸入電流；

ke—反電勢系數；

km—電機轉矩系數 ；

kr—負載轉矩常量；

R—電機本身阻抗；

Ir—電機的慣性常數。

由于四旋翼飛行器使用的小型直流電機具有很低的感應系數，故感應系數Li可以忽略不計，式（9）可以近似為：

三、四旋翼飛行器的LQR優化控制

根據四旋翼飛行器的特點和動力學特性，我們選擇LQR控制方法，LQR控制是在規定的限度下，使被控系統的性能指標達到最佳狀態的控制；即在滿足一定的約束條件下，尋求最優控制策略，使得系統性能指標取得極大值或極小值。

定理1對于給定的受控系統[7]：

以及其性能指標：

其中，Q、R—定常對稱正定矩陣。

設[A,B]可控，則存在唯一的最優控制：

最優性能指標簡化為：

式中，x*(t0)=x0，它所對應的最優軌跡是式（15）的解x*(t)：

“城市復興”視角下的古城更新規劃探索——以許昌曹魏古城城市設計為例 滕 熙 張 萍2018/04 40

矩陣P為Riccati代數方程的唯一對稱正定解。

式中，A—系統的系數矩陣；

B—系統的輸入矩陣；

Q,R—給定的定常對稱正定矩陣；

K—最優反饋增益矩陣。

定理2設[A,B]可控，[A,C]可觀，R〉0，Q=DT，D≥0，則受控系統式（11）在指標式（12）下的最優控制為：

它所對應的最優軌跡為式（18）的解x*(t)：

而常數向量為：

其中，yr—參考信號，即期望信號。

將所建立的數學模型式（7）表示為狀態空間表達式為：

（C和S分別代表三角函數cos和sin）

其中，θ、φ、Ψ—分別為俯仰角、滾轉角、偏航角；

Ixx，Iyy，Izz—分別為滾轉轉動慣量、俯仰轉動慣量、偏航轉動慣量；

kfc—電壓與升力比；

l1—前向升力距質點的距離；

ω—左右臂與X軸的夾角；

U1,U2,U3,U4—分別為前向電機電壓、左向電機電壓、右向電機電壓、后向電機電壓。輸出為：

目標函數可以寫為：

R，Q分別為系統輸入向量和狀態向量的定常正定對稱矩陣，根據實際需求要對系統實現最優控制，需要在控制輸出間尋找一個最優平衡狀態。經過多次仿真測試與多次試驗，取R=diag[1,1,1,1]，Q=diag[10,2.8,5.1,2,1,0.1]，此時能夠得到較優的控制效果。若要使得上述性能指標最小化，可由定理1中的Riccati方程求出矩陣P，并找出狀態反饋控制律：即當四旋翼飛行器趨于穩定時，可實現系統輸出跟蹤期望輸出[9]。

四、四旋翼飛行器LQR優化控制仿真

1、LQR優化控制器對未加干擾信號的跟蹤情況

根據所建立的四旋翼飛行器的模型，仿真結果如圖3～圖5所示。假設此時的俯仰角、滾轉角、偏航角的期望輸出為0°。

由圖3、圖4、圖5可以看出，在初始運行階段，系統在初始狀態隨著LQR控制器的控制參數K的調節作用，經過很短時間的動態調節過程后，其俯仰角、滾轉角、偏航角即可準確地跟蹤系統的期望輸出。

2、LQR優化控制器對加入階躍信號作為干擾信號的跟蹤情況

為考察所設計的控制器對階躍信號的抗干擾性能，我們進行假設，假設俯仰角、滾轉角、偏航角的期望值為-0.5°，在所設計的LQR控制器中加入幅值為0.1的階躍信號作為干擾，我們可以得到仿真圖如圖6～圖8所示。

從圖6、圖7、圖8可以很明顯地看出，在初始運行狀態，我們在系統中加入階躍信號作為干擾信號時，系統會隨著所設計的LQR控制器在反饋矩陣K的調節作用下，經過較短的動態調節過程后，該系統最終達到穩定狀態，即俯仰角、滾轉角、偏航角可以準確地達到系統所期望的值，并且曲線幾乎沒有波動，從而說明了所設計的LQR優化控制器具備一定的階躍信號抗干擾能力。

3、LQR優化控制器對加入白噪聲作為干擾信號的跟蹤情況

為考察所設計的控制器的對白噪聲信號的抗干擾性能，我們進行假設，假設俯仰角、滾轉角、偏航角的期望值為-0.5°，在所設計的LQR控制器中加入幅值為0.1的白噪聲信號作為干擾，我們可以得到仿真圖圖9～圖11。

從圖9、圖10、圖11可以很明顯地看出，在初始運行狀態，我們在系統中加入幅值為0.1的白噪聲作為干擾信號時，系統會隨著所設計的LQR控制器在反饋矩陣K的調節作用下，經過動態調節過程后，該系統最終達到相對穩定狀態，即俯仰角、滾轉角、偏航角可以達到系統所期望的值，但是曲線波動較大，說明了所設計的LQR優化控制器在一定條件下相對具備對白噪聲抗干擾能力。

4、LQR優化控制器對加入三角函數作為干擾信號的跟蹤情況

為考察所設計的控制器的對三角函數信號的抗干擾性能，我們進行假設，假設俯仰角、滾轉角、偏航角的期望值為-0.5°，在所設計的LQR控制器中加入幅值為0.1的三角函數信號作為干擾，我們可以得到仿真圖如圖12～圖14所示。

從圖12、圖13、圖14可以很明顯地看出，在初始運行狀態，我們在系統中加入幅值為0.1的三角函數作為干擾信號時，系統會隨著所設計的LQR控制器在反饋矩陣K的調節作用下，經過短暫的動態調節過程后，該系統最終達到穩定狀態，即俯仰角、滾轉角、偏航角可以很準確地達到系統所期望的值，并且曲線幾乎沒有波動，說明了所設計的LQR優化控制器對三角函數具備良好的抗干擾能力。

5、LQR優化控制器對加入脈沖函數作為干擾信號的跟蹤情況

為考察所設計的控制器的對脈沖函數信號的抗干擾性能，我們進行假設，假設俯仰角、滾轉角、偏航角的期望值為-0.5°，在所設計的LQR控制器中加入幅值為0.1的脈沖信號作為干擾，我們可以得到仿真圖如圖15～17所示。

從圖15、圖16、圖17可以很明顯地看出，在初始運行狀態，我們在系統中第10s時加入幅值為0.1的脈沖函數作為干擾信號時，系統會隨著所設計的LQR控制器在反饋矩陣K的調節作用下，經過短暫的動態調節過程后，該系統最終達到穩定狀態，即俯仰角、滾轉角、偏航角可以很準確地達到系統所期望的值，并且曲線幾乎沒有波動，說明了所設計的LQR優化控制器對脈沖函數具備良好的抗干擾能力。

五、結束語

本文以四旋翼飛行器為研究對象，建立了四旋翼飛行器的數學模型，以LQR控制理論為基礎，設計了四旋翼飛行器的最優控制算法，進行了仿真，并對多個干擾信號進行抗干擾性能考察，從仿真結果看，系統總體呈現穩定狀態，運動過程中，四旋翼飛行器的俯仰角、滾轉角、偏航角都能較快速地趨于穩定，取得了良好的控制效果及控制精度，并且對階躍信號、白噪聲、三角函數干擾、脈沖信號具備一定的抗干擾性能。仿真結果驗證了所設計的LQR優化控制器的有效性、可靠性、準確性。