四轴飞行器的进阶

通过博客记录分享自己的研究过程，这是第一篇，先写过去的总结，再写接下来的思路。

过去的总结

飞行控制器的选择与学习

选择TMS320F28335，主要是因为DSP运行速度快；集成FPU协处理，善于做大量浮点型数据运算；外设资源丰富，足够满足飞行器的需求。

底层驱动编写






底层驱动代码编写难点： TI官网为每个外设只提供一个或两个例程，很难满足实际需求；不提供库函数，只能用寄存器配置外设工作；中文资料少，只能看官方的数据手册。

底层驱动代码编写收获：英语阅读能力，资料搜索能力，调试方法的积累，对硬件协议底层的认识。

①4路PWM输出控制电机，周期400Hz，脉宽1000us-2000us变化

②捕获八路遥控接收脉冲，周期20Hz，脉宽1000us-2000us

③I2C读取MPU6050加速度计/陀螺仪

④SPI驱动NRF24L01

⑤定时器2.5ms程序运行周期

⑥SCI与上位机通信

⑦cmd文件编写

以上每个驱动调试至少1星期，主要方法是：将任务细化，逐步调试、验证；阿莫论坛、21IC论坛、TI技术支持论坛等各大论坛查找资料；读中文、英文手册理解每个外设工作过程，进而理解如何配置寄存器；寻找快捷调试方法。

例如SPI驱动NRF24L01。调试思路是将问题细化为：第一层SPI协议的编写(验证方式是先写NRF24L01一个寄存器，再读NRF24L01寄存器，看是写入和读出是否一样)和NRF24L01寄存器设置(验证方式是发送和接收的字节一致)；第二层若写进去和读出来的数据不一样，是写有问题还是读有问题？所以需要单独的调试写和读，读可以用读出来的数据与写进去的数据对比，但如何证明写没有问题？所以需要明白SPI协议是什么意思，SPI分四根线分别是使能、时钟、输出和输入，那么用示波器显示时钟和“写”的波形，看每个时钟对应的“写”高低电平进而确定是否和写入的字节一致；第三层若接收不到字节，是发送有问题还是接收有问题？所以需要先确保有一方一定没问题，NRF24L01能否能通信与什么芯片配置无关，至于怎么配置NRF24L01内部寄存器有关，而stm32已经有了NRF24L01的例程，故先调通stm32发stm32收，再DSP发stm32收，接着stm32发DSP收，最后DSP发DSP收。第四层单个字节调试好了再调试飞控放一直发，与上位机连接方一直收。

再例如I2C读取MPU6050姿态。调试思路将问题细化为：第一层I2C协议编写(验证方式是写一个字节再读一个字节，看写和读的字节是否一致)、MPU6050寄存器的设置(是否能读取正确的加速度和陀螺仪输出)、上位机观察MPU6050的姿态；第二层是用示波器单独调试I2C的写和读，此处难点在于TI例程只给出了16位字节的自检例程，需要看英文中文手册，理解每个寄存器的每个位的含义，对应的I2C配置的哪个具体过程，将程序改写成只写8位非自检模式；第三层因为MPU6050已经很常用了，网上查找MPU6050的例程，先用stm32配置成功，再用DSP模仿stm32配置，看读取的数据是否随模块转动而变化；第四层用已经编写好的SCI程序分别按照上位机规定的协议发送连续字节串，用已经编写好的IMU姿态解算代码解算模块当前姿态，看能否MPU6050模块转动，上位机的飞行模型是否一样转动。

CMD文件的编写：主要是因为用仿真器烧写程序是放入RAM中直接运行，RAM掉电数据就会丢失，所以实际工程中需要烧写进flash，但是flash的运行速度比RAM运行速度慢5倍左右，对于无人机这种实时控制系统是不可接受的。所以实际过程是先烧写程序进flash，在程序运行之前先把flash中的程序全部copy到RAM中，最后直接在RAM中运行。要完成上述过程，需要对DSP存储空间有个深入的认识。其实DSP将外设的寄存器全部保存在一块区域，不能改写，而其他RAM和flash都可以由用户规定代码、常数、静态变量和动态变量放在哪个地址，最后可以由.map文件查看每个变量每段段代码实际存储的地址。

