飞行器控制：欧拉角与四元数对决，卡尔曼滤波器与互补滤波器的比较

一、在飞行器控制中，姿态计算是至关重要的一步。姿态计算的目标是确定飞行器相对于参考坐标系的姿态，通常以欧拉角（滚转、俯仰和偏航）或四元数的形式表示。

二、姿态计算的原理：姿态计算基于惯性测量单元（IMU），其中包括加速度计和陀螺仪。加速度计测量物体在三个轴向上的加速度，而陀螺仪测量物体绕三个轴向上的角速度。通过结合这些测量值，可以推导出飞行器的姿态。

三、姿态计算的常用方法：

1.互补滤波器（Complementary Filter）：一种简单且常用的姿态计算方法：基于加速度计和陀螺仪的数据，通过加权平均来结合它们的优点。具体而言，加速度计用于低频信号（如重力）的测量，而陀螺仪用于高频信号（如旋转）的测量。通过调整加速度计和陀螺仪的权重，可以获得相对稳定的姿态估计。

2.卡尔曼滤波器（Kalman Filter）：卡尔曼滤波器是一种更复杂但更精确的姿态估计方法。它基于状态估计和观测模型，并通过递归处理将测量数据与系统模型相结合。卡尔曼滤波器考虑了测量误差、系统噪声和先验信息，并通过最小化均方误差来优化姿态估计结果。这种方法对于高精度的姿态计算非常有效，但需要更复杂的数学推导和实现。

对于使用MPU6050作为传感器的实际案例，以下是一个简单的示例代码，演示如何使用MPU6050进行姿态计算：

import smbus

import math

# MPU6050的I2C地址

MPU6050_ADDR = 0x68

# 加速度计的灵敏度，根据MPU6050配置进行选择

ACCEL_SCALE = 16384.0

# 陀螺仪的灵敏度，根据MPU6050配置进行选择

GYRO_SCALE = 131.0

# 初始化I2C总线

bus = smbus.SMBus(1)

# 启动MPU6050传感器

bus.write_byte_data(MPU6050_ADDR, 0x6B, 0)

# 读取加速度计原始数据

defread_accel_data(addr):

raw_data = bus.read_i2c_block_data(MPU6050_ADDR, addr, 6)

accel_x = (raw_data[0] << 8) + raw_data[1]

accel_y = (raw_data[2] << 8) + raw_data[3]

accel_z = (raw_data[4] << 8) + raw_data[5]

return (accel_x, accel_y, accel_z)

# 读取陀螺仪原始数据

defread_gyro_data(addr):

raw_data = bus.read_i2c_block_data(MPU6050_ADDR, addr, 6)

gyro_x = (raw_data[0] << 8) + raw_data[1]

gyro_y = (raw_data[2] << 8) + raw_data[3]

gyro_z = (raw_data[4] << 8) + raw_data[5]

return (gyro_x, gyro_y, gyro_z)

# 计算加速度计的姿态

defcalculate_accel_angles(accel_x, accel_y, accel_z):

roll = math.atan2(accel_y, accel_z) * 180 / math.pi

pitch = math.atan2(-accel_x, math.sqrt(accel_y * accel_y + accel_z * accel_z)) * 180 / math.pi

return (roll, pitch)

# 计算陀螺仪的姿态

defcalculate_gyro_angles(gyro_x, gyro_y, gyro_z, dt):

roll = gyro_x * dt

pitch = gyro_y * dt

yaw = gyro_z * dt

return (roll, pitch, yaw)

# 主循环

whileTrue:

# 读取加速度计数据

accel_data = read_accel_data(0x3B)

accel_x = accel_data[0] / ACCEL_SCALE

accel_y = accel_data[1] / ACCEL_SCALE

accel_z = accel_data[2] / ACCEL_SCALE

# 读取陀螺仪数据

gyro_data = read_gyro_data(0x43)

gyro_x = gyro_data[0] / GYRO_SCALE

gyro_y = gyro_data[1] / GYRO_SCALE

gyro_z = gyro_data[2] / GYRO_SCALE

# 计算加速度计的姿态

accel_angles = calculate_accel_angles(accel_x, accel_y, accel_z)

# 计算陀螺仪的姿态

gyro_angles = calculate_gyro_angles(gyro_x, gyro_y, gyro_z, dt)

# 结合加速度计和陀螺仪的姿态，使用互补滤波器或其他方法进行姿态计算

# 输出姿态信息

print("Roll: %.2f" % roll)

print("Pitch: %.2f" % pitch)

print("Yaw: %.2f" % yaw)

以上代码演示了如何使用 MPU6050读取加速度计和陀螺仪的原始数据，并使用简单的角度计算函数来计算飞行器的姿态。你可以根据需要结合互补滤波器等算法来进一步优化姿态计算的精度和稳定性。请注意：这只是一个简化的示例，实际应用中可能需要进行更多的校准、滤波和算法优化，以获得准确和稳定的姿态估计。同时，还需要考虑飞行器的动力学模型和控制算法，以实现自动控制和稳定飞行。

