微型四旋翼飞行器的设计与制作

那么显然对加速度计做不做零点校准处理都是可行的。为什么呢？经过我的分析，首先在这段代码中，我们对加速度计进行了归一化处理，我们知道在数学当中，对数值进行单位化意味着长度不变而只改变方向，对于加速度计来讲，他的”长度”就是加速度的大小，他的”方向”就是加速度的方向。所以我们对加速度计做了单位化之后，其加速度的大小我们就无从而知，但是我们利用了他的方向来进行姿态解算。就这一点来讲，无论我们做不做零点校准处理，进来的加速度的值始终都抛弃掉大小，并关注方向，与零点校准处理无关。另一方面，由于我们生活在重力场里面，那么加速度计在静止状态下测量的是重力加速度，会有一个g的输出。而我们理想的加速度计应该是输出0，而在有加速度的时候应该输出相应的加速度，但是现实是我们生活在一个重力场里面，所以必定有一个重力输出。那么零点校准处理的核心就是我们对于加速度计的理解问题，如果做了零点校准处理，那么我们使用的加速度计就成为了”真正的”加速度计，当有重力的时候他输出为0，有加速度的时候就输出加速度；当我们没有做零点校准处理的时候，那么我们使用的加速度计就成了”重力”加速度计。但是细心的你其实可以发现那个并不是真正的加速度计，我将传感器反过来放的话输出就不是0了，而是z轴上的负值输出。显然这个零点标准处理做的不那么标准。况且这种处理方式是非常粗糙的，因为加速度计的噪声十分的大，数据波动非常厉害，我做了16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，其输出仍然有1~4左右的波动。可见加速度计确实不好处理，除非用Kalman滤波。

鉴于以上两点原因，本人就没有做加速度计的零点校准处理。当需要测量飞机的加速度大小并实现定位时，那么就需要做零点校准处理了。而当我们只需要解算姿态，那么加速度计就不需要做零点校准处理。

以上是笔者对于加速度计零点校准处理的愚见，如有错误，还望共同学习。

最后想说明一点的是关于陀螺仪的数据转化，笔者在最开始编写姿态解算代码时，发现角度的变化与实时姿态差了好几个数量级，体现出来的现象就是稍微移动一下飞机，姿态就呼呼的飞速变化。之前一直以为是姿态解算中的Kp和Ki的系数问题，后来才发现是陀螺仪的数据没有转化成标准单位(°/s)输出，没有参看pdf上的量程单位，所以没有做数据转化处理，在这里提醒一下各位，不要犯笔者这种低级错误了。

PID控制：

PID控制属于自动化领域，由于笔者的本科出生于自动化专业，所以对于自动控制原理有一点理论上的认识。P是比例，I是积分，D是微分，这是最基本的定义。对于一个系统，我们想要控制他，目前的理论是引入负反馈，这个概念相当重要，是由维纳提出来的。意思是，将输出引入到输入端，并且用输入减去输出，这就是著名的负反馈系统。很显然，我们要做的是输出跟随输入，使得系统可控。也就是说要求输出和输入的误差为0，即输出等于输入。在实际的系统中，输出与输入肯定是存在误差的，这种误差就通过PID来控制使得满足输出与输入误差为0。当系统由于干扰出现误差时，此时的P参数就起到了“立竿见影”的作用，将当前系统误差第一时间反应出来，也就是当前误差多少，我就给你多少输出值来补偿你的误差。这种调节方式的特点是快速而有劲，相应来说就是发散且不稳定的；而D参数则具有一种预见性，这种预见性可以提前预知系统的行为，比如距离设定值是越来越远还是越来越近，前者D的作用越强，后者D的作用越弱。可以发现D参数与P参数具有一定的互补性质，P会引起发散，而D则是抑制发散，使系统非常敏感；最后I参数是积分，在连续系统中是时间的积分，在数字系统中是时间的累加。这种累加无疑会造成系统的不稳定，如果系统长时间处于不平衡的位置，那么由于时间的累计，I的作用会变得越来越强，甚至超过了P的作用，那么系统必定失控。但是他的作用有时候确实不可忽略的：消除静差。

在这里笔者尤其提醒大家一点，如果此时系统的输出达到了我们给定的期望值，也就是说输出与输入误差为0，即现在的PID控制器输入0，所以输出也是0，也就是说此时的执行机构是不会输出的，让设备处于自由运动阶段。而非我们认为的当你观察到一个系统处于稳定运行并达到给定值的时候，他的执行机构是一直在输出的，这是错误的。

