吳承建，沈 捷，陳乾坤

(南京工業(yè)大學 電氣工程與控制科學學院，南京 211816)

?

基于ARM的四旋翼姿態(tài)控制器設計

吳承建，沈 捷，陳乾坤

(南京工業(yè)大學 電氣工程與控制科學學院，南京 211816)

四旋翼姿態(tài)控制器采用集成了加速度計和陀螺儀的慣性測量單元，實時采集姿態(tài)數(shù)據(jù),傳輸給Cortex-M4內(nèi)核的處理芯片，利用四元數(shù)姿態(tài)解算方法，對加速度和角速度數(shù)據(jù)融合解算處理；采用位置式PID控制算法，控制4個無刷電機的轉(zhuǎn)速，實現(xiàn)控制四旋翼飛行器的飛行姿態(tài)；建立萬向云臺調(diào)試系統(tǒng)，通過實踐調(diào)試驗證該控制器能實現(xiàn)控制四旋翼姿態(tài)的穩(wěn)定性；穩(wěn)定飛行時，姿態(tài)角的平均振蕩范圍為5°。

慣性測量單元 ；PID；四元數(shù)；數(shù)據(jù)融合；姿態(tài)角；萬向云臺調(diào)試

0 引言

近年來，隨著芯片、材料等技術的進步，加快了旋翼飛行器發(fā)展速度，在諸如地震災情探查、電影電視節(jié)目錄制等方面發(fā)揮著重要的作用。四旋翼飛行器有著相對于傳統(tǒng)直升機特有的優(yōu)勢，包括便捷性、易操作和靈活性等?？紤]到四旋翼飛行器是具有6個輸入量、4個輸出量的非線性欠驅(qū)動系統(tǒng)[1]，這使得飛行控制器的設計相對比較困難。本文以四旋翼飛行器的姿態(tài)穩(wěn)定控制為目標，設計了主控制器硬件，在姿態(tài)角與加速度數(shù)據(jù)的解算中運用了卡爾曼濾波器，搭建了基于方向節(jié)的四旋翼實物調(diào)試系統(tǒng)。通過不斷調(diào)整給定參數(shù)，重復試驗，直至四旋翼能夠保持姿態(tài)穩(wěn)定。

1 控制器硬件系統(tǒng)

本文設計的四旋翼控制器采用32位ARM微控制器，運行最高時鐘頻率為168 MHz，主要處理多傳感器實時采集到的飛行器姿態(tài)數(shù)據(jù)。慣性測量單元集成了陀螺儀和加速度計，利用IIC串行總線采集并輸出數(shù)據(jù)到主控芯片，數(shù)據(jù)經(jīng)過處理后輸出4路PWM波控制電機。四旋翼飛行器系統(tǒng)結構圖如圖1所示。

圖1 四旋翼控制器結構圖

1.1 微控制器芯片

飛行控制器中的核心控制芯片采用ST意法半導體公司基于Cortex-M4內(nèi)核架構開發(fā)的STM32F407VGT6，該芯片具有通信接口資源豐富、功耗較低、成本低和外設功能較全等特點，并且支持FPU浮點運算，提高了數(shù)據(jù)運算精度[4]。芯片將慣性測量單元采集到的加速度與角速度數(shù)據(jù)解算成姿態(tài)角，經(jīng)過增量式PID控制算法處理，產(chǎn)生相應占空比的PWM波。慣性測量單元從采集到傳輸給主控制芯片耗時約3 ms，因此對微控制器響應速度有一定的要求。從STM32F407VGT6的特點來看，豐富的接口資源能夠充分的滿足上位機、串口、傳感器等對接口的要求。

1.2 慣性測量單元