姿态解算

这里姿态解算算法直接采用了网上开源的基于Mahony的互补滤波算法，算法原理是：加速度计提供长期稳定的线性加速度输出，陀螺仪提供短期的快速角速度输出，而姿态算法的原理即为对三轴角速度进行积分得到欧拉角，由于陀螺仪的漂移和偏置特性，导致积分误差不断增大，因此需要加速度计来实时校正。姿态解算所要做的工作便是描述载体坐标系与导航坐标系之间的相对关系，而描述这种关系的方法目前来说有四种：四元数、方向余弦矩阵 DCM、欧拉角和等效旋转矢量。其中以四元数的计算量小、无奇点问题而被广泛应用在工程领域中。



飞控硬件设计

先用DSP开发板作为飞控板，用杜邦线引出PWM(控制电机)、I2C接口(MPU6050读取姿态)、CAP(遥控接收器)、NRF24L01(备用以后无线传输数据)、SCI(串口发送数据给上位机)，将上述系统调试到转动飞机，上位机飞机模型可以一样的转动。将上述接口全部用排针引出，再查找资料设计DSP最小系统，30MHz晶振、JATG口、电源芯片(选择官方推荐的tps767d301)、复位电路(实验证明这个可以不要)、DSP bootload(烧写)模式选择电路，其他的还需要开关、滤波电容、led指示灯、AD采样电压的电路。以上均为飞控板上需要的东西，然后就是找封装、画原理图、PCB布局布线(参考淘宝一种最省空间最容易布线的DSP芯片布局方式：
DSP芯片的4角分别靠近飞控板的4边)。考虑到电源芯片的散热问题，特地将电源芯片底下的PCB铜箔裸露，以加大散热面积。最后大小为6×4cm。PCB板打样回来自己焊接，先焊接电源部分，以免焊接了其他部分电源电路出问题直接烧掉，电源没问题后再焊接DSP芯片，因为DSP芯片176个引脚，非常难焊，所以优先焊接DSP，预防在焊接的过程中焊接时间过长把芯片烧坏，焊接过程是断断续续的(方便散热)，并且在DSP芯片上盖上棉花撒上水，焊好DSP最小系统后就可以下程序测试DSP有没有工作。这个是第一版飞控，上面的传感器只有GY-86模块，可以用于飞行器飞行控制；第二版将加入GPS接口和光流接口，用于定高定点。最后经测试，第一版飞控第一块PCB就成功，目前调试的也是这块飞控板。



调试系统搭建

因为是中型四旋翼飞行器，为了安全，先选用烧烤模式调试，调试飞行器逻辑(即飞行器倒下的那边电机会转，另一边会停)。为了方便PID参数的调试，选择NRF24L01无线模块作为数据的“中转站”，需要实现如下信息流向功能：姿态数据流向DSP－＞NRF24L01(飞控板)－＞NRF24L01(stm32开发板) －＞stm32－＞上位机(串口)，PID参数流向上位机(串口)－＞stm32－＞ NRF24L01(stm32开发板) －＞NRF24L01(飞控板)
－＞DSP。这个数据流的搭建主要在于数据通信的协议不能乱，比如roll角度在DSP放在哪个数组里写入NRF24L01，在stm32通过NRF24L01接收在哪个数组里，最后以上位机哪部分协议发送给PC。

姿态控制

先是采用标准的PID控制方式，D项为误差的微分项，当误差定义为角度时，此时的D项即为角速度，而该类信息恰好由陀螺仪提供，因此这种有陀螺仪参与的单环 PID 控制器也是一种常规 PID 变型写法，但是飞行器无法稳定的飞行。

