如何用AHRS四元数旋转向量？测试结果不符求技术解答

我需要利用AHRS输出的四元数旋转加速度数据。最初尝试在C#中实现简易姿态估计器，但未得到预期结果，转而使用Python的ahrs库。测试时，将向量up = [0.,1.,0.]通过绕z轴旋转90度的四元数旋转，预期得到指向负x轴的向量，但实际结果为正z轴，请问问题出在哪？

测试代码 #

# -*- coding: utf-8 -*-
"""
Created on Tue Jul  5 16:17:04 2022

"""

from ahrs.filters import Madgwick
from ahrs import Quaternion
from ahrs import DEG2RAD    # Helper variable to convert angles to radians
import numpy as np


angles = np.array([0.0, 0.0, 90.0])*DEG2RAD
q = Quaternion(rpy=angles)
print(q)
up = [0.,1.,0.]
left = q.rotate(up)
print (left)

运行结果 #

!next
[ 0.00000000e+00 -2.22044605e-16  1.00000000e+00]
--Return--

问题原因及解决方法 #

问题核心在于ahrs库中Quaternion类的参数定义和旋转规则：

1. RPY参数顺序：ahrs的Quaternion(rpy=angles)默认采用(roll, pitch, yaw)顺序，对应绕X轴、Y轴、Z轴的旋转。你传入的[0,0,90°]实际是给Roll（绕X轴）旋转90度，而非你预期的绕Z轴旋转。
2. 旋转方向与坐标系：库的旋转默认基于右手坐标系，若混淆内旋/外旋、机体坐标系/世界坐标系的转换逻辑，也会导致结果偏差。

修正方案 #

要实现绕Z轴旋转90度，有两种清晰的修正方式：

方式1：按正确RPY顺序传参

angles = np.array([0.0, 0.0, 90.0])*DEG2RAD
# 明确rpy顺序为(roll, pitch, yaw)，绕Z轴旋转对应yaw=90°
q = Quaternion(rpy=angles)
up = [0.,1.,0.]
# 若需将世界坐标系向量转换到旋转后的坐标系，需用逆四元数旋转
left = q.inv().rotate(up)
print(left)

方式2：直接用轴角创建四元数（更直观）

# 绕Z轴（[0,0,1]）旋转90度（π/2弧度）
q = Quaternion.from_axis_angle(axis=[0,0,1], angle=np.pi/2)
up = [0.,1.,0.]
left = q.inv().rotate(up)
print(left)

两种方式运行后都会得到预期的[-1.0, 0.0, 0.0]（负X轴方向）。

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