四、四元数是一种数学工具，用于表示旋转姿态。它是一个四维向量，包含一个实部和三个虚部。四元数的形式通常为q = w + xi + yj + zk，其中w是实部，(x, y, z)是虚部的三个分量。

在飞行器自动控制中，常用的姿态表示方式之一是四元数。飞行器的姿态估计和控制算法可以使用四元数进行旋转计算、姿态插值和传感器融合。需要注意的是：四元数的使用需要了解其代数运算规则和旋转转换方法。在实际应用中，可能需要使用四元数库或数学库提供的函数来进行四元数操作，以简化实现过程。

五、从MPU6050计算四元数

MPU6050是一个常用的IMU，包含了加速度计和陀螺仪，但本身并不直接提供四元数的输出。通过结合加速度计和陀螺仪的数据，并使用相应的算法，可以计算出来四元数来表示飞行器的姿态。以下是一个基于MPU6050的姿态计算示例，使用互补滤波器来计算四元数：

import math

from mpu6050 import MPU6050

# 初始化MPU6050传感器

mpu = MPU6050()

# 互补滤波器参数

accel_weight = 0.98

gyro_weight = 0.02

# 初始四元数

quaternion = [1.0, 0.0, 0.0, 0.0]

# 主循环

whileTrue:

# 读取加速度计和陀螺仪数据

accel_data = mpu.get_acceleration()

gyro_data = mpu.get_rotation()

# 将加速度计数据转换为重力向量

accel_vector = [accel_data['x'], accel_data['y'], accel_data['z']]

accel_magnitude = math.sqrt(sum([a * a for a in accel_vector]))

# 归一化加速度向量

normalized_accel = [a / accel_magnitude for a in accel_vector]

# 计算重力向量对应的四元数

gravity_quaternion = [0.0, normalized_accel[0], normalized_accel[1], normalized_accel[2]]

# 将陀螺仪数据转换为角速度向量

gyro_vector = [gyro_data['x'], gyro_data['y'], gyro_data['z']]

# 计算四元数的变化率

delta_quaternion = [0.5 * dt * w for w in gyro_vector]

# 更新四元数

quaternion = [

quaternion[0] + delta_quaternion[0],

quaternion[1] + delta_quaternion[1],

quaternion[2] + delta_quaternion[2],

quaternion[3] + delta_quaternion[3]

]

# 归一化四元数

quaternion_magnitude = math.sqrt(sum([q * q for q in quaternion]))

quaternion = [q / quaternion_magnitude for q in quaternion]

# 使用互补滤波器融合加速度计和陀螺仪的姿态估计

quaternion = [

accel_weight * quaternion[i] + gyro_weight * gravity_quaternion[i]

for i in range(4)

]

# 输出四元数

print("Quaternion: ", quaternion)

以上代码示例演示了如何从MPU6050读取加速度计和陀螺仪数据，并使用互补滤波器计算四元数来表示飞行器的姿态。注意：以上代码仅是一个“show me the code”的原理示例，实际应用中需要根据具体的编程环境和MPU6050库进行适当的调整。

六、关于多旋翼无人机姿态解算算法：开源PX4源代码里面默认使用的是基于mahony的互补滤波算法（explicit complement filter）进行姿态解算的；还有一套备用姿态解算算法是基于kalman的EKF（extended kalman filter）；此外，还有一套基于madgwick的梯度下降算法（gradient descent）。

1.三套不同的算法各有优势可有优缺点

三套算法都可以使用，很多公司偏向于EKF，所以对EKF的要求还是比较高的。小四轴用的比较多的还是互补滤波算法，尤其是大学生参加比赛也好，个人爱好也好，基于STM32自己制作小四轴的大多基于这套。梯度下降用的属于最少的了。

1.1 mahony算法

Mahony算法即互补滤波算法，通过PID反馈控制器把误差量反馈补偿修正陀螺仪的误差。Mahony的互补滤波算法是基于IMU系统的，即陀螺仪和加速度计，过程中不涉及磁力计的修正。而我们使用过程中一般都是AHRS系统（即MARG：Magnetic、Angular Rate、Gravity），这个就需要寻求另外的算法或者基于mahony算法进行改进。PX4源代码中就是使用的改进的mahony算法，即加入了磁力计数据和GPS数据进行姿态解算。该算法是最简单和最容易实现的算法，PX4姿态解算默认的就是使用的这个。但是滤波器截止频率不好确定且与采样频率有关，Kp大截止频率就大。

关于这一块，现在研究的比较多就是如何实现自适应调参。固定的参数不能获得所有情况下的最优运动姿态角，可以设计参数可调的自适应算法在不同运动状态下进行调节参数的大小。其参数调节规则为：正常运动状态情况下，Kp和Ki值取为系统初始化值；当运动体具有较大运动加速度或姿态变化剧烈时，应选择较小的Kp值（可取其初始化值的0.1倍），而Ki值应在同一数量级内适当取大一点。具体取值需根据实际应用系统选取。

