吳承建，沈 捷，陳乾坤

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

?

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

吳承建，沈 捷，陳乾坤

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

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

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

0 引言

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

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

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

圖1 四旋翼控制器結(jié)構(gòu)圖

1.1 微控制器芯片

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

1.2 慣性測(cè)量單元

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

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

1.3 無線通信

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

2 控制器軟件

2.1 四元數(shù)算法

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

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

(1)

取地面參考坐標(biāo)系為N系，機(jī)體坐標(biāo)系為B系，初始時(shí)兩個(gè)坐標(biāo)系重合。四旋翼一次運(yùn)動(dòng)過程可等效為剛體定點(diǎn)旋轉(zhuǎn)過程，如圖2所示。四旋翼初始位置如A點(diǎn)所示，轉(zhuǎn)動(dòng)φ角度后位置如A′所示。

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

(2)

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

(3)

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

D=

(4)

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

(5)

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

(6)

對(duì)(1)式兩邊進(jìn)行求導(dǎo)，最終可得：

(7)

(8)

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

(9)

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

(10)

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

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

圖3 PID控制結(jié)構(gòu)圖

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

(11)

以X軸方向?yàn)槔?，輸入?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軸方向上角速度測(cè)量值。以X軸方向?yàn)槔?，輸入?yún)?shù)為偏差角速度，PID輸出控制量為：

(14)

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

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

(15)

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

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

(16)

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

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

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

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

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

3.2 PID參數(shù)整定

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

表1 PID參數(shù)

3.3 調(diào)試分析

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

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

4 結(jié)束語

圖5 加速度-時(shí)間曲線

圖6 角速度-時(shí)間曲線

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

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

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

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

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

[4] 江 斌. 小型無人低空四旋翼飛行器綜合設(shè)計(jì)[D].杭州：浙江大學(xué)，2013.

[5] 秦永元. 慣性導(dǎo)航[M].北京：科學(xué)出版社，2006.

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

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

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

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

[10] 龐慶霈，李家文，黃文浩. 四旋翼飛行器設(shè)計(jì)與平穩(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-)，男，安徽蕪湖人，工學(xué)碩士，主要從事嵌入式控制系統(tǒng)方向的研究。

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

1671-4598(2016)06-0077-03

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

TH873.7

A