自动驾驶中的坐标系与转换

lixinfu

坐标系是一种用来描述空间中物体位置的数学模型，可以用坐标表示物体在空间中的位置。在自动驾驶中，坐标系用于描述车辆和周围环境中的物体位置和方向信息，是实现各项功能的基础。今天我们来聊聊自动驾驶坐标系的那些事儿。

1.坐标系概览

在自动驾驶系统中，我们需要建立一个统一的坐标系来描述车辆的运动状态、环境地图以及其他周围车辆和障碍物的位置和运动状态。这些信息需要在不同的传感器和算法之间进行传递和共享，因此使用一个标准的坐标系可以简化整个系统的设计和实现，但满足这一要求往往需要我们在背后做很多工作。常见坐标系包括：

全球地理坐标系

局部坐标系

载体坐标系

传感器坐标系

Frenet坐标系

2.全球地理坐标系

WGS84坐标系：World Geodetic System-1984，既1984年的全球坐标系统，为原始坐标系。GPS系统使用的是WGS84坐标系，受益于GPS系统的发展，84坐标系是目前应用最为广泛的坐标系统。

WGS84坐标系的坐标原点为地球质心，地心空间直角坐标系的Z轴指向BIH （国际时间服务机构）1984.0定义的协议地球极（CTP）方向，X轴指向BIH 1984.0的零子午面和CTP赤道的交点，Y轴与Z轴、X轴垂直构成右手坐标系。


图 WGS84坐标系

GCJ02坐标系：你知道这个坐标系是什么吗？它是中国的坐标系，又被称为“火星坐标系”，因为它是为了保密而设计的，偏移了WGS84坐标系，所以使用它可以让你在中国境内的定位更加准确，但如果你的地图是基于WGS84坐标系的话，就必须得转换一下。当然，这也就意味着你的程序要对这些转换做出适当的处理,使用保密插件进行加偏，这就是另一个话题了。

3.局部坐标系

基于地球局部切线平面建立的局部坐标系，也称站心坐标系。固定在地球表面的笛卡尔坐标系通常都可以称之为局部坐标系。

    ENU坐标系：东-北-天坐标系，The Local Frame – East-North-Up，ENU，X轴指向正东方，Y轴指向正北方，Z轴指向天顶方向。



  图 ENU坐标系

     Odometry坐标系：odometry坐标系是自动驾驶中的一种坐标系，通常用于表示车辆相对于某个参考点的位置和姿态。它的命名源于机器人领域中的“odom”，意思是odometry（里程计），用于计算车辆的运动和位置。在实际应用中，通常将车辆的初始位置作为参考点，通过车辆的里程计和IMU数据来更新车辆的位置和姿态。odometry坐标系通常是车身坐标系（车身X轴指向车头，Y轴指向车侧，Z轴指向车顶），并且通常被用作控制系统中的反馈输入，例如PID控制器。虽然odometry坐标系的精度不如GNSS定位，但它仍然是自动驾驶系统中不可或缺的一部分。



  图 Odom坐标系

4.载体坐标系

     车体右前上坐标系：X轴为车辆前进方向的右侧；Y轴为车辆前进方向，对应运载体纵轴；Z轴与地面垂直，指向车顶方向，对应运载体立轴；原点位于车辆后轴中心。与之对应的还有前左上坐标系，就不过多赘述了。



图 FRU坐标系

      

5.传感器坐标系

     传感器坐标系：传感器坐标系是一个以传感器为原点的坐标系，它通常是由传感器的制造商定义的。这个坐标系与车辆坐标系或者地图坐标系是不同的，因此需要通过坐标变换才能将传感器数据映射到车辆或者地图的坐标系中。

        举个例子，假设我们有一台车，车上安装了一个激光雷达传感器，那么激光雷达坐标系就是以激光雷达为原点的坐标系。这个坐标系的X轴和Y轴通常与激光雷达的安装方向有关。在获取到传感器数据后，需要通过坐标变换将激光雷达坐标系映射到车辆坐标系或地图坐标系中，才能得到有用的信息。

6.Frenet坐标系

       Frenet坐标系：一种描述车辆运动轨迹的数学坐标系，常用于Planning模块中。该坐标系以车辆所在道路为基准，分别定义了车辆的横向位置和纵向位置，具有较好的局部精度和可解释性。

      横向位置由车辆到道路中心线的横向距离（偏差）表示，而纵向位置由车辆在道路方向上的累积距离（曲线长度）表示。通过计算车辆在Frenet坐标系下的横向位置和纵向位置，可以在车辆前方预测未来的道路曲线和转向角度，并基于此进行路径规划和控制。Frenet坐标系相比于传统的笛卡尔坐标系，可以更好地描述车辆在曲线道路上的行驶状态，适用于弯道驾驶和车道保持等自动驾驶任务。



图 Frenet坐标系

7.坐标系转换

       那么在实践中，我们如何进行坐标系的转换呢？ROS2的TF组件为我们实现坐标变换提供了良好的方法。

      在ROS2中，TF2是一个用于管理坐标系和坐标变换的库。TF2提供了一个坐标变换树，可以轻松地在不同坐标系之间进行转换。以下是TF2进行坐标转换的基本步骤：

创建一个tf2_ros::Buffer对象，用于存储坐标系变换关系；

在程序中定义需要转换的源坐标系和目标坐标系；

使用Buffer对象中的函数，例如lookupTransform()，在源坐标系和目标坐标系之间进行转换；

根据需要对转换结果进行处理。

      在进行坐标系转换时，需要注意源坐标系和目标坐标系的正确性，以及使用适当的坐标转换函数。同时，TF2还提供了其他功能，例如对坐标系的监听和广播，以及处理多个坐标系之间的变换关系等。

Notes：

坐标变换中分为静态坐标变换与动态坐标变换，静态坐标变换可以理解为从一个局部坐标系变换到另一个局部坐标系，而动态坐标变化可以理解为从车体坐标系变换到局部坐标系下。

静态坐标变换在ROS中一行代码就可以实现，也没什么可说的。

动态坐标变化由于车体是时刻移动的，所以在变换中，首先需要车体与局部坐标系的关系准确，这就涉及到定位精度，其次需要注意变换中的数据精度损耗，最后还有传输过程中的latency的问题，在工程上比较难以解决，需要注意尽量少使用到动态坐标转换。

关于TF的使用方法，参见ros wiki：http://wiki.ros.org/tf2

最后推荐最近在看的一门ROS课程，老师讲的很好，链接挂在下面，B站有全套视频课程：http://www.autolabor.com.cn/book/ROSTutorials/

——————————————————

日拱一卒无有尽，功不唐捐终入海。