基于ARM的四旋翼姿态控制器设计

吴承建，沈 捷，陈乾坤

(南京工业大学 电气工程与控制科学学院，南京 211816)



基于ARM的四旋翼姿态控制器设计

吴承建，沈 捷，陈乾坤

(南京工业大学 电气工程与控制科学学院，南京 211816)

四旋翼姿态控制器采用集成了加速度计和陀螺仪的惯性测量单元，实时采集姿态数据,传输给Cortex-M4内核的处理芯片，利用四元数姿态解算方法，对加速度和角速度数据融合解算处理；采用位置式PID控制算法，控制4个无刷电机的转速，实现控制四旋翼飞行器的飞行姿态；建立万向云台调试系统，通过实践调试验证该控制器能实现控制四旋翼姿态的稳定性；稳定飞行时，姿态角的平均振荡范围为5°。

惯性测量单元 ；PID；四元数；数据融合；姿态角；万向云台调试

0 引言

近年来，随着芯片、材料等技术的进步，加快了旋翼飞行器发展速度，在诸如地震灾情探查、电影电视节目录制等方面发挥着重要的作用。四旋翼飞行器有着相对于传统直升机特有的优势，包括便捷性、易操作和灵活性等。考虑到四旋翼飞行器是具有6个输入量、4个输出量的非线性欠驱动系统[1]，这使得飞行控制器的设计相对比较困难。本文以四旋翼飞行器的姿态稳定控制为目标，设计了主控制器硬件，在姿态角与加速度数据的解算中运用了卡尔曼滤波器，搭建了基于方向节的四旋翼实物调试系统。通过不断调整给定参数，重复试验，直至四旋翼能够保持姿态稳定。

1 控制器硬件系统

本文设计的四旋翼控制器采用32位ARM微控制器，运行最高时钟频率为168 MHz，主要处理多传感器实时采集到的飞行器姿态数据。惯性测量单元集成了陀螺仪和加速度计，利用IIC串行总线采集并输出数据到主控芯片，数据经过处理后输出4路PWM波控制电机。四旋翼飞行器系统结构图如图1所示。

图1 四旋翼控制器结构图

1.1 微控制器芯片

飞行控制器中的核心控制芯片采用ST意法半导体公司基于Cortex-M4内核架构开发的STM32F407VGT6，该芯片具有通信接口资源丰富、功耗较低、成本低和外设功能较全等特点，并且支持FPU浮点运算，提高了数据运算精度[4]。芯片将惯性测量单元采集到的加速度与角速度数据解算成姿态角，经过增量式PID控制算法处理，产生相应占空比的PWM波。惯性测量单元从采集到传输给主控制芯片耗时约3 ms，因此对微控制器响应速度有一定的要求。从STM32F407VGT6的特点来看，丰富的接口资源能够充分的满足上位机、串口、传感器等对接口的要求。

1.2 惯性测量单元

飞行控制器的惯性测量单元实时测量四旋翼飞行器的姿态数据[2]，本控制器中采用的型号为MPU6050，该单元内部集成了一个三轴MEMS加速度计和一个三轴MEMS陀螺仪，通过IIC接口与主控芯片连接。由于数据打包传输，微控制芯片在接收到数据后必须先进行离散化处理，分化为对应的X、Y、Z轴数据。MPU6050对陀螺仪和加速度计分别使用了3个16位的ADC，将测量的模拟量转换为可输出的数字量；为了精确跟踪快速和慢速的运动，规定传感器的测量范围可控。初始时刻预设四旋翼飞行器的地面参考坐标系与惯性测量单元的坐标系重合，设滚转、俯仰和偏航姿态角[3]为(α,β,θ)。

陀螺仪测量3个轴上的角速度，通过利用四元数算法将角速度解算为姿态角度。

1.3 无线通信

