一个物体进行了位移、旋转计算另一个物体在其局部坐标系中的相对坐标

问题

ego是一个按照车头朝向定义自身坐标正方向的一个车，他的全局坐标初始的位置是（0，0），初始的方向即是y轴正方向，现在已知他所旋转的yaw角度和位移（x,y）, 现在知道一个障碍物的全局坐标（x，y），现在需要计算车旋转位移后，相对于车自身坐标系中，障碍物的相对坐标为多少。可以计算车的旋转矩阵和位移矩阵再应用到障碍物坐标上
这里使用的是python代码

def relative_position_compute(self,ego,current_obstacle):
          # 获取车辆的全局位置和朝向
          ego_x = ego.position.position_local.x
          ego_y = ego.position.position_local.y
          ego_yaw = ego.yaw
          
          # 构建旋转矩阵，用于将全局坐标转换到车辆的本地坐标系中
          # 由于车辆坐标系与全局坐标系的y轴方向一致，且顺时针旋转为负，
          # 我们需要用“-ego_yaw”来构造逆时针旋转矩阵
          cos_yaw = math.cos(-ego_yaw)
          sin_yaw = math.sin(-ego_yaw)
          rotation_matrix = [
              [cos_yaw, -sin_yaw],
              [sin_yaw, cos_yaw]
          ]
          
          # 获取障碍物的全局位置
          obstacle_x = current_obstacle.x
          obstacle_y = current_obstacle.y
          
          # 计算障碍物相对于车辆全局位置的差值
          obstacle_dx = obstacle_x - ego_x
          obstacle_dy = obstacle_y - ego_y
          
          # 应用旋转矩阵到障碍物的差值坐标上
          local_obstacle_x = rotation_matrix[0][0] * obstacle_dx + rotation_matrix[0][1] * obstacle_dy
          local_obstacle_y = rotation_matrix[1][0] * obstacle_dx + rotation_matrix[1][1] * obstacle_dy
          
          # 返回障碍物相对于车辆本地坐标系的位置
          return local_obstacle_x, local_obstacle_y

解释

在二维空间中，一个点的位置可以通过一个坐标 (x, y) 来表示。如果我们有两个坐标系：一个全局坐标系和一个局部坐标系（例如车辆自身的坐标系），并且局部坐标系相对于全局坐标系进行了旋转和位移，那么在全局坐标系中观测到的一个点，在局部坐标系中的坐标将不同。

在这种情况下：

• 车辆的全局位置 (ego_x, ego_y) 表示车辆在全局坐标系中的位置。
• 车辆的朝向 ego_yaw 表示车辆相对于全局y轴正方向逆时针旋转的角度（因为车头初始朝向是y轴正方向）。
  现在，如果我们要找出某个障碍物相对于车辆自身坐标系的位置，我们需要做两件事：

1. 计算差值向量：即将障碍物的全局坐标减去车辆的全局坐标。这个过程简单说就是把车辆位置设置为局部坐标系的原点 (0, 0)，然后看障碍物在这个新坐标系下的位置在哪里。此时，障碍物的坐标变成了 (obstacle_x - ego_x, obstacle_y - ego_y)。

2. 应用旋转矩阵：但是，这个新的坐标系还没有考虑车辆的朝向。车辆可能不再是朝着全局y轴的正方向，而是旋转了 ego_yaw 角度。为了找到障碼物在车辆自身坐标系中的相对位置，我们需要将这个新坐标系按照 -ego_yaw 角度旋转回来，确保它与车辆的朝向一致。为此，我们使用旋转矩阵，该矩阵根据车辆的旋转角度来旋转点的坐标。

旋转矩阵是线性代数中的一个概念，用于描述坐标点围绕原点旋转的运算。在二维空间中，一个点 (x, y) 围绕原点逆时针旋转 θ 角度后的新坐标 (x’, y’) 可以通过以下公式计算得到：

x' = x * cos(θ) - y * sin(θ)
y' = x * sin(θ) + y * cos(θ)

这里的 (x’, y’) 是旋转后的新坐标，而 (x, y) 是旋转前的坐标。θ 是旋转角度，正值表示逆时针旋转，负值表示顺时针旋转。

既然车辆已经旋转了 ego_yaw 角度，我们需要一个旋转矩阵来逆向旋转障碍物的差值向量，使其与车辆坐标系的朝向对齐。因此，我们用 -ego_yaw 来构造旋转矩阵，这样得到的障碍物在车辆坐标系中的位置就正确地反映了它们之间的相对关系。