In this article, we will talk about how an autonomous vehicle can know its own location. But first, let’s start with a simple example.
If you are driving a car in an unfamiliar place, you usually rely on a map to navigate. But when you are close to your home, you don’t get lost. The main difference is that you have memorized landmarks such as roads and stores near your home, so you know where you are driving and how to get home.
There is a part of the human brain called the entorhinal cortex which stores a mental map that supports spatial awareness and navigation.
This article explains the relationship between maps and the brain in more depth.
In this survey (Visualizing Mental Maps of San Francisco), 22 San Francisco residents were asked to draw a picture of their neighborhood or city.
You can see that they are very rough compared to the general map shown to the right, but they are drawing places of value to themselves and things to remember.
In the same way, autonomous vehicle know where they are by comparing the landmarks they are looking at with a map. And since autonomous vehicles need more accurate maps than humans, they use a sensor called LiDAR to generate highly accurate maps.
Now let’s look at how autonomous vehicles combine their high-definition maps (HD maps) with the environment acquired by their sensors to determine their own location.
Communication of Robots and Sensors
ROS is an operating system for robots built on the publish/subscribe model.
By using ROS, it is possible to connect many sensors to the robot, acquire data (subscribe), and publish the results of control and calculations (publish).
The image below shows an example of object detection used in a typical self-driving vehicle. It detects obstacles from pictures taken by a camera in front of the vehicle to avoid collisions with pedestrians and vehicles while driving.
Connecting Maps, Sensors, and Vehicles
ROS has a library called TF (Transform Configuration) that can be used to set the positional relationships of sensors and other devices.
You can set up the positional relationship between sensors such as cameras, radar, LiDAR, etc., and the vehicle, as well as the positional relationship between the vehicle and the map, and let the robot recognize the configuration of hardware placed in the real world.
Global Coordinate Transformation
In Autoware, it is basically designed based on the following TF tree:
world: world coordinates (ECEF), latitude and longitude coordinates with the Earth at the center of gravity as the origin. In the ROS standard, the
world is called
map: coordinates with the origin at the corner of the area where the robot will run.
base_link: coordinates with the origin between the wheels of the robot.
velodyne: coordinates of sensors on the vehicle, such as LiDAR.
These layers are called frames, and each frame has one parent frame, and by linking them to the parent’s coordinate system, the range of dependencies can be reduced, and frames can be combined easily even when adding new sensors or using a map of a wide area.
Localization by Referencing a Map
Now that we know how to combine sensors and vehicles by TF, we can estimate the self-position by matching the data observed by a LiDAR with a preliminary map. Here we introduce an algorithm called NDT.
Algorithm for NDT (Normal Distribution Transform)
NDT matching involves several steps:
- divide the search space of the precreated map into a grid (normal distribution of the point cloud in each grid)
- calculate the mean and variance in the grid
- find the element corresponding to the point cloud of the input scan
- calculate the evaluation value
- update the coordinates of the point cloud of the input scan using the Newton method
- repeat the above for all input scans repeat for all input scans
NDT Matching by Autoware
In this example, we use the LiDAR data of the public nuScenes dataset, and use the following as
- Pre-created point cloud map
- Input scan (rosbag file)
Set up the TF in Autoware
You can start Autoware and configure your own TF file from the
The frame named
points_map is a pre-created point cloud, and
lidar_top is set to the point cloud of the input scan file (see how to make nuScenes rosbag files in this article).
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />
<node pkg="tf" type="static_transform_publisher" name="map_to_points_map" args="0 0 0 0 0 0 /map /points_map 10" />
<node pkg="tf" type="static_transform_publisher" name="velodyne_to_lidar_top" args="0 0 0 0 0 0 /velodyne /lidar_top 10" />
You can also use this command while the simulation is running to check the frame tree in pdf format.
rosrun tf view_frames
At this point, we would like to point out 3 things about NDT matching:
- The process of NDT matching is a heavy workload, so you can downsample the point cloud data while preserving the features of the point cloud. enable
Computingtab, but if the initial pose is not accurate, localization will fail. Carefully choose the initial position and angle.
- If the direction of the map and the vehicle model are not aligned, change the yaw setting of
Baselink to Localizerin the
Setuptab and the yaw setting of initial position in
Finally, all the settings are complete and the autonomous vehicle successfully localizes. In the following video, you can see how the scan data is matched with the pre-created map.
This article was brought to you by yodayoda Inc., your expert in automotive and robot mapping systems.
If you want to join our virtual bar time on Wednesdays at 9 pm PST/PDT, please send an email to talk_at_yodayoda.co, and don’t forget to subscribe.