暑假期间学习了STM32，对ST单片机有了初步了解，大三开学一个月左右基本开始做四轴了。首先需要对硬件电路进行设计，现在四轴开源设计的有很多，国内的有圆点博士、匿名科技以及烈火等。而四轴的硬件电路也基本相似，所以我们参照了这些电路简单地设计出我们的四轴。

四轴所用的传感器是MPU6050，通信预留了蓝牙与2.4G的接口，单片机下载采用SWD模式下载。同样将IIC接口引出来可以接IIC接口器件如HMC5883L。板子打样回来后就对电路板进行焊接，焊接时才发现电源处的100UF电容小的太明显了，没有办法，只好用了22Uf的电容进行替代，而且其他电阻电容的封装也有所偏小。最失败的地方在于2.4G引脚接口的顺序弄反了，以致不能将2.4G模块直接插在上面了。

 

电路焊接以及调试完成后，就需要用例程对它进行测试。在第一个LED测试历程中，发现LED闪烁的时间明显不正常。因为我们的程序基本是在正点原子模板在修改而来，而他的延时用的是滴答定时器，所以问题就出现在他的延时函数上。通过对比其他的例程以及相关论坛后发现，系统延时错误是因为它没有正确分频，也即没有分得72M。所以在程序首需要对时钟分频进行设置,函数如下：

void RCC_HSE_Configuration()

{

RCC_DeInit();

RCC_HSEConfig(RCC_HSE_ON);

while(RCC_WaitForHSEStartUp() != SUCCESS);

FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);/* Flash 2 wait state */

FLASH_SetLatency(FLASH_Latency_2);

RCC_HCLKConfig(RCC_SYSCLK_Div1);

RCC_PCLK1Config(RCC_HCLK_Div2);

RCC_PCLK2Config(RCC_HCLK_Div1);

RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);

RCC_PLLCmd(ENABLE);

while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET) ;

RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);

while(RCC_GetSYSCLKSource() != 0x08);   

}

加粗部分为FLASH延时。

论坛参考为：http://bbs.21ic.com/icview-359792-1-1.html

参考2：http://blog.sina.com.cn/s/blog_6cd2030b01019i9j.html

解决好时钟问题以后，我们需要对MPU6050进行测试，起初在焊接时，已通过单片机开发板接连接焊好的模块上通过IIC读取寄存器相关状态，发现已经能够读取寄存器的值。当焊接完成时，对飞控板写好MPU6050的读取函数后，通过当初预留的蓝牙串口即单片机的串口1与正点原子单片机串口相连接，将数据发送到PC端的窗口上，连接图如下

相关硬件测试完成后，就需要对控制或通信方式进行选择，由于2.4G在画板时的一些失误，所以起初我们选择了蓝牙模块。蓝牙模块选用的是正点原子的蓝牙2.0，程序也是在正点原子的例程上修改的，所以参照正点原子的不完全手册以及蓝牙模块的相关数据手册就可以很好地移植蓝牙模块通信。

 

解决好通信问题，下一步就是传感器数据读取的处理。下面进行简单的介绍：

1、坐标系统

飞行器设计两个空间直角坐标系统：地理坐标系和机体坐标系。地理坐标系采用的是东北天坐标系，而机体坐标系是固定在飞行器上的坐标系。飞行器的姿态表示和运算就是这两者坐标系的变换。

2、姿态表示

飞行器的姿态是指飞行器的的指向，一般用三个姿态角表示，包括偏航角（Yaw）、俯仰角（Pitch）和翻滚角（Roll）。姿态其实是一个旋转变换，表示机体坐标系与地理坐标系的旋转关系。

 

3、姿态测量

姿态测量是通过MPU6050读取处三轴陀螺仪三轴加速度计的数据，而在读取数据之前需要先对传感器进行校正。首先将传感器固定好，水平静止放置，这时对传感器读取出的200次数据求其平均值即得到零偏，然后将每次传感器读取的数据减去这个零偏就得到校正后的数据。

4、数据融合

数据融合就是对姿态进行结算。在更新姿态前需要对加速度计读取的数据进行简单的处理，即对其20次求平均为1次有效的加计值。姿态更新时采用了四元数的相关理论，具体流程如下：

定义四元数的四个变量，归一化四元数（每次更新完四元数均需要对其进行归一化，防止发散）。根据方向余弦矩阵的定义，可将地理坐标系的重力（0,0,1）变换到机体坐标系。

