📚 课程目录第4章点云坐标变换

4. 点云坐标变换

任务目标

无人机机载激光雷达获取的点云位于雷达自身坐标系（通常为前-左-上或前-右-下，取决于安装朝向），而飞控姿态解算与导航均在机体坐标系（前-右-下，即FRD）下进行。本案例实现将一帧点云从雷达坐标系变换到机体坐标系的刚性变换，包含旋转矩阵与平移向量，并给出可验证的最小实现。

坐标系定义与变换原理

设雷达坐标系为L，机体坐标系为B。雷达相对机体的安装位置由平移向量 t_BL（雷达原点在机体系中的坐标）和旋转矩阵 R_BL（将雷达系向量旋转到机体系）描述。对雷达系中的任意点 p_L，其在机体系中的坐标为：

p_B = R_BL · p_L + t_BL

本案例中，雷达安装于机体正下方，x轴与机体x轴同向，y轴与机体y轴反向（即雷达前-左-上坐标系对应机体前-右-下），z轴向下。安装偏移为 t_BL = [0.1, 0.0, -0.2] 米（雷达位于机体前方10cm、下方20cm处）。

关键完整代码

import numpy as np

def rotation_from_axes(x_axis, y_axis, z_axis):
    """由机体系中雷达三轴方向构造旋转矩阵 R_BL（列向量为轴方向）"""
    return np.column_stack((x_axis, y_axis, z_axis))

def lidar_to_body(points_lidar, R_BL, t_BL):
    """
    将雷达坐标系点云变换到机体坐标系
    points_lidar: (N, 3) 每行为 [x, y, z]
    R_BL: (3, 3) 旋转矩阵
    t_BL: (3,) 平移向量
    返回: (N, 3) 机体坐标系下的点云
    """
    return (R_BL @ points_lidar.T).T + t_BL

# 雷达安装姿态：雷达x轴=机体x轴，雷达y轴=-机体y轴，雷达z轴=-机体z轴
x_axis_B = np.array([1.0, 0.0, 0.0])   # 雷达x轴在机体系中的方向
y_axis_B = np.array([0.0, -1.0, 0.0])  # 雷达y轴在机体系中的方向
z_axis_B = np.array([0.0, 0.0, -1.0])  # 雷达z轴在机体系中的方向
R_BL = rotation_from_axes(x_axis_B, y_axis_B, z_axis_B)

t_BL = np.array([0.1, 0.0, -0.2])  # 米

# 示例输入：雷达正前方2米处一点（雷达系x=2, y=0, z=0）
points_lidar = np.array([[2.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0],   # 雷达系y=1（雷达左侧）
                         [0.0, 0.0, -0.5]]) # 雷达系z=-0.5（雷达上方）

points_body = lidar_to_body(points_lidar, R_BL, t_BL)
print("机体坐标系下的点云：")
print(points_body)

运行方式

需要 numpy 库。将上述代码保存为 lidar_to_body.py，在终端执行：

python lidar_to_body.py

验证方法与判定标准

代码输出应严格满足以下几何关系（可手算验证）：

点 [2, 0, 0]（雷达正前方）→ 机体系 [2.1, 0, -0.2]（前方+安装偏移）
点 [0, 1, 0]（雷达左侧1m）→ 机体系 [0.1, -1, -0.2]（机体右侧，因雷达y轴与机体y轴反向）
点 [0, 0, -0.5]（雷达上方0.5m）→ 机体系 [0.1, 0, 0.3]（机体上方0.3m，因雷达z轴向下且安装低20cm）

运行程序后，观察输出是否与上述三个预期坐标一致（允许浮点精度误差 ±1e-10）。若一致，则变换实现正确。实际工程中，R_BL 与 t_BL 应通过标定或CAD安装图获得，本案例使用固定安装参数作为演示。

上图展示了本案例的安装几何关系：雷达x轴与机体x轴同向，雷达y轴与机体y轴反向（因此雷达右侧对应机体左侧），雷达z轴与机体z轴反向（图中未画出，雷达朝下）。变换时先旋转（由 R_BL 实现轴对齐），再平移（叠加 t_BL）。