两个四元数叉乘与姿态旋转的关系

背景

最近在学习ros2的tf2库,碰到很多求解从姿态1转变到姿态2的变换矩阵的情况。实际使用中发现,使用四元数求解这个过程,运算量会远少于用矩阵,所以对四元数的运算关系浅显地做了一些简单的研究。

问题描述

已知2个四元数q1q2,物体从初始姿态(即四元数:0i+0j+0k+1)先旋转到q1,再旋转到q2。求证物体最终的姿态,是q1*q2还是q2*q1?

验证过程(无任何推导,纯举例验证)

通过查资料很容易得到一个先导结论:

四元数叉乘不满足交换律,即q1*q2不一定等于q2*q1

直接举例:

q1 = 0i+0j+1k+1
q2 = 2i-8j+1k+1

q1和q2的图像

借助一个在线工具3D Rotation Converter,我们能看到对应的姿态图像如下,第一幅是q1,第二幅是q2
q1
q2
这个3D图像工具中Euler角的Order是按照内旋方式旋转的,即图中先绕物体自身Z轴转,再绕自身Y轴,最后绕自身X轴旋转,这与tf2库中是一致的。这一点很重要,无论研究哪种姿态变换,搞清楚坐标系和内旋外旋都是第一步。
我们可以看到,q1相对原始坐标系,绕自身Z轴转了90度,q2相对原始坐标系,差不多指向了右后方(红色箭头相对黄色坐标轴),且自身Z轴基本是朝下了。

q1和q2的叉乘计算结果

使用在线工具omni calculator可以得到q1q2叉乘的2个计算结果:

q₁ * q₂ = 10i - 6j + 2k + 0
q₂ * q₁ = -6i - 10j + 2k + 0

在这里插入图片描述
画出这q1*q2q2*q1的结果,第一幅是q1*q2,第二幅是q2*q1
q1*q2
q2*q1

图像对比

通过q1和q2的图像,我们可以大致推断,先旋转q1再旋转q2,相当于把红色箭头先从黄色坐标轴绕自身Z轴转90度到绿色坐标轴,然后以不知道什么方式转到绿色坐标轴的右后方,同时把Z轴指向下方
对比两个叉乘结果的图像,很明显第一幅是接近的。实际如果去观察Euler角度,也会发现数字上是一致的。
(有意思的是,两个叉乘图像的红色箭头方向好像差了180度,我觉得这应该不是巧合,但还没确认。。。)

结论

物体从初始姿态(即四元数:0i+0j+0k+1)先旋转到q1,再旋转到q2,物体最终的姿态是q1*q2
即四元数的姿态变换顺序是左前右后,应该采用右乘方式

整个过程纯粹使用举例方式感性直观验证,结论正确性仅供参考

数学推导

To be continue…

### 关于四元数姿态解算的代码实现 以下是基于C语言的一个简单四元数姿态解算示例代码,该代码实现了基本的四元数运算以及欧拉角到四元数的转换功能。此代码片段参考了ABB机器人的相关资源[^2]。 ```c #include <stdio.h> #include <math.h> // 定义四元数结构体 typedef struct { double w, x, y, z; } Quaternion; // 辅助函数:计算两个向量的 void crossProduct(double a[3], double b[3], double result[3]) { result[0] = a[1]*b[2] - a[2]*b[1]; result[1] = a[2]*b[0] - a[0]*b[2]; result[2] = a[0]*b[1] - a[1]*b[0]; } // 函数:欧拉角转四元数 (roll, pitch, yaw) Quaternion eulerToQuaternion(double roll, double pitch, double yaw) { double cy = cos(yaw * 0.5); double sy = sin(yaw * 0.5); double cp = cos(pitch * 0.5); double sp = sin(pitch * 0.5); double cr = cos(roll * 0.5); double sr = sin(roll * 0.5); Quaternion q; q.w = cr * cp * cy + sr * sp * sy; q.x = sr * cp * cy - cr * sp * sy; q.y = cr * sp * cy + sr * cp * sy; q.z = cr * cp * sy - sr * sp * cy; return q; } // 主程序测试 int main() { // 输入欧拉角 (单位: 弧度制) double roll = M_PI / 4; // 绕X轴旋转角度 double pitch = M_PI / 6; // 绕Y轴旋转角度 double yaw = M_PI / 3; // 绕Z轴旋转角度 // 调用欧拉角转四元数函数 Quaternion q = eulerToQuaternion(roll, pitch, yaw); printf("四元数表示:\n"); printf("w: %f\n", q.w); printf("x: %f\n", q.x); printf("y: %f\n", q.y); printf("z: %f\n", q.z); return 0; } ``` 以上代码展示了如何通过输入欧拉角来获得对应的四元数表示形式。其中涉及到了三角函数的操作以及四元数的基本定义。 此外,在实际工程应用中,通常还需要考虑传感器数据融合的情况,比如利用陀螺仪、加速度计的数据进行姿态估计。这种情况下可以采用扩展卡尔曼滤波器(EKF)[^3] 或者 Madgwick算法来进行更精确的姿态解算[^4]。 #### 注意事项 - 上述代码仅作为基础示例展示用途,未包含完整的误差处理机制。 - 如果需要进一步提升精度,则需引入更多复杂的数学模型并结合硬件特性加以改进。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值