四旋翼控制器除了与遥控器通信，在调试过程中需要与上位机通信，在设计过程中综合考虑成本、效率、传输距离等因素，选择NRF24L01作为无线通信模块。该模块集成了晶振、数字接口、收发器等功能。在计算机端与飞行控制器各连接NRF24L01模块，上位机既可以向四旋翼飞行器发送指令，也可以将四旋翼飞行器的飞行参数上传至上位机，实时监测飞行姿态。飞行器需要传输给上位机的参数包括PID参数、加速度与角速度和姿态角等。

2 控制器软件

2.1 四元数算法

四元数是一个四维的复合数，它描述了刚体的定点转动的过程，包含一个旋转轴和一个旋转角度信息。四旋翼的位姿有四元数和欧拉角两种表示方式，本文利用四元数算法，并结合欧拉角坐标转换矩阵，通过对角速度进行四元数解算，得到机体旋转各个轴上欧拉角分量。

四元数[4]可表示为：

(1)

取地面参考坐标系为N系，机体坐标系为B系，初始时两个坐标系重合。四旋翼一次运动过程可等效为刚体定点旋转过程，如图2所示。四旋翼初始位置如A点所示，转动φ角度后位置如A′所示。

图2 机体旋转示意图

(2)

四元数Q对任意一个向量的作用就是绕着旋转轴旋转φ角度。式(2)可写为：

(3)

可由上式得N系与B系的坐标转换矩阵D为：

D=

(4)

若欧拉角α、β、θ表示四旋翼姿态角度，可得两坐标系间的欧拉旋转矩阵[5]为：

(5)

由于欧拉角转换矩阵等效于四元数转换矩阵，将两个转换矩阵做等式，求解等式，可得旋转的欧拉角为：

(6)

对(1)式两边进行求导，最终可得：

(7)

(8)

根据龙格-库塔法[6],有：

(9)

其中：I∈R4×4为单位矩阵。

(10)

2.2 PID 姿态控制算法

PID控制策略是一种十分经典的控制方法，在各种控制工程系统里应用十分普遍，而且更适合实践应用。本系统采用双闭环串级位置式PID控制策略，在角速度控制器处理数据之前，对角速度数据进行滑动平均滤波。其结构如图3所示。

图3 PID控制结构图

PID控制使用位置式PID方法[7]，设姿态角初始值均为0。设X、Y、Z轴3个方向的角度差值分别为Δα、Δβ、Δθ，其中αdX轴方向上角度角的目标值，α为X轴方向上角度测量值，则：

(11)

以X轴方向为例，输入参数为偏差角，PID输出控制量为：

(12)

式中，Kp1_roll、Ki1_roll和Kd1_roll分别为角度闭环的比例、微分和积分控制系数。同理，Y轴和Z轴的PID输出控制量记为u1_pit、u1_yaw。

在角速度控制闭环中，X、Y和Z轴方向上的角速度偏差表示为：

(13)

式中,ωd_roll为X轴方向上角速度期望值，ωroll为X轴方向上角速度测量值。以X轴方向为例，输入参数为偏差角速度，PID输出控制量为：

(14)

式中，Kp2_roll、Ki2_roll和Kd2_roll分别为角速度闭环的比例、微分和积分控制系数。同理Y轴和Z轴的PID输出控制量为u2_pit、u2_yaw。

由(13)式和(14)式求和，可得X轴方向的串级PID输出控制量：

(15)

同理Y轴和Z轴的串级PID输出控制量为upit、uyaw。

驱动4个无刷电机的控制量分别记为M1、M2、M3、M4，记T为遥控器油门控制量，则：

(16)

3 姿态调试

3.1 调试系统

飞行控制器与四旋翼机架、电调、电机、桨叶等器件组装完成[8]，在四旋翼室外试飞之前需要对其进行调试[9]，判定四旋翼的滚转、俯仰和偏航姿态的稳定性。考虑到四旋翼桨叶高速旋转具有一定的危险性，因此利用万向云台和四旋翼飞行器组成姿态调试系统，如图4所示。

