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

En Un Chye, A V Levenets

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

0 引言

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

1 總體設計方案

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

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

圖1 機械結構實物圖

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

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

2 四旋翼控制器的設計

2.1 四旋翼建模

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

(1)模型的中心與重心重合，模型的質量是均勻分布的，且結構是對對稱的；

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

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

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

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

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

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

圖3 當前姿態(tài)圖

圖4 槳葉位置圖

螺旋槳產生的力的方向為剛體坐標系a軸負方向，假設此時產生的力為F1，則F1在以大地為參考系的Z軸方向的分力為：Fz=F1*cosθ*cosφ

(1)

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

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

(2)

K1為常數(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 模型仿真

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

2.2.1 PID控制器

以下為PID控制器的m文件，輸入參數(shù)為目標高度，實際高度，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)響應曲線。

function Feed_Back_Control()

Height_Ref=10;%目標高度

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)響應曲線

2.2.4 歐拉角模型

如下為歐拉角模型m文件，輸入為轉速，輸出為角加速度。通過將輸出的角速度積分后得到當前的角度。

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)響應，如圖6所示。

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

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

3 使用uC/OS-Ⅲ進行多任務管理

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

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

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

圖8 程序流程圖

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

4 軟件設計

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

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

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

5 結語

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

[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.