全局坐标转局部坐标推导

全局坐标转局部坐标

问题定义：

设全局坐标系为 \(O_{world}\)，自车当前状态为：

• 位置：\((x_c, y_c)\)
• 朝向：\(\theta_c\)（与全局 X 轴的夹角，逆时针为正）

目标点状态为：

• 位置：\((x_t, y_t)\)
• 朝向：\(\theta_t\)

Step1： 平移

先将原点平移到自车位置，得到目标点在全局系下的相对偏移：

\[\begin{bmatrix} dx \ dy \end{bmatrix} = \begin{bmatrix} x_t - x_c \ y_t - y_c \end{bmatrix} \]

Step 2：旋转

全局坐标系旋转 \(\theta_c\) 后与自车局部坐标系对齐。要把全局偏移量转到局部系，需要反向旋转 \(-\theta_c\)，即乘以旋转矩阵 \(R(-\theta_c)\)：

\[R(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \ \sin\theta & \cos\theta \end{bmatrix} \quad \Rightarrow \quad R(-\theta_c) = \begin{bmatrix} \cos\theta_c & \sin\theta_c \ -\sin\theta_c & \cos\theta_c \end{bmatrix} \]

因此：

\[\begin{bmatrix} x_{local} \ y_{local} \end{bmatrix} = R(-\theta_c) \begin{bmatrix} dx \ dy \end{bmatrix} = \begin{bmatrix} \cos\theta_c & \sin\theta_c \ -\sin\theta_c & \cos\theta_c \end{bmatrix} \begin{bmatrix} dx \ dy \end{bmatrix} \]

展开得：

\[\boxed{ x_{local} = dx \cdot \cos\theta_c + dy \cdot \sin\theta_c } \]

\[\boxed{ y_{local} = -dx \cdot \sin\theta_c + dy \cdot \cos\theta_c } \]

代码实现如下：

def to_local_coords(target_x, target_y, target_yaw, curr_x, curr_y, curr_yaw):
    """全局坐标转换到自车局部坐标系"""
    curr_yaw = np.deg2rad(curr_yaw)
    target_yaw = np.deg2rad(target_yaw)
    dx = target_x - curr_x
    dy = target_y - curr_y
    local_x = dx * np.cos(curr_yaw) + dy * np.sin(curr_yaw)
    local_y = -dx * np.sin(curr_yaw) + dy * np.cos(curr_yaw)
    local_yaw = target_yaw - curr_yaw
    return np.array([local_x, local_y, np.cos(local_yaw), np.sin(local_yaw)])

旋转矩阵的推导

问题设定

设有一个向量 \(\vec{v}\)，其在原坐标系中的坐标为 \((x, y)\)，与 X 轴的夹角为 \(\alpha\)，模长为 \(r\)：

\[x = r\cos\alpha, \quad y = r\sin\alpha \]

现在将坐标系逆时针旋转 \(\theta\) 角（等价于向量顺时针旋转 \(\theta\) 角），求新坐标 \((x', y')\)。

---

Step 1：用极坐标表示原向量

\[\vec{v} = (r\cos\alpha,\ r\sin\alpha) \]

---

Step 2：旋转后用极坐标表示新向量

旋转后，向量与 X 轴夹角变为 \(\alpha + \theta\)，模长不变：

\[x' = r\cos(\alpha + \theta) \]

\[y' = r\sin(\alpha + \theta) \]

---

Step 3：展开三角函数

\[x' = r\cos(\alpha + \theta) = r(\cos\alpha\cos\theta - \sin\alpha\sin\theta) \]

\[y' = r\sin(\alpha + \theta) = r(\sin\alpha\cos\theta + \cos\alpha\sin\theta) \]

将 \(r\cos\alpha = x\)，\(r\sin\alpha = y\) 代入：

\[x' = x\cos\theta - y\sin\theta \]

\[y' = x\sin\theta + y\cos\theta \]

---

Step 4：写成矩阵形式

\[\begin{bmatrix} x' \ y' \end {bmatrix} =\underbrace{\begin{bmatrix} \cos\theta & -\sin\theta \ \sin\theta & \cos\theta \end{bmatrix}}_{R(\theta)} \begin{bmatrix} x \ y \end{bmatrix} \]

这就是逆时针旋转 \(\theta\) 的旋转矩阵 \(R(\theta)\)。