Webots ROS默认控制器的“ RosInertialUnit.cpp”中的四元数计算

Question

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

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

然后，我将场景树中给出的机器人旋转值（以轴+角度格式）与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中使用的实现相同。

Answer 1

这种反演是由于Webots和ROS坐标系不等效。

在Webots中：

• X ：向左
• Y ：向上
• 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 ：转发
• Y ：左
• Z ：向上

哪个会导致：（https://www.ros.org/reps/rep-0103.html#rotation-representation）

• 滚动：前进（ROS X）
• 螺距：向左（ROS Y）
• 偏航：向上（ROS Z）

如您所见，滚动轴和俯仰轴已切换，这就是为什么它们也在代码中切换的原因。