四轴飞行器姿态解算算法设计与仿真

徐云川

【摘 要】为设计具备稳定姿态控制的四轴飞行器，对四轴飞行器飞行姿态表示、数学模型、姿态解算算法进行了研究。硬件上采用STM32作为主控芯片，以加速度计、陀螺仪为主体的惯性测量模块作为姿态测量传感器，测量出当前飞机的三轴加速度与三轴角速度并传送给主控芯片进行四元数姿态解算，求解出当前姿态角；软件上对传感器数据进行融合，运用MATLAB工具获取实时姿态，实现对姿态的辨识和控制。实验表明，四元数姿态解算算法对飞行器具有良好的控制效果，能快速精准响应飞行状态，满足飞行器执行特殊任务需要。

【关键词】姿态解算；STM32；惯性测量模块；四元数

【Abstract】In order to design a stabilized attitude control quadcopter，we research on attitude representation，mathematical model，attitude algorithm.In the side of hardware，use STM32 processor as master which execute quaternion attitude algorithm and IMU（Inertial Measurement Unit） which consist of accelerometer and gyroscope as attitude measurement sensor，the IMU measure current acceleration and angular velocity.In the side of software，using the MATLAB gets the real-time attitude from sensor. Experiments shows that，quaternion attitude algorithm can assurance the attitude of the UAV well，and it can meet the requirement of mission job.

【Key words】Attitude algorithm；STM32；Inertial Measurement Unit；Quaternion

四轴飞行器具有结构新颖、飞行方式独特、可垂直升降特点，近几年军用、民用均占据了越来越重要的地位。常见的四轴飞行器呈轴对称结构，主要由正交“十”字型机架、飞行控制板、传感器组、电机和旋翼组成，电机和螺旋桨装置是飞行器动力装置，改变电机的转速、带动螺旋桨产生升力，为飞行器提供动力；传感器组包括由陀螺仪和加速度计组成的惯性测量模块、磁力计、气压计、GPS模块、超声波模块；飞行控制是飞行器运动的核心，飞行控制板实时收集传感器组测得的数据，解算飞行姿态。

1 姿态解算流程

惯性测量模块测量当前飞机的三轴加速度和三轴角速度数据，转化为四维向量，经线性变换后输出，利用四元数与欧拉角的关系，将当前四元数转换为欧拉角Pitch、Roll、Yaw即解算得到当前飞机的姿态。如图1是姿态解算流程图。AD值是指MPU6050的陀螺仪和加速度值，三维陀螺仪值和三维加速度值，每个值为16位精度。AD值通过姿态解算算法得到飞行器当前的姿态（姿态使用四元数表示），然后将四元数转化为欧拉角，用于姿态控制算法（PID控制）中。

刚体姿态是描述刚体的刚体坐标系与参考坐标系的角位置关系，飞行器需实时获取当前姿态，根据需要操控动作，如保持平衡、实现翻滚等。飞行器姿态解算就是将刚体坐标系与地理坐标系转化，转化有很多种表示方法，如矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉角表示最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存飞行器的姿态。在需要控制的时候，会将四元数转化为欧拉角，然后输入到姿态控制算法中。

2 姿态表示方法

2.1 欧拉角表示

欧拉角是用来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角表示，即滚转角（Roll，？准）、俯仰角（Pitch，θ）、偏航角（Yaw，ψ）。根据欧拉旋转定力，通过三次旋转即可使刚体坐标旋转到与地理坐标系重合或者飞行器平衡姿态的位置，三次坐标变换的旋转矩阵的乘积即为欧拉角姿态矩阵。图2表示刚体三次坐标旋转得到欧拉角，图中OXYZ是参考坐标系，OXYZ是刚体坐标系。

3 姿态解算算法

