自动驾驶中的坐标系转换与速度计算原理

1. 坐标系转换基础概念

在自动驾驶和机器人领域，坐标系转换是最基础也是最重要的数学工具之一。我们经常需要在不同参考系之间转换运动状态，特别是在处理传感器数据时。全局坐标系（通常称为世界坐标系或地面坐标系）是一个固定的参考系，而自车坐标系则是随着车辆移动和旋转的动态参考系。

1.1 坐标系定义

全局坐标系(G)：这是一个固定的参考系，通常以场景中的某个固定点作为原点，比如地图的起始点或停车场的某个角落。在二维情况下，我们常用x轴指向东，y轴指向北。

自车坐标系(B)：这是固定在车辆上的坐标系，通常定义如下：

• x轴：指向车辆前进方向（车头方向）
• y轴：指向车辆左侧
• z轴：垂直于地面向上（右手坐标系）

注意：不同厂商可能有不同的坐标系定义，有些系统可能将y轴指向右侧。在实际应用中，必须首先确认坐标系定义，否则会导致后续所有计算错误。

1.2 基本转换原理

坐标系转换的核心是理解"相对运动"的概念。当我们要将一个物体的速度从全局坐标系转换到自车坐标系时，实际上是在问：如果站在车辆的角度观察，这个物体的运动看起来是怎样的？

这涉及到两个关键步骤：

1. 计算物体相对于车辆的速度（在全局坐标系下）
2. 将这个相对速度转换到车辆自身的坐标系中

2. 基本速度转换方法

2.1 变量定义

让我们先明确一些基本变量：

• 自车位置：$P_{ego} = (x,y)^T$（全局坐标系下）
• 自车朝向：$\theta$（heading，相对于全局坐标系x轴的夹角）
• 自车速度标量：$speed$
• 自车速度向量（全局坐标系）：$\mathbf{v}_{ego}^G = (speed\cos\theta, speed\sin\theta)^T$
• 目标A的速度（全局坐标系）：$\mathbf{v}_A^G = (v_x, v_y)^T$
• 目标A的速度（自车坐标系）：$\mathbf{v}_A^B$

2.2 转换步骤详解

步骤1：计算相对速度（全局坐标系下）

由于自车本身也在运动，我们需要先计算目标相对于自车的速度：

$$
\mathbf{v}_{rel}^G = \mathbf{v}A^G - \mathbf{v}^G
$$

展开分量形式：

$$
\begin{pmatrix}v_{rel_x} \ v_{rel_y}\end{pmatrix} = \begin{pmatrix} v_x - speed\cos\theta \ v_y - speed\sin\theta\end{pmatrix}
$$

这个相对速度仍然是在全局坐标系下表达的。

步骤2：坐标系旋转

为了得到自车坐标系下的速度，我们需要将这个相对速度旋转到自车坐标系中。这通过旋转矩阵实现：

$$
\mathbf{v}A^B = \mathbf{R} \cdot \mathbf{v}^G
$$

从全局到自车坐标系的旋转矩阵为：

$$
\mathbf{R} = R(-\theta) = \begin{pmatrix} \cos\theta & \sin\theta \ -\sin\theta & \cos\theta \end{pmatrix}
$$

为什么是这个形式？因为我们要将全局坐标系的向量"反向"旋转θ角度，相当于应用一个-θ的旋转。

步骤3：完整计算公式

将上述两步结合起来，得到完整的转换公式：

$$
\mathbf{v}_A^B = \begin{pmatrix} \cos\theta & \sin\theta \ -\sin\theta & \cos\theta \end{pmatrix} \begin{pmatrix} v_x - speed\cos\theta \ v_y - speed\sin\theta\end{pmatrix}
$$

展开后：

$$
v_{A,x}^B = (v_x - speed\cos\theta)\cos\theta + (v_y - speed\sin\theta)\sin\theta \
v_{A,y}^B = -(v_x - speed\cos\theta)\sin\theta + (v_y - speed\sin\theta)\cos\theta
$$

2.3 几何解释

这个过程可以直观理解为：

1. 首先"减去"自车的运动，得到目标相对于自车的运动
2. 然后"旋转视角"，从自车的角度观察这个相对运动

3. 考虑横摆角速度的进阶转换

在实际车辆运动中，除了直线运动外，还有旋转运动（转弯）。这时就需要考虑横摆角速度（yaw rate）带来的影响。

3.1 新增变量定义

• 自车的横摆角速度（yaw rate）：$\omega$（单位：rad/s，正值表示逆时针旋转）
• 目标A相对于自车的位置：$\mathbf{r} = (r_x, r_y)^T$（自车坐标系下）

3.2 旋转补偿原理

当坐标系本身在旋转时，经典力学告诉我们，观测到的相对速度需要加上旋转带来的牵连速度。完整的速度关系为：

$$
\mathbf{v}A^G = \mathbf{v}^G + \mathbf{\omega} \times \mathbf{r} + \mathbf{v}_A^B
$$

其中：

• $\mathbf{v}_A^G$：目标在全局坐标系下的速度
• $\mathbf{v}_{ego}^G$：自车在全局坐标系下的速度
• $\mathbf{\omega} \times \mathbf{r}$：旋转补偿项
• $\mathbf{v}_A^B$：目标在自车坐标系下的速度

重新排列得到：

$$
\mathbf{v}_A^B = \mathbf{v}A^G - \mathbf{v}^G - \mathbf{\omega} \times \mathbf{r}
$$