飛行控制器的慣性測量單元實時測量四旋翼飛行器的姿態(tài)數(shù)據(jù)[2]，本控制器中采用的型號為MPU6050，該單元內(nèi)部集成了一個三軸MEMS加速度計和一個三軸MEMS陀螺儀，通過IIC接口與主控芯片連接。由于數(shù)據(jù)打包傳輸，微控制芯片在接收到數(shù)據(jù)后必須先進行離散化處理，分化為對應的X、Y、Z軸數(shù)據(jù)。MPU6050對陀螺儀和加速度計分別使用了3個16位的ADC，將測量的模擬量轉(zhuǎn)換為可輸出的數(shù)字量；為了精確跟蹤快速和慢速的運動，規(guī)定傳感器的測量范圍可控。初始時刻預設四旋翼飛行器的地面參考坐標系與慣性測量單元的坐標系重合，設滾轉(zhuǎn)、俯仰和偏航姿態(tài)角[3]為(α,β,θ)。

陀螺儀測量3個軸上的角速度，通過利用四元數(shù)算法將角速度解算為姿態(tài)角度。

1.3 無線通信

四旋翼控制器除了與遙控器通信，在調(diào)試過程中需要與上位機通信，在設計過程中綜合考慮成本、效率、傳輸距離等因素，選擇NRF24L01作為無線通信模塊。該模塊集成了晶振、數(shù)字接口、收發(fā)器等功能。在計算機端與飛行控制器各連接NRF24L01模塊，上位機既可以向四旋翼飛行器發(fā)送指令，也可以將四旋翼飛行器的飛行參數(shù)上傳至上位機，實時監(jiān)測飛行姿態(tài)。飛行器需要傳輸給上位機的參數(shù)包括PID參數(shù)、加速度與角速度和姿態(tài)角等。

2 控制器軟件

2.1 四元數(shù)算法

四元數(shù)是一個四維的復合數(shù)，它描述了剛體的定點轉(zhuǎn)動的過程，包含一個旋轉(zhuǎn)軸和一個旋轉(zhuǎn)角度信息。四旋翼的位姿有四元數(shù)和歐拉角兩種表示方式，本文利用四元數(shù)算法，并結合歐拉角坐標轉(zhuǎn)換矩陣，通過對角速度進行四元數(shù)解算，得到機體旋轉(zhuǎn)各個軸上歐拉角分量。

四元數(shù)[4]可表示為：

(1)

取地面參考坐標系為N系，機體坐標系為B系，初始時兩個坐標系重合。四旋翼一次運動過程可等效為剛體定點旋轉(zhuǎn)過程，如圖2所示。四旋翼初始位置如A點所示，轉(zhuǎn)動φ角度后位置如A′所示。

圖2 機體旋轉(zhuǎn)示意圖

(2)

四元數(shù)Q對任意一個向量的作用就是繞著旋轉(zhuǎn)軸旋轉(zhuǎn)φ角度。式(2)可寫為：

(3)

可由上式得N系與B系的坐標轉(zhuǎn)換矩陣D為：

D=

(4)

若歐拉角α、β、θ表示四旋翼姿態(tài)角度，可得兩坐標系間的歐拉旋轉(zhuǎn)矩陣[5]為：

(5)

由于歐拉角轉(zhuǎn)換矩陣等效于四元數(shù)轉(zhuǎn)換矩陣，將兩個轉(zhuǎn)換矩陣做等式，求解等式，可得旋轉(zhuǎn)的歐拉角為：

(6)

對(1)式兩邊進行求導，最終可得：

(7)

(8)

根據(jù)龍格-庫塔法[6],有：

(9)

其中：I∈R4×4為單位矩陣。

(10)

2.2 PID 姿態(tài)控制算法

PID控制策略是一種十分經(jīng)典的控制方法，在各種控制工程系統(tǒng)里應用十分普遍，而且更適合實踐應用。本系統(tǒng)采用雙閉環(huán)串級位置式PID控制策略，在角速度控制器處理數(shù)據(jù)之前，對角速度數(shù)據(jù)進行滑動平均濾波。其結構如圖3所示。

圖3 PID控制結構圖

