c++ - Webots ROS默认 Controller “RosInertialUnit.cpp”中的四元数计算

今天，我仔细研究了作为默认ROS Controller 一部分的“RosInertialUnit.cpp”文件中使用的四元数计算。

我想使用“keyboard_teleop.wbt”-世界尝试InterialUnit，并将传感器添加到Pioneer机器人中。

然后，我正在将场景树中给出的机器人旋转值(以轴+ Angular 格式)与ROS中的传感器输出(方向转换为四元数)进行比较。您可以在下面的屏幕截图中看到两者:

在我看来，四元数输出与场景树中给出的值不匹配。当使用MATLAB的函数“quat = axang2quat(axang)”时，对于上述示例，我将获得以下内容:

quat = 0.7936    0.0131   -0.6082    0.0104  % w x y z

与ROS消息比较时，表明y和z已切换。我不太确定这是否是故意的(也许是不同的约定？)。我不想立即开始请求请求，但是想在这里讨论这个问题。

我正在更改版本的“RosInertialUnit.cpp”中测试以下实现，它为我提供了预期的结果(与MATLAB中计算的结果相同)。

  double halfRoll = mInertialUnit->getRollPitchYaw()[0] * 0.5;   // turning around x
  double halfPitch = mInertialUnit->getRollPitchYaw()[2] * 0.5;  // turning around y
  double halfYaw = mInertialUnit->getRollPitchYaw()[1] * 0.5;    // turning around z
  double cosYaw = cos(halfYaw);
  double sinYaw = sin(halfYaw);
  double cosPitch = cos(halfPitch);
  double sinPitch = sin(halfPitch);
  double cosRoll = cos(halfRoll);
  double sinRoll = sin(halfRoll);

  value.orientation.x = cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll;
  value.orientation.y = sinYaw * cosPitch * sinRoll + cosYaw * sinPitch * cosRoll;
  value.orientation.z = sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll;
  value.orientation.w = cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll;

这与this wikipedia article中使用的实现相同。

最佳答案

这种反演是由于Webots和ROS坐标系不相等这一事实造成的。

在Webots中:

X :左

Y :up

Z :转发

导致:(https://cyberbotics.com/doc/reference/inertialunit#field-summary)

卷:左(Webots X)

音调:前进(Webots Z)

偏航:向上(Webots Y)

在ROS中:(https://www.ros.org/reps/rep-0103.html#axis-orientation)

X :转发