1

今天，我仔细研究了作为默认 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;

这与本维基百科文章中使用的实现相同。

4

1 回答 1

2

这种倒置是因为 Webots 和 ROS 坐标系不等价。

在网络机器人中：

  • X：左
  • 是：向上
  • 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）

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

于 2019-05-10T12:58:48.963 回答