浅谈完PID，对于四旋翼的控制，笔者采用的就是经典控制论中的PID控制，利用的是期望姿态(pitch=0,roll=0,yaw=0)与当前姿态的误差，通过PID的控制作用输出四路不同的PWM驱动电机让飞机调整自己的姿态满足当前姿态与期望姿态的误差为0的目标，这也是PID控制器的目标。

以下是笔者的PID控制代码：

[cpp]view plaincopyprint?

2. voidQuadrotor_Control(constfloatExp_Pitch,constfloatExp_Roll,constfloatExp_Yaw)
3. {
4. s16outputPWM_Pitch,outputPWM_Roll,outputPWM_Yaw;
6. //---得到当前系统的误差-->利用期望角度减去当前角度
7. g_Attitude_Error.g_Error_Pitch=Exp_Pitch-g_Pitch;
8. g_Attitude_Error.g_Error_Roll=Exp_Roll-g_Roll;
9. g_Attitude_Error.g_Error_Yaw=Exp_Yaw-g_Yaw;
11. //---倾角太大，放弃控制
12. if(fabs(g_Attitude_Error.g_Error_Pitch)>=55||fabs(g_Attitude_Error.g_Error_Roll)>=55)
13. {
14. PWM2_LED=0;//蓝灯亮起
15. PWM_Set(0,0,0,0);//停机
16. return;
17. }
18. PWM2_LED=1;//蓝灯熄灭
20. //---稳定指示灯,黄色.当角度值小于3°时，判定为基本稳定,黄灯亮起
21. if(fabs(g_Attitude_Error.g_Error_Pitch)<=3&&fabs(g_Attitude_Error.g_Error_Roll)<=3)
22. PWM4_LED=0;
23. else
24. PWM4_LED=1;
26. //---积分运算与积分误差限幅
27. if(fabs(g_Attitude_Error.g_Error_Pitch)<=20)//积分分离-->在姿态误差角小于20°时引入积分
28. {//Pitch
29. //累加误差
30. g_Attitude_Error.g_ErrorI_Pitch+=g_Attitude_Error.g_Error_Pitch;
32. //积分限幅
33. if(g_Attitude_Error.g_ErrorI_Pitch>=PITCH_I_MAX)
34. g_Attitude_Error.g_ErrorI_Pitch=PITCH_I_MAX;
35. elseif(g_Attitude_Error.g_ErrorI_Pitch<=-PITCH_I_MAX)
36. g_Attitude_Error.g_ErrorI_Pitch=-PITCH_I_MAX;
37. }
38. if(fabs(g_Attitude_Error.g_Error_Roll)<=20)
39. {//Roll
40. //累加误差
41. g_Attitude_Error.g_ErrorI_Roll+=g_Attitude_Error.g_Error_Roll;
43. //积分限幅
44. if(g_Attitude_Error.g_ErrorI_Roll>=ROLL_I_MAX)
45. g_Attitude_Error.g_ErrorI_Roll=ROLL_I_MAX;
46. elseif(g_Attitude_Error.g_ErrorI_Roll<=-ROLL_I_MAX)
47. g_Attitude_Error.g_ErrorI_Roll=-ROLL_I_MAX;
48. }
50. //---PID运算-->这里的微分D运算并非传统意义上的利用前一次的误差减去上一次的误差得来
51. //---而是直接利用陀螺仪的值来替代微分项,这样的处理非常好,因为巧妙利用了硬件设施,陀螺仪本身就是具有增量的效果
52. outputPWM_Pitch=(s16)(g_PID_Kp*g_Attitude_Error.g_Error_Pitch+
53. g_PID_Ki*g_Attitude_Error.g_ErrorI_Pitch-g_PID_Kd*g_MPU6050Data_Filter.gyro_x_c);
54. outputPWM_Roll=(s16)(g_PID_Kp*g_Attitude_Error.g_Error_Roll+
55. g_PID_Ki*g_Attitude_Error.g_ErrorI_Roll-g_PID_Kd*g_MPU6050Data_Filter.gyro_y_c);
56. outputPWM_Yaw=(s16)(g_PID_Yaw_Kp*g_Attitude_Error.g_Error_Yaw);
58. //---给出PWM控制量到四个电机-->X模式控制
59. //特别注意，这里输出反相了，因为误差是反的
60. g_motor1_PWM=g_BasePWM+outputPWM_Pitch+outputPWM_Roll+outputPWM_Yaw;
61. g_motor2_PWM=g_BasePWM-outputPWM_Pitch+outputPWM_Roll-outputPWM_Yaw;
62. g_motor3_PWM=g_BasePWM-outputPWM_Pitch-outputPWM_Roll+outputPWM_Yaw;
63. g_motor4_PWM=g_BasePWM+outputPWM_Pitch-outputPWM_Roll-outputPWM_Yaw;
65. //---PWM反向清零,因为没有反转
66. if(g_motor1_PWM<0)
67. g_motor1_PWM=0;
68. if(g_motor2_PWM<0)
69. g_motor2_PWM=0;
70. if(g_motor3_PWM<0)
71. g_motor3_PWM=0;
72. if(g_motor4_PWM<0)
73. g_motor4_PWM=0;
75. //---PWM限幅
76. if(g_motor1_PWM>=g_MaxPWM)
77. g_motor1_PWM=g_MaxPWM;
78. if(g_motor2_PWM>=g_MaxPWM)
79. g_motor2_PWM=g_MaxPWM;
80. if(g_motor3_PWM>=g_MaxPWM)
81. g_motor3_PWM=g_MaxPWM;
82. if(g_motor4_PWM>=g_MaxPWM)
83. g_motor4_PWM=g_MaxPWM;
85. if(g_Fly_Enable)//允许起飞,给出PWM
86. PWM_Set(g_motor1_PWM,g_motor2_PWM,g_motor3_PWM,g_motor4_PWM);
87. else
88. PWM_Set(0,0,0,0);//停机
89. }

