基于嵌入式系统的四旋翼飞行控制器设计

En Un Chye, A V Levenets

(太平洋国立大学 自动化与系统工程系，哈巴罗夫斯克 哈巴罗夫斯克 999081)

0 引言

uC/OS-Ⅲ是由Micro开发的一个可升级、可固化的嵌入式实时性内核，该内核在II的基础上又升级了一步，它对任务的个数支持无限。并且它是一个基于优先级的、具有可剥夺性的实时性内核，在最大程度上保证了系统任务的实时性。这个小巧的系统支持现代实时内核所期望的大部分功能，例如资源管理、同步、任务通信等。将四旋翼的控制系统搭载上uC/OS-Ⅲ，为软件平台的设计和扩展提供了极大的便利。为后续的任务扩展创造了基础框架，开发人员可以像开发Windows程序一样专注于API的实现，而不用设计底层的编写。

1 总体设计方案

本设计主要包括硬件和软件两部分。其中，硬件设计中包含机械结构的设计。四旋翼的结构主要分为两种,通常根据姿态传感器放置位置分为X型和十字型。X型的X轴和Y轴与陀螺仪的X轴和Y轴构成45度夹角，而十字形的X轴Y轴与陀螺仪的X轴Y轴重合。X型结构的飞行器自由度多，控制难；十字形则自由度相对较少，但控制简单。如图1所示为本次设计中采用的机械结构。

硬件电路的搭建主要采用了以STM32F103单片机为核心结合惯性测量传感器和其他辅助传感器作为此次设计的硬件平台。硬件平台主要负责信息的采集和为四旋翼的控制系统包括uC/OS-Ⅲ提供一个运行的平台。如图2所示为硬件的方案设计。

图1 机械结构实物图

图2 整体系统框架

在此硬件平台的基础上，结合搭建底层操作系统，将飞行器的控制算法和扩展应用加入其中，即完成了本次的软件结构的设计。

2 四旋翼控制器的设计

2.1 四旋翼建模

为了简化模型的建立和计算的难度，在建立模型前提出如下假设：

(1)模型的中心与重心重合，模型的质量是均匀分布的，且结构是对对称的；

(2)由于角度之间的变换具有非常强的耦合性，会引入很多未知的参数，因此，假设模型处于悬停状态，且角度的变化非常小，彼此没有耦合；

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

(4)两个坐标系，静止的以东北方向为参考的静止坐标系和固定于刚体的坐标系；

(5)飞行器的升力与转速的平方成正比。

基于以上假设，设模型的质量为m(Kg)，重力加速度为g(N.m^2)，高度为H(m)，螺旋桨转速为ω=[ω1,ω2,ω3,ω4]。

图3为当前飞行器姿态，a,b,c为刚体坐标系，X,Y,Z为大地参考系，θ,φ为当前的横滚角和俯仰角。假设此时某一桨叶位置如图4所示。

图3 当前姿态图

图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为常数。

设飞行器在横滚方向、俯仰方向和偏航方向所受的力矩为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为常数。

2.2 模型仿真

结合上述控制算法和数学模型，使用Matlab进行验证仿真。本次仿真采用m文件形式对各个模块进行源代码编写。

2.2.1 PID控制器

以下为PID控制器的m文件，输入参数为目标高度，实际高度，PID参数等。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 直流电机模型

直流电机模型采用通用的模型，模型参数由电机资料获取。普通直流电机的模型如式(6)，ω为角速度，v为输入电压，其他为常数。

(6)

2.2.3 高度闭环

根据上述几个模型，即可做出高度的闭环模型，如下m文件源代码，图5为闭环响应曲线。

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 高度闭环响应曲线

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控制器后得到闭环的姿态响应，如图6所示。

图6 欧拉角闭环响应

如图为给定Yaw=0.15，Pitch=0.1，Roll=0.06时的闭环响应曲线。响应的稳定性和快速性都能满足要求。

3 使用uC/OS-Ⅲ进行多任务管理

uC/OS是一个可剥夺性的多任务管理内核。可剥夺性代表高优先级的任务可以抢占或优先使用CPU，这极大保证了高优先级任务的实时性。多任务管理代表的是在多个任务间调度和切换CPU的使用权的方法，合理的分配CPU的使用权，利用uc/OS用户可以轻松地管理这些应用程序。

每一个任务都是一个独立的C函数，在任务中不允许返回。任务通常有两种存在形式：一是运行至完成型，这种形式的任务在程序运行结束时通过调用OSTaskDel()函数来将自己删除以结束自己的生命周期，操作系统不会再调用此函数；二是无限循环型，这种形式的任务以无限循环的形式存在，所以它的生命周期将从始至终一直存在，并且受操作系统的调配。另外，每一个无限循环的任务(系统创建的空闲任务例外)都必须有一个激励信号，该激励信号可以使得该任务就绪，也就是任务需要等待一个激励信号到来后才开始执行。这个激励信号可以是一个延时或外部输入等信号。因为如果一个任务没有这种激励源，那将会导致其成为一个真正的无限循环，一直处于就绪状态，会导致比它优先级低的任务无法执行。

图7 任务状态框图

图8 程序流程图

图7为典型的任务状态图。任务在系统中以不同状态存在，由工程人员编写好后，当休眠状态的任务通过调用OSTaskCreate()函数时，任务就可以接受系统的管理。当任务的激励信号到来后，任务准备运行即进入就绪状态，等待系统分配CPU。用户也可以通过调用OSTaskDel()将任务从系统中删除，则该任务将不再受系统的调配，但物理内存仍然存在。当一系列的任务就绪后，操作系统会根据任务的优先级来决定哪一个获得CPU的使用权，获得CPU 使用权的任务即进入运行状态。该任务在运行过程中，如果有更高优先级的任务就绪，则会转而执行高优先级的任务。这就是内核的可剥夺性，即使运行中的任务也要无条件交出CPU的使用权，将自己挂起，当高优先级的任务执行完后才能继续执行。这其实非常类似CPU的中断过程，所以也可以称其为软中断。而一个真正的中断到来之后，系统会自动将当前任务挂起，去执行中断服务程序。执行中断服务函数的过程中可能会接受的中断，当所有中断服务函数执行完后，系统会继续执行之前被中断的任务，或者可以进行新一轮的任务调度。任务执行结束后，需要等待下个激励信号的到来，任务转而进入等待状态。处于等待状态的任务一旦收到激励信号就会进入就绪状态，等待系统调遣。这些状态的转移就构成了系统的任务管理的过程。

4 软件设计

软件算法的实现主要基于意法半导体的STM32F103，STM32是基于CortexM3内核的高性能低成本低功耗的嵌入式微控制器，STM32F103的主频可达72MHZ，片上具有丰富的外设，有多达256K的Flash和64K的SRAM，本次设计主要以STM32为硬件平台，进行操作系统的搭建移植和算法的实现。程序流程图如图8所示。

如表1所示为具体的任务函数。

表1 系统任务分配表

5 结语

本系统设计比较复杂，由于篇幅所限不能将所有内容叙述详尽，尤其是算法实现部分。uC/OS是一个可剥夺性基于优先级的内核，其任务调度就是系统调用应用函数的过程。虽然本文基本达成了设计目标，但也有很多可以进行优化和改进的地方。

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