基于嵌入式系統(tǒng)的四旋翼飛行控制器設(shè)計

En Un Chye, A V Levenets

(太平洋國立大學(xué) 自動化與系統(tǒng)工程系，哈巴羅夫斯克 哈巴羅夫斯克 999081)

0 引言

uC/OS-Ⅲ是由Micro開發(fā)的一個可升級、可固化的嵌入式實時性內(nèi)核，該內(nèi)核在II的基礎(chǔ)上又升級了一步，它對任務(wù)的個數(shù)支持無限。并且它是一個基于優(yōu)先級的、具有可剝奪性的實時性內(nèi)核，在最大程度上保證了系統(tǒng)任務(wù)的實時性。這個小巧的系統(tǒng)支持現(xiàn)代實時內(nèi)核所期望的大部分功能，例如資源管理、同步、任務(wù)通信等。將四旋翼的控制系統(tǒng)搭載上uC/OS-Ⅲ，為軟件平臺的設(shè)計和擴展提供了極大的便利。為后續(xù)的任務(wù)擴展創(chuàng)造了基礎(chǔ)框架，開發(fā)人員可以像開發(fā)Windows程序一樣專注于API的實現(xiàn)，而不用設(shè)計底層的編寫。

1 總體設(shè)計方案

本設(shè)計主要包括硬件和軟件兩部分。其中，硬件設(shè)計中包含機械結(jié)構(gòu)的設(shè)計。四旋翼的結(jié)構(gòu)主要分為兩種,通常根據(jù)姿態(tài)傳感器放置位置分為X型和十字型。X型的X軸和Y軸與陀螺儀的X軸和Y軸構(gòu)成45度夾角，而十字形的X軸Y軸與陀螺儀的X軸Y軸重合。X型結(jié)構(gòu)的飛行器自由度多，控制難；十字形則自由度相對較少，但控制簡單。如圖1所示為本次設(shè)計中采用的機械結(jié)構(gòu)。

硬件電路的搭建主要采用了以STM32F103單片機為核心結(jié)合慣性測量傳感器和其他輔助傳感器作為此次設(shè)計的硬件平臺。硬件平臺主要負(fù)責(zé)信息的采集和為四旋翼的控制系統(tǒng)包括uC/OS-Ⅲ提供一個運行的平臺。如圖2所示為硬件的方案設(shè)計。

圖1 機械結(jié)構(gòu)實物圖

圖2 整體系統(tǒng)框架

在此硬件平臺的基礎(chǔ)上，結(jié)合搭建底層操作系統(tǒng)，將飛行器的控制算法和擴展應(yīng)用加入其中，即完成了本次的軟件結(jié)構(gòu)的設(shè)計。

2 四旋翼控制器的設(shè)計

2.1 四旋翼建模

為了簡化模型的建立和計算的難度，在建立模型前提出如下假設(shè)：

(1)模型的中心與重心重合，模型的質(zhì)量是均勻分布的，且結(jié)構(gòu)是對對稱的；

(2)由于角度之間的變換具有非常強的耦合性，會引入很多未知的參數(shù)，因此，假設(shè)模型處于懸停狀態(tài)，且角度的變化非常小，彼此沒有耦合；

(3)模型的被控量目前只能控制飛行器的高度和歐拉角，因此不考慮模型的水平方向位移；

(4)兩個坐標(biāo)系，靜止的以東北方向為參考的靜止坐標(biāo)系和固定于剛體的坐標(biāo)系；

(5)飛行器的升力與轉(zhuǎn)速的平方成正比。

基于以上假設(shè)，設(shè)模型的質(zhì)量為m(Kg)，重力加速度為g(N.m^2)，高度為H(m)，螺旋槳轉(zhuǎn)速為ω=[ω1,ω2,ω3,ω4]。

圖3為當(dāng)前飛行器姿態(tài)，a,b,c為剛體坐標(biāo)系，X,Y,Z為大地參考系，θ,φ為當(dāng)前的橫滾角和俯仰角。假設(shè)此時某一槳葉位置如圖4所示。

圖3 當(dāng)前姿態(tài)圖

圖4 槳葉位置圖

螺旋槳產(chǎn)生的力的方向為剛體坐標(biāo)系a軸負(fù)方向，假設(shè)此時產(chǎn)生的力為F1，則F1在以大地為參考系的Z軸方向的分力為：Fz=F1*cosθ*cosφ

(1)

同理可得，其他螺旋槳在Z軸方向的分力，計算Z軸方向的分力之和即為飛行器的升力。各個螺旋槳產(chǎn)生的力與螺旋槳的轉(zhuǎn)速的平方成正比，則可得Z軸的分力之和即升力U為：

U1=k1(w1＾2+w2＾2+w3＾2+w4＾2)*cosθ*cosφ

(2)

K1為常數(shù)。

設(shè)飛行器在橫滾方向、俯仰方向和偏航方向所受的力矩為U2,U3,U4，則：

U2=k2(-w2＾2+w4＾2)

