原文链接：https://www.toutiao.com/article/7505704799000232460/?channel=&source=news

在自动驾驶中存在多种坐标系表示方式，也经常需要用到坐标系变化，本文对此做一个总结。

–1 坐标系定义：

自动驾驶中我们会用到哪些坐标系呢？

（1）世界坐标系

地理坐标系：WGS84 是地心坐标系 即经纬度、海拔 单位度

投影坐标系：UTM 通用横轴墨卡托投影坐标 单位是米

（2）车体坐标系

用于描述车辆周围和本车相对位置关系，右手定则，前向x 左侧y 上方z。

（3）Frenet坐标系

在 Frenet 坐标系中，s 代表沿道路的距离称为纵坐标，d表示与纵向线的位移称为横坐标。

（4）相机相关坐标系

相机坐标系：一个三维直角坐标系，原点位于镜头的光心处，x,y分别与像面的两边平行，Z轴为镜头光轴，与像面垂直。

图像坐标系：一个二维直角坐标系，原点是光轴与像面的交点（又称主点），即图像的中心点，x,y分别与像面的两边平行。

像素坐标系：一个二维直角坐标系，反应了相机（CCD/CMOS）芯片中像素的排列情况。原点位于图像的左上角，下x,y分别平行与像面。像素坐标与图像坐标实际是平移的关系（下面会详细描述）。像素坐标中坐标轴单位为像素。

（5）激光雷达坐标系：球坐标系，径向距离 r 俯仰角 θ 方位角 ϕ 组成。

–2 坐标系变化的常用操作：

这里介绍常见的两种：

（1）点的旋转与平移 （主动变换）

（2）点的位置不变，其相对的坐标系旋转与平移 （被动变换）

–3 点变换数学表达：

（1）基于函数方程进行表达：

• 原始点 P=(x,y)
• 绕原点逆时针旋转角度 θ
• 再平移向量 t=(tx,ty)
• 得到新点 P′=(x′,y′)

（2）基于变换矩阵进行表达

使用齐次坐标，可以将旋转和平移统一为一个矩阵乘法：

–4 坐标系变换数学表达：

（1）基于函数方程进行表达：

• 点 P 的实际位置不变；
• 新坐标系由原坐标系绕原点逆时针旋转 θ，并平移到 t=(tx,ty)
• 求点 P 在新坐标系下的坐标 (x′,y′)

（2）基于变换矩阵进行表达：

–5 车体坐标系和世界坐标变换：

坐标系变换其实不太好理解什么时候应该先旋转再平移，什么时候应该先平移再旋转，这里距离自动驾驶中实际应该的车体坐标到世界坐标系变换的案例来帮助理解。

下诉代码展示了，将车体坐标系转到世界坐标系以及将世界坐标系再转回车体坐标系的过程。

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle, FancyArrow

# 定义主车和目标车的参数
main_car_length = 4  # 主车长度
main_car_width = 2  # 主车宽度
main_car_dx = 1  # 主车在世界坐标系中的x位置
main_car_dy = 1  # 主车在世界坐标系中的y位置
main_car_theta = 30  # 主车在世界坐标系中的朝向角度（度）

target_car_length = 4  # 目标车长度
target_car_width = 2  # 目标车宽度
target_car_dx = 10  # 目标车在车体坐标系中的x位置
target_car_dy = 5  # 目标车在车体坐标系中的y位置
target_car_theta = 30  # 目标车在车体坐标系中的朝向角度（度）


# 将目标车从车体坐标系转换到世界坐标系
def car_body_to_world(dx, dy, theta, main_car_dx, main_car_dy, main_car_theta):
    # 将角度转换为弧度
    theta_rad = np.radians(theta)
    main_car_theta_rad = np.radians(main_car_theta)

    # 旋转矩阵
    rotation_matrix = np.array(
        [
            [np.cos(main_car_theta_rad), -np.sin(main_car_theta_rad)],
            [np.sin(main_car_theta_rad), np.cos(main_car_theta_rad)],
        ]
    )

    # 目标车在车体坐标系中的位置
    target_car_position_body = np.array([dx, dy])

    # 旋转
    target_car_position_rel = rotation_matrix @ target_car_position_body

    # 平移
    target_car_position_world = target_car_position_rel + np.array([main_car_dx, main_car_dy])

    # 目标车在世界坐标系中的朝向角度
    target_car_theta_world = theta + main_car_theta

    return target_car_position_world, target_car_theta_world