在这段代码中，首先得到期望值与当前值的误差，然后经过积分分离与抗积分饱和处理后，计算PID输出，关键点在于三轴PID输出与四电机的融合处理，接着对运算结果进行反向清零和正向限幅处理。

我们知道四旋翼目前有两种运行模式，一种成为+模式，一种成为x模式。前者表示四旋翼的运动方向与其中一对电机的轴线重合，后者则是将前一种方式旋转45度的结果。相对而言，x模式稳定一些。但如果要完成翻跟头等特技动作，可能需要用+模式。笔者观看了网易公开课关于四旋翼的TED，他们的四轴运动方式全部是+模式。笔者在这里就不细讲+模式与x模式怎么融合，这部分网上都有，其实也不难，想好符号和力矩关系，自己都可以写出来。笔者就是这么过来的。

而对于PID的参数整定来讲，因为笔者制作的是小四轴，惯性小，很灵敏。所以P和D参数的耦合比大四轴严重很多，在调试的时候注意两者的关系。先整定P，再整定D，然后反过来迭代P，再迭代D，直到找到一个最佳值。如果发现无论如何都找不到更好的效果时，考虑降低参数，因为可能在迭代的过程中已经超过了极值。

加速度计滤波：

在前面的姿态解算部分已经提到有必要对加速度计的值进行滤波。笔者为了达到滤波的最佳效果，当没有考虑实时性时，采用了方才讨论的16深度的窗口滑动滤波再加19阶的kaiser窗FIR低通滤波，效果确实理想很多，但是代价就是延迟较为严重；而在考虑实时性的要求之后，笔者去除了FIR低通滤波，只用了8深度的窗口滑动滤波。虽然效果来讲肯定没有前述的要好，但是对于姿态解算的误差来讲，静止时波动差不多在0.1~0.2°左右(有FIR滤波则稳定不动)。针对于本四旋翼的设计，0.1~0.2°的误差显得微不足道，所以就放弃了高阶的FIR滤波。当然，这只是在静止状态下的测试，如果打开电机，引入电机的高频机械震动，那么加速度计的值又会产生新的噪声。笔者将四旋翼拿在手上测试他的角度变化，发现在大油门时大致有4°左右的偏差，这个误差还是较为严重的。鉴于此，笔者才做FIR滤波。但是在实际飞行过程中，当只有8深度的窗口滑动滤波时，似乎可以平衡，没有拿在手上测试的4°误差。所以在这里笔者就偷懒了，直接采用8深度的窗口滑动滤波，放弃了FIR低通滤波。具体的原因，如果有网友愿意讨论可以联系我。

以下是笔者的8深度窗口滑动滤波代码，算法经过优化，减少了数组的移动和求和运算，利用了循环队列的原理避免了求和运算：

[cpp]view plaincopyprint?

