四旋翼無人機飛控系統設計與實現

胡徐勝 鄭睿 陶彬彬

【摘要】為了提升四旋翼無人機的飛行控制效果，采用PID控制算法和PWM控制對無人機的飛行進行控制。通過對無人機飛行運動狀態的研究，對無人機進行數學建模。引進了PID控制算法和PWM控制，并設計實驗驗證了算法的穩定性和良好效果。無人機的飛行控制一般通過避障來驗證，避障實驗證明，該算法避障效果好、魯棒性強。

【關鍵詞】無人機;四旋翼;避障;PWM

〔中圖分類號〕TP273 〔文獻標識碼〕A 〔文章編號〕1674-3229（2021）04-0046一05

0 引言

隨著通信技術、遙控技術以及智能控制技術的發展，無人機的應用領域越來越廣泛，而對無人機的控制系統也提出了越來越高的要求。四旋翼無人機與普通固定翼無人機不同，關閉電源后，四旋翼無人機將失去動力成自由落體下落，并進行滑行式飛行[1]。四旋翼無人機由四臺電動機控制，其可控性較固定翼無人機更高，可以實現快速垂直起降、定點控制等特殊功能。

1 四旋翼無人機運動狀態

四旋翼無人機簡易模型如圖1所示。X、Y、Z軸為三維空間坐標，通過調整四個電動機的速度變化來控制四旋翼無人機的各種飛行姿態、速度和動作[2]。

1.1 升降狀態

無人機起飛的條件是有向上的加速度，觸發條件則是無人機向Z軸產生的作用力大于無人機自重[3]。

圖1 四旋翼無人機簡易模型在無人機降落或向Z軸反方向運動的時候，需要向下的加速度，觸發條件則變為機身自重大于向Z軸正方向產生的作用力，加速度大小決定了無人機上升下降的速度。當四個電動機所產生的向Z軸正方向的作用力等于無人機的自重時，無人機Z軸方向上的力達到平衡，此時無人機處于懸停狀態。

1.2 俯仰狀態

在升降狀態下，四旋翼無人機會通過降低M4電動機的速度以降低升力，提高M2電動機的速度以提高升力[4]。如果無人機X軸的時間扭矩不平衡，則將產生負X軸扭矩。當向上移動時，在水平面上會產生向前的力，而無人機會受到充分的振動以完成向前的飛行。

1.3 橫滾狀態

在橫滾狀態下，四旋翼無人機穩定懸吊在空中停止時，通過降低M1電動機速度可使相應的升力降低，提高M3電動機速度可獲取相應的升程上升量，其中無人機的Y軸扭矩不平衡，并且產生正Y軸扭矩。由于側傾角現在為正，因此它將在水平面上創建一個正確的方向，無人機可以向右加速并完成向右飛行。

1.4 偏航狀態

當四旋翼無人機飛行時，它是一個相對獨立的系統。當UAV旋翼和電動機旋翼旋轉時，相互作用是基于力的，旋翼和旋翼向機身施加反扭矩，并且電動機和轉子為順時針和逆時針。當無人機的偏航條件保持不變時—保持偏航角，M2和M4施加的逆時針扭矩、M1和M3施加的作用力以平衡偏航扭矩應等于方向轉矩[5]。當無人機在水平飛行時，控制無人機的偏航角以使無人機頭部的左側偏斜可以增加M4和M2電動機的轉速，并降低M3和M1電動機的速度[6]。

2 四旋翼無人機坐標系定義及變換關系

四旋翼無人機動態建模中一般使用兩個坐標系，即機身坐標系A-XYZ和地面坐標系E-XYZ[7]。對二者關系的研究將為四旋翼無人機的飛行控制起到重要作用。

2.1 無人機機身坐標系A-XYZ

四旋翼無人機的重心是機身坐標系A-XYZ的原點，Y軸是四旋翼無人機的縱向對稱軸，并且在無人機的旋翼平面中，機頭的方向為正，它也位于無人機旋翼平面上與Y軸垂直的位置，表示從機身尾部到機身頂部的機身右側，X軸和Y-Z軸滿足直角坐標系，機身坐標系固定連接至無人機機身[8]。

2.2 地面坐標系E-XYZ

四旋翼無人機在飛行過程中，為了實現避障、俯沖等功能，將參照地面坐標系。地面坐標系E-XYZ如圖2所示。

地面坐標系E-XYZ繞Z軸旋轉方位角Ψ，得到中間坐標系0-x2y2z2，其變換公式為：

x2=CDx2·D（1）

其中，變換矩陣為：

中間坐標系0-x2y2z2繞x2軸旋轉俯仰角θ，得到中間坐標系0-x1y1z1，其中變換公式為（3）

中間坐標系0-x1y1z1繞y1軸旋轉橫滾角φ，得到機身坐標系A-XYZ，其中變換公式為：

經過上述三個方向的詳細研究，可以獲得四旋翼無人機機身坐標系與地面坐標系的變換關系：

由此定義地面坐標系到機體坐標系的姿態變換矩陣為

由此定義機體坐標系到地面坐標系的姿態變換矩陣為

3 飛控系統軟件設計

本文設計的飛控系統的軟件核心算法全部采用一塊STM32F4芯片控制[9]。在主程序中建立與地面站指令系統、GPS信號及氣壓傳感器、陀螺儀等信號的通信，并設置中斷機制來處理外部事件[10]。對中斷的處理支持優先級決策，即級別高的中斷具有優先處理特權。這種方式既能保持一定的系統實時性，實現起來也較簡單。具體如圖3所示。

3.1 主程序軟件流程