3.3 旋转补偿项计算

在二维情况下，角速度向量$\mathbf{\omega}$垂直于地面（只有z分量），相对位置向量$\mathbf{r}$在平面内。叉乘结果为：

$$
\mathbf{\omega} \times \mathbf{r} = (-\omega r_y, \omega r_x)^T
$$

这个结果的物理意义是：

• 如果车辆正在左转（$\omega > 0$），在自车坐标系中会看到静止物体向右移动
• 如果车辆正在右转（$\omega < 0$），会看到静止物体向左移动

3.4 完整转换公式

结合旋转和平移，完整的转换公式为：

$$
\mathbf{v}_A^B = \mathbf{R}(\theta) \cdot (\mathbf{v}A^G - \mathbf{v}^G) - \mathbf{\omega} \times \mathbf{r}
$$

其中$\mathbf{R}(\theta)$是之前定义的旋转矩阵。

4. 实际应用中的注意事项

4.1 坐标系一致性

在实际系统中，必须确保所有变量使用相同的坐标系：

• 位置和速度必须在同一全局坐标系下
• 角度定义必须一致（通常是x轴为0，逆时针为正）
• 时间同步：确保所有状态量是同一时刻的

4.2 传感器安装位置补偿

如果传感器（如雷达、相机）的安装位置与自车坐标系原点不一致，需要进行额外补偿：

$$
\mathbf{v}_{sensor}^B = \mathbf{v}A^B + \mathbf{\omega} \times \mathbf{r}
$$

其中$\mathbf{r}_{sensor}$是传感器相对于自车坐标系原点的位置。

4.3 数值稳定性处理

在实际代码实现中，需要注意：

• 小角度近似：当$\theta$很小时，可以使用近似$\sin\theta \approx \theta$，$\cos\theta \approx 1$
• 角度归一化：确保heading角在合理范围内（如$[-\pi, \pi]$）
• 异常值处理：对输入速度进行合理性检查

5. 代码实现示例

以下是Python实现的示例代码：

python复制import numpy as np

def global_to_vehicle_frame(v_global, ego_state):
    """
    将全局坐标系下的速度转换到自车坐标系
    参数:
        v_global: 目标在全局坐标系下的速度 [vx, vy]
        ego_state: 自车状态字典，包含:
            'x': 全局x位置
            'y': 全局y位置
            'heading': 朝向角(弧度)
            'speed': 速度大小
            'yaw_rate': 横摆角速度(可选)
            'rx': 目标相对位置x(自车坐标系，可选)
            'ry': 目标相对位置y(自车坐标系，可选)
    返回:
        v_vehicle: 目标在自车坐标系下的速度 [vx, vy]
    """
    # 计算自车速度向量
    ego_heading = ego_state['heading']
    v_ego = np.array([
        ego_state['speed'] * np.cos(ego_heading),
        ego_state['speed'] * np.sin(ego_heading)
    ])
    
    # 计算相对速度(全局坐标系)
    v_rel_global = np.array(v_global) - v_ego
    
    # 构建旋转矩阵
    R = np.array([
        [np.cos(ego_heading), np.sin(ego_heading)],
        [-np.sin(ego_heading), np.cos(ego_heading)]
    ])
    
    # 初步转换
    v_vehicle = R @ v_rel_global
    
    # 如果有横摆角速度和相对位置，进行旋转补偿
    if 'yaw_rate' in ego_state and 'rx' in ego_state and 'ry' in ego_state:
        omega = ego_state['yaw_rate']
        rx, ry = ego_state['rx'], ego_state['ry']
        # 计算旋转补偿项
        v_rot = np.array([-omega * ry, omega * rx])
        v_vehicle -= v_rot
    
    return v_vehicle

6. 常见问题与调试技巧

6.1 结果验证方法

验证坐标系转换是否正确，可以采用以下方法：

1. 静态目标测试：设置一个静止的全局目标，检查自车坐标系下的速度是否合理
2. 自车静止测试：自车速度为0时，转换结果应与简单旋转一致
3. 对称性测试：转换后再反向转换，应得到原始值

6.2 典型错误排查

1. 角度符号错误：检查heading角定义是否符合约定
2. 坐标系定义不一致：确认所有输入数据使用相同的坐标系
3. 时间不同步：确保速度、位置、角度是同一时刻的
4. 单位不一致：检查角度是弧度还是度，速度单位是否统一

6.3 性能优化建议

对于需要高频计算的场景：

• 预先计算旋转矩阵
• 使用查表法近似三角函数
• 考虑使用固定点数运算
• 利用SIMD指令并行计算

7. 扩展应用

7.1 三维空间扩展

在三维情况下，原理相同但计算更复杂：

• 旋转矩阵变为3x3
• 角速度向量有3个分量
• 叉乘计算更复杂

7.2 多传感器融合

在自动驾驶中，常需要融合多个传感器的速度观测：

1. 将所有观测转换到统一坐标系
2. 考虑各传感器的噪声特性
3. 使用时序信息进行滤波

7.3 动态障碍物预测

准确的速度转换是预测障碍物运动的基础：

1. 将障碍物速度转换到自车坐标系
2. 基于相对运动进行轨迹预测
3. 考虑不确定性传播

在实际工程实现中，我发现最常出现的问题是坐标系定义不一致。不同团队、不同传感器可能使用不同的坐标系约定，这会导致难以察觉的错误。建议在系统设计早期就明确坐标系定义，并编写完善的文档和测试用例。