又可根据传感器测出的在机体坐标系XYZ上的重力向量，求他们之间的误差向量，就是陀螺仪积分后的姿态和加计测出来的姿态之间的向量误差，可以用向量叉积来表示。这个叉积向量仍旧位于机体坐标系上的，而陀螺仪积分误差也是在机体坐标系上的，而且叉积的大小与陀螺仪的积分误差成正比，正好拿来纠正陀螺仪。由于陀螺仪是对机体直接积分，所以对陀螺仪的纠正量会直接体现在对机体坐标系的纠正。

 

//根据四元数转换为方向余弦矩阵中的第三列的三个元素

  vx = 2*(q1q3 - q0q2);

  vy = 2*(q0q1 + q2q3);

  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

//叉积误差

  ex = (ay*vz - az*vy) ;                             

  ey = (az*vx - ax*vz) ;

  ez = (ax*vy - ay*vx) ;

 

//用叉积误差来做PI修正陀螺仪零偏

  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;

 

最后用四元数更新算法对四元数进行更新：

//四元数更新算法 一阶库陇法

 // integrate quaternion rate and normalise   

  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;

  // normalise quaternion

  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

 

5、姿态控制

姿态控制可以用四元数进行控制或者欧拉角进行控制机，但欧拉角控制更为直观。

 

 

 

 

 

 

 

 

而对于PID控制概念的理解，我觉得别人举的例子很形象，很适合刚接触PID的人来理解：

 

PID 中的比例控制是最容易理解的，比例控制就是把角度的误差乘以一个常数作为输出驱动。假定我们有一个理想模型的电机，1V 电压的变化会带来小四轴1 度的角度改变。假定现在电机控制电压是5V,小四轴在某一轴上的偏角是5 度，目标角度是100 度。我们把当前的电压量定义为Vin,把输出控制量定义为Vout。假定P 等于0.2，那么比例控制的结果就是：

第一次：Vout=Vin+(100-5)*P=5V+19V=24V，得到电机电压是24V，对应的小四轴角度是24 度,距离目标角度的误差是100-24=76 度。

第二次：Vout=Vin+(100-24)*P=24V+15V=39V, 从而引起的角度是39 度。

我们看到，在这么的一个比例控制系统下，小四轴角度在慢慢地向目标角度靠近。

 

PID 中的积分控制就是把把所有角度误差相加起来，然后乘上一个常数作为输出驱动。在上述例子中，假定I=0.2,我们来看看比例和积分控制同时起作用下的系统反应。

第一次：Vout=Vin+(100-5)*P+(100-5)*I=5V+19V+19V=43V，这时候小四轴角度为43 度。由于第一次控制前的误差是100-5=95,第二次控制前的误差是100-43=57，所以积分结果是152。

