姿态解算

关键字: IMU 数据融合.

姿态解算是指根据加速度计/陀螺仪/磁力计等的数据融合在一起, 得出飞行器的空中姿态.

\[ 加速度计 \rightarrow 加速度 \xrightarrow{积分} 速度 \xrightarrow{积分} 位置 \\ 陀螺仪 \rightarrow 角速度 \xrightarrow{积分} 角度 \]

!!! info 本文使用右手坐标系, Y 表示垂直方向.

姿态角

陀螺仪: 可以得到相对的姿态欧拉角, 但存在累计误差和零偏误差.
磁力计: 可以得到绝对的 yaw 角.
加速度计: 可以得到绝对的 pitch 和 roll 角.

当加速度计只受到重力作用的时候, 由于已知重力矢量的方向, 可以进一步推出加速度计的姿态角.

位置

加速度计: 可以得到相对的 X, Y, Z 坐标, 但存在累计误差.
气压计: 只能获取绝对的 Z 坐标.
GPS: 可以得到绝对的 X, Y, Z 坐标, 但 Z 坐标的精度较低, 且存在无 GPS 信号的情况.

IMU

6 轴 IMU: 三轴加速度计和三轴陀螺仪.
9 轴 IMU: 在 6 轴 IMU 的基础上添加了三轴磁力计.
10 轴 IMU: 在 9 轴 IMU 的基础上添加了气压计.

MPU-6050

MPU-6050 是 InvenSense 公司推出的一款集成了 3 轴陀螺仪和 3 轴加速度计的 6 轴运动处理器, 它还具有一个第二 IIC 接口, 可以连接外部磁力计, 结合内置的数字运动处理器(DMP, Digital Motion Processor), 能够在硬件层面对传感数据进行融合和处理, 并通过主 IIC 接口向 MCU 输出 9 轴姿态融合演算数据.

控制分配

电机的旋转方向与 yaw 的调节有关, pitch 和 roll 与电机相对与质心的位置有关.

绕 Z 轴正转 (yaw): 绕 Z 轴反转 (顺时针) 的电机加速, 绕 Z 轴正转 (逆时针) 的电机减速, 提供一个绕 Z 轴正转的力矩.
绕 Y 轴正转 (pitch): 位于 -X 方向 (后方) 的电机加速, 位于 X 方向 (前方) 的电机减速, 提供一个绕 Y 轴正转的力矩.
绕 X 轴正转 (roll): 位于 Y 方向 (左方) 的电机加速, 位于 -Y 方向 (右方) 的电机减速, 提供一个绕 X 轴正转的力矩.

+ 型:

前方和后方的电机顺时针旋转, 左方和右方的电机逆时针旋转.

front_speed = throttle + yaw - pitch;
back_speed  = throttle + yaw + pitch;
right_speed = throttle - yaw - roll;
left_speed  = throttle - yaw + roll;

x 型:

左前和右后的电机顺时针旋转, 右前和左后的电机逆时针旋转.

在进行俯仰和滚转时需要使用两个电机模拟 + 型一个电机的效果, 所以需要乘以一个系数.

假设 front_left 和 back_left 的方向与 Y 方向之间的夹角为 45° 且中心对称:

constexpr float scale = 0.70710678118654752440084436210485f; // cos(45°)
pitch *= scale;
roll  *= scale;

front_right_speed = throttle - yaw - pitch - roll;
front_left_speed  = throttle + yaw - pitch + roll;
back_right_speed  = throttle + yaw + pitch - roll;
back_left_speed   = throttle - yaw + pitch + roll;

对于非 + 型的多旋翼无人机, 需要将旋转的调整量分配到不同的电机上. 比如 + 型需要使左侧电机产生一个升力 F, x 型应该要对这个力进行分解, 使多个电机产生的合力也等于 F.

动态调整节流阀上限, 为姿态控制留出余量.

throttle_max = motor_speed_max - std::max({yaw - pitch + roll, yaw + pitch - roll, -yaw + pitch + roll});

可同时支持多种多旋翼, 甚至允许电机位置和数量在运行时发生变化:

constexpr size_t motor_count = 4;

const EulerAngles[motor_count] scalings = {
    // yaw pitch                                 roll
    {-1.0, -0.70710678118654752440084436210485f, -0.70710678118654752440084436210485f}, // front right
    { 1.0, -0.70710678118654752440084436210485f,  0.70710678118654752440084436210485f}, // front left
    { 1.0,  0.70710678118654752440084436210485f, -0.70710678118654752440084436210485f}, // back right
    {-1.0,  0.70710678118654752440084436210485f,  0.70710678118654752440084436210485f}, // back left
};

float control_speed[motor_count]; // 调节姿态的控制量
float throttle_max = std::numeric_limits<float>::max();
for(uint8_t i = 0; i < motors.size(); i++)
{
    control_speed[i] = scalings[i].yaw * yaw + scalings[i].pitch * pitch + scalings[i].roll * roll;
    throttle_max = std::max(motor_speed_max - control_speed[i], throttle_max);
}

const float throttle_limited = std::min(throttle, throttle_max);
for(uint8_t i = 0; i < motors.size(); i++)
    motor_speed[i] = throttle_limited + control_speed[i];

滤波

在飞行器飞行的过程中, 加速度计由于震动会产生大量噪声. 需要通过滤波算法对传感器的输出进行处理, 减少噪声对姿态评估的影响.

平均值滤波 (average filtering).
均值滤波 (mean filtering).
卡尔曼滤波 (Kalman filtering).

框架 (Frame).
电机 (Motors).
桨叶 (Propellers).
电子调速器 (Electric Speed Controller, ESC), 电调.
配电板 (Power Distribution Board, PDB).
飞行控制系统 (Flight Controller), 飞控.
电池 (Battery).
信号接收器 (Reciever).
摄像机 (Camera).
视频发射机 (Video Transmitter, VTX), 图传.
传感器 (Sensors).

姿态解算

姿态角

位置

IMU

MPU-6050

控制分配

滤波

评论