无人机姿态表示之四元数篇

无人机姿态表示之四元数篇

又是没有公式的一篇文档，继续欠着。

1 四元数表示三维旋转

四元数常用于表示三维空间的旋转变换，注意它表示一个变换，不是一个状态，它描述了一个旋转轴u和旋转角θ表示旋转。

四元数要素是旋转轴和旋转角体现机体坐标系相对于地面坐标系之间的姿态旋转关系。机体坐标系和地球参考系可以通过四元数旋转得到，即机体坐标系绕固定点的某一瞬时旋转轴转过某个角度的1次转动获得可获得坐标系的位置关系。即可用四元数表示机体系和地球系之间的姿态变化矩阵。

2 四元数的表达方式

四元数的表达方式最常用的就是复数式和三角式。

2.1 复数式表达

四元数 q 由一个实部和三个虚部组成： q = q0+ q1i + q2j + q3k

其中：

• q0是实部
• q1, q2, q3 是虚部
• i, j, k 是虚数单位，满足特定的乘法规则

q = q0 + (q1, q2, q3)

• q0：标量部分（实部）
• (q1, q2, q3)：向量部分（虚部）

2.1.1 代码表示

typedef struct {
    float q0;   // 实部
    float q1;   // 虚部x分量
    float q2;   // 虚部y分量
    float q3;   // 虚部z分量
} Quaternion;

2.1.2 物理意义

• q0表示旋转的整体信息
• (q1, q2,q3) 表示旋转轴的方向和大小

2.2 三角式表达

2.2.1 标准形式

q = cos(θ/2) + sin(θ/2)u

其中：

• θ 是旋转角度，用θ /2，而不是θ ，是进行角度限制，是避免奇异点
• u是单位旋转轴
• i, j, k 是虚数单位

2.2.2 详细分解

• cos(θ/2)：实部
• sin(θ/2)u：虚部

2.3 物理意义详解

2.3.1 复数式物理解释

1. 实部 q0

• 表示旋转的"稳定性"
• 接近1表示旋转角度小
• 接近0表示旋转角度接近180度

2. 虚部 (q1, q2, q3)

• 编码旋转轴的方向
• 长度反映旋转的"强度"

2.3.2 三角式物理解释

1. 为什么使用 θ/2

• 避免重复表示
• 提供更稳定的数值表示

1. 单位旋转轴 u

• 确定旋转平面
• 垂直于旋转轴
• 长度始终为1

2.4 实际应用示例

2.5 飞行器姿态旋转

// 绕X轴旋转45度
Quaternion rollRotation = createRotationQuaternion(
    45.0f,  // 角度
    {1, 0, 0}  // X轴单位向量
);

// 绕Y轴旋转30度 
Quaternion pitchRotation = createRotationQuaternion(
    30.0f,  // 角度
    {0, 1, 0}  // Y轴单位向量
);

// 复合旋转
Quaternion combinedRotation = quaternionMultiply(
    rollRotation, 
    pitchRotation
);

2.6 单位四元数

在四元数是单位四元数的时候，四元数的共轭和逆是相等的。在用四元数表示旋转的时候，特别是我们的姿态表示，必须要将四元数变成单位四元数。要对四元数进行归一化，后续第二章有讲，详见。：

归一化四元数不仅是数学上的要求，更是保证旋转变换物理准确性的关键。通过始终保持单位四元数，我们确保了：

1. 纯粹的旋转变换
2. 避免意外缩放
3. 数学变换的连续性
4. 系统长期稳定性

2.7 四元数的共轭和逆

1. 共轭（Conjugate）
   对于单位四元数 q*=[cos(θ/2),sin(*θ/2)u]（表示绕单位轴u轴旋转角度 θ），其共轭 q∗=[cos⁡(θ/2), −sin⁡(θ/2)u]q∗=[cos(θ/2),−sin(θ/2)u] 对应的物理意义是逆旋转，即定轴反角，或者反轴定角旋转。

   即绕同一轴 u 旋转 −θ 角度（或等价于绕轴 −u 旋转 θ 角度）。

2. 逆（Inverse）
   单位四元数的逆 q−1q−1 定义为 q−1=q∗/∥q∥2q−1=q∗/∥q∥2。对于单位四元数（∥q∥=1∥q∥=1），逆与共轭相同，即 q−1=q∗q−1=q∗。其物理意义同样是逆旋转，用于撤销原旋转操作。

