全局坐标转局部坐标推导

全局坐标转局部坐标

问题定义：

设全局坐标系为 OworldO_{world}Oworld，自车当前状态为：

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

目标点状态为：

位置：(xt,yt)(x_t, y_t)(xt,yt)
朝向：θt\theta_tθt

Step1：平移

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

dx dy\]=\[xt−xc yt−yc\] \\begin{bmatrix} dx \\ dy \\end{bmatrix} = \\begin{bmatrix} x_t - x_c \\ y_t - y_c \\end{bmatrix} \[dx dy\]=\[xt−xc yt−yc

Step 2：旋转

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

R(θ)= $cosθ-sinθ sinθcosθ$ ⇒R(−θc)= $cosθcsinθc -sinθccosθ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} R(θ)= $cosθ-sinθ sinθcosθ$ ⇒R(−θc)= $cosθcsinθc -sinθccosθc$

因此：

xlocal ylocal\]=R(−θc)\[dx dy\]=\[cos⁡θcsin⁡θc −sin⁡θccos⁡θc\]\[dx dy\] \\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} \[xlocal ylocal\]=R(−θc)\[dx dy\]=\[cosθcsinθc −sinθccosθc\]\[dx dy

展开得：

xlocal=dx⋅cos⁡θc+dy⋅sin⁡θc \boxed{ x_{local} = dx \cdot \cos\theta_c + dy \cdot \sin\theta_c } xlocal=dx⋅cosθc+dy⋅sinθc

ylocal=−dx⋅sin⁡θc+dy⋅cos⁡θc \boxed{ y_{local} = -dx \cdot \sin\theta_c + dy \cdot \cos\theta_c } ylocal=−dx⋅sinθc+dy⋅cosθc

代码实现如下：

python 复制代码

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)])

旋转矩阵的推导

问题设定

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

x=rcos⁡α,y=rsin⁡α x = r\cos\alpha, \quad y = r\sin\alpha x=rcosα,y=rsinα

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

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

v⃗=(rcos⁡α, rsin⁡α) \vec{v} = (r\cos\alpha,\ r\sin\alpha) v =(rcosα, rsinα)

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

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

x′=rcos⁡(α+θ) x' = r\cos(\alpha + \theta) x′=rcos(α+θ)

y′=rsin⁡(α+θ) y' = r\sin(\alpha + \theta) y′=rsin(α+θ)

Step 3：展开三角函数

x′=rcos⁡(α+θ)=r(cos⁡αcos⁡θ−sin⁡αsin⁡θ) x' = r\cos(\alpha + \theta) = r(\cos\alpha\cos\theta - \sin\alpha\sin\theta) x′=rcos(α+θ)=r(cosαcosθ−sinαsinθ)

y′=rsin⁡(α+θ)=r(sin⁡αcos⁡θ+cos⁡αsin⁡θ) y' = r\sin(\alpha + \theta) = r(\sin\alpha\cos\theta + \cos\alpha\sin\theta) y′=rsin(α+θ)=r(sinαcosθ+cosαsinθ)

将 rcos⁡α=xr\cos\alpha = xrcosα=x，rsin⁡α=yr\sin\alpha = yrsinα=y 代入：

x′=xcos⁡θ−ysin⁡θ x' = x\cos\theta - y\sin\theta x′=xcosθ−ysinθ

y′=xsin⁡θ+ycos⁡θ y' = x\sin\theta + y\cos\theta y′=xsinθ+ycosθ

Step 4：写成矩阵形式

x′ y′\]=\[cos⁡θ−sin⁡θ sin⁡θcos⁡θ\]⏟R(θ)\[x y\]\\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} \[x′ y′\]=R(θ) \[cosθ−sinθ sinθcosθ\]\[x y

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

全局坐标转局部坐标推导