Webots中移动机器人激光雷达点云全局坐标获取方法咨询

核心数学原理 #

3D空间中点云的全局坐标转换分为两步：

1. 旋转：将激光雷达局部坐标系下的点云，通过机器人的3D旋转矩阵转换到全局坐标系姿态
2. 平移：将旋转后的点云叠加机器人的全局位置（由GPS获取）

旋转矩阵可通过机器人的欧拉角（滚转roll、俯仰pitch、偏航yaw）推导，或直接基于四元数转换。Webots中机器人的姿态本质是局部坐标系到全局坐标系的变换矩阵。

Webots Compass数据的正确解析 #

Webots的Compass传感器getValues()返回的是全局坐标系中磁北方向在机器人局部坐标系中的单位向量（格式为[x, y, z]）。通过该向量可推导关键姿态角：

• 偏航角（yaw）：机器人绕全局Z轴的旋转角度，由向量在XY平面的投影与全局X轴的夹角计算
• 俯仰角（pitch）：机器人绕局部Y轴的旋转角度，由向量与XY平面的夹角计算
• 滚转角（roll）：仅靠Compass无法单独推导，轻微倾斜场景可近似为0，高精度需求需搭配IMU传感器

代码示例（Python） #

方案1：手动计算旋转矩阵 #

import math
from controller import Robot, GPS, Compass, Lidar

robot = Robot()
timestep = int(robot.getBasicTimeStep())

# 初始化设备
gps = robot.getDevice("gps")
compass = robot.getDevice("compass")
lidar = robot.getDevice("lidar")
gps.enable(timestep)
compass.enable(timestep)
lidar.enable(timestep)

def euler_to_rotation_matrix(roll, pitch, yaw):
    # Z-Y-X顺序的旋转矩阵（局部坐标系→全局坐标系）
    cr, sr = math.cos(roll), math.sin(roll)
    cp, sp = math.cos(pitch), math.sin(pitch)
    cy, sy = math.cos(yaw), math.sin(yaw)
    
    return [
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp, cp*sr, cp*cr]
    ]

def get_robot_orientation(compass_values):
    x, y, z = compass_values
    # 计算偏航角与俯仰角，滚转近似为0
    yaw = math.atan2(y, x)
    pitch = math.asin(-z)
    return 0.0, pitch, yaw

while robot.step(timestep) != -1:
    # 获取机器人全局位置
    gps_pos = gps.getValues()
    # 解析Compass数据得到欧拉角
    roll, pitch, yaw = get_robot_orientation(compass.getValues())
    # 生成旋转矩阵
    rot_matrix = euler_to_rotation_matrix(roll, pitch, yaw)
    # 获取激光雷达局部点云
    lidar_points = lidar.getPointCloud()
    
    # 转换为全局坐标
    global_points = []
    for p in lidar_points:
        # 旋转局部点
        x_rot = rot_matrix[0][0]*p[0] + rot_matrix[0][1]*p[1] + rot_matrix[0][2]*p[2]
        y_rot = rot_matrix[1][0]*p[0] + rot_matrix[1][1]*p[1] + rot_matrix[1][2]*p[2]
        z_rot = rot_matrix[2][0]*p[0] + rot_matrix[2][1]*p[1] + rot_matrix[2][2]*p[2]
        # 叠加平移量
        global_points.append([
            x_rot + gps_pos[0],
            y_rot + gps_pos[1],
            z_rot + gps_pos[2]
        ])
    
    # 此处可将global_points用于建图逻辑

方案2：使用Webots内置姿态接口（更简便） #

Webots的Robot节点提供getOrientation()方法，直接返回局部坐标系到全局坐标系的4x4变换矩阵（前3x3为旋转部分），无需手动处理Compass：

while robot.step(timestep) != -1:
    gps_pos = gps.getValues()
    # 获取内置变换矩阵（列优先存储）
    full_matrix = robot.getOrientation()
    # 提取3x3旋转矩阵
    rot_matrix = [
        [full_matrix[0], full_matrix[4], full_matrix[8]],
        [full_matrix[1], full_matrix[5], full_matrix[9]],
        [full_matrix[2], full_matrix[6], full_matrix[10]]
    ]
    # 点云转换逻辑同方案1
    lidar_points = lidar.getPointCloud()
    global_points = []
    for p in lidar_points:
        x_rot = rot_matrix[0][0]*p[0] + rot_matrix[0][1]*p[1] + rot_matrix[0][2]*p[2]
        y_rot = rot_matrix[1][0]*p[0] + rot_matrix[1][1]*p[1] + rot_matrix[1][2]*p[2]
        z_rot = rot_matrix[2][0]*p[0] + rot_matrix[2][1]*p[1] + rot_matrix[2][2]*p[2]
        global_points.append([
            x_rot + gps_pos[0],
            y_rot + gps_pos[1],
            z_rot + gps_pos[2]
        ])

关键注意事项 #

• Webots坐标系遵循右手定则，全局Z轴向上
• Compass数据易受场景内金属物体干扰，高精度场景建议搭配IMU获取完整姿态
• 若激光雷达与机器人基座存在相对位姿偏移，需先将雷达点云转换到机器人局部坐标系，再做全局变换

内容的提问来源于stack exchange，提问作者RavenCloud