四旋翼無人機飛行控制算法設計及仿真分析

王建山 盧利中 丁 偉 張天宇 王光璞

（國網吉林省電力有限公司吉林供電公司，吉林 吉林 132012）

由于無人機技術日趨成熟，無人機逐漸成為一種新型智能裝備，由于其具有靈巧度高、體積小以及可以定點懸停等特性，因此應用范圍較廣[1]。四旋翼飛行器因其獨特的飛行方式，起降時所需空間較小，有利于在障礙物密集的環境中保持高機動性飛行。四旋翼無人機具備簡單的機械構造，主要由十字形框架和4 個旋翼組成。當對其進行數學建模時可以發現，它是一個強耦合、非線性和欠驅動的六自由度系統。通過調整4 個螺旋槳的速度就可以實現不同的飛行姿態[2]。由于非線性強耦合系統的特性，因此控制四旋翼飛行器從初始位置到目標位置且保持當前運動狀態比較困難[3]。為了達到該目的，需要對非線性四旋翼飛行器模型進行線性近似處理。該文研究了四旋翼飛行器的雙閉環結構，將外環速度環的控制輸入設置為內環角度環的輸出，以實現對飛行器姿態進行控制的功能。該文分析了四旋翼的非線性模型和雙閉環系統，建立了飛行器的動力學模型，在該模型的基礎上，設計了雙閉環控制系統，并采用MATLAB 進行控制系統穩定性仿真試驗。

1 四旋翼無人機動力系統模型

無人機動力系統模型是四旋翼無人機飛行控制算法設計研究的基礎，模型建立的準確性將直接影響控制算法設計的合理性和有效性。該文建立無人機位移運動方程、旋轉運動方程、動力學方程和系統控制分配模型，為建立控制算法和搭建仿真平臺奠定基礎。

1.1 運動學方程

1.1.1 位移運動方程

在已知四旋翼無人機機體坐標系下的三維運動速度（u，v，z）的情況下[4]，可以采用旋轉變換理論計算四旋翼無人機地面坐標系下的三維線運動速度（，，），如公式（1）所示。

式中：θ為俯仰角；γ為橫滾角；φ為偏航角。

1.1.2 旋轉運動

在已知四旋翼無人機機體坐標系下的三維姿態角速度（p，q，r）的情況下[5]，可以采用旋轉變換理論計算四旋翼無人機地面坐標系下的三維線姿態角加速度（，，），如公式（2）所示。

1.2 動力學方程

采用Newton-Euler 式來推導飛行器的動力學方程[6]，動力學方程如公式（3）所示。

1.3 系統控制分配模型

飛行控制有升降運動、滾轉運動、俯仰運動以及偏航運動4 種狀態，由安裝在翼端的4 個電機驅動螺旋槳轉動完成，分別用U1～U4表示，如公式（4）所示。

式中：b為飛行器的槳葉的升力系數；d為飛行器槳葉的阻力系數；Ωi（i=1，2，3，4）為飛行器的電機轉速。

2 控制算法設計

無人機控制算是整個飛行系統的控制核心[7]，決定無人機軌跡飛行的穩定性和安全性。隨著無人機在各領域的廣泛應用，無人機飛行控制算法受到了各國學者的廣泛關注[8]，并取得了顯著的研究成果。無人機飛行控制算法是無人機的大腦，一個好的控制算法可以保障無人機穩定、安全飛行[9]。目前，滑模控制算法和模糊控制算法是前沿研究理論，但是還未應用于實際工程中，當前普遍采用PID 算法完成位置控制器和姿態控制器算法的設計工作。

2.1 位置控制器算法設計

采用PID 算法求出四旋翼飛行器在地面坐標系下的線加速度，如公式（5）所示。

式中：zd為目標給定值；z為反饋高度；d為槳葉的阻力系數；kp3、kd3為2 個PD 算法參數。

通過俯仰角θ、橫滾角γ得到飛行器的電機輸入信號，如公式（6）所示。

式中：P1為飛行器的電機輸入信號；g為重力加速度。

2.2 姿態控制器算法設計

采用PID 算法求出四旋翼飛行器的電機輸入信號P2、P3和P4，分別如公式（7）～公式（9）所示。

