SH3001姿态解算

姿态角，机体坐标系相对于参考坐标系的角度

旋转的非交换性

姿态角指的是载体坐标系与参考系之间的关系，

欧拉角是描述物体与某个轴(X,Y,Z)的关系，欧拉角属于姿态角的一种常见描述形式，另外两种描述形式就是四元数和和旋转矩阵

飞控绕各个轴旋转的角度相同但顺序不同，最终的姿态通常是不同的。这是因为旋转顺序对最终姿态有重要影响。这种现象在数学上被称为“旋转的非交换性”（non-commutativity of rotations）。

旋转的非交换性

在三维空间中，绕不同轴的旋转顺序会影响最终的姿态。具体来说，绕X轴、Y轴和Z轴的旋转顺序不同，会导致不同的最终姿态。这可以通过旋转矩阵或四元数来直观地理解。

示例说明

假设我们有一个初始姿态为单位矩阵 II 的物体，我们需要绕X轴、Y轴和Z轴各旋转 θθ 角度。我们来看看不同旋转顺序的结果。

1. 绕X轴 → 绕Y轴 → 绕Z轴

绕X轴旋转 θθ 角度：

Rx(θ)=(1000cos⁡(θ)−sin⁡(θ)0sin⁡(θ)cos⁡(θ))Rx(θ)=1000cos(θ)sin(θ)0−sin(θ)cos(θ)

绕Y轴旋转 θθ 角度：

Ry(θ)=(cos⁡(θ)0sin⁡(θ)010−sin⁡(θ)0cos⁡(θ))Ry(θ)=cos(θ)0−sin(θ)010sin(θ)0cos(θ)

绕Z轴旋转 θθ 角度：

Rz(θ)=(cos⁡(θ)−sin⁡(θ)0sin⁡(θ)cos⁡(θ)0001)Rz(θ)=cos(θ)sin(θ)0−sin(θ)cos(θ)0001

最终的旋转矩阵为：

R=Rz(θ)⋅Ry(θ)⋅Rx(θ)R=Rz(θ)⋅Ry(θ)⋅Rx(θ)

2. 绕Y轴 → 绕Z轴 → 绕X轴

绕Y轴旋转 θθ 角度：

Ry(θ)=(cos⁡(θ)0sin⁡(θ)010−sin⁡(θ)0cos⁡(θ))Ry(θ)=cos(θ)0−sin(θ)010sin(θ)0cos(θ)

绕Z轴旋转 θθ 角度：

Rz(θ)=(cos⁡(θ)−sin⁡(θ)0sin⁡(θ)cos⁡(θ)0001)Rz(θ)=cos(θ)sin(θ)0−sin(θ)cos(θ)0001

绕X轴旋转 θθ 角度：

Rx(θ)=(1000cos⁡(θ)−sin⁡(θ)0sin⁡(θ)cos⁡(θ))Rx(θ)=1000cos(θ)sin(θ)0−sin(θ)cos(θ)

最终的旋转矩阵为：

R′=Rx(θ)⋅Rz(θ)⋅Ry(θ)R′=Rx(θ)⋅Rz(θ)⋅Ry(θ)

比较结果

尽管每个单独的旋转矩阵都是相同的，但由于矩阵乘法的非交换性，RR 和 R′R′ 通常是不同的。这意味着最终的姿态也会不同。

四元数表示

同样的道理也适用于四元数表示。四元数 qq 表示旋转时，旋转顺序的不同也会导致不同的最终姿态。例如：

绕X轴旋转 θθ 角度：

qx=cos⁡(θ/2)+sin⁡(θ/2)iqx=cos(θ/2)+sin(θ/2)i

绕Y轴旋转 θθ 角度：

qy=cos⁡(θ/2)+sin⁡(θ/2)jqy=cos(θ/2)+sin(θ/2)j

绕Z轴旋转 θθ 角度：

qz=cos⁡(θ/2)+sin⁡(θ/2)kqz=cos(θ/2)+sin(θ/2)k

