如何将蓝牙IMU输出的四元数转换为2D屏幕坐标（Android Java）

你的核心需求是将两个IMU的相对旋转四元数映射到2D屏幕坐标，通过绿色圆圈与橙色目标点的距离，直观展示传感器的对齐状态。结合你对两个传感器关注轴的限制（红：Y/Z轴；蓝：X/Y轴），以下是针对性的修正方案与技巧：

1. 先确认相对四元数的正确性 #

你计算的q_rel = 红传感器四元数共轭 × 蓝传感器四元数是正确的，这个四元数代表从红色传感器坐标系到蓝色传感器坐标系的旋转。零位对齐时，q_rel应为单位四元数（w=1, x=y=z=0），此时两个传感器姿态完全匹配。

2. 原代码的问题点 #

你提供的ChatGPT生成代码存在两个核心问题：

• 欧拉角提取公式对应的是Z-Y-X航空坐标系，未匹配你传感器的轴关注逻辑；
• 直接用sin(angle)映射坐标，且未适配Android屏幕Y轴向下的坐标系方向，导致零位显示偏移。

3. 修正后的转换实现 #

方案一：基于欧拉角的定向映射（适配你的轴关注需求） #

直接提取相对旋转中两个传感器各自关注的轴角度，再映射到屏幕坐标：

private PointF quaternionTo2D(Quaternion q) {
    // 提取相对旋转的欧拉角（对应传感器标注的XYZ轴）
    // 滚转角（X轴，蓝色传感器关注）
    float roll = (float) Math.atan2(2.0f * (q.w() * q.x() + q.y() * q.z()), 1.0f - 2.0f * (q.x() * q.x() + q.y() * q.y()));
    // 俯仰角（Y轴，两个传感器共同关注）
    float pitch = (float) Math.asin(2.0f * (q.w() * q.y() - q.z() * q.x()));
    // 偏航角（Z轴，红色传感器关注）
    float yaw = (float) Math.atan2(2.0f * (q.w() * q.z() + q.x() * q.y()), 1.0f - 2.0f * (q.y() * q.y() + q.z() * q.z()));

    // 映射到归一化坐标[-1,1]：X对应蓝传感器关注的X轴，Y对应共同关注的Y轴
    float normX = roll / (float) Math.PI;
    float normY = pitch / ((float) Math.PI / 2); // 俯仰角范围[-π/2, π/2]，放大到[-1,1]

    // 适配Android屏幕Y轴向下的特性，反转Y坐标
    normY = -normY;

    // 若需映射到实际屏幕像素，可再乘以屏幕半宽/半高，加上屏幕中心坐标
    // int centerX = getWidth() / 2;
    // int centerY = getHeight() / 2;
    // return new PointF(centerX + normX * centerX, centerY + normY * centerY);

    return new PointF(normX, normY);
}

方案二：基于向量投影的方法（避免欧拉角万向锁问题） #

通过旋转参考向量并投影到2D平面，更直观地对应物理姿态：

private PointF quaternionTo2D(Quaternion q) {
    // 定义零位时的参考向量（比如红色传感器的Y轴方向：(0,1,0)）
    float[] refVec = {0, 1, 0};
    // 用相对四元数旋转参考向量
    float[] rotatedVec = rotateVecByQuat(q, refVec);

    // 投影到2D：X对应蓝传感器关注的X分量，Y对应红传感器关注的Z分量
    float screenX = rotatedVec[0];
    float screenY = rotatedVec[2];

    // 适配屏幕Y轴方向
    screenY = -screenY;

    return new PointF(screenX, screenY);
}

// 四元数旋转向量的工具方法
private float[] rotateVecByQuat(Quaternion q, float[] vec) {
    float w = q.w(), x = q.x(), y = q.y(), z = q.z();
    float vx = vec[0], vy = vec[1], vz = vec[2];

    // 四元数旋转公式：v' = q*v*q_conj
    float xPrime = w*w*vx + 2*y*w*vz - 2*z*w*vy + x*x*vx + 2*y*x*vy + 2*z*x*vz - z*z*vx - y*y*vx;
    float yPrime = 2*x*y*vx + y*y*vy + 2*z*y*vz + 2*w*z*vx - z*z*vy + w*w*vy - 2*x*w*vz - x*x*vy;
    float zPrime = 2*x*z*vx + 2*y*z*vy + z*z*vz - 2*w*y*vx - y*y*vz + 2*w*x*vy - x*x*vz + w*w*vz;

    return new float[]{xPrime, yPrime, zPrime};
}

4. 零位校准与调试技巧 #

• 零位校准：若零位时相对四元数不是单位四元数，需记录零位时两个传感器的原始四元数qRedZero、qBlueZero，后续计算相对旋转时先校准：qRel = (qRed.conjugate().multiply(qRedZero)).multiply(qBlueZero.conjugate().multiply(qBlue))；
• 调试步骤：先打印零位时的相对四元数值，确认是否为(1,0,0,0)；再打印提取的欧拉角/旋转向量值，观察旋转变化是否符合预期；最后再映射到屏幕像素；
• 范围控制：如果角度偏移过大导致圆圈超出屏幕，可通过tanh()函数限制坐标范围，避免显示溢出。

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