2、坐标系与坐标变换：车辆坐标系、世界坐标系、欧拉角与旋转矩阵、齐次变换

做车辆控制这些年，我最大的感触就是：坐标系搞不清楚，代码写得再漂亮也是白搭。你想想看，你让车往左转，结果它往右跑了——十有八九是坐标系定义反了。今天咱们就把这个基础打牢。

2.1 车辆坐标系——车自己的"小世界"

每辆车都有自己的坐标系。我习惯这么定义：

• X轴：车头方向为正
• Y轴：车身左侧为正（注意！很多教材用右侧，但我个人觉得左侧更符合右手定则）
• Z轴：车顶方向为正

原点一般放在后轴中心，或者质心位置。嗯，这里要注意：后轴中心更常用，因为阿克曼转向几何的推导会方便很多。

2.2 世界坐标系——车在哪儿？

世界坐标系就是大地坐标系。说白了，就是告诉车：你现在在哪个经纬度，朝哪个方向。

常用的有：

• ENU坐标系：东-北-天，适合小范围场景
• NED坐标系：北-东-地，航空航天常用
• UTM坐标系：通用横轴墨卡托，适合大范围

我个人做自动驾驶时，ENU用得最多。为什么？因为直观。东就是东，北就是北，不用绕来绕去。

2.3 欧拉角与旋转矩阵——让车"转"起来

车辆坐标系和世界坐标系之间怎么转换？靠旋转。

欧拉角有三个：

• 横摆角（Yaw）：绕Z轴旋转，也就是车头的朝向
• 俯仰角（Pitch）：绕Y轴旋转，上下坡时用
• 侧倾角（Roll）：绕X轴旋转，过弯时车身侧倾

旋转顺序我习惯用Z-Y-X（横摆→俯仰→侧倾）。为什么是这个顺序？因为车辆运动时，横摆角变化最快，放在最后处理可以减少计算误差。

旋转矩阵长这样（绕Z轴旋转θ角）：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

嗯，看着眼熟吧？其实就是二维旋转加了个Z轴维度。

2.4 齐次变换——把旋转和平移打包

光有旋转不够，车还会移动。齐次变换就是把旋转矩阵和平移向量塞到一个4x4矩阵里：

T = [R   t]
    [0   1]

其中R是3x3旋转矩阵，t是3x1平移向量。

举个例子：世界坐标系下有个点P_w，想转到车辆坐标系：

P_v = T_vw * P_w

这里的T_vw就是从世界到车辆的齐次变换矩阵。

2.5 实战：坐标系转换的Python实现

光说不练假把式。咱们写个简单的转换函数：

import numpy as np
from scipy.spatial.transform import Rotation as R

def vehicle_to_world(x_v, y_v, yaw, x_w_origin, y_w_origin):
    """
    将车辆坐标系下的点转换到世界坐标系
    :param x_v, y_v: 车辆坐标系下的点
    :param yaw: 车辆横摆角 (rad)
    :param x_w_origin, y_w_origin: 车辆在世界坐标系下的位置
    :return: 世界坐标系下的点 (x_w, y_w)
    """
    # 构建旋转矩阵（绕Z轴）
    rot = R.from_euler('z', yaw).as_matrix()[:2, :2]
    
    # 旋转
    point_rotated = rot @ np.array([x_v, y_v])
    
    # 平移
    x_w = point_rotated[0] + x_w_origin
    y_w = point_rotated[1] + y_w_origin
    
    return x_w, y_w

# 测试
x_w, y_w = vehicle_to_world(1.0, 0.0, np.pi/4, 10.0, 5.0)
print(f"世界坐标: ({x_w:.2f}, {y_w:.2f})")
# 输出: 世界坐标: (10.71, 5.71)

这段代码我用了好几年，基本没出过问题。你拿去用的时候，记得检查角度单位——我吃过这个亏，一度把弧度当角度传进去了，结果车在原地转圈。

2.6 常见坑点总结

坑点	后果	解决方法
坐标系方向搞反	控制量反向	统一用右手定则，写注释
旋转顺序错误	姿态计算偏差	固定用Z-Y-X顺序
角度单位混淆	车乱转	全程用弧度，接口处转换
齐次变换顺序颠倒	位置偏移	先旋转后平移

好了，坐标系这块就聊到这儿。下一章咱们讲车辆运动学模型——到时候你会发现，坐标系搞清楚了，模型推导就是水到渠成的事。