PID控制使用位置式PID方法[7]，設姿態(tài)角初始值均為0。設X、Y、Z軸3個方向的角度差值分別為Δα、Δβ、Δθ，其中αdX軸方向上角度角的目標值，α為X軸方向上角度測量值，則：

(11)

以X軸方向為例，輸入?yún)?shù)為偏差角，PID輸出控制量為：

(12)

式中，Kp1_roll、Ki1_roll和Kd1_roll分別為角度閉環(huán)的比例、微分和積分控制系數(shù)。同理，Y軸和Z軸的PID輸出控制量記為u1_pit、u1_yaw。

在角速度控制閉環(huán)中，X、Y和Z軸方向上的角速度偏差表示為：

(13)

式中,ωd_roll為X軸方向上角速度期望值，ωroll為X軸方向上角速度測量值。以X軸方向為例，輸入?yún)?shù)為偏差角速度，PID輸出控制量為：

(14)

式中，Kp2_roll、Ki2_roll和Kd2_roll分別為角速度閉環(huán)的比例、微分和積分控制系數(shù)。同理Y軸和Z軸的PID輸出控制量為u2_pit、u2_yaw。

由(13)式和(14)式求和，可得X軸方向的串級PID輸出控制量：

(15)

同理Y軸和Z軸的串級PID輸出控制量為upit、uyaw。

驅(qū)動4個無刷電機的控制量分別記為M1、M2、M3、M4，記T為遙控器油門控制量，則：

(16)

3 姿態(tài)調(diào)試

3.1 調(diào)試系統(tǒng)

飛行控制器與四旋翼機架、電調(diào)、電機、槳葉等器件組裝完成[8]，在四旋翼室外試飛之前需要對其進行調(diào)試[9]，判定四旋翼的滾轉(zhuǎn)、俯仰和偏航姿態(tài)的穩(wěn)定性?？紤]到四旋翼槳葉高速旋轉(zhuǎn)具有一定的危險性，因此利用萬向云臺和四旋翼飛行器組成姿態(tài)調(diào)試系統(tǒng)，如圖4所示。

圖4 姿態(tài)調(diào)試平臺

使用萬向云臺調(diào)試具有很強優(yōu)勢，一方面只對四旋翼機體施加向下的拉力，不影響飛行姿態(tài)，另一方面可以實現(xiàn)小范圍調(diào)試同時保證安全。

3.2 PID參數(shù)整定

調(diào)試過程中，針對四旋翼的姿態(tài)需要不斷調(diào)整PID參數(shù)，Kp1— roll,Ki1—roll,Kd1—roll分別表示角速度閉環(huán)的滾轉(zhuǎn)姿態(tài)的PID參數(shù)[10]。

PID參數(shù)整定從P值開始，P值是PID中最重要的部分，直接決定飛行器的飛行結果。首先取I和D值為零，改變滾轉(zhuǎn)和俯仰的P值直到飛行器變得難以隨意傾斜；根據(jù)偏航姿態(tài)，重復改變偏航P值，直至四旋翼對偏航具有較大的阻尼力。其次，保證P值不變和D值為零，改變I值，直至機體的修正力度較大且機體振蕩不變。最后，保持P和I值，改變滾轉(zhuǎn)和俯仰的D值，使用遙控器發(fā)送姿態(tài)動作指令，直至四旋翼返回初始位置的動作能夠快速響應。根據(jù)上述調(diào)試方法，重復調(diào)試實驗，當PID的參數(shù)取表1中的值時，四旋翼的姿態(tài)可以保持平穩(wěn)。

表1 PID參數(shù)

3.3 調(diào)試分析

取上述表1中的參數(shù)，數(shù)據(jù)采集周期為40ms，NRF24L01無線模塊將采集的加速度、角速度和姿態(tài)角等數(shù)據(jù)上傳至上位機并保存，如圖5～7所示。