3. 数学定义

   共轭仅对四元数的虚部取反，即 q∗=[q0,q1,q2,q3]

   逆则是共轭除以模长的平方：q−1=q∗/∥q∥2q−1=q∗/∥q∥2。对于非单位四元数，逆与共轭不同。

   在旋转操作中，四元数必须为单位长度以保证旋转的保距性（无缩放）。此时 q−1=q∗q−1=q∗，两者等价。

   对于非单位四元数，逆需额外归一化，而共轭直接取反虚部。但非单位四元数一般不用于表示纯旋转。

3 四元数的归一化处理

四元数在多次运算后会存在累积误差，这是由于浮点数运算的精度限制和连续运算过程中的数值不稳定性造成的。将详细解释误差产生的原因和归一化处理方法。

3.1 四元数误差产生的原因

1. 浮点数计算精度限制
   • 计算机使用浮点数表示实数，存在固有的精度误差
   • 每次四元数运算（乘法、旋转等）都会引入微小的数值误差
   • 多次连续运算会使这些微小误差逐步累积
2. 数值不稳定性
   • 四元数理论上应该是单位四元数（模长为1）
   • 连续运算会导致四元数的模长逐渐偏离1
   • 模长变化会引起旋转表示的不准确

3.2 误差累积的具体过程

考虑一个典型的旋转序列：

Q1 * Q2 * Q3 * Q4 * ... * Qn

每次乘法都可能引入微小误差，导致最终结果的四元数不再是标准的单位四元数。

3.3 归一化处理方法

归一化的目的是将四元数的模长调整回1，保持其旋转表示的准确性。

3.3.1 方法1：标准化（最简单的归一化）

void normalizeQuaternion(Quaternion* q) {
    float magnitude = sqrt(q->0 * q->0 + 
                           q->1 * q->1 + 
                           q->2 * q->2+ 
                           q->3 * q->3);
    
    // 防止除零
    if (magnitude > 0.000001f) {
        q->0 /= magnitude;
        q->1 /= magnitude;
        q->2 /= magnitude;
        q->3 /= magnitude;
    }
}

3.3.2 方法2：快速近似归一化

Quaternion fastNormalizeQuaternion(Quaternion q) {
    // 快速平方求模长的近似算法
    float modulus_squared = q.0 * q.0 + 
                            q.1 * q.1 + 
                            q.2 * q.2 + 
                            q.3 * q.3;
    
    // 使用牛顿迭代法快速逼近平方根的倒数
    float approx_inv_norm = 1.5f - 0.5f * modulus_squared;
    
    return (Quaternion){
        q.0 * approx_inv_norm,
        q.1 * approx_inv_norm,
        q.2 * approx_inv_norm,
        q.3* approx_inv_norm
    };
}

3.3.3 方法3：累积误差修正

typedef struct {
    Quaternion quaternion;
    float error_accumulator;
} AdaptiveQuaternion;

AdaptiveQuaternion updateQuaternionWithErrorCorrection(
    AdaptiveQuaternion current, 
    Quaternion delta) 
{
    // 执行四元数乘法
    Quaternion newQ = quaternionMultiply(current.quaternion, delta);
    
    // 计算模长偏差
    float deviation = fabs(1.0f - quaternionMagnitude(newQ));
    
    // 累积误差
    current.error_accumulator += deviation;
    
    // 定期执行强制归一化
    if (current.error_accumulator > ERROR_THRESHOLD) {
        newQ = normalizeQuaternion(newQ);
        current.error_accumulator = 0;
    }
    
    current.quaternion = newQ;
    return current;
}

3.4 归一化的数学原理

1. 单位四元数满足：|q| = √(q0² + q1² + q2² + q3²) = 1
2. 归一化本质是将四元数投影回单位超球面
3. 保持四元数表示的旋转方向不变，仅调整其长度

3.5 实际应用建议

1. 在高精度要求的场景（如航空航天）每隔一定次数执行归一化
2. 选择计算复杂度和精度平衡的归一化方法
3. 对于实时系统，可以使用快速近似归一化算法

3.6 性能与精度权衡

• 标准化：最准确，计算开销最大
• 快速近似：计算速度快，精度略有损失
• 累积误差修正：平衡性能和精度

3.7 示例应用场景

// 无人机姿态估计中的四元数更新
void updateDroneAttitude(Quaternion* attitude, 
                         Vector3 gyroData, 
                         float deltaTime) {
    // 陀螺仪数据转换为旋转四元数
    Quaternion rotationDelta = gyroToQuaternion(gyroData, deltaTime);
    
    // 更新姿态并归一化
    *attitude = quaternionMultiply(*attitude, rotationDelta);
    normalizeQuaternion(attitude);
}

四元数计算，未完待续。。。