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

Question

今天我仔细研究了作为默认 ROS Controller 一部分的“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 坐标系不等价。

在网络机器人中:

• X:左
• Y:向上
• Z:向前

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

• 滚动:向左(Webots X)
• 投球:前锋(Webots Z)
• yaw:向上 (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 )

• roll:向前(ROS X)
• 俯仰:左(ROS Y)
• 偏航:向上 (ROS Z)

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