无人机IMU姿态校正及测距仪高度补偿技术咨询

问题背景 #

搭建的无人机搭载IMU（左手坐标系：X轴向下、Z轴向后、Y轴向右）与垂直于机身平面向下的超声波测距仪，存在两个核心需求：

• 校正IMU四元数，消除IMU与无人机轴不对齐的误差，获取无人机真实姿态；
• 结合无人机俯仰/横滚角校正测距仪测量值，得到真实地面高度。

已完成的IMU姿态校正（验证有效） #

通过采集水平表面下的5个IMU四元数并取平均，计算与零旋转四元数的差值，实现姿态校正：

qs = quaternion.as_quat_array(sensor_data)
avg_imu_pos = np.average(qs)
base_q = quaternion.from_rotation_vector([[0, 0, 0]])[0]
diff = base_q * avg_imu_pos.conjugate()

后续通过orientation_true = diff * orientation（orientation为IMU原始输出四元数）得到真实姿态，经可视化验证有效。

测距值修正的异常问题 #

编写的校正函数假设使用校正后的姿态四元数，通过计算与零旋转四元数的夹角，用cos(angle)修正测距值，但实际测试中，即使仅绕垂直轴旋转，校正后的测量值也会大幅降低，明显不符合预期：

def correct_distance(self, measured_distance: float, orientation: quaternion) -> float:
    q1 = quaternion.from_rotation_vector([[0, 0, 0]])[0]
    diff = q1.conjugate() * orientation
    if diff.w < -1:
        diff.w = -1
    elif diff.w > 1:
        diff.w = 1
    angle = 2 * math.acos(diff.w)
    return measured_distance * math.cos(angle)

整体思路判断 #

整体思路方向正确：先校正IMU与机身的轴对齐误差，再基于真实姿态修正测距值。但测距修正的逻辑存在关键错误。

错误原因拆解 #

当前函数的核心问题是用总旋转角计算校正系数：

• 2 * math.acos(diff.w)计算的是无人机姿态相对于水平状态的总旋转角度，该角度包含了偏航（绕垂直轴旋转）的分量；
• 但超声波测距仪始终垂直于机身平面，偏航旋转不会改变机身平面与地面的夹角，完全不影响测距值的垂直投影，因此不应该纳入校正计算；
• 这就导致仅偏航时，总旋转角也会变化，cos(angle)变小，测距值被错误地大幅降低。

修正方案 #

正确的做法是计算测距仪指向与真实垂直向下方向的夹角，仅保留俯仰、横滚的影响，忽略偏航。可以通过向量旋转与点积实现：

import numpy as np
import quaternion

def correct_distance(self, measured_distance: float, orientation_true: quaternion) -> float:
    # 机身坐标系中测距仪的方向：垂直机身向下，对应校正后的机身X轴方向
    v_body = np.array([1.0, 0.0, 0.0])
    # 将机身坐标系下的测距向量旋转至世界坐标系
    v_world = quaternion.rotate_vectors(orientation_true, v_body)
    # 世界坐标系中的真实垂直向下向量（与无人机水平时的测距方向一致）
    v_down = np.array([1.0, 0.0, 0.0])
    # 计算向量点积，即夹角的余弦值（点积=|v1||v2|cosθ，单位向量下直接为cosθ）
    cos_theta = np.dot(v_world, v_down)
    # 修正数值误差导致的cosθ超出[-1,1]的情况
    cos_theta = np.clip(cos_theta, -1.0, 1.0)
    # 真实高度=测量距离×cosθ
    return measured_distance * cos_theta

方案说明 #

• 利用quaternion.rotate_vectors将机身坐标系下的测距方向向量转换到世界坐标系；
• 通过向量点积计算测距方向与真实垂直向下方向的夹角余弦值，该值仅受俯仰、横滚影响，偏航时向量方向不变，点积始终为1，测距值不会被错误修正；
• 加入np.clip避免数值计算误差导致的余弦值超出有效范围。

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