一文看懂自动驾驶中的坐标变换

自动驾驶中的坐标变换

在自动驾驶任务中，我们通过各种传感器对周围环境进行感知，获取图片。点云等各种数据，由于传感器的位置、数据的形式、不同数据的含义等差异，在进行环境感知任务前，需要进行坐标、数据形式的统一，其中坐标变换是极其重要的一环。

自动驾驶中的坐标系

根据参照物的不同，坐标系主要分为世界坐标系、传感器坐标系和汽车坐标系。不同的传感器对应不同的数据类型和坐标系。

每个传感器都有其自身的坐标系，按照其测量原理可以分为相对测量（测量环境信息）和绝对测量（测量自身信息）。相对测量的传感器如相机，激光雷达是通过测量环境信息来进行间接定位，而绝对测量如 IMU，轮速计，GPS 则是通过直接测量自身的运动状态（速度、位置、姿态等）来进行定位。对于相对测量的传感器而言，其测量值通常是在其自身传感器下；而绝对测量的测量结果一般则是自身坐标系相对于其他坐标系（局部坐标系或世界坐标系等）。下面对常见的测量传感器进行归纳。

1. 相机坐标系

下图中左图为世界坐标系和相机坐标系的关系示意图，在相机坐标系中原点为成像平面的中心，与z轴垂直的平面对应焦距平面，右图为像素图像坐标系和物理图像坐标系的关系示意图，像素图像坐标系的原点在图像的左上角。

世界坐标系：在环境中选择一个基准坐标系来描述摄像机的位置，并用它描述环境中任何物体的位置，该坐标系称为世界坐标系。

相机坐标系：以相机的聚焦中心为原点，以光轴为Z轴建立的三维直角坐标系。

像素图像坐标系：在真实世界中的影像通过小孔成像后得到的图像中，以像素为单位，坐标原点在图像左上角的坐标系。

物理图像坐标系：用物理单位表示像素的位置，坐标原点为摄像机光轴与图像物理坐标系的交点位置。

相机是通过小孔成像原理，将物理世界中的影像投影到成像平面，成像平面在物理世界中是真实存在的，有对应的物理尺寸，通常比较小，以毫米为单位。而我们最终得到的图像是以像素为单位的，因此关于图像的坐标系分为像素图像坐标系和物理图像坐标系。

2. 激光雷达坐标系

• 64线激光雷达进行环境扫描过程：单帧扫描数据

• 64线激光雷达进行环境扫描过程：全周扫描数据

• 激光雷达坐标系统

旋转式激光雷达一般选择激光发射中心作为坐标系原点，向上为Z轴正方向，X轴和Y轴构成水平平面。图中红色线条为激光雷达发出的激光束，在任意时刻形成平行于Z轴，垂直于XY平面的扇形扫描区。每束出射激光在竖直方向上的俯仰角${\theta }_j$为固定值，在设计制造时确定，属于激光雷达的内部参数。扇形扫描平面绕Z轴旋转的角度 φ ( t ) φ(t)φ(t) 随时间变化，并会在原始测量数据中给出。

第i束激光在$t_0$时刻照射到某物体表面的P点，测距结果显示P点与激光雷达间的距离为L，则该测量点P的原始测量数据可以用极坐标形式$(\phi (t_0),{\theta }_i,L)$来表示。同时，P点在激光雷达的正交坐标系$(X_L,Y_L,Z_L)$中表示为$P(x_L,y_L,z_L)$，存在如下转换关系。用正交坐标系表示的点云数据在实际中更为常用。
$$\begin{gathered} X_L=Lcos({\theta }_i)cos(\phi (t_0)) \\ {Y}_L=Lcos({\theta }_i)sin(\phi (t_0)) \\ {Z}_L=Lsin({\theta }_i) \end{gathered}$$

3. 车体坐标系

车体坐标系用来描述车辆周围的物体和本车之间的相对位置关系。目前学术界和工业界有几种比较常用的车体坐标系定义方式。分别是ISO国际标准定义，SAE（Society of Automotive Engineers）汽车工程师协会定义，和基于惯性测量单元IMU的坐标定义。

在车辆动力学分析中，ISO定义的车体坐标系较为常见。SAE定义的车体坐标系与航空航天领域常用的机体坐标系相一致。基于IMU定义的车体坐标系，则在IMU的相关应用中较为常见。无论使用哪一种坐标系定义，只要使用正确，都可以完成对车身位姿的描述，以及确定周围物体和本车间的相对位置关系。研发人员可以根据应用需求和使用习惯来选择车体坐标系。

4. 世界坐标系

