笔者目前在读研究生，研究的方向正是飞行器的定位与导航。去年10月份开始设计的微型四旋翼，近日才完成整个系统的设计并且飞机可以较为稳定的悬停在空中。
下面就将笔者最近整理的制作过程梳理一遍，还希望与有兴趣的网友共同讨论。
笔者将分为硬件设计与软件设计两大部分来分别阐述系统的构成：
硬件设计：总体思路：
整个机架采用PCB板，将四个电机固定在PCB板的四个角，外接电池。
硬件模块：
单片机、惯性测量模块(IMU)、无线通讯模块、电机驱动模块、续流二极管、电源管理模块(稳压与充放电)、直流有刷电机、大电流放电电池、遥控器。
硬件选型：
模块名称
元件名称
数量
单片机
STM32F103CBT6
1
惯性测量模块(IMU)
MPU6050(三轴加速度计+三轴陀螺仪)
1
无线通讯模块
NRF24L01
1
电机驱动模块
AO3400 5.8A
4
续流二极管
SS34 3A
4
电源管理模块
稳压
TPS79333 3.3V
1
充放电
TP4057 USB兼容5V充电
1
直流有刷电机
空心杯有刷直流电机7*16mm
4
大电流放电电池
250mAh 20C
1
遥控器
JOYPAD游戏手柄
1
硬件工作综述：
单片机负责整个系统的协调工作；惯性测量模块(IMU)负责测量四旋翼的姿态；无线通讯模块负责四旋翼与遥控器的通讯；电机驱动模块负责驱动电机；续流二极管负责对电机进行续流；电源管理模块中的稳压模块负责整个系统的供电，电源管理模块中的充放电模块负责对电池充电；有刷电机负责提供四旋翼的飞行动力；大电流放电电池负责四旋翼的能量来源；遥控器负责对四旋翼进行遥控和控制。
硬件设计功能模块图：
实际效果图与相关参数：
尺寸：对角电机轴距10x10cm
重量：33.2g(带电池)
软件设计：总体思路：
惯性测量模块(IMU)测量出当前飞机的三轴加速度与三轴角速度并传送给单片机处理，由单片机进行基于四元数的姿态解算，求解出当前飞机的pitch、roll、yaw三个角度值，然后根据这三个角度经过PID控制运算，输出四路PWM控制四个直流有刷电机的加减速从而达到飞机的平衡悬停。
其中，惯性测量模块(IMU)的加速度计由于噪声比较大，所以需要对其进行滤波处理；而遥控器则是对飞机进行实时的姿态控制；最后由于四旋翼制作的特殊性，在调试PID参数阶段会频繁的烧写程序，鉴于此，笔者开发了基于NRF24L01的Bootloader技术，免除了烧写Flash的物理连线限制，可实现远程程序一键下载。
姿态解算：
姿态解算属于四旋翼制作的核心部分，如果姿态解算能够实时的反应出飞机的状态，那么对于控制来讲就相对来说比较容易了。而姿态结算所要做的事情就是两个坐标系之间的正确转化(地理坐标系与载体坐标系)，这种转化有很多种表示方法，例如欧拉角法、方向余弦矩阵法、四元数法、旋转矢量法等。笔者采用的是应用广泛的四元数法，而旋转矢量法则是一种基于四元数法的改进四元数法。
四元数本是用于描述四维空间向量的一种方法，对于他的线性变换也就是在四维空间中的拉伸和旋转，显而易见，我们用四元数的向量乘法来表示三维空间中的旋转是绰绰有余的。
通过惯性测量模块(IMU)传送过来的当前飞机的三轴加速度和三轴角速度的值，这样一个三维的向量，转化为四维向量，然后在四维空间中做线性变换(也可以说在三维空间中旋转)后输出，利用四元数与欧拉角的关系(一定要注意旋转顺序)，将当前四元数转换为欧拉角pitch、roll、yaw即得到当前飞机的姿态。
以下给出笔者姿态融合的代码，该代码网上都有，笔者在这里做了些许注释，方便理解。
/*********************************************************************************函数名：void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az)说明：IMU单元数据融合，更新姿态四元数入口：float gx陀螺仪x分量float gy陀螺仪y分量float gz陀螺仪z分量float ax加速度计x分量float ay加速度计y分量float az加速度计z分量出口：无备注：核心思想:利用陀螺仪来计算高速动态下的姿态,利用加速度计来进行角度修正**********************************************************************************/void IMUupdata(float gx, float gy, float gz, float ax, float ay, float az){float recipNorm;//平方根的倒数float halfvx, halfvy, halfvz;//在当前载体坐标系中，重力分量在三个轴上的分量float halfex, halfey, halfez;//当前加速度计测得的重力加速度在三个轴上的分量与当前姿态在三个轴上的重力分量的误差,这里采用差积的方式float qa, qb, qc;gx = gx * PI / 180;//转换为弧度制gy = gy * PI / 180;gz = gz * PI / 180;//如果加速度计处于自由落体状态，可能会出现这种情况，不进行姿态解算，因为会产生分母无穷大的情况if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {//单位化加速度计,意义在于在变更了加速度计的量程之后不需要修改Kp参数,因为这里归一化了recipNorm = invSqrt(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm;//将当前姿态的重力在三个轴上的分量分离出来//就是方向余弦旋转矩阵的第三列,注意是地理坐标系(n系)到载体坐标系(b系)的,不要弄反了.如果书上是b系到n系,转置即可//惯性测量器件测量的都是关于b系的值，为了方便，我们一般将b系转换到n系进行导航参数求解。但是这里并不需要这样做，因为这里是对陀螺仪进行补偿halfvx = g_q1 * g_q3 – g_q0 * g_q2;halfvy = g_q0 * g_q1 + g_q2 * g_q3;halfvz = g_q0 * g_q0 – 0.5f + g_q3 * g_q3;//计算由当前姿态的重力在三个轴上的分量与加速度计测得的重力在三个轴上的分量的差,这里采用三维空间的差积(向量积)方法求差//计算公式由矩阵运算推导而来 公式参见 中的Mnemonic部分halfex = (ay * halfvz – az * halfvy);halfey = (az * halfvx – ax * halfvz);halfez = (ax * halfvy – ay * halfvx);//积分求误差,关于当前姿态分离出的重力分量与当前加速度计测得的重力分量的差值进行积分消除误差if(g_twoKi > 0.0f) {g_integralFBx += g_twoKi * halfex * CNTLCYCLE;//Ki积分g_integralFBy += g_twoKi * halfey * CNTLCYCLE;g_integralFBz += g_twoKi * halfez * CNTLCYCLE;gx += g_integralFBx;//将积分误差反馈到陀螺仪上，修正陀螺仪的值gy += g_integralFBy;gz += g_integralFBz;}else//不进行积分运算，只进行比例调节{g_integralFBx = 0.0f;g_integralFBy = 0.0f;g_integralFBz = 0.0f;}//直接应用比例调节，修正陀螺仪的值gx += g_twoKp * halfex;gy += g_twoKp * halfey;gz += g_twoKp * halfez;}//以下为四元数微分方程.将陀螺仪和四元数结合起来，是姿态更新的核心算子//计算方法由矩阵运算推导而来//. 1//q = – * q x Omega 式中左边是四元数的倒数,右边的x是四元数乘法,Omega是陀螺仪的值(即角速度)//2// .//[q0] [0-wx-wy-wz][q0]// .//[q1][wx 0wz-wy][q1]// . =* //[q2][wy -wz 0wx ][q2]// .//[q3][wz wy-wx0 ][q3]gx *= (0.5f * CNTLCYCLE);gy *= (0.5f * CNTLCYCLE);gz *= (0.5f * CNTLCYCLE);qa = g_q0;qb = g_q1;qc = g_q2;g_q0 += (-qb * gx – qc * gy – g_q3 * gz);g_q1 += ( qa * gx + qc * gz – g_q3 * gy);g_q2 += ( qa * gy – qb * gz + g_q3 * gx);g_q3 += ( qa * gz + qb * gy – qc * gx);//单位化四元数,意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换recipNorm = invSqrt(g_q0 * g_q0 + g_q1 * g_q1 + g_q2 * g_q2 + g_q3 * g_q3);g_q0 *= recipNorm;g_q1 *= recipNorm;g_q2 *= recipNorm;g_q3 *= recipNorm;//四元数到欧拉角转换，转换顺序为Z-Y-X,参见<Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors>.pdf一文,P24//注意此时的转换顺序是1-2-3，即X-Y-Z。但是由于画图方便，作者这里做了一个转换，即调换Z和X，所以顺序没变g_Yaw = atan2(2 * g_q1 * g_q2 + 2 * g_q0 * g_q3, g_q1 * g_q1 + g_q0 * g_q0 – g_q3 * g_q3 – g_q2 * g_q2) * 180 / PI; //Yawg_Roll = asin(-2 * g_q1 * g_q3 + 2 * g_q0* g_q2) * 180 / PI;//Rollg_Pitch = atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1 * g_q1 – 2 * g_q2* g_q2 + 1) * 180 / PI;//Pitch}
注意其中的快速开方函数来自维基百科，精度0.175%。并且注意输入的陀螺仪必须是弧度制(这一点在进入函数的时候已经做了转换)，否则姿态解算是错误的。
如果心在远方，只需勇敢前行，梦想自会引路，