在飛控系統正常啟動工作后，四旋翼無人機搭載的速度傳感器、氣壓傳感器等不斷向飛控系統傳回實時數據，并快速進行譯碼、讀取，包括機身所處狀態、無人機在坐標系中的位置、陀螺儀、水平儀、GPS以及其他各傳感器的指令消息[11]。

3.2 中斷程序軟件流程

在整個四旋翼無人機的控制系統運行期間，此定時器中斷按設定以相應的間隔量切入主程序，此時獲得無人機飛行狀態、位置坐標等數據。通過與基站、控制終端或飛控系統內置的GPS導航信息進行通信，輸出對應的控制指令與數據，并將四個電機、舵機的數據，輸出到相應的接口，實施控制[12]。與定時器中斷不同的是，外部接口中斷通過GPIO口下拉的方式來切換中斷觸發條件，無人機接受導航指令便執行該中斷，例如接收到遙控器的PWM指令時，中斷切入之后獲得相應的導航指令，以此完成導航階段的控制過程。

4 實驗與數據

4.1 實驗模型建立

無人機在低速下超聲波折線多障礙物避障的實驗設計如圖4所示。

以水平方向為X軸，障礙物所在方向的豎直方向為Y軸，墻壁的垂直方向為Z軸建立空間直角坐標系，墻壁右側為坐標原點，夾角用αi、βi表示，假設撞擊到障礙物時的空間位置為（X2，Y2，Z2）與（X3，Y3，Z3），距離分別為H1、H2。

4.2 模型數據計算

4.3 實驗結果

表1為UAV飛行數據的監測，可以看出該次實驗的數據極為可靠，成功的次數較多，這說明UAV的避障飛行即使在轉向并且有障礙物的情況下也能實現終點的到達，為本次研究提供了有力的數據支持。

表2為折線多障礙飛行避障情形，從最后的總得分來看，其相對效果沒有單獨直線避障的效果好，卻有其獨特的優勢。UAV在飛行時難免會轉彎，并且轉彎后遇到突發事件，這為UAV的多重避障提供了良好的模型。導致本次實驗得分降低的原因有：一是折線多重避障使UAV的活動范圍大大減小，UAV在飛行過程中，由于轉向的問題，會使得撞擊墻壁的次數明顯增加，也使飛行成功到達終點的概率有所降低;二是綜合以上實驗知道，UAV不能自主選擇路徑進行繞行，多重障礙物的疊加更加增加了UAV的飛行限制，由于障礙物以及折線路徑的共同作用，UAV的路徑更加遭到了限定，到達的終點的方式減少，使得成功到達終點的概率減小。

5 結論

本文通過對四旋翼無人機飛行運動狀態的研究，對無人機進行數學建模，引進了經典的PID控制算法和PWM控制，并設計避障實驗驗證算法的穩定性和良好效果。結果表明，該算法避障效果好，驗證了設計的有效性和穩定性。

[參考文獻]

[1]韓敏，閆闊，秦國帥.基于改進KAZE的無人機航拍圖像拼接算法[J].自動化學報，2019，45（2）：70-74.

[2]史豪斌，徐夢，劉珈妤，等.一種基于Dyna-Q學習的旋翼無人機視覺伺服智能控制方法[J].控制與決策，2018，46（11）：335-337.

[3]胡徐勝，劉娟一種帶多路選擇的智能循跡小車優化設計[J].控制工程，2020，27（7）：1131-1135.

[4]王杰，張永輝，李憲鵬.無人機組合導航系統姿態控制方法仿真[J].計算機仿真，2019，36（6）：73-76+82.

[5]余亮，韓芳，王鵬，等.局部閉鏈碼垛機器人運動學分析及運動控制系統設計[J].廊坊師范學院學報（自然科學版），2020，20（2）：46-49+54.

[6]陳章寶，侯勇.網球檢球機器人視覺導航系統設計[J].廊坊師范學院學報（自然科學版），2020，20（2）：42-45.

[7]Wu Yu，Gou Jinzhan，Hu Xinting，et al.A new consensustheory-based method for formation control and obstacleavoidance of UAVs[J].Aerospace Science and Technology，2020，107（Dec.）：106332.1-106332.18.

[8]Shiqi Li，Shuai Zhang，Yan Fu，et al.Task-based obstacleavoidance for uncertain targets based on semantic object ma-trix[J]Control Engineering Practice，2020（105）：134-137.

[9]Binrong Wen，Xinliang Tian，Xingjian Dong，et al.Designapproaches of performance-scaled rotor for wave basin mod-el tests of floating wind turbines[J].Renewable Energy，2020，148（Apr.）：573-584.

[10] Shrishti P，Pandey，Prabhat K.Singh Basic Orange 21：Amolecular rotor probe for fluorescence turn-on sensing ofamyloid fibrils[J].Journal of Molecular Liquids，2020（303）：112618.

[11]Lijun Song，Hongwei Zhang，Yuhong Liu，et al.Researchon negative-buoyancy autorotating-rotor autonomous un-derwater vehicles[J].Applied Ocean Research，2020（99）：102123.

[12]Fanjing Huang，Panlong Wu，Xingxiu Li.DistributedFlocking Control of Quad-rotor UAVs with ObstacleAvoidance Under the Parallel-triggered Scheme[J].Inter-national Journal of Control，Automation and Systems，2021，19（3）：1375-1383.

[收稿日期]2021-05-18

[基金項目]安徽高校自然科學研究重點項目（KJ2019A1275）;安徽省智能機器人信息融合與控制工程實驗室資助項目（IFCIR2020003）

[作者簡介]胡徐勝（1982-），男，碩士，皖江工學院電氣信息工程學院副教授，研究方向：電氣自動化、智能控制。