姿态解算的目的是获取飞行器实时准确的姿态角度，惯性测量模块包含了加速度计和陀螺仪两种测量姿态角度的传感器[2]。加速度计静态性能良好，在飞行器静止状态下，能准确测量当前机体相对地理坐标系的姿态角度；陀螺仪有良好的动态性能，通过积分获取机体姿态角度相对初始角度的变化量，缺点是偏差会随着积分时间无限增大。飞行器要实现平衡可控飞行就需要从惯性测量模块获得具有噪声小、误差小、动态性能好的姿态信号，如前所述，飞行器姿态信号指其姿态角。加速度计输出三轴重力分量信号，陀螺仪输出绕三轴旋转的角速度信号。飞行器在飞行过程中，电机的高速旋转引起的机体震动会给陀螺仪、加速度计的输出信号带来噪声影响，实际的频谱分析图（如图3所示）也恰恰印证了电机震动带来噪声影响，大量噪声的引入会使控制系统对飞行器的姿态解算会出现很大的误差，为减小误差对控制系统的影响，需要对加速度计、陀螺仪测量数据进行融合滤波处理得到更加有效的飞行器姿态信号，弥补各自缺陷。

为获得比较精确的飞行姿态角度，对加速度计采集到的数据进行低通滤波去除高频噪声，对陀螺仪采集到的数据进行高通滤波消除积分漂移的影响，为保证姿态数据准确，需要对加速度计和陀螺仪设定量程和精度如表1所示。

采集得到的加速度计（ax，ay，az）和陀螺仪（gx，gy，yz）需通过四元数解算姿态数据。运用传感器数据融合思想，设计实时更新姿态四元数算法，具体计算方式是，初始化四元数向量，从传感器获得加速度和角速度，将加速度值规范化，用四元数求取重力分量，采用互补滤波方式，计算重力分量与加速度计测量误差，用所得误差修正陀螺仪，用修正后的陀螺仪值（gx，gy，gz）对四元数进行更新，由新的四元数和下一个周期采集得到的加速度计和陀螺仪的原始数据重复计算，循环获得陀螺仪的值，更新四元数，将更新后的四元数规范化，应用式（4）转化为欧拉角，得到飞行姿态。

四元数解算方法最终可以得到飞行器运动过程中的滚转角（Roll，？准）、俯仰角（Pitch，θ）、偏航角（Yaw，ψ），准确的姿态解算为四轴飞行器姿态的精准可知提供必要的条件。飞控板执行姿态解算程序，将采集到的原始加速度数据、陀螺仪数据等通过MATLAB串口采集到上位机，并实时通过波形显示数据，MATLAB实时姿态显示界面如图4所示，界面左侧是串口设置参数以及MPU6050原始数据，界面中间是加速度计和陀螺仪解算结果，界面右侧是解算得到的姿态角和姿态图。

在姿态图中，设定OXYZ是地理坐标系，OABC是机体坐标系，姿态角一旦有偏移可以立马反映到姿态图上。从MATLAB实时姿态显示数据曲线可以看到，通过一系列的滤波处理，可以实现对加速度计、陀螺仪原始数据的去噪声处理，同时通过四元数姿态解算可以得到用于飞行器姿态控制的有效姿态数据，通过姿态融合方法有效消除了加速度计的高频噪声和陀螺仪的积分漂移的影响。

4 结论

本文详细分析了四轴飞行器的飞行运动原理、飞行姿态的欧拉角和四元数姿态表示，建立了相应的数学模型，设计并制作了飞行控制系统硬件电路，包括电源供电电路、用于姿态检测的多传感器电路等，着重研究和分析了惯性测量模块MPU6050中加速度计和陀螺仪信号噪声，设计了一种姿态解算算法，并运用MATLAB工具辅助设计了互补滤波传感器融合飞行姿态实时显示系统。

【参考文献】

[1]何瑜.四轴飞行器控制系统设计及其姿态解算和控制算法研究[D].成都：电子科技大学，2015：14-16.

[2]刘浩蓬.植保四轴无人飞行器关键技术研究[D].武汉：华中农业大学，2015：29-31.

[3]冀亮，钱正洪，白茹.基于四元数的四轴无人机姿态的估计和控制[J].现代电子技术，2015，11（38）：112-116.

[4]秦永元.惯性导航[M].北京：科学出版社，2006.

[5]刘峰，吕强，王国胜，等.四轴飞行器姿态控制系统设计[J].计算机测量与控制，2011，19（3）：583-585.

[6]赵建军，陈滨，杨利斌.基于四元数的大地坐标转换算法及实现[J].计算机工程与应用，2013，49（4）：202-205.

[责任编辑：汤静]