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

时间:2015-04-23 17:08:01

标签: c++ transformation eigen quaternions euler-angles

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

我写了一个函数来获取用户想要在每个方向上旋转手臂的数量(滚动,俯仰,偏航)并输出新的四元数。我将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(),但是没有它们工作。

2 个答案:

答案 0 :(得分:3)

您的示例几乎与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感到困惑。如果滚动,俯仰,偏航对应于某个固定的参考方向,那么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的评论中也提到了这一点

答案 1 :(得分:0)

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

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;