如何正确计算两个四元数之间的旋转四元数?
问题描述
我在学习四元数旋转相关知识时,写了两段代码实现「将沿X轴的单位向量旋转至Y轴」的功能,代码如下:
// 方法1 Eigen::Vector3f a(1,0,0), b(0,1,0); Eigen::Quaternionf qr; qr.setFromTwoVectors(a,b); // 方法2 Eigen::Quaternionf q1(0,1,0,0), q2(0,0,1,0), qr_alt; qr_alt = q1.inverse() * q2; // 我以为q2 = q1 * delta_q,delta_q就是要找的旋转四元数
两种方法输出的四元数结果不一致:转成欧拉角后,qr_alt算出的偏航角是π弧度,而qr算出的π/2弧度偏航角才是正确结果。想知道计算两个四元数之间旋转量的正确方法是什么。
错误根源
你的第二种方法从根上就错了,混淆了纯向量四元数和旋转四元数的概念,而且记错了四元数旋转向量的公式:
q1(0,1,0,0)、q2(0,0,1,0)只是分别对应X轴、Y轴单位向量的纯四元数,根本不是「把向量转到X轴/Y轴方向」的旋转四元数,直接对这两个值求逆相乘没有任何物理意义。- 四元数对向量做旋转的正确公式是「三明治结构」:如果用旋转四元数
q旋转向量v(对应纯四元数v_q),旋转后的向量对应纯四元数为v'_q = q * v_q * q.inverse(),你写的q2 = q1 * delta_q直接漏掉了右侧乘逆的项,公式本身不成立。
你可以手动算下你写的qr_alt结果:q1是单位纯四元数,逆等于共轭即(0,-1,0,0),和q2(0,0,1,0)做四元数乘法得到的是(0,0,0,-1),对应绕Z轴旋转π,也就是你看到的偏航角180度的错误结果,和你要的90度旋转完全无关。
正确计算方法
分两种场景对应不同计算方式:
场景1:已知两个向量,求把向量a转到向量b的旋转四元数
直接用你写的第一种方法setFromTwoVectors(a,b)就行,这是Eigen官方实现的标准接口,会自动处理旋转轴、旋转角的计算,还能处理两个向量不共线、反向等边界情况,不需要自己手动造轮子。
如果要手动实现,逻辑也很简单:- 旋转轴取两个向量的叉乘,归一化:
Eigen::Vector3f axis = a.cross(b).normalized(); - 旋转角取两个向量的夹角:
float theta = std::acos(a.dot(b)); - 按四元数定义构造:
Eigen::Quaternionf q(std::cos(theta/2), axis.x()*std::sin(theta/2), axis.y()*std::sin(theta/2), axis.z()*std::sin(theta/2));
算出来的结果和接口返回值完全一致。
- 旋转轴取两个向量的叉乘,归一化:
场景2:已知两个旋转四元数
q_from、q_to(都是相对于同一个全局初始坐标系的旋转),求从q_from姿态转到q_to姿态的增量旋转delta_q
从三明治公式推导可得正确计算式为:// 注意Eigen的四元数乘法顺序是「右乘先应用」,如果用其他3D库要先确认库的乘法约定,部分库顺序相反 Eigen::Quaternionf delta_q = q_to * q_from.inverse();验证逻辑:对
q_from坐标系下的任意向量v,先做q_from旋转转到全局系,再做delta_q旋转,最终结果和直接做q_to旋转完全一致。
内容的提问来源于stack exchange,提问作者Simit Pradhan
相关产品推荐
相关产品推荐

