全局坐标转局部坐标
问题定义：
设全局坐标系为 \(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_xdy = target_y - curr_ylocal_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_yawreturn 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)\)。