式中：kpγ、kdγ、kiγ、kpθ、kdθ、kiθ、kpφ、kdφ和kiφ為9 個PID算法參數；eγ、eθ和eφ分別為飛行器的俯仰角、橫滾角和偏航角的歐拉角。

3 仿真分析

四旋翼無人機初始歐拉角和初始歐拉角速度分別為（0，0，0）rad、（0，0，0）r/s；四旋翼無人機初始位置、初始線速度為分別為（0.1，0.1，1.0）m、（0，0，0）m/s。四旋翼無人機期望飛行位置為（0，0，1.26）m。

在MATLAB 軟件中搭建了一個四旋翼仿真系統，以驗證飛行器控制系統在無風和有風干擾下的穩定性。為了驗證控制算法遇到有風干擾時的穩定性，分別在無風干擾和有風干擾2 種狀況下對控制系統的穩定性進行仿真驗證。有無東、北風干擾下的飛行器位置變化曲線如圖1 所示，有無東、北風干擾的飛行器沿地面坐標系飛行速度曲線如圖2 所示，有無東、北風干擾的飛行器姿態角響應曲線如圖3 所示。

圖1 有無東、北風干擾的飛行器沿地面坐標系飛行位置變化

圖2 有無東、北風干擾的飛行器沿地面坐標系飛行速度曲線

圖3 有無東、北風干擾的飛行器姿態角響應曲線

由圖1（a）可知，飛行器在起飛初始時刻的位置坐標為（0.1，0.1，1.0）。在經過20 s 的飛行后，飛行器成功達到目標坐標（0，0，1.26）處并保持了穩定的位置。整個系統確保了飛行器在期望的位置上進行穩定懸停飛行，說明控制算法在無風干擾下具有飛行穩定性，可以滿足特定場景下的飛行任務。由圖1（b）可知，在30 s 和60 s 的時間點，使用幅值為0.1、脈寬為4 s 的單脈沖方波模擬自然界的北風和東風干擾，導致飛行器在x軸和y軸上發生擾動。經過10 s，飛行器再次返回到目標位置（0，0，1.26）并保持不變。綜上所述，在無風干擾的情況下，四旋翼飛行器可以準確執行飛行、懸停動作，當有風干擾時，飛行器在發生小的飛行擾動后依然可以抵達目標位置，說明控制算法具有有效性和合理性，可以保證飛行器在干擾下的飛行穩定性。

由圖2（a）可知，在0 s～20 s 飛行器的速度波動較大，約20 s 時，速度接近0 m/s，表明此時飛行器已基本到達目標位置，其位置不再發生變化。整個飛行器控制系統確保了飛行器在期望的位置上進行穩定懸停飛行，說明控制算法在無風干擾下具有飛行穩定性。由圖2（b）可知，飛行器在30 s 受到了北風干擾，在60 s 受到了東北風干擾，導致在x軸和y軸方向的速度發生擾動。然而，大約經過10 s 后，飛行器速度再次穩定為0 m/s，使其進入懸停狀態，飛行器保持靜默狀態。綜上所述，飛行器飛行速度抗干擾性較強，即使遇到較大的風干擾，也可以迅速調整恢復到設定飛行速度，保證飛行器執行任務時的飛行穩定性。

由圖3（a）可知，飛行器的姿態角度在20 s 變為0°并保持穩定。整個飛行器控制系統系統確保了飛行器姿態的穩定，說明飛行器控制算法在無風干擾下具有飛行穩定性。由圖3（b）可知，在30 s 施加北風干擾，然后在60 s施加東北風干擾，飛行器的俯仰角和橫滾角受到一定強度的干擾。然而，經過約10 s 的時間，飛行器的姿態角恢復到初始的0°并保持了穩定的姿態角度。綜上所述，飛行器飛行姿態抗干擾性較強，在經歷干擾后可以迅速恢復到初始0°的位置，具有較高的飛行穩定性。

4 結語

該文采用Newton-Euler公式構建四旋翼無人機的動力系統模型，采用PID 控制算法設計了雙閉環控制系統，并在MATLAB 軟件中構建四旋翼仿真平臺。仿真結果顯示，設計的四旋翼無人機飛行控制系統可以抑制風干擾，具有較高的飛行穩定性，可以滿足工程應用的設計要求。