四轴飞行器姿态控制算法
谢兆清
【摘要】 近年四轴飞行器发展相当迅速.越来越多的人开始关注并研究四轴飞行器。本文将分析一种常见的四轴飞行器姿态解算方法并用matlab实现姿态显示。希望能给学习四轴飞行器的朋友们带来帮助。
【关键字】 四轴飞行器 姿态显示
一、matlab串口初始化
matlab串口采用回调函数，类似于中断方式。但是mtalb的串口没有多线程，而我们在中断方式里面需要进行波形显示，四元素旋转等各种数据操作，这就导致我们的数据平率不能很高。matlab采用符号‘！为结束符，碰到这个符号matlab就会调用回调函数，中间的数据都是逗号隔开的，數据顺序一次为accex，accey，accez，temp，gyrox，gyroy，gyroz，c pu_major，q0，q1，q2，q3发送，数据通过sprintf进行格式化，然后通过rt_kprintf函数发送。temp是MPU6050读出的温度数，cpu_major是CPU使用率，q0，q1，q2，q3分别对应四元素的四个参数，q0是实数，其他分别对应i，j，k的参数。
matlab数据处理：收到数据后，标准的处理方式是用matlab的regexp函数，用正则表达式将数据读取出来.将数据中的空格、结束符感叹号、间隔符逗号，去掉后调用str2num函数将字符串转换为数字就行。
二、四元素表示的坐标变化和四元素与欧拉角的关系
当获取MPU6050的数据后，一是加速度，一是角速度。有了加速度我们就可以算出pitch和roll。因为飞机是动态的，飞机动的过程中自己本身机体也会产生加速度，所以我们要分辨地球重力产生的重力加速度和机体的加速度才能算出集体的pitch和roll。加速度在震动情况下输出的值是波动很大的，而陀螺在动态下输出就好很多。陀螺输出的是角速度，我们用时间乘以角速度就可以的得到角度，每次积分的角度和上次的角度相加，就可以达到集体的xyz三个轴的角度。虽然积分的时候可以使用龙格-库塔积分方法，但是角速度积分仍然存在误差，这时我们要短期相信陀螺仪，然后长期相信加速度.其中w，x，y，z就是四元素的四个元素，W为实数部分，xyz对应ijk的三个变量。四元素的微分方程：
可以写为
三、matlab界面姿态显示
添加控制四个电机的pwm数值现实和pid控制器中yaw pitch roll目标值的显示， PID的控制效果和对齐就可以显示出来了。如图1。
左边中间两个方框，左边那个33.76是mpu6050读出来的温度数值，右边的7是代表CPU使用率为7%。
四 、结束语
飞控数据采集线程和数据处理线程的安排，类似于生产者与消费者的关系。生产者只管采集数据，消费者消费数据.我们有两个变量互斥量和计数来同步两个任务。生产者只管统计数据.但是需要做些处理，拿到数据后对数据均值滤波，就是除以统计的次数，同时将原来的数值清0。互斥量的存在，可以保证数据操作的同步和安全性。
参 考 文 献
[1] 王俊； 鲁晓天.对四轴飞行器基于姿态算法的分析与实现 [J].河南科技，2015-01-25
[2] 徐云川.四轴飞行器姿态解算算法设计与仿真 [J].科技视界，2016-08-15.
