自动驾驶坐标变换意义与类型

自动驾驶的关键挑战之一是准确感知和解释车辆周围环境的能力。这需要使用各种传感器，如相机、激光雷达和雷达，来捕捉周围环境的数据。

然而，这些传感器捕获的数据通常位于与车辆自身坐标系不同的坐标系中。这意味着必须将数据转换到车辆的坐标系中，以便对导航和控制有用。

上面提到的BEV感知，通过视觉算法我们可以在每一帧都提取出特征点。如果在时间序列上，我们想利用这些特征点，我们就无法避免的要使用坐标变换了。

自动驾驶中常用的坐标变换有几种类型，包括：

1. 平移

平移包括在同一坐标系中将对象从一个位置移动到另一个位置。在自动驾驶中，平移通常用于对齐激光雷达和相机等不同传感器捕获的数据，使它们处于同一坐标系中。

2. 旋转

旋转包括围绕同一坐标系中的固定点旋转对象。在自动驾驶中，旋转通常用于对齐安装在车辆上不同角度的传感器捕获的数据。

3. 齐次变换

齐次变换包括将平移和旋转组合成单个变换矩阵。在自动驾驶中我们会用到齐次变换，将数据从传感器坐标系转换到车辆坐标系。

为了执行这些坐标变换，使用了各种数学技术，如矩阵乘法和四元数旋转。这些技术是在软件库中实现的，例如C++的Egengen库和Python的NumPy库。

二维数据坐标变换的常见场景

情况1

情况2

python代码

写一个python代码，来验证坐标变换

import matplotlib.pyplot as plt
plt.title('Calibration')
plt.xlabel('x (m)')
plt.ylabel('y (m)')
plt.axis('equal')


# 坐标轴长度
coord_length = 0.2


# 两个车辆位姿
x1 = 1
y1 = 4
theta1 = 0.1
print("car pose1: ["+str(x1), ", "+str(y1) + ", "+str(theta1)+"]")
x2 = 3
y2 = 5
theta2 = 1.3
print("car pose2: ["+str(x2), ", "+str(y2) + ", "+str(theta2)+"]")


# 位姿1下有一个点p
x = 2
y = 1
print("["+str(x), ", "+str(y) + "] in pose1")


# 画位姿1的坐标轴
x_cord = math.cos(theta1) * coord_length + x1
y_cord = math.sin(theta1) * coord_length + y1
plt.plot([x1, x_cord], [y1, y_cord], 'r')
x_cord = math.cos(theta1 + math.pi/2) * coord_length + x1
y_cord = math.sin(theta1 + math.pi/2) * coord_length + y1
plt.plot([x1, x_cord], [y1, y_cord], 'b')
# 画位姿2的坐标轴
x_cord = math.cos(theta2) * coord_length + x2
y_cord = math.sin(theta2) * coord_length + y2
plt.plot([x2, x_cord], [y2, y_cord], 'r')
x_cord = math.cos(theta2 + math.pi/2) * coord_length + x2
y_cord = math.sin(theta2 + math.pi/2) * coord_length + y2
plt.plot([x2, x_cord], [y2, y_cord], 'b')


# p点在世界坐标系下的位置
x_world = math.cos(theta1) * x - math.sin(theta1) * y + x1
y_world = math.sin(theta1) * x + math.cos(theta1) * y + y1
print("["+str(x_world), ", "+str(y_world) + "] in world")
plt.plot(x_world, y_world, 'r*')


# p点在位姿2下的位置
x_ = math.cos(theta2) * (x_world-x2) + math.sin(theta2) * (y_world-y2)
y_ = -math.sin(theta2) * (x_world-x2) + math.cos(theta2) * (y_world-y2)
print("["+str(x_), ", "+str(y_) + "] in pose2")


# 用计算出来的p点位姿2下的位置，来再算出在世界坐标系下的位置
x_world_ = math.cos(theta2) * x_ - math.sin(theta2) * y_ + x2
y_world_ = math.sin(theta2) * x_ + math.cos(theta2) * y_ + y2
print("use 2 to check ["+str(x_world_), ", "+str(y_world_) + "]")
plt.plot(x_world_, y_world_, 'b*')


plt.show()