关于在Webots中获取激光雷达点云全局坐标的问题

Question

我需要在Webots中使用移动机器人使用激光雷达对周围环境进行自定义映射。我用的是:

• GPS获取机器人位置。
• 机器人方向指南针。
• 用于获取周围信息的激光雷达。

也许有人熟悉Webots，可以展示基本的代码示例或解释背后的数学或有一个方法，我错过了在Webots?

我做了激光雷达相对点的平移和旋转，当机器人在平面上(2D旋转)时效果很好。但无论我多么努力，我都不知道如何从点云相对点获得准确的全局坐标，当机器人甚至有点倾斜(3D旋转)时。

我的猜测是，它应该使用空间变换矩阵，但我不确定如何使用Webots指南针值在旋转矩阵.

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中添加真实的机器人坐标。最后你会得到你需要的点的全局坐标。