为了增加飞机的阻尼特性、提高飞机角度响应速度，便引入串级PID 控制器。该类串级 PID 控制器其实在很多国外开源飞控中已经被广泛应用，比如Crazyflie、MWC、APM、PX4等姿态控制器无一例外都是串级PID。该串级PID控制器分为外环-角度控制器和内环-角速度控制器，分别控制了飞机的角度和角速度，而后者角速度相当于角度的导数，也就是说增加了系统的控制阶次，使得飞机具有更强的阻尼效果。调试串级 PID 的过程也更加深入理解了参数对于系统的性能影响：外环P
直接影响飞机跟踪摇杆的速度，值越大，跟随性越强，响应越快；内环 P 项直接影响飞机姿态的自调整速度，过大的 P 会导致飞机自振荡；内环 I 项与外环 I 项一般而言只保留一个，因为积分环节会破坏系统稳定性，当飞机存在角度静差时，必须引入积分项消除静差；内环 D 项属于辅助项，过大的 D 项会导致 PID 输出毛刺过多，直接导致电机频换变速和高频振荡。前述的姿态控制只限于水平面内的角度控制，偏航角的控制与姿态控制一致，用串级 PID 也可以达到较好的效果。但是实际飞行过程中发现偏航角的响应速度并没有非常快，因此本人尝试利用前馈控制提高响应速度：即遥控器输入直连到执行机构输出，实验证明这种处理方法大幅度提升了偏航角的响应速度。



调试定高

定高常用的传感器为超声波模块和气压计。虽然超声波精度可达1-2cm，但只适应低空并且需要平面地形来反射超声波，所以选用气压计MS5611。调试主要分为3个步骤：①读气压传感器数据②与加速度的二重积分互补融合得到可用的高度数据③用于串级PID控制模式得到PWM。因为纯用加速度积分得到的高度长时间会漂移，所以需要气压计来补偿矫正，主要控制思想是二阶互补融合滤波：第一阶是对融合后的高度微分得到速度与加速度计的一重积分得到的速度融合，得到可用的速度，作为串级PID内环控制输入；第二阶将气压计得到的高度与一阶融合得到的速度进行积分再融合，得到可用的高度，作为串级PID的外环控制。主要程序框架已经搭建好了，主要问题是气压计会漂移，网上查找原因，可能是气压计受光线影响或者气压计没有被包起来，容易受周边环境影响，所以下一步调试将优化飞控板，将飞控板放在小盒子中，气压计上再盖上黑色海绵。

调试定点

可选用的传感器有GPS和光流。GPS适用于开阔的环境，光流适用于近地面。想要实现的效果是飞行器高空用GPS定点，降落用光流定点，实现飞行器的自主飞行。目前定点仍在调试，GPS选择ublox公司的NEO-M8N模块，已用串口可以正确读取GPS发回的经纬度等信息。光流选用的是pixhawk的光流模块，上面集成了超声波模块，光流处理算法内部已经写好了，只需要用I2C接口读取速度和高度。控制方式也是串级PID：对水平速度积分得到水平位移作为外环输入，外环输出是期望姿态，速度作为内环输入与外环输出做差得到控制量。最重要的是算出相对地面位移，再补偿回来。目前只获取了光流模块的速度、位移(速度的积分)，这个也需要在下一版的飞控中做好接口。

调试滤波

现程序中的MPU6050、MS5611、HMC5883L等传感器原始数据滤波主要是滑动窗口滤波，还没有对机械振动对传感器数据影响作频域分析。已做好相关的准备工作：将需要分析的数据通过匿名上位机保存成.CSV格式文件，运用MATLAB导入文件数据，并画出波形。进一步将使用MATLAB里 的FFT函数分析机械振动带来的是哪些频率的干扰，进而针对此干扰设计相应的滤波器。

现在的研究思路

研究思路是基于pix开源平台ardupilot代码继续开发，学习C++、Nuttx操作系统，了解pix开源平台结构和相关算法，再做最新研究机器视觉、模式识别等。因为时间问题，不能面面俱到，把pix架构搞清楚后选择想研究的部分深入。