A vehicle tracking method based on Autoware
By using robosense lidar and NDTmapping algorithm on Autoware, combined with A* and pure tracking algorithms, the accuracy and simplification of the vehicle tracking method are solved, and a higher accuracy and simplified vehicle tracking process is achieved.
Patent Information
- Application Number
- CN202111054142.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-09-09
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2041-09-09
AI Technical Summary
The existing vehicle tracking methods have problems such as low accuracy and cumbersome implementation process. Especially under the influence of weather and other factors, the GPS+IMU method will cause a large deviation in the tracking position compared with the lidar method, and The path planning process is complex and prone to errors.
Using Autoware-based vehicle tracking method, data is collected using robosense lidar, maps are built with NDTmapping algorithm, and post-processing is performed through the PCCL point cloud library, downsampling and point cloud map area division to form the final map point cloud map, using A *Algorithms and pure tracking algorithms form vehicle tracking trajectories.
Improves vehicle tracking accuracy and simplification of steps, and completes all steps only on Autoware, which is more accurate and simpler than the traditional GPS+IMU method.
Smart Images

Figure CN113850915B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a vehicle tracking method and belongs to the technical field of automatic driving control. Background Art
[0002] With the development of smart cars and autonomous driving technology, vehicles can now realize autonomous driving based on road and environmental information. An important part of realizing autonomous driving is to accurately track the vehicle. There are many known vehicle tracking methods, but they are basically implemented using GPS+IMU.
[0003] Since GPS+IMU is subject to factors such as weather, the tracking positioning points will have a larger deviation compared to the method using lidar, and the use of GPS+IMU to complete path planning requires multiple software, which is not only prone to errors but also the process is cumbersome. Therefore, the existing vehicle tracking methods have the problems of low accuracy and cumbersome implementation process. Summary of the invention
[0004] In order to solve the problems of complicated implementation process and large deviation in the existing vehicle tracking methods, the present invention further proposes a vehicle tracking method based on Autoware.
[0005] A vehicle tracking method based on Autoware comprises the following steps:
[0006] S1, collect data based on the robosense laser radar installed on the vehicle;
[0007] S2, use NDTmapping algorithm to build map in Autoware;
[0008] S3, post-process the pcd point cloud map created by NDT_mapping using the pcl point cloud library, the post-processing process includes downsampling and point cloud map area division;
[0009] In the process of downsampling, voxel_leaf_size is used to reduce the volume of point cloud files, and the point cloud files are voxel_filtered through the pcl point cloud library; in the process of point cloud map area division, the point cloud map is divided into regions by setting the grid size;
[0010] Post-processing to obtain the final map point cloud;
[0011] S4. Based on the map point cloud, use the simulation function module and select waypoint_saver to form a csv path node file;
[0012] S5. Load the csv path node file in the map point cloud, use astar_avoid and velocity_set based on the A* algorithm to form the vehicle tracking trajectory, and use the tracking instructions pure_pursuit and twist_filter based on the pure tracking algorithm to obtain the vehicle tracking result.
[0013] Furthermore, before the Robosense LiDAR collects data, it is necessary to calibrate and measure the height of the Robosense LiDAR, and perform rosbag recording. When performing rosbag recording, the topic points_raw needs to be checked.
[0014] Furthermore, when building a map using the NDTmapping algorithm, point cloud registration is required. The process of point cloud registration includes the following steps:
[0015] (1) First, fix a point cloud data as the target point cloud and divide the target point cloud into grids;
[0016] (2) After the target point cloud is divided into a grid, the point cloud is loaded into the grid and the mean vector is calculated first.
[0017]
[0018] in, represents the coordinates of the scanning point k in the grid; m represents the number of scanning points in the grid;
[0019] By Calculate the covariance matrix;
[0020] Finally, by Find the normal distribution probability density function in each grid;
[0021] (3) Determine the initial coordinate transformation parameters of the source point cloud relative to the target point cloud;
[0022] The source point cloud is a point cloud that needs to be registered with the target point cloud;
[0023] (4) According to the initial coordinate transformation parameters, the source point cloud coordinates are transformed into the target point cloud coordinate system. At this time, the source point cloud will be distributed in the target point cloud grid. The probability that the source point cloud coordinates are X' is calculated from the normal distribution probability density function of the corresponding grid, that is, the probability of the source point cloud in the target point cloud grid after the coordinate transformation is determined;
[0024] (5) Find the maximum target likelihood function:
[0025] Multiply the probabilities of each point as the target likelihood function;
[0026]
[0027] Where n is the number of source point clouds distributed in the target point cloud grid;
[0028] (6) Use Newton iteration method to optimize and find the optimal transformation parameter p to complete point cloud registration.
[0029] Furthermore, in the process of determining the initial coordinate transformation parameters in step (3), more than half of the source point cloud needs to be in the coordinate plane of the target point cloud.
[0030] Furthermore, the process of optimizing by using the Newton iteration method in step (6) to find the optimal transformation parameter p to complete the point cloud registration includes the following steps:
[0031] Solving the objective function formula
[0032] Where H is the Hessian matrix of the objective function, is the gradient vector of the objective function; is the vector change value of the optimal transformation parameter p;
[0033] Iterative update formula T = T + ΔT;
[0034] Among them, ΔT is the change value of the coordinate transformation parameter T, and the iteration parameter T is the coordinate transformation parameter T.
[0035] Furthermore, in the process of building a map using the NDTmapping algorithm in Autoware, the parameter resolution is selected as 1.
[0036] Furthermore, the process of reducing the volume of the point cloud file using voxel_leaf_size selects leaf_size as 0.1-0.2.
[0037] Furthermore, in the process of forming the CSV path node file, the autoware is used to generate the vehicle's posture information in the x, y, and z directions, as well as the vehicle's yaw angle around the z axis.
[0038] Furthermore, the process of forming a vehicle tracking trajectory using astar_avoid and velocity_set based on the A* search algorithm includes the following steps:
[0039] (511) Rasterize the map and store the map data in a two-dimensional list;
[0040] (512) Construct an evaluation function: F(n)=H(n)+G(n);
[0041] H(n) is the distance evaluation function, n represents each square on the map; G(n) is the cost function from the starting point to the end point;
[0042] (513) The search is performed based on the A* search algorithm. Each time the A* search searches for a path, it will look for the node with the minimum evaluation value.
[0043] Furthermore, the process of searching based on the A* search algorithm includes the following steps:
[0044] A* search involves an open list and a closed list; the algorithm first adds the starting point to the open list, then traverses the open list, finds the node with the smallest evaluation function as the current search start node, and then adds the point to the closed list as a node that has been traversed; explores each node adjacent to the node with the smallest evaluation function that is not in the closed list, and calculates their H, G, and F values; and takes the node with the minimum evaluation function as the parent node of these nodes, and adds these nodes as exploration nodes to the node set Q; if the node in Q is not in the open list, add it to the open list; if the node in Q already exists in the open list, compare the F value of these nodes with their F value in the open list to see which is smaller. If the F value in the open list is smaller or the two are equal, do not make any changes, otherwise replace the nodes in the open list with the nodes in Q; if the end point is in the open list, exit the search and determine the optimal path.
[0045] Beneficial effects:
[0046] The advantages of using LiDAR to complete mapping and path planning compared to traditional GPS+IMU positioning are high accuracy and simple steps, and all steps can be completed on autoware alone. BRIEF DESCRIPTION OF THE DRAWINGS
[0047] Figure 1 Take a screenshot of the voxel_filter code;
[0048] Figure 2 The final point cloud map after using LiDAR to build the map;
[0049] Figure 3 is a route diagram obtained based on two-dimensional information (x, y);
[0050] Figure 4 is a schematic diagram of the route obtained based on three-dimensional information (x, y, z);
[0051] Figure 5 This is a simplified schematic diagram of the smart car in the pure tracking algorithm;
[0052] Figures 6 to 15 They are schematic diagrams of autoware vehicle tracking simulation. DETAILED DESCRIPTION Specific implementation method one:
[0054] The vehicle tracking method based on Autoware described in this embodiment includes the following steps:
[0055] S1. Set the Robosense LiDAR on the roof and connect the vehicle to the Robosense LiDAR.
[0056] Before connecting, the Robosense LiDAR needs to be calibrated and the height measured, and the rosbag recording is performed, that is, the calibration data and height data of the LiDAR are recorded in a data packet with the .bag suffix. When performing the rosbag recording, the topic points_raw needs to be checked; setting the correct LiDAR height is highly correlated with the accuracy of completing the vehicle tracking point search;
[0057] S2. Use the NDTmapping algorithm in Autoware to build a map. Perform simulation through the recorded package to complete the NDTmapping map. Here, in outdoor map building, the parameter resolution is selected as 1 considering the influence of data and running speed.
[0058] Point cloud registration is required when using the NDTmapping algorithm to build a map. In the process of building a laser point cloud map, due to the limitation of the laser radar scanning distance, one scan cannot obtain the entire target environment, and the farther away from the laser radar, the sparser the point cloud will become. Therefore, after multiple continuous scans and the registration and splicing of the point cloud data of each scan, a complete laser point cloud map can be finally formed. The point cloud data obtained by scanning the same scene from different angles is uniformly converted to the same coordinate system, which is called point cloud registration. It is to splice discrete point cloud data into a complete point cloud data in a unified coordinate system. Usually, the point cloud registration algorithm can use the minimum distance between two point sets to obtain the transformation relationship between the two point sets, so that the point cloud can achieve the effect of transformation registration.
[0059] The process of performing point cloud registration includes the following steps:
[0060] (1) Gridded target point cloud:
[0061] Since the matching process first fixes a point cloud data as the target point cloud, and then matches and splices the source point cloud (another point cloud) data with the fixed point cloud through translation and rotation; therefore, it is necessary to first mesh the target point cloud using a cube so that the laser point is in the cube;
[0062] (2) Registering laser point cloud:
[0063] After the target point cloud is divided into a grid, the point cloud is loaded into the grid and the mean vector is calculated first.
[0064]
[0065] in, represents the coordinates of the scanning point k in the grid; m represents the number of scanning points in the grid;
[0066] By Calculate the covariance matrix;
[0067] Finally, by Find the normal distribution probability density function in each grid
[0068] (3) Calculate the initial coordinate transformation parameters of the source point cloud relative to the target point cloud:
[0069] Find an initial coordinate transformation so that most (more than half) of the source point cloud is in the coordinate plane of the target point cloud, provide the initial value of the transformation parameter, and provide the initial value close to the optimal point for the next iteration of the transformation parameter;
[0070] (4) The source point cloud undergoes initial coordinate transformation and calculates the probability of being in the target point cloud grid:
[0071] The source point cloud coordinates are transformed into the target point cloud coordinate system according to the initial coordinate transformation parameters. At this time, the source point cloud will be distributed in the target point cloud grid. The probability that the source point cloud coordinates are X' is obtained from the normal distribution probability density function of the corresponding grid, that is, the probability of the source point cloud in the target point cloud grid after the coordinate transformation is determined;
[0072] (5) Find the maximum target likelihood function:
[0073] Multiply the probabilities of each point as the target likelihood function;
[0074]
[0075] Where n is the number of source point clouds distributed in the target point cloud grid;
[0076] (6) Use Newton iteration method to optimize and find the optimal transformation parameter p to complete point cloud registration;
[0077] Solving the objective function formula
[0078] Where H is the Hessian matrix of the objective function, is the gradient vector of the objective function; is the vector change value of the optimal transformation parameter p;
[0079] Iterative update formula T = T + ΔT;
[0080] Wherein, ΔT is the change value of coordinate transformation parameter T, and iteration parameter T is coordinate transformation parameter T;
[0081] S3. Post-process the pcd point cloud image created by NDT_mapping using the pcl point cloud library to improve the point cloud clarity of the image, reduce the file size and increase the running speed; post-processing includes downsampling and area division;
[0082] The process of downsampling and region division of the point cloud map generated by NDT_mapping using the pcl point cloud library includes the following steps:
[0083] Since there are many overlapping points in the established point cloud file, in order to filter out the overlapping points, it is necessary to use voxel_leaf_size (used to set the voxel size) to reduce the volume of the point cloud file, and reduce its volume to less than half of the original by downsampling; in the process of using voxel_leaf_size to reduce the volume of the point cloud file, it is necessary to maintain the image clarity within the range of leaf_size of 0.1-0.2;
[0084] Due to the characteristics of NDT, downsampling will not affect the final matching and positioning results;
[0085] Use the pcl point cloud library to perform voxel_filter (voxel grid filtering) on the point cloud file, such as Figure 1 As shown, then save it to a new file;
[0086] Point cloud map grid division: By setting a certain grid size, the point cloud map is divided into regions. After the map is divided into regions, it is ordered. After verification, the loading speed is much faster than before.
[0087] The present invention uses voxel grid filtering for downsampling. The size of the voxel grid is very important. If the voxel grid is too large, point cloud distortion may occur easily. For example, as shown in Table 1, in the simulation results of some embodiments, when the voxel side length changes from 10 cm to 5 cm (expanded by 2 times), the maximum CPU occupancy of the mapping algorithm does not increase significantly, but the number of point clouds increases by 54%. This is because the voxel setting is too small, the overall number of point clouds is too large, and the number of voxels exceeds the setting value of the Voxel_Grid function, resulting in data overflow. Therefore, when using voxel grid filtering in large scenes, it is necessary to set the voxel size and / or divide the point cloud into blocks (region division) according to the actual situation to avoid data overflow.
[0088] Table 1
[0089]
[0090] When the voxel size is set to a large value, it can be observed that the depth information of the point cloud map is relatively sparse and cannot truly reflect the environmental features. When the voxel size is gradually reduced, the environmental features become clearer, and there is no overflow of the downsampled data. Therefore, in small scenes, the voxel size can be appropriately reduced (the size setting is reduced to 2-4 times) to obtain updated and rich environmental depth information.
[0091] Choosing different voxel sizes will bring different display effects to the laser point cloud map. Smaller voxels can significantly increase the density of the laser point cloud map, making the map's expression of environmental information more delicate, and can provide depth information for more image pixels in the data fusion algorithm of the laser radar and camera. Compared with large scenes, the increase in point cloud density using the voxel grid filtering algorithm is more obvious in small scenes such as corridors and indoors, and it is not easy to have voxel overflow. Smaller voxels will increase the amount of processor computing, and it may be impossible to build maps in real time, so it is necessary to choose between the real-time performance of the algorithm and the point cloud density.
[0092] After using LiDAR to build the map, we get the final point cloud map, such as Figure 2 shown.
[0093] S4. Based on the map point cloud, use the simulation function module and select waypoint_saver to form a csv path node file (the file contains the real-time position information and speed of the vehicle);
[0094] After the map is built, the autoware is used to generate the vehicle's position information in the x, y, and z directions, including the vehicle speed, and the yaw angle of the vehicle around the z axis. The approximate route is obtained through two-dimensional information (x, y) or three-dimensional information (x, y, z), such as Figure 3 to Figure 4 As shown;
[0095] S5. Load the csv path node file in the map point cloud, use astar_avoid and velocity_set based on the A* algorithm to form a vehicle tracking trajectory, and use the tracking instructions pure_pursuit and twist_filter based on the pure tracking algorithm to obtain the vehicle tracking result;
[0096] The process of forming a vehicle tracking trajectory using astar_avoid and velocity_set based on the A* search algorithm includes the following steps:
[0097] (511) Rasterized map:
[0098] The first step of the A* algorithm is to rasterize the map and store the map data in a large two-dimensional list;
[0099] (512) Construct an evaluation function:
[0100] F(n)=H(n)+G(n)
[0101] H(n) is the distance evaluation function, n represents each square on the map; G(n) is the cost function from the starting point to the end point;
[0102] The evaluation function F(n) of a node n is the sum of the distance estimate H(n) of the point and the cost function G(n); each path search of A* will look for the node with the minimum evaluation value;
[0103] (513) Search steps:
[0104] A* search involves two important lists, the open list and the closed list. The algorithm first adds the starting point to the open list, then traverses the open list, finds the node with the smallest evaluation function as the current search start node, and then adds the point to the closed list as the node that has been traversed. Explore each node adjacent to the node with the smallest evaluation function that is not in the closed list, and calculate their H, G, and F values. Take the node with the smallest evaluation function as the parent node of these nodes, and add these nodes as exploration nodes to the node set Q. If the node in Q is not in the open list, add it to the open list. If the node in Q already exists in the open list, compare the F value of these nodes with their F value in the open list. The smaller the F value, the better the path. If the F value in the open list is smaller or the two are equal, no change is made. Otherwise, the node in the open list is replaced with the node in Q. If the end point is in the open list, exit the search and determine the optimal path. The optimal path is the path that goes back along the parent node to the corresponding starting point.
[0105] The tracking process of the pure tracking algorithm includes the following steps:
[0106] The pure tracking algorithm is a trajectory tracking method based on geometric tracking. The pure tracking algorithm simplifies the smart car into a bicycle model, such as Figure 5 As shown, with the center of the rear axle of the vehicle as the tangent point and the longitudinal body of the vehicle as the tangent line, by controlling the steering angle δ of the front wheels, the vehicle can travel along an arc passing through the target path point (the vehicle travels along an arc passing through the preview point);
[0107] The next waypoint is tracked by the rear axle of the vehicle passing through the target path point, that is, on the established global path, the rear axle of the vehicle is controlled to pass through these waypoints; according to the sine theorem, the following expression can be obtained
[0108]
[0109]
[0110]
[0111] In the formula, l d represents the distance from the rear axle of the vehicle to the target waypoint, R represents the vehicle turning radius, and α represents the vehicle deviation angle; the above formula can also be expressed as
[0112]
[0113] Where κ is the calculated curvature of the arc, then the expression for the front wheel turning angle δ is
[0114] δ = tan -1 (κL)
[0115] Where L represents the distance between the front axle and the rear axle of the vehicle;
[0116] The final expression of the control quantity of the pure pursuit algorithm is obtained
[0117]
[0118] Some schematic diagrams of the autoware vehicle tracking simulation are as follows Figures 6 to 15 shown.
[0119] The advantages of using LiDAR to complete mapping and path planning compared to traditional GPS+IMU positioning are high accuracy and simple steps (all steps are completed only on autoware); while using GPS+IMU to complete path planning requires multiple software and is prone to errors.
Claims
1. A vehicle tracking method based on Autoware, characterized in that: The following steps are involved: S1, collecting data based on a laser radar installed on a vehicle; S2, use NDTmapping algorithm to build map in Autoware; S3, post-process the pcd point cloud map created by NDT_mapping using the pcl point cloud library, the post-processing process includes downsampling and point cloud map area division; In the process of downsampling, voxel_leaf_size is used to reduce the volume of point cloud files, and the point cloud files are voxel_filtered through the pcl point cloud library; in the process of point cloud map area division, the point cloud map is divided into regions by setting the grid size; Post-processing to obtain the final map point cloud; S4. Based on the map point cloud, use the simulation function module and select waypoint_saver to form a csv path node file; S5. Load the csv path node file in the map point cloud, use astar_avoid and velocity_set based on the A* algorithm to form a vehicle tracking trajectory, and use the tracking instructions pure_pursuit and twist_filter based on the pure tracking algorithm to obtain the vehicle tracking result; When using the NDTmapping algorithm to build a map, point cloud registration is required. The process of point cloud registration includes the following steps: (1) First, fix a point cloud data as the target point cloud and divide the target point cloud into grids; (2) After the target point cloud is divided into a grid, the point cloud is loaded into the grid and the mean vector is calculated first. in, represents the coordinates of the scanning point k in the grid; m represents the number of scanning points in the grid; By Calculate the covariance matrix; Finally, by Find the normal distribution probability density function in each grid; (3) Determine the initial coordinate transformation parameters of the source point cloud relative to the target point cloud; The source point cloud is a point cloud that needs to be registered with the target point cloud; (4) According to the initial coordinate transformation parameters, the source point cloud coordinates are transformed into the target point cloud coordinate system. At this time, the source point cloud will be distributed in the target point cloud grid. The probability that the source point cloud coordinates are X' is calculated from the normal distribution probability density function of the corresponding grid, that is, the probability of the source point cloud in the target point cloud grid after the coordinate transformation is determined; (5) Find the maximum target likelihood function: Multiply the probabilities of each point as the target likelihood function; Where n is the number of source point clouds distributed in the target point cloud grid; (6) Use Newton iteration method to optimize and find the optimal transformation parameter p to complete point cloud registration.
2. The vehicle tracking method based on Autoware according to claim 1, characterized in that: Before the LiDAR collects data, it is necessary to calibrate and measure the height of the LiDAR, and record it using rosbag. When recording using rosbag, you need to select the topic points_raw.
3. The vehicle tracking method based on Autoware according to claim 1, characterized in that: In the process of determining the initial coordinate transformation parameters in step (3), more than half of the source point cloud needs to be in the coordinate plane of the target point cloud.
4. The vehicle tracking method based on Autoware according to claim 3, characterized in that: The process of optimizing by Newton iteration method in step (6) to find the optimal transformation parameter p to complete point cloud registration includes the following steps: Solving the objective function formula Where H is the Hessian matrix of the objective function, is the gradient vector of the objective function; is the vector change value of the optimal transformation parameter p; Iterative update formula T = T + ΔT; Among them, ΔT is the change value of the coordinate transformation parameter T, and the iteration parameter T is the coordinate transformation parameter T.
5. The vehicle tracking method based on Autoware according to claim 4, characterized in that: In Autoware, when using the NDTmapping algorithm to build a map, select the parameter resolution as 1.
6. The vehicle tracking method based on Autoware according to claim 5, characterized in that: The process of using voxel_leaf_size to reduce the volume of point cloud files selects leaf_size as 0.1-0.
2.
7. A vehicle tracking method based on Autoware according to any one of claims 1 to 6, characterized in that: In the process of forming the CSV path node file, autoware is used to generate the vehicle's position information in the x, y, and z directions, as well as the vehicle's yaw angle around the z axis.
8. The vehicle tracking method based on Autoware according to claim 7, characterized in that: The process of forming a vehicle tracking trajectory using astar_avoid and velocity_set based on the A* search algorithm includes the following steps: (511) Rasterize the map and store the map data in a two-dimensional list; (512) Construct an evaluation function: F(n)=H(n)+G(n); H(n) is the distance evaluation function, n represents each square on the map; G(n) is the cost function from the starting point to the end point; (513) The search is performed based on the A* search algorithm. Each time the A* search searches for a path, it will look for the node with the minimum evaluation value.
9. The vehicle tracking method based on Autoware according to claim 8, characterized in that: The process of searching based on the A* search algorithm includes the following steps: A* search involves an open list and a closed list; the algorithm first adds the starting point to the open list, then traverses the open list, finds the node with the smallest evaluation function as the current search start node, and then adds the point to the closed list as a node that has been traversed; explores each node adjacent to the node with the smallest evaluation function that is not in the closed list, and calculates their H, G, and F values; and takes the node with the minimum evaluation function as the parent node of these nodes, and adds these nodes as exploration nodes to the node set Q; if the node in Q is not in the open list, add it to the open list; if the node in Q already exists in the open list, compare the F value of these nodes with their F value in the open list to see which is smaller. If the F value in the open list is smaller or the two are equal, do not make any changes, otherwise replace the nodes in the open list with the nodes in Q; if the end point is in the open list, exit the search and determine the optimal path.