2. voidIMU_Filter(void)
3. {
4. s32resultx=0;
5. statics32s_resulttmpx[ACC_FILTER_DELAY]={0};
6. staticu8s_bufferCounterx=0;
7. statics32s_totalx=0;
9. s32resulty=0;
10. statics32s_resulttmpy[ACC_FILTER_DELAY]={0};
11. staticu8s_bufferCountery=0;
12. statics32s_totaly=0;
14. s32resultz=0;
15. statics32s_resulttmpz[ACC_FILTER_DELAY]={0};
16. staticu8s_bufferCounterz=0;
17. statics32s_totalz=0;
19. //加速度计滤波
20. s_totalx-=s_resulttmpx[s_bufferCounterx];//从总和中删除头部元素的值，履行头部指针职责
21. s_resulttmpx[s_bufferCounterx]=g_MPU6050Data.accel_x;//将采样值放到尾部指针处，履行尾部指针职责
22. s_totalx+=g_MPU6050Data.accel_x;//更新总和
24. resultx=s_totalx/ACC_FILTER_DELAY;//计算平均值，并输入到一个固定变量中
25. s_bufferCounterx++;//更新指针
26. if(s_bufferCounterx==ACC_FILTER_DELAY)//到达队列边界
27. s_bufferCounterx=0;
28. g_MPU6050Data_Filter.accel_x_f=resultx;
30. s_totaly-=s_resulttmpy[s_bufferCountery];
31. s_resulttmpy[s_bufferCountery]=g_MPU6050Data.accel_y;
32. s_totaly+=g_MPU6050Data.accel_y;
34. resulty=s_totaly/ACC_FILTER_DELAY;
35. s_bufferCountery++;
36. if(s_bufferCountery==ACC_FILTER_DELAY)
37. s_bufferCountery=0;
38. g_MPU6050Data_Filter.accel_y_f=resulty;
40. s_totalz-=s_resulttmpz[s_bufferCounterz];
41. s_resulttmpz[s_bufferCounterz]=g_MPU6050Data.accel_z;
42. s_totalz+=g_MPU6050Data.accel_z;
44. resultz=s_totalz/ACC_FILTER_DELAY;
45. s_bufferCounterz++;
46. if(s_bufferCounterz==ACC_FILTER_DELAY)
47. s_bufferCounterz=0;
48. g_MPU6050Data_Filter.accel_z_f=resultz;
49. }

基于NRF24L01的Bootloader：

这一块内容属于独立与四旋翼开发的部分，因为在最初设计之时，想到PID调试需要反复整定参数，就需要不断的烧写程序来变更参数，这样就需要重复的插拔连线，显得麻烦。所以笔者就在无线模块NRF24L01的基础之上，开发了Bootloader技术，使得下载程序通过无线模块下载程序到Flash中，这样极大的简化了参数整定的过程。

笔者在这里就不详细介绍Bootloader的原理了，简单点说就是在Flash中开辟两个区域：A区域和B区域。其中A区域称之为Bootloader，用以实现Flash的烧写工作，相当于代替了J-LINK；B区域就是我们运行代码的区域，也就是Bootloader将要操作的Flash区域，我们的代码就在这里运行。单片机在开机后首先运行A区域的Bootloader代码，这段代码等待NRF24L01接收二进制程序代码，在接收的同时，就一边将接收到的二进制程序代码烧写进B区域中。等全部接收完毕，同时也烧写完毕。之后通过在汇编修改栈顶指针并跳转到程序的APP代码起始位置即可。

以下为Bootloader中的APP函数跳转关键代码：

[cpp]view plaincopyprint?

2. voidIAP_Load_App(u32appxaddr)
3. {
4. if(((*(vu32*)appxaddr)&0x2FFE0000)==0x20000000)//检查栈顶地址是否合法
5. {
6. Jump_To_App=(IAP_FunEntrance)*(vu32*)(appxaddr+4);//用户代码区第二个字为程序开始地址(复位地址)-->详见startup.sLine61
7. //(vu32*)(appxaddr+4)-->将FLASH的首地址强制转换为vu32的指针
8. //*(vu32*)(appxaddr+4)-->解引用该地址上存放的APP跳转地址,即main函数入口
9. //(IAP_FunEntrance)*(vu32*)(appxaddr+4)-->将main函数入口地址强制转换为指向函数的指针给Jump_To_App
10. MSR_MSP(*(vu32*)appxaddr);//初始化APP堆栈指针(用户代码区的第一个字用于存放栈顶地址)
11. Jump_To_App();//跳转到APP,执行APP
12. }
13. }

尤其注意Jump_To_App和Jump_To_App()的用法，前提是Jump_To_App本身就是一个指向函数的指针。定义：

[cpp]view plaincopyprint?

1. typedefvoid(*IAP_FunEntrance)(void);//定义一个指向函数的指针
2. IAP_FunEntranceJump_To_App;