第二次：Vout=Vin+(100-43)*P+((100-5)+(100-43)）*I=43V+11V+30V=84V, 这时候小四轴角度变为84 度。

第三次：Vout=Vin+(100-84)*P+((100-5)+(100-43)+（100-84）*I=84+3V+33V=120V。这时小四轴角度变为120 度。

我们看到，在增加了积分控制后，小四轴角度在快速向目标角度靠近。

 

PID 中的微分控制就是把角度的变化乘上一个常数来作为电机驱动输出。

 

在上述例子中，假定D=0.2, 我们来看看比例,积分和微分共同控制下的系统反应。假定第一次前，电机转速保持5 转，那么第一次前的角度变化为0。

第一次：Vout=Vin+(100-5)*P+(100-5)*I-（5-5）*D=5V+19V+19V-0V=43V，这时候小四轴角度为43 度。和上一次相比，角度从5 度变化到了43 度，所以小四周角度变化是43-5=38 度。

第二次：Vout=Vin+(100-43)*P+((100-5)+(100-43)）*I-（43-5）*D=43V+11V+30V-7V=77V, 这时候小四周角度77 度。

 

1、单独比例控制的时候，数据慢慢接近目标（图表中的红色线）

2、加入积分控制之后，数据快速接近目标（图表中的蓝色线）

3、微分控制起到抑制变化的作用。（图表中的绿色线）

 

 

在实际写PID代码时，微分控制部分的误差可以用陀螺仪的角速度代替，对于微型四轴飞行器积分控制可以不需要。

 

 

 

 

学习完相关的理论以后，参考网上开源的程序可以移植一个到我们的飞控板上。首先要确定桨安装是否正确，正确安装如下图

总之无论正桨还是反桨，风总之向下吹的。检验电机的顺序和桨是否安装正确，可以先随便给一个PID的值，然后烧写到板子上，左右前后倾斜，看电机的转速是否改变，改变的位置是否正确。例如上图，将四轴左倾，3和11号电机应该加速，9和10号电机应该减速。

 

当一切就绪后就可以调节PID了，当然需要一个方向先调，所以需要把四轴固定起来，使其只可以绕一个轴旋转。PID的整定虽说靠的是经验，但是别人的经验也是可以参考的。调节PID时，有人说先给P值，然后再给D值，但是我们这样调节起来发现如果只是给P值得话根本无法等幅翻滚，一直不停地绕圈，稍稍加了点D值还能出现小幅翻滚。后来看见别人在论坛中说，单个地调P或D根本无法小幅抖动，必须两个一起调。但是无论怎么样，我们最后还是没有调节出来。

 

 

这样我们就需要重新考虑我们的程序了，因为我们的程序基本参考的是匿名科技的，而上位机就是简单的串口调试助手安卓软件，通过蓝牙通信。但是我们在调试过程中发现来发送PID值经常发送失败，经常需要按很多次才能成功，而且经常丢数据。后来发现蓝牙数据接收是在串口1的中断里接收的，而程序中有许多中断，如果串口中断的优先级较低的话，接收数据时经常会发生其他高优先级中断。

因为匿名科技主要是用2.4G通信，再加上蓝牙没有较为合适的上位机，我们不清楚我们的硬件是否正常，所以我们决定移植匿名的程序。找到匿名的程序后参照我们电路的引脚将他的程序做相应的修改，只是匿名用的是硬件IIC，而我们硬件IIC的相关引脚并没有引出来，所以我们用了软件IIC代替了他的硬件IIC。然后熟悉一下他的通信协议做了相应的修改，就算移植成功了。其实匿名的上位机还是不错的，能看姿态也能看波形，还能控制，界面截图如下：

 

 

 

 

用上位机看了一下姿态后发现一切 ，然后下面就是PID调节了。因为以前弄了很久都没有弄出来，如果这次也失败的话基本会丧失信心的，应为毕竟这是别人好的程序。所以我们按捺不住激动与紧张的心情，用匿名默认的PID参数（3,0.002,0.06）来试飞，让我们激动不已的是，它竟然飞了起来，虽然飞的不稳，但是比我们先前所有的效果都比较好，这就让我们看到了一些希望，所以我们更有激情去调PID了，调节PID时，我们发现无论我们怎么调节PID，它都不能达到平稳，看波形也是在抖动，我们就怀疑：是不是不一定要完全平稳，根据试飞的效果来看，好像是这样的。在加上在逛论坛时发现有人说ROll与PITCH的PID参数一样，所以我们就用了一个我们认为好的参数来试飞（3，0.001,0.04）,f发现效果还是不错的，只不过飞行中可能还是有些偏向，所以飞的时候还是往一边飞。

 

在以后的几天里我们一直试飞，起初没有做好防护措施，在室内不停地乱撞和摔，以及控制的不好时，一直向上飞装上天花板厚直接摔下，以致摔坏了一架。就连单片机用来指示的灯都不亮了，更不谈电机转了。起初我们认为是单片机的问题，所以将单片机吹了下来重新换了一块新的，换上之后发现还不行，查找相关资料后发现可能是晶振摔坏了，为了验证我们吹下晶振接到51的板子上，发现确实坏了。为了做对比，我们找了直插的晶振，提前确定是好的，然后摔了几下，发现确实摔坏了。只可惜那一架再也挽救不回来了，因为焊吹的次数太多，不知道晶振为甚么还是不能起振，只好放弃那一架，幸亏我们还有一架，不然又得重新调试硬件。

 

 

 

 

 

在以后的试飞日子里主要发下如下问题：

1、飞的时候总是朝一个方向飞，而我们并未给期望角，有时起飞时就会遇到这种情况（但有时起飞时会很平稳起飞）；

猜测是PID没有调节完好使其平稳时有一定的倾斜。

2、飞的时候稍微有一点外力如撞上物品都会打破它的平衡，飞的时候给一个期望角后就会失去平衡。

猜想：飞行器完全起飞时，油门基本已接近最大值，而留给PID调节的油门很小，如果这是两侧电机转速理论相差很大，实际是办不到的，因为已达到油门上限。

*博客内容为网友个人发布，仅代表博主个人观点，如有侵权请联系工作人员删除。