如何去实现一种四旋翼自主飞行器的设计呢

　　1.方案设计
　　1.1姿态传感器的选择
　　1.1.1加速度计
　　加速度检测芯片有ADXL345、BMA180以及MPU6050等。ADXL345是ADI公司于2008年推出的具有SPI和IIC数字接口的三轴加速度计，有体积小、功耗低、可变量程、高分变率的特点。但是，在工作时电压要稳定，否则会降低测量的精确度，因为每一个工作电压点的零点漂移不一致，所以使用该芯片时，对供电系统的要求比较高。MPU6050是一款整合了3轴加速度计和3轴陀螺仪的六轴测量仪器，而且可以从第二个IIC接口连接其器传感器，比如磁力传感器，这样可以从IIC端口以单一数据流的形式，向应用端输出完整的9轴数据，MPU6050自带了硬件加速器引擎（DMP： Digital Motion Processor），可以直接输出欧拉角数据。
　　1.1.2陀螺仪
　　角速度检测的芯片有L3GD2H、IGT32000和MPU6060等，IGT3200是世界上第一个单芯片，IIC总线接口数字输出，3轴MEMS陀螺仪集成电路，对3D鼠标、游戏和三维远程控制有广泛的应用，其测量的角速度精度为16位，分辨率为14.375deg/s，最大测量范围位2000deg/s，工作电压2.6V。
　　1.1.3电子罗盘
　　电子罗盘有AK8975、HML5883L等，它是一种导航工具，能够提供机体的航向和姿态。电子罗盘是一种重要的导航工具，能实时提供移动物体的航向和姿态。HMC5883L是一种贴片式的高集成模块，内部含有12位的模数转换器，并带有数字接口的弱磁传感器芯片，可将模拟量转换为数字量输出。测量范围为+-8高斯，导航精度可达2度。AK8975内置13位的AD转换器，灵敏度为0.3μT/LSB，含有IIC和四线SPI接口，内置振荡器，用于内部时钟源，内部还安置了磁源，可以自检。
　　1.1.4融合了角速度计和角速度计的模块
　　市面上有姿态角测量的模块，直接通过串口TX输出欧拉角，模块内部带有MPU，比如JY901，该模块融合了地磁测量，数据输出频率达到200Hz，含有GPS接口。姿态解算含有动态卡尔曼滤波，使用该模块可以直接串口读数据，去除了主控姿态解算的压力。但是不能再提高获取原始数据的速度，而且在工作时不能有强烈的振动，否则，获取的数据会出错。
　　经过上面各种器件的比较，选取MPU9150作为姿态检测芯片，其内部集成了MPU6050和AK8975，是一款九轴惯性传感芯片。这样保证了角速度、加速度、电磁场的测量的精度，又使飞控板设计的面积大大小，减少了成本。
　　1.2循迹传感器的选择
　　1.2.1反射式光电开关
　　反射式光电开光是一种红外线产品，是一种小型光电元器件，能够检测出放射的光强的变化。它由一个红外发射管和一个红外接收管组成，发射波长是780nm-1mm，发射器内置一个校准镜头，经过反射射向接收器，当有物体经过，或者物体的颜色改变，都会改变反射光线的强度。白色和黑色对光的吸收程度有很大的差别，根据该原理可以识别轨迹。反射式光电开光在壁障，循迹中应用广泛。但是光照因素对其检测有很大的影响，不同的关照环境下反射的光线强度大有不同，会使调试带来麻烦。
　　1.2.1光流传感器
　　光流发法是图像分析的重要方法，特别是在视觉运动研究中有着举足轻重的作用，它的原理是从二维的图像序列中检测物体的运动、提取运动参数，并且分析物体运动的相关规律。PX4Flow是一款智能光学流动传感器。传感器拥有原生 752×480像素分辨率，计算光学流的过程中采用了4倍分级和剪裁算法，计算速度达到250Hz（白天，室外），具备非常高的感光度。与其他滑鼠传感器不同，它可以以120Hz（黑暗，室内）的计算速度在室内或者室外暗光环境下工作，而无需照明LED。你也可以对它重新编程，用于执行其他基础的，高效率的低等级机器视觉任务。PX4FLOW模块使用USB和UART2输出MAVLink协议包，可以使用地面站软件QGroundControl从模块读取数据，主控可以通过IIC端口来获取相应的数据，
　　主要数据是x轴、y轴的移动速度和移动距离。光流算法是基于每个像素点变化来计算的，所以光流传感器测量时要求被测面有纹理特征，特别不能是单色面，否则计算就不准确。而我们要循的轨迹是一条以白色为底面黑色的直线，所以光流传感器不太适合。
　　1.2.3摄像头
　　摄像头的功能是图像采集，通过对图像的分析来寻找轨迹，并计算出机体偏离轨迹的程度，所以用摄像头循迹，是种很好的选择。不同型号的摄像头的分辨率不同，一般分辨率决定了摄像头产品的价格，在循迹的应用中，对摄像头的分辨率要求不高，设计从成本的角度考虑，选择OV2640摄像头来进行图像采集。
　　1.3测高传感器的选择
　　1.3.1气压计
　　在同一环境下，气压是随高度的变化而变化的，每升高12m，大气压下降1mmHg（1毫升水银柱）或者每上升9m，大气压降低100Pa。根据该原理，可以通过气压计测量不同位置的气压变化来计算出高度的变化。MS5611是由瑞士推出的一款I²C和SPI总线接口的高分辨率气压传感器。MS5611提供了一个精确的24位数字温度值和压力值以及不同的操作模式，可以提高转换速度并优化电流消耗，分辨率达到10cm，体积尺寸是5mmX3mmX1mm，可以集成在移动设备中。由于机体飞行高度只要求在0到4m的范围内，所以，10cm的误差对于定高来说还是比较大的。
　　1.3.2超声波传感器
　　超声波测距原理是超声波发射装置向某一方向发射超声波，与此同时开始计时，超声波在空气中传播，当碰到障碍物就立刻返回来，超声波接收器收到反射波就停止计时。（超声波在空气中的传播速度为340m/s，根据发送到接收经过的时间t，就可以计算出发射点距障碍物的距离（s），即：s=340t/2）常见的超声波测距模块有US-100、URF02、HC_SR04等。US-100超声波测距模块可测范围是2cm~4.5m，拥有2.4~5.5V的宽电压输入范围，静态功耗低于2mA，内部自带了温度传感器，可对测距结果进行校正，内带看门狗，工作稳定可靠，同时具有GPIO，串口两种通信方式，为了获取数据的方便，选择串口方式获取数据。
　　2.硬件设计
　　2.1 结构与功能
　　飞行器由机架、电机、电调、螺旋桨、电池、飞控板、图像处理系统、超声波模块、2.4G无线模块等组成，四旋翼飞行器结构形式如图2.1所示。
　　机架为整个飞行器提供骨架支撑，采用 F330型号的机架，材质为尼龙和纤，硬度强，耐炸机。电机和桨提供动力，电调将直流电转换成三相交流，为电机提供电力，电池则为整个系统提供电力。
　　飞控板则为飞行器的大脑，控制其飞行。飞控板应比赛要求则自主研发，采用TM4C123G为微处理器。飞控板中含有姿态检测芯片，以获取飞行姿态数据。图像处理系统为飞行器的眼睛，对地面轨迹图像经过二值化及滤波处理，再经过特定的计算得出飞飞行器相对地面轨迹的位置。最后以数据包通过串口发送给飞控。
　　超声波模块检测飞行高度，以串口的形式发给飞控。2.4G无线模块是飞行器与地面站的沟通桥梁，通过地面站可以监控飞行器的飞行状态，还可以用地面站调整PID参数。
　　2.2 TM4C123G基本应用系统
　　TM4C123G基本系统由时钟系统、复位系统、SWD调试及下载系统等组成。
　　时钟系统为单片机工作提供节拍，石英晶体振荡器比TM4C123G内部的RC振荡更精准、更稳定。设计采用外部16Mhz的晶体振荡器，再由TM4C123G内部的PLL将时钟达到80Mhz，作为系统时钟。具体电路如图1.2.1所示，其中12pF的电容的作用是帮助晶体起振。OSC_0和OSC_1分别接TM4C123G的OSC0输入管脚和OSC1输出管脚。
　　复位系统包含手动复位和上电复位。手动复位是当程序跑飞或者需要程序从头开始运行时，通过按键来进行复位；上电复位是单片机在上电启动时，为了能使CPU以系统各部件处于确定的状态，并从初态开始工作，通过RC充电原理进行自动复位。复位电路如图2.2.2所示，图中100欧姆的电阻起保护电容的作用。
　　对于SWD调试及下载系统，可以直接将下载器的信号线接入TM4C123G的SWCLK和SWDIO引脚，再将下载器和目标芯片供地即可。下载接口采用1X4的单排针，引脚排序为+3.3V、SWCLK、GND、SWDIO。
　　2.3MPU9150电路设计
　　MPU9150和MPU有两种通信方式，一中是SPI，另一种是IIC，用SPI方式至少的用三根信号线，而IIC只需要两根线，为了降低设计的复杂性采用IIC通信模式。MPU6050外围元件的配置，可以通过它的数据手册来确定。I2C接口采用的是OpenDrain机制，器件本身只能输出低电平，不能主动输出高电平，只能通过外部上拉电阻RP将信号线拉至高电平，所以IIC总线上的上拉电阻是必须的。上拉电阻的大小会对时序有影响，对信号的上升和下降时间也有影响。IIC上拉电阻有一个计算公式：Rmin={Vdd（min）-0.4V}/3mA，Vdd是供电电压。Rmax=（T/0.874）*c，（T=1us100KHz，T=0.3us400KHz），其中C是Buscapacitance。Rp最大值由总线最大容限（Cbmax）决定，Rp最小值由Vdd与上拉驱动电流（最大取3mA）决定。系统的Vdd是3.3V，MPU9105灌入端口的电流是3mA而TM4C123G的最小灌入电流是2mA，所以上拉驱动电流取3mA，Rmin≈1K。MPU9105的Cb=10pF，TM4C123G的Cb=50pF。通信速率要求是400kHz，所以Rmax≈2k。所以上拉电阻选取常用值1.5k。具体电路如图2.3.1所示。
　　2.4循迹单元的设计
　　图像处理对CPU的要求比较高，如果用主控TM4C123G来处理，由于多种任务的处理频率和内存的限制，很难实现。所以采用协处理器（STM32F407）来对摄像头数据进行处理，在通过串口将处理的数据传给主控。为了方便安装摄像头，将图像处理做成一个单独的模块，再通过连接线与主控相连。摄像头和处理器的接线有很多，其中含有SCCB接口，是一种控制接口，控制图像传感器芯片的运行，和IIC接口兼容；数字视频接口（Digital Video Port），包含了信号线和数据线，是个10位的数字接口，其支持8为位数据，ALIENTEK OV2640 模块采用默认的 8位连接方式。在布线布线时得注意，常用的设计规则有：（1）时钟源到负载的连线尽量短，线应较宽。（2）信号的负载过大时，在一个驱动器上不要驱动其他信号，特别是其他时钟。（3）注意不同时钟之间，时钟与其他信号之间的干扰，避免多种信号平行布线（可采用屏蔽层隔离的办法）（4）多个设备需要严格的同步时钟时，可采用H形网络，以保证时钟源到每个负载的延迟相等。（5）驱动高频时钟的器件应很好地去耦。（6）屏蔽线和双绞线一般能得到比线路板走线更好的效果。SCL和SDA属于控制线，走线时，尽量不要和D0-D7的数据线在一起。D0-D7尽量并行走线，且长度尽量相等。
　　2、软件设计
　　2.1姿态解算
　　姿态解算方法有四元数法和方向余弦法，四元素法是通过四元素来记录机体的状态，再将四元数转换成欧拉角，以欧拉角作为直观的数据。
　　一种比较常见的四元数法是Mahony互补滤波算法，特点是加速度对角速度进行补偿，达到消除积分误差的方法。从角速度计获取的角度是通过对角度的积分而得到的，由于角度速度计短期测得的值比较准确，但时间一长，由于温漂的问题，进过积分之后，得到的角度会有很大的误差。加速度计短期值不太准确，噪声比较大，但通过低通滤波之后，长期的值比较准确。根据两者的特点，利用加速度计对角速度计进行补偿来消除积累误差。对机体姿态解算，需要得到x轴、y轴、z轴的角速度和角速度，还有解算的周期。加速度和角速度就可以通过MPU9150来获取，周期就有主控的定时器产生。下面是姿态解算的步骤：
　　1.将加速度原始数据归一化，转换成单位加速度，公式如下：
　　norm = sqrt（ax*ax + ay*ay + az*az）;
　　ax = ax / norm;
　　ay = ay / norm;
　　az = az / norm;
　　2.把四元数转换换成“方向余弦矩阵”中的第三列的三个元素。地理坐标系的重力向量，转到机体坐标系，正好是这三个元素。所以vx、vy、vz，其实就是机体坐标参照系上，换算出来的重力单位向量，公式如下：
　　vx = 2*（q1*q3 - q0*q2）;
　　vy = 2*（q0*q1 + q2*q3）;
　　vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
　　3.对加速度单位向量和重力单位向量进行叉积，该叉积的向量仍在机体坐标系上，而陀螺仪的积分误差也是在机体坐标系上的，而且和叉积成正比，正好可以用来修正角速度计，所以对角速度计的修正直接体现在对机体的修正，公式如下：
　　ex = （ay*vz - az*vy）;
　　ey = （az*vx - ax*vz）;
　　ez = （ax*vy - ay*vx）;
　　4.由于具体的比例系数未知的，所以采用PI调节来对角速度计进行补偿，可以通过调节Kp、Ki两个参数来控制加速度计对加速度计修正的速度。
　　exInt = exInt + ex*Ki;
　　eyInt = eyInt + ey*Ki;
　　ezInt = ezInt + ez*Ki;
　　gx = gx + Kp*ex + exInt;
　　gy = gy + Kp*ey + eyInt;
　　gz = gz + Kp*ez + ezInt;
　　5.得到了修正后的角速度就可以将其转换成四元数，来记录机体状态，下面用一阶龙格-库塔法将角速度转换成四元素。
　　q0 = q0 + （-q1*gx - q2*gy - q3*gz）*halfT;
　　q1 = q1 + （q0*gx + q2*gz - q3*gy）*halfT;
　　q2 = q2 + （q0*gy - q1*gz + q3*gx）*halfT;
　　q3 = q3 + （q0*gz + q1*gy - q2*gx）*halfT;
　　6.最后就是将四元素转换成欧拉角了。
　　Roll = atan2（2*（q0*q1+q2*q3），1-2*（q1*q1+q2*q2））;
　　Pitch = asin（2*（q1*q3-q0*q2）;
　　Yaw = atan2（2*（q1*q2-q0*q3），2*（q0*q0-q1*q1）-1）;
　　2.2姿态控制
　　谈论四轴的姿态控制，首先得知道控制原理，如图2.1所示，以电机1和电机4一侧为前方，下面来分析四轴的基本运动。
　　1.垂直运动：垂直运动相对来说比较简单。结构图显示相邻的电机做反向运动，相对的电机做同向运动。因此有两对电机转向相反，可以平衡机身的反转扭矩，当同时增大电机的转速，是的总的拉力增大，当拉力比机体本身的重力大时，怎机体所受合力垂直向上，机体向上飞，当机体重力大于拉力，机体向上飞。
　　2.滚转运动：滚转运动是机体做左右翻滚的运动。以向右翻滚为例，为了使机体反向扭矩的平衡，不让机体旋转，又要使机体垂直的拉力不变。使电机3和电机4的转速降低，电机1和电机2，的转速增加，由于降低和增加的分别是一对转速相反的电机，机体的反向扭矩依然平衡，使增加和降低的转速一样，则总的拉力没有改变。但由于力的改变，机体会向右翻滚。
　　3.俯仰运动：由于结构的对称性，原理和滚转运动一样，使电机2和电机3的转速降低，电机1和电机4转速增加，会使机体向上仰。
　　4.偏航运动：机体的偏航则利用机体反转扭矩的不平衡。当电机1和电机3的转速增加，电机2和电机4的转速降低时，旋翼1和旋翼3产生的反扭矩大于旋翼2和旋翼4产生的反扭矩，机身便在总反扭矩的作用下绕z轴转动，实现机体的偏航，转向与电机1、电机3的转向相反。
　　机体的前进后退和向左向右，就是由分别由俯仰运动和滚转运动来实现。让机体俯仰或翻滚到某一程度，则会使机体倾斜，拉力会产生一个与水平面平行的力，使机体往平行水平面的方向飞行。所以姿态的保持，即机体额欧拉角保持某个值不变，是姿态控制的主要要求。
　　数字控制器有很多种，最常用和经典的算法那就是PID算法了，机体姿态控制器采用串级PID。控制结构如图2.2.1所示，图中的角度就是姿态解算出来的欧拉角，角速度是直接从MPU9150读出的值，内环是角速度控制，有加强稳定的作用，因为角度为位移量，不能突变，所以变换成角速度来突变；外环就是角度控制了，用来控制机体的姿态。串级控制输入就是期望姿态，通过设置期望姿态就能控制机体的飞行方向。
　　执行机构是四个电机，四个电机是由PWM来控制的，下面是PWM的公式：
　　posture_value［0］ = + out_roll - out_pitch + out_yaw ;
　　posture_value［1］ = + out_roll + out_pitch - out_yaw ;
　　posture_value［2］ = - out_roll + out_pitch + out_yaw ;
　　posture_value［3］ = - out_roll - out_pitch - out_yaw ;
　　posture_value是保存四个PWM的变化量，out_roll、out_pitch、out_yaw就是执行机构的输入了。比如机体往左右时，out_pitch、out_yaw为零，out_roll为正值，则电机1和电机2转速增加，电机3和电机4转速降低。
　　2.3图像处理
　　要实现自主飞行，就必须使机体能够感知周围环境，感知器采用的是摄像头，所以图像处理是也是设计的重要内容。在赛题要求中飞行环境不是很复杂，只有两种颜色，白色和黑色，白色为地面，黑色为飞行的轨迹。所以图像处理大致可分为两步，图像的二值化和对轨迹的计算。
　　2.3.1图像的二值化
　　二值化的基本原理是把图像的每个点的灰度通过适当的阀值设置成0或255，使整个图像呈现黑白的效果。图像的二值化有利于对图像进一步处理，使图像变得简单，处理的数据量也减少了很多。为了能够得到较好的二值化图像，一般采用连通和封闭的边界来定义不交叠的区域。灰度大于或等于阀值的像素置为255，用来表示特定点，小于则为0，用来表示背景。从OV2640获取的图片是RGB565的格式，其每个像素点是16位的，首先得经过灰度化再二值化。将RGB565格式的图片灰度化公式如下：
　　Red = （PixVal & 0xF800） 》》 8;
　　Green = （PixVal & 0x07E0） 》》 3;
　　Blue = （PixVal & 0x001F） 《《 3;
　　Grey = （Red * 76 + Green * 150 + Blue * 30） 》》 8。
　　其中PixVal就是像素点的值，Red、Green、Blue为三种基色。Grey就是灰度值（0-255）。
　　得到灰度值之后就可以用OTSU或Kittle算法进行二值化，Kittle算法是一种快速的全局阈值法，其效果虽然比OTSU差一点，但速度快很多倍，在循迹控制中要求协控处理图像的速度要快，所以选择Kittle算法。关于Kittle算法有很多文献资料，再此就不多说了。
　　2.3.1轨迹的计算方法
　　由于循迹的轨迹是直线，所以只要计算黑线在图像中心位置的偏移。对二值化后的图像进行扫描，计算每行的黑点横坐标的平均值，从某行进行分析，该平均值就体现出了该行连续黑点在整行的位置，把所有行的平均值再求平均之后就可得到整条黑线在图像的位置，该位置表现出左右的偏移。对于黑圆该方法同样适用，将其看成二维的即可，和对每行进行扫描一样，对每一列进行像素点的扫描就可以得到圆在图像上前后的偏移，图像是机体上摄像头获取的，黑线或圆在图像的偏移，就是机体对于黑线或圆的偏移。
　　2.4循迹和定点
　　循迹和定点的控制原理相同，区别在于循迹控制是一维的，定点是二维的，所以可以使用同一种控制器，采用PID算法，PID算法具有很好的跟随性，而且响应速度也很快。PID控制器由比例（P）、积分（I）和微分（D）组成，位置式PID公式如下：
　　u0为初始期望的输出值，通过实际情况来估算出u0，一般为零，T是采用周期，即控制器两次计算之间的间隔。e（n）是第n次期望和实际值得偏差，由于期望是黑线或者圆在图像正中心，所以期望值是不变的，偏差就是一个固定的值减去图像处理后的数据，这里固定的值为128。
　　Kp、Kp/Ti、Kp*Td为控制器PID的参数，它们的作用如下：
　　1.比例参数的作用是加快系统的响应速度，太小系统响应太慢，太大会使系统振荡，参产生超调，比例环节是PID控制器中不可缺少的环节，调参时一开始就要估算出比例系数；
　　2.积分的作用是来消除系统误差的，积分越大，消除静差的时间越短，但也不能太大，否则会产生积分饱和的现象。从传递函数来分析，增加了积分环节，相位会有一个90度的延迟，而且控制器从一阶变成了二阶系统，会降低系统的稳定性；
　　3.微分的作用是改善系统的动态性能，在响应过程中可以抑制偏差向任何方向的变化，对偏差变化进行提前预知，防止超调，从某一角度分析可理解为微分环节在控制器中有相反的作用，当偏差越小，就会使调节器的调节作用越小，微分系数不能太大，不然会增加系统的调节时间。
　　经过对三个参数的分析，选取PD控制器最为合适，即将积分系数置为零，比例和微分设为非零合适置，在进行调参时，先把微分系数，比例设为一个合适的值，对机体进行测试，如果测得值在期望值附近产生振荡，则合理的比例系数就在此时比例系数的附近了。在调试过程中，很难用个肉眼观察出系统是否产生了等幅振荡，所以通过2.4G无线传输将机体对黑线的偏移传输给上位机，通过上位机来显示波形，这就很明了了。如图2.4.1就是选取比例系数时的波形图，可以很直观的看出其在128附近振荡。
　　比例系数选取好之后就要加入微分环节了，给个合适的微分参数，如果机体振荡更加严重了，那么是微分系数太大了，得减小微分。如果没有发生振荡，那么微分就发挥了作用了，它会使振荡的幅度变小，即波形更加平缓，当然微分太小，其作用也是很小的。微分也有个缺点，那就是降低系统的响应，当然调试出一组折中的参数，系统的稳定性达到要求，系统的响应速度也要达到要求。如果在比例不变的情况下只改微分，达不到要求，那就要适当的改变比例系数了，利用比例系数和微分系数两者的特点来进行选取。
　　2.5高度控制
　　定高函数的作用是使飞行器在指定高度飞行，在飞行过程中会有网子，这样会影响超声波测的得数据，因此在数据采集后需要滤除，滤除的方法采用低通滤波加滑动中值滤波。滤波之后采用串级PID对高度进行控制，其中速度控制是内环控制，外环是高度控制，PID的输出量就是油门量，串级PID的结构和姿态控制的结构是一样的，其中Z轴的速度就是对角速度计进行积分得到的，高度通过超声波来测量。一阶低通滤波的公式如下：
　　Yn=A*Xn+（1-A）*Yn-1
　　其中Yn是滤波后的值，Yn-1是上次滤波的值，Xn是当前测得的值，A为滤波系数，截止频率f=A/（2πt）。在编程中实现方式如下：
　　height_lpf += 0.8*2*3.14f*t*（height - height_lpf）;//低通滤波