(3)

U3=k2(-w1＾2+w3＾2)

(4)

U4=k1(-w1＾2-w3＾2+w2＾2+w4＾2)

(5)

其中，w2，w4為左右槳，w1，w3為前后槳，K2為常數(shù)。

2.2 模型仿真

結(jié)合上述控制算法和數(shù)學(xué)模型，使用Matlab進(jìn)行驗證仿真。本次仿真采用m文件形式對各個模塊進(jìn)行源代碼編寫。

2.2.1 PID控制器

以下為PID控制器的m文件，輸入?yún)?shù)為目標(biāo)高度，實際高度，PID參數(shù)等。PID控制器采用上述提到的微分先行的控制方式。

Pid_Out=Pid_Controler(Ref,FeedBack,P,I,D,Integration,FeedBack_Last)

Error=0;

error=Ref-FeedBack;

Integration=Integration+error*I;

Output=P*error+(FeedBack_Last-FeedBack)*D+Integration;

%Output=Output+error*I;

Pid_Out=[Output,Integration];

2.2.2 直流電機模型

直流電機模型采用通用的模型，模型參數(shù)由電機資料獲取。普通直流電機的模型如式(6)，ω為角速度，v為輸入電壓，其他為常數(shù)。

(6)

2.2.3 高度閉環(huán)

根據(jù)上述幾個模型，即可做出高度的閉環(huán)模型，如下m文件源代碼，圖5為閉環(huán)響應(yīng)曲線。

function Feed_Back_Control()

Height_Ref=10;%目標(biāo)高度

Height_Back=0;%測量高度

Heignt_Integration=0;

w1=0;

w2=0;

w3=0;

w4=0;

theta=0;

fai=0;

H_FeedBack_Last=0;

for i=1:100

B=PID_Contorler(Height_Ref,Height_Back,0.04,0.001,0.056,Height_Integration,H_FeedBack_Last);

H_FeedBack_Last=Height_Back;

Heignt_Integration=B(2);

S_Acc=Motor(B(1),B(1),B(1),B(1),w1,w2,w3,w4);

w1=S_Acc(1)+w1;

w2=S_Acc(2)+w2;

w3=S_Acc(3)+w3;

w4=S_Acc(4)+w4;

H_Acc=Height(w1,w2,w3,w4,theta,fai);

Height_Back=Height_Back+H_Acc;

H(i)=Height_Back;

S(i)=w1;%Heignt_Integration;

end

plot(H);

end

圖5 高度閉環(huán)響應(yīng)曲線

2.2.4 歐拉角模型

如下為歐拉角模型m文件，輸入為轉(zhuǎn)速，輸出為角加速度。通過將輸出的角速度積分后得到當(dāng)前的角度。

function angle=Eula(w1,w2,w3,w4)

U2=(-(w2^2)+w4^2)*0.5;

U3=(-(w1^2)+w3^2)*0.5;

U4=w2^2+w4^2-(w1^2)-(w3^2);

Ixx=1;

Iyy=1;

Izz=1.5;

angle(1)=U2/Ixx;

angle(2)=U3/Iyy;

angle(3)=U4/Izz;

加入PID控制器后得到閉環(huán)的姿態(tài)響應(yīng)，如圖6所示。

圖6 歐拉角閉環(huán)響應(yīng)

如圖為給定Yaw=0.15，Pitch=0.1，Roll=0.06時的閉環(huán)響應(yīng)曲線。響應(yīng)的穩(wěn)定性和快速性都能滿足要求。

3 使用uC/OS-Ⅲ進(jìn)行多任務(wù)管理

uC/OS是一個可剝奪性的多任務(wù)管理內(nèi)核?？蓜儕Z性代表高優(yōu)先級的任務(wù)可以搶占或優(yōu)先使用CPU，這極大保證了高優(yōu)先級任務(wù)的實時性。多任務(wù)管理代表的是在多個任務(wù)間調(diào)度和切換CPU的使用權(quán)的方法，合理的分配CPU的使用權(quán)，利用uc/OS用戶可以輕松地管理這些應(yīng)用程序。

每一個任務(wù)都是一個獨立的C函數(shù)，在任務(wù)中不允許返回。任務(wù)通常有兩種存在形式：一是運行至完成型，這種形式的任務(wù)在程序運行結(jié)束時通過調(diào)用OSTaskDel()函數(shù)來將自己刪除以結(jié)束自己的生命周期，操作系統(tǒng)不會再調(diào)用此函數(shù)；二是無限循環(huán)型，這種形式的任務(wù)以無限循環(huán)的形式存在，所以它的生命周期將從始至終一直存在，并且受操作系統(tǒng)的調(diào)配。另外，每一個無限循環(huán)的任務(wù)(系統(tǒng)創(chuàng)建的空閑任務(wù)例外)都必須有一個激勵信號，該激勵信號可以使得該任務(wù)就緒，也就是任務(wù)需要等待一個激勵信號到來后才開始執(zhí)行。這個激勵信號可以是一個延時或外部輸入等信號。因為如果一個任務(wù)沒有這種激勵源，那將會導(dǎo)致其成為一個真正的無限循環(huán)，一直處于就緒狀態(tài)，會導(dǎo)致比它優(yōu)先級低的任務(wù)無法執(zhí)行。