最终的四元数为：

q=qz⋅qy⋅qxq=qz⋅qy⋅qx

或

q′=qx⋅qz⋅qyq′=qx⋅qz⋅qy

这两个四元数 qq 和 q′q′ 也是不同的，因此最终的姿态也会不同。

结论

飞控绕各个轴旋转的角度相同但顺序不同，最终的姿态通常是不同的。这是因为旋转的非交换性，即旋转矩阵或四元数的乘法顺序不同会导致不同的结果。因此，在设计和实现飞控系统时，必须明确旋转顺序，以确保达到预期的姿态。

atan的，反正切，atan(0.75) = 37度，常见三角形状勾3股4玄5

矩阵相乘顺序导致结果不同

根据方向余弦矩阵得到roll（横滚角）pitch（俯仰角）解算不出来偏航角

单纯用加速度计有缺陷

用陀螺仪检测姿态角

角速度进行积分得到角度，角速度有误差，一直积分会放大误差

角度融合

计算角度：

角速度积分-》积分漂移（低频误差）

加速度正交-》震动误差（高频误差）

所以有了互补滤波（向量外积补偿）

PI比例，积分项进行陀螺仪误差补偿

姿态的表现形式之一四元数和旋转矩阵欧拉角表示姿态会出现万向节死锁

万向节死锁

万向节死锁（Gimbal Lock）是使用欧拉角表示旋转时可能出现的一种问题。当两个旋转轴对齐时，系统会失去一个自由度，导致无法唯一确定某些姿态。这个问题在航空、航天和机器人等领域中尤其重要，因为它们依赖于精确的姿态控制。

万向节死锁的成因

万向节死锁发生在使用欧拉角表示旋转时，当两个旋转轴对齐时，系统会失去一个自由度。具体来说，当俯仰角（Pitch Angle） θθ 接近 ±90∘±90∘（即 θ=±π2θ=±2π）时，滚转角（Roll Angle） ϕϕ 和偏航角（Yaw Angle） ψψ 的旋转轴会重合，导致无法唯一确定某些姿态。

图解说明

假设我们使用 Z-Y-X 欧拉角顺序（航向-俯仰-滚转）来描述旋转：

初始状态：

物体的初始姿态为 OXYZOXYZ。

绕Z轴旋转 ψψ 角度：

