搜索
硬件设计：总体思路：整个机架采用PCB板，将四个电机固定在PCB板的四个角，外接电池。硬件模块：单片机、惯性测量模块(IMU)、无线通讯模块、电机驱动模块、续流二极管、电源管理模块(稳压与充放电)、直流有刷电机、大电流放电电池、遥控器。硬件选型：模块名称元件名称数量单片机STM32F103CBT61惯性测量模块(IMU)MPU6050(三轴加速度计+三轴陀螺仪)1无线通讯模块NRF24L011电机驱动模块AO34005.8A4续流二极管SS343A4电源管理模块稳压TPS793333.3V1充放电TP4057USB兼容5V充电1直流有刷电机空心杯有刷直流电机7*16mm4大电流放电电池250mAh20C1遥控器JOYPAD游戏手柄1硬件工作综述：单片机负责整个系统的协调工作；惯性测量模块(IMU)负责测量四旋翼的姿态；无线通讯模块负责四旋翼与遥控器的通讯；电机驱动模块负责驱动电机；续流二极管负责对电机进行续流；电源管理模块中的稳压模块负责整个系统的供电，电源管理模块中的充放电模块负责对电池充电；有刷电机负责提供四旋翼的飞行动力；大电流放电电池负责四旋翼的能量来源；遥控器负责对四旋翼进行遥控和控制。硬件设计功能模块图：实际效果图与相关参数：尺寸：对角电机轴距10x10cm重量：33.2g(带电池)软件设计：总体思路：惯性测量模块(IMU)测量出当前飞机的三轴加速度与三轴角速度并传送给单片机处理，由单片机进行基于四元数的姿态解算，求解出当前飞机的pitch、roll、yaw三个角度值，然后根据这三个角度经过PID控制运算，输出四路PWM控制四个直流有刷电机的加减速从而达到飞机的平衡悬停。其中，惯性测量模块(IMU)的加速度计由于噪声比较大，所以需要对其进行滤波处理；而遥控器则是对飞机进行实时的姿态控制；最后由于四旋翼制作的特殊性，在调试PID参数阶段会频繁的烧写程序，鉴于此，笔者开发了基于NRF24L01的Bootloader技术，免除了烧写Flash的物理连线限制，可实现远程程序一键下载。姿态解算：姿态解算属于四旋翼制作的核心部分，如果姿态解算能够实时的反应出飞机的状态，那么对于控制来讲就相对来说比较容易了。而姿态结算所要做的事情就是两个坐标系之间的正确转化(地理坐标系与载体坐标系)，这种转化有很多种表示方法，例如欧拉角法、方向余弦矩阵法、四元数法、旋转矢量法等。笔者采用的是应用广泛的四元数法，而旋转矢量法则是一种基于四元数法的改进四元数法。四元数本是用于描述四维空间向量的一种方法，对于他的线性变换也就是在四维空间中的拉伸和旋转，显而易见，我们用四元数的向量乘法来表示三维空间中的旋转是绰绰有余的。通过惯性测量模块(IMU)传送过来的当前飞机的三轴加速度和三轴角速度的值，这样一个三维的向量，转化为四维向量，然后在四维空间中做线性变换(也可以说在三维空间中旋转)后输出，利用四元数与欧拉角的关系(一定要注意旋转顺序)，将当前四元数转换为欧拉角pitch、roll、yaw即得到当前飞机的姿态。以下给出笔者姿态融合的代码，该代码网上都有，笔者在这里做了些许注释，方便理解。/*********************************************************************************函数名：voidIMUupdata(floatgx,floatgy,floatgz,floatax,floatay,floataz)说明：IMU单元数据融合，更新姿态四元数入口：floatgx陀螺仪x分量floatgy陀螺仪y分量floatgz陀螺仪z分量floatax加速度计x分量floatay加速度计y分量floataz加速度计z分量出口：无备注：核心思想:利用陀螺仪来计算高速动态下的姿态,利用加速度计来进行角度修正**********************************************************************************/voidIMUupdata(floatgx,floatgy,floatgz,floatax,floatay,floataz){floatrecipNorm;//平方根的倒数floathalfvx,halfvy,halfvz;//在当前载体坐标系中，重力分量在三个轴上的分量floathalfex,halfey,halfez;//当前加速度计测得的重力加速度在三个轴上的分量与当前姿态在三个轴上的重力分量的误差,这里采用差积的方式floatqa,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_twoKi0.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=-*qxOmega式中左边是四元数的倒数,右边的x是四元数乘法,Omega是陀螺仪的值(即角速度)//2//.//[q0][0-wx-wy-wz][q0]//.//[q1][wx0wz-wy][q1]//.=*//[q2][wy-wz0wx][q2]//.//[q3][wzwy-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,参见RepresentingAttitude:EulerAngles,UnitQuaternions,andRotationVectors.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%。并且注意输入的陀螺仪必须是弧度制(这一点在进入函数的时候已经做了转换)，否则姿态解算是错误的。针对上述代码我还想说明一个笔者发现的问题：有很多网友和许多外国的四轴代码(CrazyFlie)在这个姿态解算的加速度计部分都做了零点校准处理。意思就是在开机的时候读取一定次数的加速度计的值，然后平均一下，得到一个初始状态的偏移量，最后在输出的时候加速度计减掉这个值，然后再给到姿态解算代码部分。而笔者在刚开始移植代码的时候是没有做这个零点校准处理的(当然，陀螺仪必须要做零点处理，因为陀螺仪的原理，必须要在静止时输出为0)，包括现在依旧没有对加速度计做零点校准处理，仍然可以获得较为实时的姿态。那么显然对加速度计做不做零点校准处理都是可行的。为什么呢？经过我的分析，首先在这段代码中，我们对加速度计进行了归一化处理，我们知道在数学当中，对数值进行单位化意味着长度不变而只改变方向，对于加速度计来讲，他的”长度”就是加速度的大小，他的”方向”就是加速度的方向。所以我们对加速度计做了单位化之后，其加速度的大小我们就无从而知，但是我们利用了他的方向来进行姿态解算。就这一点来讲，无论我们做不做零点校准处理，进来的加速度的值始终都抛弃掉大小，并关注方向，与零点校准处理无关。另一方面，由于我们生活在重力场里面，那么加速度计在静止状态下测量的是重力加速度，会有一个g的输出。而我们理想的加速度计应该是输出0，而在有加速度的时候应该输出相应的加速度，但是现实是我们生活在一个重力场里面，所以必定有一个重力输出。那么零点校准处理的核心就是我们对于加速度计的理解问题，如果做了零点校准处理，那么我们使用的加速度计就成为了”真正的”加速度计，当有重力的时候他输出为0，有加速度的时候就输出加速度；当我们没有做零点校准处理的时候，那么我们使用的加速度计就成了”重力”加速度计。但是细心的你其实可以发现那个并不是真正的加速度计，我将传感器反过来放的话输出就不是0了，而是z轴上的负值输出。显然这个零点标准处理做的不那么标准。况且这种处理方式是非常粗糙的，因为加速度计的噪声十分的大，数据波动非常厉害，我做了16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，其输出仍然有1~4左右的波动。可见加速度计确实不好处理，除非用K