通过欧拉角输入旋转四元数

Question

我正在编写代码来控制 3D 机械臂 space。机械臂通过四元数处理旋转，但我希望用户通过改变偏航、俯仰和滚动来控制它，因为人类使用这些更明智。

我编写函数来获取用户想要在每个方向（滚动、俯仰、偏航）上旋转手臂的量并输出新的四元数。我将 current_quaternion 保存为全局变量。

我正在使用 C++ 和 Eigen。

Eigen::Quaterniond euler2Quaternion( const double roll,
              const double pitch,
              const double yaw)
{

Eigen::AngleAxisd rollAngle(roll,Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(pitch,Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(yaw,Eigen::Vector3d::UnitZ());

Eigen::Quaterniond q = rollAngle*pitchAngle*yawAngle;
current_q=q*current_q;
return current_q;
}

我尝试了很多方法来改变乘以角度的顺序以及将 UnitX()、UnitY() 和 UnitZ() 乘以 current_q.toRotationMatrix()，但没有一个奏效。

Answer 1

您的示例与 the example 几乎相同

Matrix3f m;
m = AngleAxisf(0.25*M_PI, Vector3f::UnitX())
  * AngleAxisf(0.5*M_PI,  Vector3f::UnitY())
  * AngleAxisf(0.33*M_PI, Vector3f::UnitZ());

您是否尝试打印组合旋转矩阵的结果？当角度为零时，我敢打赌它是对角线 1,1,1。

我对您对 current_q 的使用感到困惑。如果roll、pitch、yaw对应于某个固定的参考方向，那么q就是整个旋转。在那种情况下，这个：

current_q=q*current_q;
return current_q;

应该就是

current_q=q;
return current_q;

如果 roll, pitch, yaw 意味着改变当前的欧拉旋转角度（从某个固定的参考方向开始），存储这些角度并相应地改变它们会更容易：

double m_roll=0, m_pitch=0, m_yaw=0;
 . . .
Eigen::Quaterniond euler2Quaternion(double roll,
              double pitch,
              double yaw)
{
 m_roll+=roll;
 m_pitch+=pitch;
 m_yaw+=yaw;

 Eigen::AngleAxisd rollAngle(m_roll,Eigen::Vector3d::UnitX());
 Eigen::AngleAxisd pitchAngle(m_pitch,Eigen::Vector3d::UnitY());
 Eigen::AngleAxisd yawAngle(m_yaw,Eigen::Vector3d::UnitZ());

 Eigen::Quaterniond q = rollAngle*pitchAngle*yawAngle;
 current_q=q;
 return current_q;
}

sbabbi 在评论中也提出了这一建议

Answer 2

为什么不直接构建四元数？

Eigen::Quaterniond rollQuaternion(cos(0.5*roll), sin(0.5*roll), 0.0, 0.0);
Eigen::Quaterniond pitchQuaternion(cos(0.5*roll), 0.0, sin(0.5*roll), 0.0);
Eigen::Quaterniond yawQuaternion(cos(0.5*roll), 0.0, 0.0, sin(0.5*roll));
Eigen::Quaterniond finalOrientation = rollQuaternion*pitchQuaternion*yawQuaternion*current_q;

通过欧拉角输入旋转四元数

Rotate a quaternion by Euler angles input

c++

transformation

eigen

quaternions

euler-angles