图4 姿态调试平台

使用万向云台调试具有很强优势，一方面只对四旋翼机体施加向下的拉力，不影响飞行姿态，另一方面可以实现小范围调试同时保证安全。

3.2 PID参数整定

调试过程中，针对四旋翼的姿态需要不断调整PID参数，Kp1— roll,Ki1—roll,Kd1—roll分别表示角速度闭环的滚转姿态的PID参数[10]。

PID参数整定从P值开始，P值是PID中最重要的部分，直接决定飞行器的飞行结果。首先取I和D值为零，改变滚转和俯仰的P值直到飞行器变得难以随意倾斜；根据偏航姿态，重复改变偏航P值，直至四旋翼对偏航具有较大的阻尼力。其次，保证P值不变和D值为零，改变I值，直至机体的修正力度较大且机体振荡不变。最后，保持P和I值，改变滚转和俯仰的D值，使用遥控器发送姿态动作指令，直至四旋翼返回初始位置的动作能够快速响应。根据上述调试方法，重复调试实验，当PID的参数取表1中的值时，四旋翼的姿态可以保持平稳。

表1 PID参数

3.3 调试分析

取上述表1中的参数，数据采集周期为40ms，NRF24L01无线模块将采集的加速度、角速度和姿态角等数据上传至上位机并保存，如图5～7所示。

图5中，X、Y轴加速度从初始值逐渐趋于零，图6中3个轴上的角速度减小并趋于相同，图7中姿态角大小在小幅的范围内振荡，从3个变量的变化趋势中可以推断出四旋翼的随着时间逐渐趋于稳定。

4 结束语

图5 加速度-时间曲线

图6 角速度-时间曲线

图7 姿态角-时间曲线

必然会导致其比采用多测量单元控制器的精度低，为了提高精确度，在数据处理层面使用结合卡尔曼滤波算法的PID控制策略，使得控制器误差保持在可控范围，能够满足稳定性要求。通过建立四旋翼飞行器的调试系统，在排除室外环境的干扰下对PID参数进行整定，调试结果表明在姿态数据处理过程中，采用卡尔曼滤波器的控制器能够控制四旋翼的稳定飞行，说明该控制器设计是合理的。从结果可知，在万向云台上，四旋翼趋于稳定，机体存在振荡，其振荡姿态角度在5度之内，满足稳定要求。

[1] 魏丽文．四旋翼控制系统设计[D].哈尔滨：哈尔滨工业大学，2010.

[2] 翼 明，等.基于Cortex-M4的四旋翼飞行控制系统设计[J].计算机测量与控制,2013，21(7)：1816-1821.

[3] 刘 乾，孙志锋. 基于ARM的四旋翼无人飞行器控制系统[J].机电工程，2011,28(10)：1237-1240.

[4] 江 斌. 小型无人低空四旋翼飞行器综合设计[D].杭州：浙江大学，2013.

[5] 秦永元. 惯性导航[M].北京：科学出版社，2006.

[6] 袁信等. 捷联式惯性导航原理[M].北京：国防工业出版社，2003.

[7] 刘焕晔.小型四旋翼飞行器飞行控制系统研究与设计 [D].上海：上海交通大学，2009.

[8] 王 帅，魏 国.卡尔曼滤波在四旋翼飞行器姿态测量中的应用[J].兵工自动化， 2011, 30(1): 73-74.

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

[10] 庞庆霈，李家文，黄文浩. 四旋翼飞行器设计与平稳控制仿真研究[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-)，男，安徽芜湖人，工学硕士，主要从事嵌入式控制系统方向的研究。

沈 捷(1976-)，男 ，安徽铜陵人，副教授，硕士生导师，主要从事机器人、嵌入式系统方向的研究。

1671-4598(2016)06-0077-03

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

TH873.7

A