坐标系转换公式

坐标系转换2种情况：

一、XOY坐标系不动，点P(x, y) 沿顺时针方向旋转 θ \thetaθ，得在XOY坐标系的坐标为P'(x′, y′)

设某点与原点连线和X轴夹角为A度，以原点为圆心，逆时针转过θ度  , 原点与该点连线长度为R, [x,y]为变换前坐标， [X,Y]为变换后坐标。

  x = Rcos(A) ; y = Rsin(A);

  X = Rcos(A+θ) = RcosAcosθ - RsinAsinθ = xcosθ - ysinθ; (合角公式)

  Y = Rsin(A+θ) = RsinAcosθ + RcosAsinθ = xsinθ + ycosθ;

  用矩阵表示：

                                cosθ   sinθ  0

 [X, Y, 1] = [x, y, 1][-sinθ  cosθ  0  ] 

                                 0        0     1

  cosθ   sinθ  0

 [-sinθ  cosθ  0]  为旋转变换矩阵

   0       0     1 

参考文档：矩阵运算——平移，旋转，缩放_求矩阵变换的旋转和倾斜-CSDN博客

参考文档：几何变换详解：平移、缩放、旋转_三维轮廓缩放几何表达式-CSDN博客

二、点P(x, y)不动，坐标轴XOY沿着逆时针方向旋转 θ \thetaθ，得到新坐标轴X'OY'，此时点P在X'OY'坐标为(x′, y′) 

即：有点P（Xa，Ya），当坐标由 x –> y 旋转 θ 度后，求该点在新坐标轴的坐标

参考文档：https://www.cnblogs.com/meteoric_cry/p/7987548.html

python示例：

def trans_cord(x_c, y_c, yaw_c, x, y):
    '''
    x_c,y_c,yaw_c 基准车辆在基准（全局）坐标系下的位姿
    x,y 待转换的车辆在基准（全局）坐标系下坐标
    return 待转换车辆在基准车辆基准（全局）坐标系下的相对坐标
    ^Y
    |  |y_c .(x,y)
    |  |.___x_c|yaw_c
    ---------->X
    '''
    cos_theta = np.cos(yaw_c)#弧度
    sin_theta = np.sin(yaw_c)#弧度
    
    x_trans = x - x_c
    y_trans = y - y_c

    x_rotated = x_trans*cos_theta + y_trans*sin_theta
    y_rotated = (-1)*x_trans*sin_theta + y_trans*cos_theta
    
    return x_rotated, y_rotated