圖7 任務(wù)狀態(tài)框圖

圖8 程序流程圖

圖7為典型的任務(wù)狀態(tài)圖。任務(wù)在系統(tǒng)中以不同狀態(tài)存在，由工程人員編寫好后，當(dāng)休眠狀態(tài)的任務(wù)通過調(diào)用OSTaskCreate()函數(shù)時，任務(wù)就可以接受系統(tǒng)的管理。當(dāng)任務(wù)的激勵信號到來后，任務(wù)準(zhǔn)備運行即進(jìn)入就緒狀態(tài)，等待系統(tǒng)分配CPU。用戶也可以通過調(diào)用OSTaskDel()將任務(wù)從系統(tǒng)中刪除，則該任務(wù)將不再受系統(tǒng)的調(diào)配，但物理內(nèi)存仍然存在。當(dāng)一系列的任務(wù)就緒后，操作系統(tǒng)會根據(jù)任務(wù)的優(yōu)先級來決定哪一個獲得CPU的使用權(quán)，獲得CPU 使用權(quán)的任務(wù)即進(jìn)入運行狀態(tài)。該任務(wù)在運行過程中，如果有更高優(yōu)先級的任務(wù)就緒，則會轉(zhuǎn)而執(zhí)行高優(yōu)先級的任務(wù)。這就是內(nèi)核的可剝奪性，即使運行中的任務(wù)也要無條件交出CPU的使用權(quán)，將自己掛起，當(dāng)高優(yōu)先級的任務(wù)執(zhí)行完后才能繼續(xù)執(zhí)行。這其實非常類似CPU的中斷過程，所以也可以稱其為軟中斷。而一個真正的中斷到來之后，系統(tǒng)會自動將當(dāng)前任務(wù)掛起，去執(zhí)行中斷服務(wù)程序。執(zhí)行中斷服務(wù)函數(shù)的過程中可能會接受的中斷，當(dāng)所有中斷服務(wù)函數(shù)執(zhí)行完后，系統(tǒng)會繼續(xù)執(zhí)行之前被中斷的任務(wù)，或者可以進(jìn)行新一輪的任務(wù)調(diào)度。任務(wù)執(zhí)行結(jié)束后，需要等待下個激勵信號的到來，任務(wù)轉(zhuǎn)而進(jìn)入等待狀態(tài)。處于等待狀態(tài)的任務(wù)一旦收到激勵信號就會進(jìn)入就緒狀態(tài)，等待系統(tǒng)調(diào)遣。這些狀態(tài)的轉(zhuǎn)移就構(gòu)成了系統(tǒng)的任務(wù)管理的過程。

4 軟件設(shè)計

軟件算法的實現(xiàn)主要基于意法半導(dǎo)體的STM32F103，STM32是基于CortexM3內(nèi)核的高性能低成本低功耗的嵌入式微控制器，STM32F103的主頻可達(dá)72MHZ，片上具有豐富的外設(shè)，有多達(dá)256K的Flash和64K的SRAM，本次設(shè)計主要以STM32為硬件平臺，進(jìn)行操作系統(tǒng)的搭建移植和算法的實現(xiàn)。程序流程圖如圖8所示。

如表1所示為具體的任務(wù)函數(shù)。

表1 系統(tǒng)任務(wù)分配表

5 結(jié)語

本系統(tǒng)設(shè)計比較復(fù)雜，由于篇幅所限不能將所有內(nèi)容敘述詳盡，尤其是算法實現(xiàn)部分。uC/OS是一個可剝奪性基于優(yōu)先級的內(nèi)核，其任務(wù)調(diào)度就是系統(tǒng)調(diào)用應(yīng)用函數(shù)的過程。雖然本文基本達(dá)成了設(shè)計目標(biāo)，但也有很多可以進(jìn)行優(yōu)化和改進(jìn)的地方。

[1] Jistsukawa N,Ogawa T,Kanada H,et al.Time-frequency analysis of impact sound of composite materials[J].Sice Sice Conference,2003,37(8):1076-1079.

[2] D Salomon.Data compression[M].New York:Springer,2004.

[3] I V Bogachev,A V Levenets.Telemetry data compression algorithms[C].Theory and practice of a modern science:materials of XVIII International scientifically-practical conference,Moscow,Russia,Institut strategicheskikh issledovanijj:2015:53-60.

[4] D Vatolin,A Ratushnjak,M Smirnov,et al.Data compression methods.Structure of the archivers,image and video compression[M].Moscow:Dialog-Mifi,2003.

[5] T Takaoka.Efficient algorithms for the maximum subarray problem by distance matrix multiplication[J].Electronic Notes in Theoretical Computer Science,2002(61):191-200.