Webots中從親戚處獲取激光雷達點雲全局坐標的問題

Question

我需要使用 Webots 中的移動機器人使用激光雷達對周圍環境進行自定義映射。 我用什么：

• GPS 用於獲取機器人 position。
• 獲得方向機器人的指南針。
• 用於獲取周圍信息的激光雷達。

也許熟悉 Webots 的人可以展示基本代碼示例或解釋其背后的數學原理，或者我在 Webots 中錯過了一種方法？

我對激光雷達的相對點進行了平移和旋轉，當機器人在平面上時效果很好（2D 旋轉）。 但是無論我嘗試了多少，當機器人稍微傾斜（3D 旋轉）時，我都無法弄清楚如何從點雲相對點獲得准確的全局坐標。

我的猜測是它應該使用空間變換矩陣，但我不確定如何在旋轉矩陣中使用 Webots Compass 值。

Answer 1

在StackExchange中獲得一些有用的信息后。 Python 解決方案的基本示例：

from scipy.spatial.transform import Rotation as Rotation

RobotPoint = gps.getValues()
STR = Rotation.from_quat(InertialUnit.getQuaternion())
for RelativeCloudPoint in lidar.getPointCloud():
  Point2 = STR.apply(RelativeCloudPoint)
  GlobalCloudPoint = RelativeCloudPoint + RobotPoint

使用 InternalUnit 獲取空間旋轉矩陣的四元數。 然后將其應用於相對坐標。 之后添加來自 GPS 的真實機器人坐標。最后您將獲得所需點的全局坐標。