1、四轴飞行器遥感平台的实现方案摘 要四轴飞行器作为低空低成本的遥感平台，在各个领域应用广泛。相比其他类型的飞行器，四轴飞行器硬件结构简单紧凑，而软件复杂。本文介绍四轴飞行器的一个实现方案，重点讲软件算法，包括加速度计校正、姿态计算和姿态控制三部分。校正加速度计采用最小二乘法。计算姿态采用姿态插值法、梯度下降法或互补滤波法，需要对比这三种方法然后选出一种来应用。控制姿态采用欧拉角控制或四元数控制。最后比较各种方法的效果，并附上C 语言的算法实现代码。关键词：四轴飞行器；姿态；控制IIOne Method to Make a QuadcopterXUAN Yong-junAbstractQuadco
2、pter is a low-cost low-altitude remote sensing platforms, which widely used in variousfields. Compared to other types of aircraft, the quadcopter has simpler hardware, result in morecomplex software. This paper describes an implementation of the quadcopter, focusing onsoftware algorithms, including
3、the calibration of accelerometer, estimation of attitude, and controlof attitude. It uses the Least Squares Method to calibration the accelerometer. It uses AttitudeInterpolation Method, Gradient Descent Method or Complementary Filter Method to estimationthe attitude. It uses Euler angles or quatern
4、ion to control the attitude. Finally, there arecomparisons of the methods. The C-language implementation of the methods is appended.Key words: quadcopter; attitude; controlIII目 录1. 引言. 12. 飞行器的构成. 12.1. 硬件构成. 12.1.1. 机械构成. 12.1.2. 电气构成. 22.2. 软件构成. 32.2.1. 上位机. 32.2.2. 下位机. 33. 飞行原理. 43.1. 坐标系统. 43.
5、2. 姿态的表示和运算. 43.3. 动力学原理. 54. 姿态测量. 64.1. 传感器校正. 64.1.1. 陀螺仪. 64.1.2. 加速度计和电子罗盘. 64.2. 数据融合. 94.2.1. 概述. 94.2.2. 姿态插值法. 94.2.3. 梯度下降法. 114.2.4. 互补滤波法. 135. 姿态控制. 145.1. 欧拉角控制. 145.2. 四元数控制. 156. 算法效果. 156.1. 加速度计校正. 156.2. 姿态计算. 177. 结论及存在的问题. 19参考文献. 20致 谢. 21附 录. 2211. 引言四轴飞行器最开始是由军方研发的一种新式飞行器1。随着
6、MEMS 传感器、单片机、电机和电池技术的发展和普及，四轴飞行器成为航模界的新锐力量。到今天，四轴飞行器已经应用到各个领域，如军事打击、公安追捕、灾害搜救、农林业调查、输电线巡查、广告宣传航拍、航模玩具等，已经成为重要的遥感平台2 3 4 5 6 7。以农业调查为例，传统的调查方式为到现场抽样调查或用航空航天遥感。抽样的方式工作量大，而且准确性受主观因素影响；而遥感的方式可以大范围同时调查，时效性和准确性都有保证，但只能得到大型作物的宏观的指标，而且成本很高。不连续的地块、小种作物等很难用上遥感调查。因此，低空低成本遥感技术显得相当重要，而四轴飞行器正符合低空低成本遥感平台的要求。目前应用广泛
7、的飞行器有：固定翼飞行器和单轴的直升机。与固定翼飞行器相比，四轴飞行器机动性好，动作灵活，可以垂直起飞降落和悬停，缺点是续航时间短得多、飞行速度不快；而与单轴直升机比，四轴飞行器的机械简单，无需尾桨抵消反力矩，成本低8。本文就小型电动四轴飞行器，介绍四轴飞行器的一种实现方案，重点讲解四轴飞行器的原理和用到的算法，并对几种姿态算法进行比较。2. 飞行器的构成四轴飞行器的实现可以分为硬件和软件两部分。比起其他类型的飞行器，四轴飞行器的硬件比较简单，而把系统的复杂性转移到软件上，所以本文的主要内容是软件的实现，特别是算法、公式的推导。2.1. 硬件构成飞行器由机架、电机、螺旋桨和控制电路构成。2.1
8、.1. 机械构成机架呈十字状，是固定其他部件的平台，本项目采用的是尼龙材料的机架。电机采用无刷直流电机，固定在机架的四个端点上，而螺旋桨固定在电机转子上，迎风面垂直向下。螺旋桨按旋转方向分正桨和反桨，从迎风面看逆时针转的为正桨，四个桨的中心连成的正方形，正桨反桨交错安装。整体如图2-1。2图2-1 四轴飞行器整机2.1.2. 电气构成电气部分包括：控制电路板、电子调速器、电池，和一些外接的通讯、传感器模块。控制电路板是电气部分的核心，上面包含MCU、陀螺仪、加速度计、电子罗盘、气压计等芯片，负责计算姿态、处理通信命令和输出控制信号到电子调速器。电子调速器简称电调，用于控制无刷直流电机。电气连接
9、如图2-2 所示。图2-2 四轴飞行器电气连接图电调 电调电机电调电机电调电机电池控制电路板BEC 供电遥控 遥控接收机串口调试接口电机3硬件清单如表2-1。表2-1 四轴飞行器硬件清单器件 型号 主要参数机架 风火轮Z450 桨距0.45m，尼龙材料，重量241g。电机 新西达A2212 13 极，1000KV。螺旋桨 1045 直径10 英寸，桨叶角45。电子调速器 新西达HW-30A 额定电流30A。电池 Lion Power 11.1V ，2200mAh，30C，重量179g。MCU STM32F405RGT6 主频168MHz。陀螺仪 MPU6050 量程2000dps，16 位分辨
10、率。加速度计 MPU6050 量程8G ，16 位分辨率。电子罗盘 HMC5883 量程8Gass，12 位分辨率。气压计 BMP085 0.25m 分辨率。遥控及其接收机 天地飞WFT06X-A2.4G 2.4GHz 信号，5 比例通道+1 开关通道。2.2. 软件构成2.2.1. 上位机上位机是针对飞行器的需要，在Qt SDK 上写的一个桌面程序，可以通过串口与飞行器相连，具备传感器校正、显示姿态、测试电机、查看电量、设置参数等功能，主界面如图(2-3)。图2-3 上位机主界面2.2.2. 下位机下位机为飞行器上MCU 里的程序，主要有三个任务：计算姿态、接受命令和输出控制。下位机直接控制
11、电机功率，飞行器的安全性、稳定性、可操纵性都取决于它。下位机的三个任务实时性都4要求很高，所以计算姿态的频率设为200Hz，输出控制的频率为 100Hz，而接收到命令后，立即处理。因为电子调速器接受的信号为PWM 信号，高电平时间在1ms2ms 之间，所以控制信号输出频率也不能太高。3. 飞行原理3.1. 坐标系统飞行器涉及两个空间直角坐标系统：地理坐标系和机体坐标系。地理坐标系是固连在地面的坐标系，机体坐标系是固连在飞行器上的坐标系。四轴飞行器运动范围小，可以不考虑地面曲率，且假设地面为惯性系。地理坐标系采用“东北天坐标系”，X 轴指向东，为方便罗盘的使用，Y 轴指向地磁北，Z 轴指向天顶。
12、机体坐标系原点在飞行器中心，xy 平面为电机所在平面，电机分布在|x|=|y|,z=0的直线上，第一象限的电机带正桨，z 轴指向飞行器上方。如图3-1 所示。图3-1 地理坐标系和机体坐标系图示（坐标系方向重合时）3.2. 姿态的表示和运算飞行器的姿态，是指飞行器的指向，一般用三个姿态角表示，包括偏航角(yaw)、俯仰角(pitch)和滚转角(roll) 。更深一层，姿态其实是一个旋转变换，表示机体坐标系与地理坐标系的旋转关系，这里定义姿态为机体坐标系向地理坐标系的转换。旋转变换有多种表示方式，包括变换矩阵、姿态角、转轴转角、四元数等。在本文中，矩阵用加粗大写字母表示，如EA R ，左上标和左
13、下标表示从机体坐标系（Aircraft）变换到地理坐标系（Earth）；四元数用加粗小写字母表示，如EA q ，上下标意义与变换矩阵一样；向量用带箭头加粗小写字母表示，如Av ，左上标A 表示向量的值是在机体坐标系的坐标值。因为姿态实质是一个旋转变换，根据刚体有限转动的欧拉定理，旋转变换是可以串联的，所以一个姿态可以经过一个旋转变换，变成另一个姿态。类比点和向量的概念，姿态相当于点，旋转相当于向量，点可以通过加向量，变成另一个点。如果用矩阵表示旋转，旋转的串联由矩阵乘法来实现。如果用四元数表示旋转，则由四元数的乘法来实现旋转串联。用四元数来表示旋转，组合旋转时比用其他方法运算量更少，所以无论在
14、计算机图形学、飞行5器控制等涉及刚体旋转的领域，四元数都有举足轻重的地位9。飞行器的姿态计算是围绕姿态四元数进行的，下面简要介绍一下四元数的运算。1 个四元数由4 个实数组成。Tq wq xq yq zq (3-1)规范化的四元数可以表示旋转，见(3-2)式，为旋转的转角，单位向量T x y z 为旋转的转轴。cos2sin2sin2sin2wx xy yz zqqqq(3-2)记四元数乘法的符号为。四元数乘法跟矩阵一样，有结合律，没有交换律。运算过程见(3-3)式。w w w x x y y z zx w x x w y z z yy w y x z y w z xz w z x y y x
15、 z wr p q p q p q p qr p q p q p q p qr p q p q p q p qr p q p q p q p qr p q (3-3)四元数转成矩阵的函数记为R，具体过程见(3-4) 式。2 22 22 21 2 2 2 2 2 2R 2 2 1 2 2 2 22 2 2 2 1 2 2y z x y w z x z w yx y w z x z y z w xx z w y y z w x x yq q q q q q q q q qq q q q q q q q q qq q q q q q q q q qq (3-4)3.3. 动力学原理螺旋桨旋转时，把空
16、气对螺旋桨的压力在轴向和侧向两个方向分解，得到两种力学效应：推力和转矩。当四轴飞行器悬停时，合外力为0，螺旋桨的推力用于抵消重力，转矩则由成对的正桨反桨抵消。当飞行器运动时，因为推力只能沿轴向，所以只能通过倾斜姿态来提供水平的动力，控制运动由控制姿态来间接实现。假设四轴为刚体，根据质点系动量矩定理，角速度和角加速度由外力矩决定10，通过控制四个螺旋桨，可以产生需要的力矩。首先对螺旋桨编号：第一象限的为0 号，然后逆时针依次递增，如图(3-1)。同步增加0 号和1 号、减小 2 号和3 号桨的功率，可以在不改变推力的情况下，提供x 轴的力矩；同步增加1 号和2 号、减小0 号和3 号桨的功率，可
17、以在不改变推力的情况下，提供y 轴的力矩；同步增加1 号和3 号、减小0 号和2 号桨的功率，可以在不改变推力的情况下，提供z 轴的力矩。以上“增加”和“减小”只是表明变化的方向，可以增加负数和减小负数，提供的力矩就沿对应轴的负方向了。把三个轴的力矩叠加起来，就得到各螺旋桨功率变化与提供的力矩的对应关系，可以用一个矩阵等式表示，见(3-5)式。T 是螺旋桨的功率变化量，为41 矩阵，每行分别对6应0 到3 号螺旋桨；m 是力矩，为31 矩阵。mx 、my 和mz 是各轴的力矩系数，用于把力矩转换成功率变化量，具体数值与电机力矩特性、电机安装位置等有关。1 1 11 1 1diag m , m
18、, m1 1 11 1 1x y zT m (3-5)各个电机实际输出的功率记为output T ，推力油门对应的功率量为 base T ，则有：output base T T T (3-6)4. 姿态测量获取当前姿态是控制飞行器平稳飞行的基础，姿态的测量要求低噪声、高输出频率，当采用陀螺仪等需要积分的传感器时，还需要考虑积分发散等问题。近年来MEMS 传感器越来越成熟、应用广泛，成为低成本姿态测量的首选器件11，因此该项目使用的传感器全部都是MEMS 传感器，见表(2-1)。在使用传感器的值进行姿态计算之前，有必要校正传感器4.1. 传感器校正由于实验条件限制，传感器的校正只有两项，分别对应
19、两种类型的传感器：陀螺仪静止时0输出的传感器、加速度计与罗盘测量某向量场强度的传感器。4.1.1. 陀螺仪对于陀螺仪等静止时0 输出的传感器，可以很方便地校正零偏。把传感器固定好，这时对输出值f X 求平均，得到的A 即为零偏，如(4-1) 式。实际使用时，把测得的值减去零偏，得到的值就是校正值。实际应用的公式如(4-2)， A 为零偏值，31 矩阵，单位：LSB ； i Y 为校正好的值，31矩阵，单位：rad/s； i X 为测量原始值，单位： LSB； gain 为转换系数，单位：(rad/s)/LSB，由传感器的数据手册给出。1fn nA X (4-1)gain i i Y X A (
20、4-2)4.1.2. 加速度计和电子罗盘加速度计和罗盘都是测量所在点的某个向量场的值的传感器，静态时加速度计测的是等效重力加速度场，电子罗盘测的是地磁场。下面仅介绍加速度计的校正，罗盘的校正同理。加速度计测量的对象是比力，也就是等效重力加速度和运动加速度的和，当静止时，运动加速度为0，加速度计的测量值为等效重力加速度，可以利用这一点校正加速度计。加速度计的校正的思路为：对测量值平移和缩放，把测量值拟合到重力加速度。因此校正的任务为：寻找最佳的平移和缩放参数，使总体测量数据的更靠近重力加速度。记测量值为Tm m m x y z ，校正后的值为Tc c c x y z ，平移参数为Tx y z o
21、 o o ，缩放参数为Tx y z g g g ，他们之间的关系为(4-3)式。7c m x xc m y yc m z zx x o gy y o gz z o g(4-3)定义误差u 为测量值长度与重力加速度常数G 的平方差。2 2 2 2c c c u x y z G (4-4)把(4-3)式代入 (4-4)式，得：2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 2 x m x x m x x y m y y m y y z m z z m z z u g x o g x o g g y o g y o g g z o g z o g G (4-5)记2 2 2 1 T m m m m m m V x y z x y z (4-6)a b c d e f gT P (4-7)2222222 2 2 2 2 2 2222xyzxyzxyzx x y y z za gb gc gd o ge o gf o gg o g o g o g G(4-8)则u 可以表示成2 2 2 T
