Rust中UnitQuaternion连续乘法数值误差问题及解决咨询

一、误差成因 #

理论上两个单位四元数相乘的结果仍为单位四元数，但浮点数运算的精度损失会导致每次乘法后四元数的模长轻微偏离1。随着连续乘法次数增加，这种偏差不断累积，最终引发明显的旋转误差。

你观察到的"重启程序用相同参数误差归零"现象，是因为初始状态下四元数模长严格为1，误差尚未开始累积；长期运行后，多次乘法的精度损失叠加，才会显现出可观测的误差。你的推测完全正确：线性代数库默认认为单位四元数相乘无需归一化，但实际数值计算中必须考虑精度损失的累积效应。

二、短期规避方案 #

核心思路是每次四元数运算后手动执行归一化，强制将四元数拉回单位球面，抵消精度损失。

1. 修改复现测试代码 #

在每次乘法后添加归一化步骤：

#[test]
pub fn __accumulate_unit_quaternion() {
    use rand::Rng;

    let mut rng = rand::thread_rng();

    let mut qq = UnitQuaternion::from_euler_angles(0.0, 0.0, 0.0);

    let unit_vec = Vector3::new(1.0, 0.0, 0.0);

    loop {
        let u1 = rng.gen_range(-1.58f32..1.58);
        let u2 = rng.gen_range(-1.58f32..1.58);
        let u3 = rng.gen_range(-1.58f32..1.58);

        let random = UnitQuaternion::from_euler_angles(u1, u2, u3);

        qq = random * qq;
        // 关键：手动归一化抵消精度损失
        qq = qq.normalize();

        let reconstruction_error = {
            let reconstructed = UnitQuaternion::from_axis_angle(&qq.axis().unwrap(), qq.angle());
            let error = (qq * unit_vec - reconstructed * unit_vec).norm();
            error
        };

        println!("error={}", reconstruction_error);

        assert!(reconstruction_error < 0.001);
    }
}

2. 修改get_correction相关函数 #

在生成修正四元数后、乘法运算后执行归一化：

fn get_correction_raw(
    acc: &Vector3<f32>,
    rotation: &UnitQuaternion<f32>,
) -> Option<UnitQuaternion<f32>> {
    static UP_FRD: Vector3<f32> = Vector3::new(0.0, 0.0, -9.81);

    let uncorrected = rotation.transform_vector(&UP_FRD);
    let correction_opt = UnitQuaternion::rotation_between(&uncorrected, &acc);
    // 对生成的修正四元数归一化
    correction_opt.map(|q| q.normalize())
}

fn get_correction_verified_internal(
    acc: &Vector3<f32>,
    rotation: &UnitQuaternion<f32>,
) -> (Option<UnitQuaternion<f32>>, f32) {
    let raw = Self::get_correction_raw(acc, rotation);
    match raw {
        Some(correction) => {
            //round trip verification
            let corrected = (correction * rotation).normalize(); // 乘法后归一化

            let should_be_zero = Self::get_correction_raw(acc, &corrected);

            let zero = should_be_zero.unwrap().angle();
            if zero > 0.01 {
                println!("residual={}", zero);
                println!(
                    "compute: {}, {} => {}",
                    acc.transpose(),
                    rotation,
                    correction
                );
            }

            (Some(correction.normalize()), zero) // 返回前确保归一化
        }
        None => (raw, 0.0),
    }
}

三、长期库修复方案 #

1. 运算符重载添加归一化：在UnitQuaternion的Mul实现中默认加入归一化步骤，同时提供两种乘法接口：一种是不做归一化的快速乘法，另一种是带归一化的安全乘法（默认使用安全版本）。
2. 内部维护模长一致性：在UnitQuaternion内部，当模长偏离1超过阈值时自动执行近似归一化，平衡精度与性能。
3. 文档明确提示：在UnitQuaternion的文档中明确说明连续乘法的数值误差问题，提醒用户在长期累积运算中需手动归一化，或使用库提供的安全运算方法。

内容的提问来源于stack exchange，提问作者tribbloid