# 将目标车从世界坐标系转换到车体坐标系
def car_world_to_body(x_w, y_w, theta_w, main_car_dx, main_car_dy, main_car_theta):
    # 将角度转换为弧度
    main_car_theta_rad = np.radians(main_car_theta)

    # 逆旋转矩阵
    inverse_rotation_matrix = np.array(
        [
            [np.cos(main_car_theta_rad), np.sin(main_car_theta_rad)],
            [-np.sin(main_car_theta_rad), np.cos(main_car_theta_rad)],
        ]
    )

    # 目标车在世界坐标系中的位置
    target_car_position_world = np.array([x_w, y_w])

    # 平移
    target_car_position_rel = target_car_position_world - np.array([main_car_dx, main_car_dy])

    # 逆旋转
    target_car_position_body = inverse_rotation_matrix @ target_car_position_rel

    # 目标车在车体坐标系中的朝向角度
    target_car_theta_body = theta_w - main_car_theta

    return target_car_position_body, target_car_theta_body


# 绘制车辆
def draw_car(ax, position, length, width, theta, color="blue"):
    # 将角度转换为弧度
    theta_rad = np.radians(theta)

    # 计算矩形的四个角
    corners = np.array(
        [[-length / 2, -width / 2], [length / 2, -width / 2], [length / 2, width / 2], [-length / 2, width / 2]]
    )

    # 旋转矩形
    rotation_matrix = np.array([[np.cos(theta_rad), -np.sin(theta_rad)], [np.sin(theta_rad), np.cos(theta_rad)]])
    rotated_corners = corners @ rotation_matrix.T

    # 平移矩形
    translated_corners = rotated_corners + position

    # 绘制矩形
    rect = Rectangle(position - np.array([length / 2, width / 2]), length, width, angle=theta, color=color, fill=False)
    ax.add_patch(rect)

    # 绘制航向箭头
    arrow_length = 1.5
    arrow_dx = arrow_length * np.cos(theta_rad)
    arrow_dy = arrow_length * np.sin(theta_rad)
    arrow = FancyArrow(position[0], position[1], arrow_dx, arrow_dy, color=color, width=0.2)
    ax.add_patch(arrow)

    # 标注车辆位置和航向
    ax.text(
        position[0],
        position[1] - 1,
        f"({position[0]:.2f}, {position[1]:.2f})\nθ={theta:.2f}°",
        fontsize=12,
        color=color,
    )


# 创建图形和轴
fig, ax = plt.subplots(figsize=(10, 10))
ax.set_xlim(-20, 20)
ax.set_ylim(-20, 20)
ax.set_aspect("equal")
ax.grid(True)

# 绘制主车
main_car_position = np.array([main_car_dx, main_car_dy])
draw_car(ax, main_car_position, main_car_length, main_car_width, main_car_theta, color="red")

# 车体坐标系到世界坐标系
target_car_position_world, target_car_theta_world = car_body_to_world(
    target_car_dx, target_car_dy, target_car_theta, main_car_dx, main_car_dy, main_car_theta
)
draw_car(ax, target_car_position_world, target_car_length, target_car_width, target_car_theta_world, color="blue")

# 世界坐标系到车体坐标系
target_car_position_body, target_car_theta_body = car_world_to_body(
    target_car_position_world[0],
    target_car_position_world[1],
    target_car_theta_world,
    main_car_dx,
    main_car_dy,
    main_car_theta,
)
draw_car(ax, target_car_position_body, target_car_length, target_car_width, target_car_theta_body, color="gray")

# 打印主车和目标车的信息
print(f"主车的世界坐标: ({main_car_dx}, {main_car_dy})")
print(f"主车的航向: {main_car_theta}°")
print(f"目标车在车体坐标系下的坐标: ({target_car_dx}, {target_car_dy})")
print(f"目标车在车体坐标系下的航向: {target_car_theta}°")
print(f"目标车在世界坐标系下的坐标: ({target_car_position_world[0]:.2f}, {target_car_position_world[1]:.2f})")
print(f"目标车在世界坐标系下的航向: {target_car_theta_world:.2f}°")

# 添加图例
ax.legend(["Main Car", "Target Car in World", "Target Car in Body"])

# 显示图形
plt.show()

–5.1 车体坐标系下的目标变换到世界坐标系下表达：先旋转再平移，因为你知道的是主车在世界坐标系下的坐标，这个时候你希望将目标车所属的车体坐标系旋转变换到世界坐标系下，结合你知道的信息，你需要先旋转到和世界坐标系一样的朝向，然后再根据主车的车体坐标进行平移。

–5.2 世界坐标系下的目标要在车体坐标系下表达：先平移再旋转，因为你知道的是主车在世界坐标系下的坐标，这个时候你希望将目标车所属的世界坐标系旋转变换到主车下，结合你知道的信息，如果你先旋转，那你旋转之后的需要的平移值其实你是不知道的，它和你目前知道的主车坐标不是一个数，因此你只能先用现在知道的主车坐标先平移再旋转。

综上：

1. 旋转 + 平移都是在现有坐标系下进行的
2. 旋转和平移的顺序，取决于你知道的平移坐标是哪个坐标系相对于哪个坐标系的