自动驾驶场景实现：路径点跟踪

引言

之前的文章介绍了如何使用 LG 和 Autoware 搭建自动驾驶仿真平台。文章的最后用一个简单的自动驾驶场景测试了这个平台的有效性。本文就介绍一下实现该场景的具体操作细节。

Autoware 作为一个自动驾驶套装，将 SLAM 建图、定位、决策、控制等自动驾驶环节整合在一起，并且提供了操作非常方便的图形界面 Runtime Manager。在设置自动驾驶的整个过程中，不需要输入任何代码，只要在图形界面中选择相应的功能即可。

在 Autoware manual 中介绍了如何操作 Autoware，不过那里是基于实车的操作，我们是基于仿真平台，细节有些区别，基本流程差不多。

收集数据

• 按照之前的文章设置好 LG + Autoware 的仿真环境，确保两者已经连通。

• 在 LG 中选择一个城市环境，这里我们用 SimpleMap 。城市的选择并不影响操作过程。

• 进入 LG 的驾驶界面。在本文实现的场景中，只需要启用 LIDAR 。可以通过 Sensor Effect 确认一下 LIDAR 是否开启。如果车身周围出现放射状的红点，则说明 LIDAR 已启用。

• 在 Autoware 所在电脑上，保险起见可以通过如下命令再次确认一下 Autoware 能接收到 LIDAR 的数据

   rostopic echo /points_raw
  

  如果出现了密密麻麻的 0~255 之间的数字，则说明 Autoware 能接收到 LIDAR 数据。
  这一步确认也可以不用 ROS 命令。Autoware 为我们提供了 rostopic echo 的功能。操作如下：
  Runtime Manager -> Topics -> Refresh -> 选择 /point_raw -> Echo
  同样的，如果有数字显示，则表示能接收到 LIDAR 数据。

• 记录 LIDAR 数据。在 Runtime Manager 的右下角点击 ROSBAG，refresh 之后选择 /points_raw topic，设定好数据包的名字，点击 start 开始记录数据。

• 手动驾驶汽车，收集 LIDAR 数据。在 LG 中驾驶汽车行驶一段路，这一段路就是之后希望汽车自动驾驶的路段，手动驾驶汽车行驶的路线也就是自动驾驶时要跟踪的路线。

• 驾驶结束，在 ROSBAG 中停止记录数据。假设得到的数据包名字为 lidar.bag。

建图

• 首先要关闭 LG 端的 LIDAR，否则会干扰建图。然后进行后边的操作。

• 在 Runtime Manager -- Setup 界面中 TF 设置为 x: 1.2, y: 0, z: 2. 这是 LIDAR 传感器在车身坐标系中的位置。设置这一 transform 是为了建立 LIDAR 坐标系与车身坐标系的转换关系。

• 在 Computing 界面选择 ndt_mapping。这里是借助 NDT 算法实现 SLAM。

• 在 Simulation 界面选择刚才生成的数据包 lidar.bag，点击 play。等待结束，点 stop.

• 在 Computing 界面选择 ndt_mapping[app]，设定保存点云地图 (point cloud data map) 的路径，然后点击 PCD OUTPUT，等待建图结束。

• 在建图过程中，为了确认建图成功，可以打开 rviz，选择配置文件 Autoware/ros/src/.config/rviz/ndt_mapping.rviz。
  如果一切顺利，应该可以在 rviz 中看到 pcd 地图逐步建立的过程。

• 建图不一定每次都成功，有时候 NDT 算法匹配的不好，地图可能很混乱。我们的经验是，在收集 LIDAR 数据的时候车辆行驶慢一些，如果建图不成功，就多尝试几次，每次都重新收集一次数据，另外，运行 Autoware 的电脑配置也应该足够高，至少达到 Autoware 的推荐配置，如下：

Recommended System Specifications

Number of CPU cores: 8
RAM size: 32GB
Storage size: 64GB+

生成路径点

• 在 Runtime Manager -- Setup 界面中 TF 设置为 x: 1.2, y: 0, z: 2

• 在 Map 界面加载刚才创建的 Point Cloud 地图

• 在 Map 界面加载 TF 文件。这个文件可以在Autoware 官方 demo 文件中找到，可以用如下命令下载

   wget http://db3.ertl.jp/autoware/sample_data/sample_moriyama_data.tar.gz
  

  要加载的文件为 data/tf/tf.launch，但是要将其中的参数修改为

  name="world_to_map" args="0 0 0 0 0 0 /world /map 10" 
  

  这个文件中的 transform 是将 map 坐标系和 world 坐标系关联起来。这里可以令 map 坐标系与 world 坐标系重合，也就是 tf 中 x,y,z,roll,pitch,yaw 都为 0.

• 在 Sensing 界面选择 Point Downsampler 中的 voxel_grid_filter 。

• 在 Computing 界面选择 lidar_localizer 中的 ndt_matching，这里要注意，要在 app 中选择 initial pose，数值全为 0.

• 在 Computing 界面选择 autoware connector 中的 vel_pose_connect ，这里是将 ndt 估计出的 pose 和 velocity 名字改为 current_pose, current_velocity，以便后续 pure-pursuit node 使用.

• 在 Computing 界面选择 waypoing_maker 中的 waypoint_saver，设置好路径点文件的名字和保存路径。

• 再次在 Simulation 界面选择数据包 lidar.bag，点击 play。等待结束，点 stop。如果一切顺利，应该可以看到在指定位置生成了路径点文件，扩展名为 .csv。如果对某些路径点不太满意，可以修改这个 csv 文件，调整路径点的位置和期望速度。

路径点跟踪

最后要实现的就是让汽车通过 LIDAR 定位，自动沿路径点行驶。

• 在 Setup 界面设置 TF，与前边相同

• 在 Map 界面加载 pcd 地图以及 TF 文件，与前边相同

• 在 LG 中启动 LIDAR。

• 在 Sensing 界面选择 Point Downsampler 中的 voxel_grid_filter 。

• 在 Computing 界面选择 lidar_localizer 中的 ndt_matching，这里还是要注意，在 app 中选择 initial pose，数值全为 0，这是 NDT 点云匹配的初始位置。相应的， LG 中的车辆也应该在 pcd 地图的初始位置，两者相近才更容易实现点云匹配。

• 在 Computing 界面选择 autoware connector 中的 vel_pose_connect

• 在 Computing 界面选择 waypoing_maker 中的 waypoint_loader，加载刚才生成的路径点文件。

• 在 Computing 界面选择 lane_planner 中的 lane_rule 和 lane_select

• 在 Computing 界面选择 lattice_planner 中的 lattice_velocity_set 和 path_select

• 在 Computing 界面选择 waypoint_follower 中的 pure_pursuit 和 twist_filter

如果一切顺利，此时 LG 端的车辆会进入自动驾驶状态，通过 LIDAR 定位，沿给定的路径点行驶。

总结

Autoware 基本操作非常简单（如果你知道该选择 Runtime Manager 中哪些功能的话）。Autoware 本质上是一大堆 ROS node 的集合，每个 node 包含了实现某些特定功能的算法程序。每次选择 Runtime Manager 中的某个功能，实际上在 terminal 中都会有对应的 ROS 命令。借助图形界面，Autoware 隐藏了输入命令的过程。如果要通过 Autoware 深入学习自动驾驶，依然需要深入到 ROS node 层面，理清系统结构，然后尝试在某些 node 中改进算法或者加入自己全新的 node，让自动驾驶功能更完善！