圖5中，X、Y軸加速度從初始值逐漸趨于零，圖6中3個軸上的角速度減小并趨于相同，圖7中姿態(tài)角大小在小幅的范圍內(nèi)振蕩，從3個變量的變化趨勢中可以推斷出四旋翼的隨著時間逐漸趨于穩(wěn)定。

4 結束語

圖5 加速度-時間曲線

圖6 角速度-時間曲線

圖7 姿態(tài)角-時間曲線

必然會導致其比采用多測量單元控制器的精度低，為了提高精確度，在數(shù)據(jù)處理層面使用結合卡爾曼濾波算法的PID控制策略，使得控制器誤差保持在可控范圍，能夠滿足穩(wěn)定性要求。通過建立四旋翼飛行器的調(diào)試系統(tǒng)，在排除室外環(huán)境的干擾下對PID參數(shù)進行整定，調(diào)試結果表明在姿態(tài)數(shù)據(jù)處理過程中，采用卡爾曼濾波器的控制器能夠控制四旋翼的穩(wěn)定飛行，說明該控制器設計是合理的。從結果可知，在萬向云臺上，四旋翼趨于穩(wěn)定，機體存在振蕩，其振蕩姿態(tài)角度在5度之內(nèi)，滿足穩(wěn)定要求。

[1] 魏麗文．四旋翼控制系統(tǒng)設計[D].哈爾濱：哈爾濱工業(yè)大學，2010.

[2] 翼 明，等.基于Cortex-M4的四旋翼飛行控制系統(tǒng)設計[J].計算機測量與控制,2013，21(7)：1816-1821.

[3] 劉 乾，孫志鋒. 基于ARM的四旋翼無人飛行器控制系統(tǒng)[J].機電工程，2011,28(10)：1237-1240.

[4] 江 斌. 小型無人低空四旋翼飛行器綜合設計[D].杭州：浙江大學，2013.

[5] 秦永元. 慣性導航[M].北京：科學出版社，2006.

[6] 袁信等. 捷聯(lián)式慣性導航原理[M].北京：國防工業(yè)出版社，2003.

[7] 劉煥曄.小型四旋翼飛行器飛行控制系統(tǒng)研究與設計 [D].上海：上海交通大學，2009.

[8] 王 帥，魏 國.卡爾曼濾波在四旋翼飛行器姿態(tài)測量中的應用[J].兵工自動化， 2011, 30(1): 73-74.

[9] Sikiric V.Control of quadrocopter [D].Stockholm:Royal Institute of Technology,2008:8-30.

[10] 龐慶霈，李家文，黃文浩. 四旋翼飛行器設計與平穩(wěn)控制仿真研究[J].電光與控制，2012, 19(3):51-55.

Design of Four Rotor Attitude Controller Based on ARM

Wu Chengjian, Shen Jie，Chen Qiankun

(College of Automation and Electrical Engineering, Nanjing Tech University, Nanjing 211816，China )

Four rotor attitude controller adopts the inertial measurement unit which integrated accelerometer with gyro, gather attitude data real-time, then transferred to the Cortex-M4 core processing chips, Using the attitude of quaternion calculation method, the data integration of acceleration and angular velocity is calculated. And use positional PID control algorithm to control four brushless motor speed to achieve controlled four-rotor aircraft flight attitude. Establish universal platform debug system, through the practice of testing to verify controller can achieve the stability control of four rotor profile. The stable flight attitude average oscillation angle range is 5 degree.

IMU; positional PID; quaternion; data fusion；attitude；universal platform debug system

2015-05-17；

2015-06-03。

吳承建(1990-)，男，安徽蕪湖人，工學碩士，主要從事嵌入式控制系統(tǒng)方向的研究。

沈 捷(1976-)，男 ，安徽銅陵人，副教授，碩士生導師，主要從事機器人、嵌入式系統(tǒng)方向的研究。

1671-4598(2016)06-0077-03

10.16526/j.cnki.11-4762/tp.2016.06.021

TH873.7

A