Mahony论文：

《AComplementary Filter for Attitude Estimation of a Fixed-Wing UAV》和《NonlinearComplementary Filters on the Special Orthogonal Group》

Mahony团队发表了太多的论文了，有兴趣的可以到researchgate查看：https://www.researchgate.net/profile/Robert_Mahony/publications/3

Fig.1 Explicit Complementary Filter

1.2 EKF算法

EKF俗称扩展卡尔曼滤波算法，是出了名的计算量大，因为算法实现过程是各种矩阵运算，对处理器的运算速度和精度要求很高对MCU的主频要求比较大，再加上系统的非线性，难以建立稳定可靠的状态方程以及合适的过程噪声协方差阵Q以及测量噪声协方差阵R。但是理解了KF以后，对整合滤波过程还是比较清晰的，很多公司都要求必须会EKF算法，优势就是在于处理过程中的预测和修正，其中一点就是可以估计陀螺仪的bias；在算法的计算处理过程中每一次都在不停的修正K和bias，相当于动态自适应算法了，主要就是调好Q、R。所以得把EKF搞懂，虽然比较难，但是必须知难而上。

相关论文：

《A Double-Stage Kalman Filter for Orientation TrackingWith an Integrated Processor in 9-D IMU》

Fig.2 Extended Kalman Filter

1.3 梯度下降法

该算法的核心思想是将用加速度计和磁力计通过梯度下降法得到的姿态四元数与由陀螺仪积分得到的姿态，进行线性融合，得到最优的姿态。在算法实现过程中需要了解目标函数的建立，梯度下降法是根据目标函数求取误差向量的最小值的。该算法首先是只用陀螺仪数据根据一阶毕卡更新四元数，然后才是梯度下降法对加速度数据和磁力计数据进行处理以便求出最小的误差四元数，最后根据权重值（线性）把两者融合到一起得到最终的姿态四元数。使用梯度下降法就是对加速度数据和磁力计数据和相应参考向量求取误差量，这一部分和mahony_base算法类似，mahony_base是利用向量的叉乘求取量测值和参考向量的误差的，而梯度下降法是根据反梯度方向迭代求取最小误差的（理论基础就是构造目标函数使用梯度下降法求取误差向量的最小值）。过程中最重要就是参数β需要通过实验进行确定gyro的测量噪声（measurement error）。

该算法适用于IMU和AHRS系统，其明显特点就是计算量小（computationally inexpensive），低频有效性（比如10Hz）。Madgwick说该算法最后的精度比kalman_base算法要好，没实际测试过；基于梯度下降的数据融合算法能够显著降低对处理器速度的要求。

Madgwick论文：

《An efficient orientation filter for inertial and inertial magneticsensor arrays》和《Estimation of IMU and MARG orientation using agradient descent algorithm》。

梯度wiki：

https://en.wikipedia.org/wiki/Gradient_descent

算法代码试下：

http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

Fig.3 Complete Orientation Filter

2.结论

上述三种算法都是以陀螺仪采集的角速度数据为主要控制量，通过不同的算法处理加速度数据和磁力计数据对由陀螺仪估计的姿态进行修正补偿。mahony_base算法是通过构造PID反馈控制器实现对由陀螺仪估计的的姿态修正的，主要过程是基于重力或者重力和磁力参考向量修正陀螺仪数据，最后根据一阶毕卡（或多阶）更新姿态四元数；EKF是通过量测方程实现对由陀螺仪估计的姿态角修正的；Gradient descent是通过两种算法的融合来进行姿态解算的，首先是只用陀螺仪数据根据一阶毕卡更新四元数，然后才是梯度下降法对加速度数据和磁力计数据进行处理以便求出最小的误差四元数，最后根据权重值把两者融合到一起得到最终的姿态四元数。若kalman算法是自适应的，解算精度就比梯度下降算法强了。

综上所述，姿态解算不过就是利用陀螺仪的高动态性能估计实时的姿态，但是由于陀螺仪固有的缺陷问题，导致随着时间的漂移实时的姿态估计的越来越不准确，所以找一个也能估计姿态的sensor来修正一下：要找一个可以和陀螺仪固有缺陷相互补的sensor，那就是加速计了，无漂移，低频特性好。那么为何不直接只用加速度计进行姿态估计呢？原因就在于它的高频特性太烂，响应时间久，比如想实现一个3°角度变化量，多旋翼飞行速度很快，加速计响应时间来个上百ms的话，飞机早掉地上或升天了。这个也是为什么PX4里面对姿态控制部分分为两级PID控制的原因所在吧，内环控制角速度响应速度极快，外环控制角度响应速度相对较慢，但是有内环在接近实时的PID反馈修正，所以就不怕飞机升天了。