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

Question

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

我编写函数来获取用户想要在每个方向(滚动、俯仰、偏航)上旋转 ARM 的量并输出新的四元数。我将 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 的评论中也有建议