广义上讲，世界坐标系是一种描述物体在世界中位置的坐标系，一般是指一个描述地球上位置关系的系统，其主要分为地理坐标系统和投影坐标系统。地理坐标系统主要是指以地心为原点的球坐标系或椭球体坐标系，一般以经纬度表示地球表面某地的位置，常见的标准有WGS-84经纬坐标系。投影坐标系统主要是指以米为单位表示距离和大小的平面直角坐标系，需要进行一种坐标变换或映射关系，将用于定位的地理坐标系统中的经纬度信息转换为以米为单位的平面直角坐标，常见的标准有UTM坐标系。考虑到环境感知本身不需要对定位相关的内容做深入研究，在此按下不表，详细了解可参考这两个链接：(1), (2)。

• 地理坐标系统（英文简写GCS，Geographical Coordinate System）地理坐标系单位 度°分’秒"–>擅长定位

  

• 投影坐标系统（英文简写PCS，Projection Coordinate System）投影坐标系单位 米m–>擅长计算

坐标系变换

坐标系之间的关联

在实际应用中，我们需要通过不同传感器确定障碍物与自动驾驶汽车间的相对位置，同时需要知道本车在世界坐标和地图中的位置与姿态。这就需要将不同的坐标系关联起来，并建立他们之间的转换关系。下图是自动驾驶问题中常用的一些坐标系的示意图。

在自动驾驶问题中的坐标变换主要分为两类，第一类是物理空间中不同位置的坐标系之间的坐标变换，涉及原点的平移对齐和坐标轴的旋转对齐，一般我们将这类变换的标定参数称为外参；第二类是诸如相机等传感器内部，由于其本身的特性，导致获得的数据与真实世界有一定的对应关系，为了充分利用不同传感器的数据，需要尽可能将它们统一到相同的格式、坐标系下，这类变换的标定参数称为内参。

外参的使用

基于O1的世界坐标系与基于O2的坐标系如下所示：

在描述机器人运动时，我们常常提及“位姿”，其实位姿是一个合成词，我们可以将其拆解为“位置+姿态”。位置就是指“机器人某个运动关节/测量传感器在世界坐标系中的具体位置，姿态就是”基于该点的坐标系相较于世界坐标系所进行的旋转“，如下所示：

坐标变换本质上可分解为平移和旋转，其中平移很好理解，两个坐标系经过旋转后，只差一个$O_1{O}_2$向量，进行向量加法即可完成，下面着重介绍一下坐标变换中的旋转。

我们知道，一个单位向量在某一空间直角坐标系中可以分解为x, y, z轴上的投影，如图所示。单位向量在坐标系中的投影正好为向量P与各个坐标轴夹角的余弦值。

我们将坐标系A作为参考坐标系（world坐标系），基于坐标系A表示坐标系B的各个坐标轴并且将各个向量单位化，由此我们得到一个旋转矩阵，示意图及旋转矩阵各个元素的含义如下：

我们先前提到过，向量坐标的计算无非就是投影，那么向量坐标从坐标系B转换至坐标系A本质上就是两次投影：

向量P的转换过程涉及三个轴的投影以及累加，使用矩阵乘法的形式可以非常简洁地表示这个过程：

在理解了坐标变换的本质，即旋转和平移后，坐标系之间的转换公式如下：
$$P_C=\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\iff P_c=R\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]+T$$

相机内参的使用

在自动驾驶的坐标变换中，相机内参负责相机坐标系和像素图像坐标系之间的坐标变换，其中又可以细分为物理图像坐标系与相机坐标系的变换，以及物理图像坐标系与像素图像坐标系的变换。

像素图像坐标系与物理图像坐标系的变换

当我们拿到了图像坐标系的数据，我们这个时候想把其转换成像素坐标系，如果我们已知图像坐标系的一个点p(x,y),我们还应该知道，横坐标和纵坐标中1个像素占多少毫米（mm）。即有如下公式：
$$p(u,v)=p(x/dx+u_0,y/dy+v_0)$$
用矩阵表示如下：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}1/dx& 0& u_0\\ 0& 1/dy& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

物理图像坐标系与相机坐标系的变换

根据小孔成像原理，物理世界中的点P，光心O，以及成像平面上的对应点$P^{\prime }$所在的线段，z轴，成像平面以及P点对应平面形成了相似三角形，根据相似三角形有：
$$\frac{X_c}{x}=\frac{Z_c}{f}=\frac{Y_c}{y}\Rightarrow x=f\frac{X_c}{Z_c},y=f\frac{Y_c}{Z_c}$$
化为矩阵形式得：
$$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\end{array}\right]$$
综上所述，对于相机，由世界坐标系到图像像素坐标系的坐标变换可以用如下矩阵进行。

参考链接

https://blog.csdn.net/fb_941219/article/details/124444541

http://imgtec.eetrend.com/blog/2021/100556031.html

https://zhuanlan.zhihu.com/p/561726864

https://zhuanlan.zhihu.com/p/453081175

https://blog.csdn.net/weixin_45590473/article/details/122848202