物体绕Z轴旋转 ψψ 角度，新的姿态为 ( OX'Y'Z' \）。

绕Y'轴旋转 θθ 角度：

物体绕新的Y'轴旋转 θθ 角度，新的姿态为 ( OX''Y''Z'' \）。

绕X''轴旋转 ϕϕ 角度：

物体绕新的X''轴旋转 ϕϕ 触度，最终姿态为 ( OX'''Y'''Z''' \）。

当 θ=±π2θ=±2π 时，新的Y'轴与Z轴对齐，此时X''轴和Z轴重合，导致无法区分滚转角 ϕϕ 和偏航角 ψψ 的旋转效果。

数学解释

使用旋转矩阵表示 Z-Y-X 欧拉角顺序的旋转：

解决方法

使用四元数（Quaternions）：

四元数是一种用于表示旋转的数学工具，由四个分量组成：一个实部和三个虚部。
四元数可以避免欧拉角中的奇异点问题（如万向节死锁）。
四元数通常表示为 q=[w,x,y,z]q=[w,x,y,z]，其中 ww 是实部，x,y,zx,y,z 是虚部。

使用旋转矩阵（Rotation Matrices）：

旋转矩阵是一个3x3的矩阵，用于表示从一个坐标系到另一个坐标系的旋转。
旋转矩阵可以直接用于坐标变换，且易于理解和实现。

示例代码

以下是一个简单的示例代码，展示如何使用四元数表示旋转并避免万向节死锁：
python

深色版本
import numpy as np

def euler_to_quaternion(roll, pitch, yaw):
    cy = np.cos(yaw * 0.5)
    sy = np.sin(yaw * 0.5)
    cp = np.cos(pitch * 0.5)
    sp = np.sin(pitch * 0.5)
    cr = np.cos(roll * 0.5)
    sr = np.sin(roll * 0.5)
    
    w = cr * cp * cy + sr * sp * sy
    x = sr * cp * cy - cr * sp * sy
    y = cr * sp * cy + sr * cp * sy
    z = cr * cp * sy - sr * sp * cy
    
    return np.array([w, x, y, z])

def quaternion_to_rotation_matrix(q):
    w, x, y, z = q
    return np.array([
        [1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
        [2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w],
        [2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]
    ])

# 示例：绕Z轴旋转45度，绕Y轴旋转90度，绕X轴旋转30度
roll = np.radians(30)
pitch = np.radians(90)
yaw = np.radians(45)

# 将欧拉角转换为四元数
q = euler_to_quaternion(roll, pitch, yaw)

# 将四元数转换为旋转矩阵
R = quaternion_to_rotation_matrix(q)

print("四元数:", q)
print("旋转矩阵:\n", R)
总结

万向节死锁：当使用欧拉角表示旋转时，如果俯仰角 θθ 接近 ±90∘±90∘，系统会失去一个自由度，导致无法唯一确定某些姿态。
解决方法：使用四元数或旋转矩阵可以避免万向节死锁问题，确保姿态的唯一性和准确性。

2.姿态融合理论和代码部分

四元数求得欧拉角

载体的姿态解算需要实时更新四元数，通过构建四元数和时间的微分方程

得到这4个q就能得到欧拉角

运用一阶龙格库塔法求解微分方程

代入矩阵，求解

想要数值更精确，就需要更高阶次的龙格库塔法

校准

理想情况， XY轴输出的应该是0，而Z轴输出的应该是1g，所以

陀螺仪水平放置的时候各轴输出应该也是为0，所以也需要对陀螺仪进行三维校准

校准方法：

开发板水平静止放置的时候，采集一两百次求个平均作为校准值保存起来，然后工作状态下各轴的输出就是采集的数据减去各轴的校准值

第三行和代码对应

误差

加速度计得到实际重力加速度和理论重力加速度分量沿X,Y,Z轴的分量的误差

误差的积分的解算对应中间的代码

所以补偿误差通过构建PI(比例-积分)控制器来控制补偿值的大小与精度补偿到陀螺仪代码如下

对于四元数的求解，采用的是二阶龙格库塔法

网址详解互补滤波四元数中向量叉积与陀螺仪角速度补偿问题（Mahony算法）_陀螺仪陀螺仪互补滤波程序c语言-CSDN博客

采用的是一阶龙格库塔法，如下

通过得到的姿态修正融合得到的四元数来求解欧拉角

由于加速度的数据容易受电机等高频干扰，所以这里进行了一阶低通滤波

加权

加权（Weighting） 是一种在数据分析和处理中常用的技术，通过赋予不同数据点不同的权重来反映它们在整体中的重要性或可信度。加权可以用于各种场景，如统计分析、机器学习、信号处理和决策制定等。

加权的基本概念

权重（Weight）：

权重是一个数值，表示某个数据点在整体中的相对重要性或可信度。
权重通常是非负的，且所有权重之和通常为1（归一化）。

加权的应用场景

统计分析：

加权平均：在计算平均值时，某些数据点可能比其他数据点更重要。例如，计算学生的综合成绩时，不同科目的权重可能不同。
加权回归：在回归分析中，某些数据点的误差可能更大，可以赋予这些数据点较小的权重。

机器学习：

加权损失函数：在训练模型时，某些样本的重要性可能更高，可以通过加权损失函数来反映这一点。
加权投票：在集成学习中，不同模型的预测结果可以赋予不同的权重，以提高整体预测的准确性。

信号处理：

加权滤波：在滤波器设计中，可以赋予不同频率成分不同的权重，以突出或抑制某些频率。

决策制定：

加权评分：在多准则决策中，不同准则的重要程度不同，可以通过加权评分来综合评估备选方案。