WO2022016320A1

WO2022016320A1 - Map update method and apparatus, computer device, and storage medium

Info

Publication number: WO2022016320A1
Application number: PCT/CN2020/103042
Authority: WO
Inventors: 解婧玮
Original assignee: 深圳元戎启行科技有限公司
Priority date: 2020-07-20
Filing date: 2020-07-20
Publication date: 2022-01-27
Also published as: CN114930391A

Abstract

A map update method and apparatus, a computer device, and a storage medium. The method comprises: acquiring a new key frame of a map to be updated and a historical pose graph of the map to be updated (S201); taking the new key frame as a new node and adding same to the historical pose graph to obtain a new pose graph (S202); identifying a node to be updated in the new pose graph (S203); and, in the map to be updated, updating a map area corresponding to the node to be updated (S204). By using the method, the purpose of updating a map to be updated according to a new key frame of said map and a historical pose graph of the map to be updated is achieved, thereby avoiding the defect of the low accuracy of updated map data caused by directly matching new map data to old map data, and thus improving the accuracy of map updating.

Description

Map update method, device, computer equipment and storage medium

technical field

The present application relates to the technical field of map processing, and in particular, to a map updating method, apparatus, computer equipment and storage medium.

Background technique

Map data is an important supporting element in autonomous driving technology, which is used to provide a priori information of driving scenarios to perception, planning and other modules; in order to ensure the timeliness of map data, the map needs to be updated continuously.

However, the current map update method generally matches the newly added map data to the old map data as a whole, and then performs incremental update; however, the new map data is directly matched to the old map data as a whole, and the old map data is The accuracy will limit the accuracy of the mapping, resulting in lower accuracy of the updated map data, resulting in lower accuracy of the map update.

SUMMARY OF THE INVENTION

Based on this, it is necessary to provide a map update method, device, computer equipment and storage medium capable of map update accuracy in response to the above technical problems.

A map update method, the method includes:

Obtain the new key frame of the map to be updated and the historical pose graph of the map to be updated;

adding the new key frame as a new node to the historical pose graph to obtain a new pose graph;

Identifying nodes to be updated in the new pose graph;

The map area corresponding to the node to be updated in the map to be updated is updated.

In one embodiment, adding the new key frame as a new node to the historical pose graph to obtain a new pose graph, including:

Use the new key frame as a new node;

Performing loop closure detection between the new node and the historical nodes in the historical pose graph to obtain the constraint relationship between the new node and the historical nodes in the historical pose graph;

According to the constraint relationship, the new node is added to the corresponding position in the historical pose graph to obtain a new pose graph.

In one of the embodiments, the loopback detection is performed between the new node and the historical nodes in the historical pose graph to obtain the difference between the new node and the historical nodes in the historical pose graph. constraints, including:

Perform loopback detection between the new node and the historical node in the historical pose graph to obtain a historical node adjacent to the new node in time or position;

The constraint relationship between the new node and the historical node in the historical pose graph is obtained according to the historical nodes adjacent to the new node in time or position.

In one embodiment, the identifying the node to be updated in the new pose graph includes:

When the number of the new nodes in the new pose graph reaches a first preset number, a first node set, a second node set and a third node set are constructed; the first node set is the A set composed of new nodes, the second node set is a set composed of historical nodes that have a constraint relationship with the new node and do not belong to the first node set, and the third node set is a set of The historical nodes in the second node set have a constraint relationship, and do not belong to the set formed by the historical nodes of the first node set and the second node set;

By using the pre-trained optimization model, the carrier state quantities in the first node set, the second node set and the third node set are optimized until the number of nodes in the first node set reaches a second preset number;

If the number of nodes in the first node set reaches the second preset number, the nodes in the first node set are used as nodes to be updated.

In one embodiment, the updating the map area corresponding to the node to be updated in the map to be updated includes:

obtaining the map data corresponding to the node to be updated;

determining the map area covered by the map data;

The map area in the to-be-updated map is updated.

In one embodiment, the acquiring a new key frame of the map to be updated includes:

Obtain the sensor data collected by the sensor installed on the carrier device on the target route of the map to be updated;

According to the sensor data, determine the initial carrier state quantity of the carrier device at each moment;

From the initial carrier state quantities of the carrier device at each moment, an initial carrier state quantity that satisfies a preset condition is screened out as a new key frame of the map to be updated.

In one embodiment, after updating the map area corresponding to the node to be updated in the map to be updated, the method further includes:

Get the updated map;

converting the updated map into a preset type of autopilot map;

The automatic driving map is sent to the automatic driving vehicle terminal, and the automatic driving vehicle terminal is triggered to control the driving of the corresponding automatic driving vehicle.

A map update device, the device includes:

an acquisition module for acquiring a new key frame of the map to be updated and a historical pose graph of the map to be updated;

An adding module is used to add the new key frame as a new node to the historical pose graph to obtain a new pose graph;

an identification module for identifying the node to be updated in the new pose graph;

An update module, configured to update the map area corresponding to the node to be updated in the map to be updated.

A computer device includes a memory and a processor, the memory stores a computer program, and the processor implements the following steps when executing the computer program:

A computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, the following steps are implemented:

The above-mentioned map updating method, device, computer equipment and storage medium, by acquiring a new key frame of the map to be updated and a historical pose graph of the map to be updated; adding the new key frame as a new node to the historical pose graph, Obtain a new pose graph; identify the node to be updated in the new pose graph; update the map area corresponding to the node to be updated in the to-be-updated map; realize the new key frame of the to-be-updated map and the history of the to-be-updated map Pose map, the purpose of updating the map to be updated, avoids the defect of directly matching the new map data to the old map data, resulting in the low accuracy of the updated map data, thereby improving the accuracy of the map update; At the same time, the new key frame is first added as a new node to the historical pose graph to obtain a new pose graph, and then the nodes to be updated in the new pose graph are identified, and then the to-be-updated map is updated. The nodes to be updated in the new pose graph do not directly use the new nodes to update the map to be updated, which further improves the accuracy of map update.

Description of drawings

Fig. 1 is the application environment diagram of the map update method in one embodiment;

2 is a schematic flowchart of a map update method in one embodiment;

3 is a schematic flowchart of a step of identifying a node to be updated in a new pose graph in one embodiment;

4 is a schematic flowchart of a step of obtaining a new key frame of a map to be updated in one embodiment;

5 is a schematic flowchart of a map update method in another embodiment;

6 is a schematic flowchart of a map update method in yet another embodiment;

7 is a structural block diagram of a map updating apparatus in one embodiment;

FIG. 8 is a diagram of the internal structure of a computer device in one embodiment.

detailed description

In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application will be described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, but not to limit the present application.

The map update method provided in this application can be applied to the application environment shown in FIG. 1 . Among them, the sensor 110 communicates with the server 120 through the network, and the sensor 110 is arranged on the carrier device to collect the corresponding sensor data during the movement of the carrier device on the target route of the map to be updated, and transfer the sensor data through the network. Upload to the server 120; the server 120 determines the new key frame of the map to be updated according to the received sensor data, and obtains the historical pose graph of the map to be updated; adds the new key frame as a new node to the historical pose graph , obtain a new pose graph; identify the node to be updated in the new pose graph; update the map area corresponding to the node to be updated in the to-be-updated map to obtain the updated map. Wherein, the sensors can be, but are not limited to, various cameras, GPS (Global Positioning System) receivers, lidars, IMUs (Inertial Measurement Unit, inertial measurement units), wheel speedometers, etc. The server 120 can use an independent It is implemented by a server or a server cluster composed of multiple servers.

In one embodiment, as shown in FIG. 2 , a map updating method is provided, which is described by taking the method applied to the server in FIG. 1 as an example, including the following steps:

Step S201, acquiring a new key frame of the map to be updated and a historical pose graph of the map to be updated.

The map to be updated refers to a map that needs to be updated, such as a point cloud map, a grid map, a semantic map, and the like. The new key frame refers to the carrier state quantity determined by the newly collected sensor data, and specifically refers to the carrier state quantity at a certain moment, which may include position and attitude values, and may also include other carrier state quantities to be optimized. The constraints between key frames refer to the relationship between the state quantities of the carrier at different times (such as the relative motion relationship between key frames measured by sensors, or the relative position relationship between key frames obtained from loop closure detection, which can be used as constraints) . Pose graph refers to a graph optimization model composed of key frames as nodes and constraints between key frames as edges. Specifically, it refers to the state information such as the position and attitude of the carrier device, and the change relationship of the state quantity of the carrier between different times. The historical pose graph refers to a graph composed of historical keyframes as nodes and constraints between historical keyframes as edges.

Specifically, the server receives the new sensor data collected by the sensor, and determines the carrier state quantity at the critical moment according to the new sensor data as a new key frame of the map to be updated; obtains the map identifier of the map to be updated, such as map number, map Name, etc., according to the map identification query and store the historical pose graph corresponding to multiple map identifications, obtain the historical pose graph corresponding to the map identification of the map to be updated, as the historical pose graph of the map to be updated. In this way, by acquiring new keyframes and historical pose graphs of the map to be updated, it is beneficial to subsequently add new keyframes as new nodes to the historical pose graph to obtain a new pose graph.

Further, the server can also obtain the historical pose graph in the following manner: obtain the historical sensor data collected by the sensor installed on the carrier device on the target route; according to the historical sensor data, determine the initial carrier state quantity of the carrier device at each historical moment. ; From the initial carrier state quantities of the carrier device at each historical moment, the initial carrier state quantities that meet the preset conditions are screened out as historical key frames; the historical key frames are used as nodes to construct a historical pose graph.

Step S202, adding the new key frame as a new node to the historical pose graph to obtain a new pose graph.

The new pose graph refers to a pose graph formed by adding nodes corresponding to new key frames on the basis of the historical pose graph.

Specifically, the server uses the new key frame as a new node, and obtains the historical nodes in the historical pose graph; obtains a preset loopback detection instruction, and determines the new node and the historical pose graph according to the preset loopback detection instruction The constraint relationship between the historical nodes in ; according to the constraint relationship, new nodes are added to the historical pose graph to obtain a new pose graph. In this way, by adding a new key frame as a new node to the historical pose graph, a new pose graph is obtained, which is beneficial to comprehensively consider the nodes in the new pose graph later to determine the new pose graph. Node to be updated.

Step S203, identifying the node to be updated in the new pose graph.

The node to be updated refers to a node that needs to be updated.

Specifically, the server obtains the carrier state quantity of the node in the new pose graph, and optimizes the carrier state quantity of the node in the new pose graph through the pre-trained optimization model, and obtains the state quantity of the node in the new pose graph. The node to be optimized is used as the node to be updated in the new pose graph. In this way, it is beneficial to update the map area corresponding to the to-be-updated node in the to-be-updated map, comprehensively considering the to-be-updated node in the new pose graph, instead of directly using the new node to update the to-be-updated map. Further improved the accuracy of map updates.

Further, the server may also obtain a preset identification instruction of the node to be updated, and analyze and process the nodes in the new pose graph according to the preset identification instruction of the node to be updated, so as to obtain the node to be updated in the new pose graph; Wherein, the preset identification instruction of the node to be updated is an instruction that can automatically identify the node to be updated in the pose graph.

Step S204, update the map area corresponding to the node to be updated in the to-be-updated map.

Specifically, the server obtains the node corresponding to the node to be updated in the map to be updated, and determines the map area to be updated in the map to be updated according to the node corresponding to the node to be updated in the map to be updated; Update the updated map area to get the updated map. In this way, the defect of directly matching the new map data to the old map data, resulting in low accuracy of the updated map data, is avoided, thereby improving the accuracy of map update.

The above map update method obtains a new key frame of the map to be updated and a historical pose graph of the map to be updated; adds the new key frame as a new node to the historical pose graph to obtain a new pose graph; recognizes The node to be updated in the new pose graph; the map area corresponding to the node to be updated in the map to be updated is updated; it is realized that the map to be updated is updated according to the new key frame of the map to be updated and the historical pose graph of the map to be updated. The purpose of the update is to avoid the defect of directly matching the new map data to the old map data, resulting in the low accuracy of the updated map data, thereby improving the accuracy of the map update; Add it to the historical pose graph as a new node, obtain a new pose graph, identify the nodes to be updated in the new pose graph, and then update the to-be-updated map, taking into account the to-be-updated map in the new pose graph Updating the node does not directly use the new node to update the map to be updated, which further improves the accuracy of the map update.

In one embodiment, in the above step S202, a new key frame is added to the historical pose graph as a new node to obtain a new pose graph, including: taking the new key frame as a new node; Perform loopback detection with the historical nodes in the historical pose graph to obtain the constraint relationship between the new node and the historical nodes in the historical pose graph; according to the constraint relationship, add the new node to the historical pose graph. Corresponding position, get a new pose graph.

The constraint relationship refers to the positional relationship between nodes, or the relationship between the times corresponding to the nodes.

Specifically, the server obtains a preset loopback detection instruction and a historical node in the historical pose graph; uses the new key frame as a new node, and uses the preset loopback detection instruction to display the new node and historical pose graph in the new node and the historical pose graph. The loopback detection is performed between the historical nodes in the past, and if a constraint relationship between the new node and the historical node in the historical pose graph is detected, the constraint relationship between the new node and the historical node in the historical pose graph is constructed; According to the constraint relationship, the corresponding position of the new node in the historical pose graph is determined; the new node is added to the corresponding position in the historical pose graph to obtain a new pose graph.

For example, when a new keyframe is added to the historical pose graph, that is, the new keyframe is used as a newly created node in the historical pose graph, the server will add the newly created node and the historical node in the historical pose graph Perform loopback detection between the nodes, try to establish a constraint relationship between the newly added node and the historical node in the historical pose graph, so as to add the new node to the corresponding position in the historical pose graph to obtain a new pose graph.

In the above embodiment, by adding a new key frame as a new node to the historical pose graph, a new pose graph is obtained, which is conducive to the subsequent comprehensive consideration of the nodes in the new pose graph to determine the new pose graph. The node to be updated in .

In one embodiment, loop closure detection is performed between the new node and the historical node in the historical pose graph to obtain the constraint relationship between the new node and the historical node in the historical pose graph, including: in the new node Perform loopback detection with the historical nodes in the historical pose graph to obtain the historical nodes adjacent to the new node time or position; obtain the new node and historical position according to the historical nodes adjacent to the new node time or position Constraints between historical nodes in the pose graph.

For example, for a newly established node, it can construct two types of edges with other nodes, one is the edge between adjacent time nodes, and the relationship between the adjacent time frames provided by the local database; the other One is the edge between nodes at related locations, from the relationship between spatially adjacent frames provided by the local database. For example, if there is a key frame at time T1 and time T2, respectively, corresponding to nodes V1 and V2 in the graph, and containing state quantities state_1 and state_2, when the wheel speedometer data is used, the vehicle can be measured from time T1 to time T2, Based on the movement of Trans_12_wheel based on the carrier coordinate system frame1, and the covariance matrix of the observed value Trans_12_wheel is Cov_12_wheel, the constraints on the key frames T1 and T2 can be set according to the measured value of the wheel speedometer, and the residual is set as: Residual_12_wheel=Trans_12_wheel -1·Trans01-1·Tans02. The constraint established between the key frames T1 and T2 at adjacent times is that the optimized state quantities Trans01 and Trans02 are expected to make the value of Residual_12_wheelT·Cov_12_wheel·Residual_12_wheel as small as possible. If the key frame at time T4 is stored in the graph as node V4, and the module S04 compares the point cloud data contained in key frame T4 and key frame T1, a loopback is detected between them, and the loop is detected by ICP (Iterative Closest Point , iterative closest point) algorithm to obtain the observation value Trans_14_icp of the coordinate transformation relationship between the two frames and the covariance matrix of this observation value is Cov_14_icp, then the observation value Trans_14_icp can be transformed between V1 and V4 according to the space between the two key frames. A constraint is established between two position-related nodes, and the residual is set as Residual_14_icp=Trans_14_icp-1·Trans01-1·Tans04. It is hoped that the optimized state quantities Trans01 and Trans04 can make the value of Residual_14_icpT·Cov_14_icp·Residual_14_icp as small as possible.

In the above-mentioned embodiment, by performing loopback detection between the new node and the historical node in the historical pose graph, the constraint relationship between the new node and the historical node in the historical pose graph is obtained, which is conducive to the follow-up according to the constraint relationship, Add the new node to the corresponding position in the historical pose graph to get a new pose graph.

In one embodiment, as shown in FIG. 3, the above step S203, identifying the node to be updated in the new pose graph, specifically includes the following steps:

Step S301, when the number of new nodes in the new pose graph reaches a first preset number, construct a first node set, a second node set and a third node set.

Among them, the first node set is a set composed of new nodes, the second node set is a set composed of historical nodes that have a constraint relationship with the new node and do not belong to the first node set, and the third node set is related to the first node set. The historical nodes in the two node sets have a constraint relationship, and do not belong to the set formed by the historical nodes of the first node set and the second node set.

Step S302, optimize the carrier state quantities in the first node set, the second node set and the third node set by using the pre-trained optimization model until the number of nodes in the first node set reaches a second preset number.

The pre-trained optimization model refers to a model that can optimize the nodes in the pose graph.

Specifically, through the pre-trained optimization model, the server uses the residual items defined between the nodes in the first node set, the second node set and the third node set as constraints, and uses the first node set and the second node set as constraints. The contained carrier state quantity is used as the carrier state quantity to be optimized, and the carrier state quantity contained in the third node set is set as a fixed variable, and the result is obtained by optimization; for the nodes contained in the second node set, compare the carrier state before and after optimization. If the updated value of the carrier state quantity for a node in the second node set exceeds the corresponding threshold, the node will be moved from the second node set to the first node set, and the third node set will be moved to the first node set. The target node that has a constraint relationship with the node moves to the second node set, and finds from the new pose graph that there is a constraint relationship with the target node and does not belong to the first node set, and the nodes in the second node set are added to the third node set , and perform the above process again; if no update value of the carrier state quantity in the second node set is greater than the corresponding threshold, the optimization is ended, or the number of nodes in the first node set reaches the second preset number, the optimization is ended.

Step S303, if the number of nodes in the first node set reaches a second preset number, the nodes in the first node set are used as nodes to be updated.

Wherein, the second preset number is greater than the first preset number.

For example, the server selects the nodes that need to be optimized from the new pose graph, and optimizes the new pose graph at a certain frequency; in order to realize the continuous update of the map construction, it will involve the optimization of a large number of carrier state quantities, so by selecting some carriers The state quantity is optimized, as follows:

Step1: Whenever K=20 nodes are added to the pose graph, go to step2.

Step 2: The set G1 is the node that needs to be updated for this round of optimization, and is initialized as a set composed of these K newly added nodes. Set G2 is a node that may need to be updated in this round of optimization. It is initialized as a node that has a constraint relationship with the K newly added nodes, but is not in set G1. Set G3 is a node that does not need to be updated in this round of optimization. Initialized There is a constraint relationship with the nodes in the set G2, but not the nodes in G1 and G2. After initialization, go to Step3.

Step3: Using the optimization algorithm, the residual term defined between the nodes in G1, G2 and G3 is used as a constraint, the state quantities contained in G1 and G2 are set as the variables to be optimized, and the state quantities contained in G3 are set as fixed variables, Optimizing to get the result. For the nodes contained in G2, compare the difference between the state quantities before and after optimization, if for a node Vn in G2, the update value of its state quantity exceeds the threshold d_threshold=[0.05m, 0.05m, 0.05m, 0.1degree, 0.1 degree, 0.1degree], then move the node Vn from the set G2 to G1, move each node Vn' in G3 that has a direct constraint relationship with Vn to G2, and find the existence of each Vn' from the pose graph The nodes that are constrained and not in G1 and G2 are added to G3, and Step3 is executed again. If no update value of the state quantity in G2 is greater than the threshold, the optimization ends. In order to control the amount of computation during optimization, an upper limit can be set on the number of elements in the set G1. For example, when the number of nodes in the set G1 is greater than the threshold n_max_node=10000, the loop is stopped.

It should be noted that the optimization algorithm can use linear or nonlinear least squares, such as Gauss-Newton's method, etc., and existing optimization algorithm libraries, such as ceres solver, g2o, etc., can be directly used in the implementation. In addition, when there are not many variables involved in the optimization problem, the traditional back-end optimization method can be used to optimize the entire pose graph after waiting for all new keyframes to be added to the pose graph. For example, using an optimization algorithm library such as ceres, the previously defined carrier state quantities and residual items are input into the optimization model, and all carrier state quantities are optimally estimated to obtain the optimized state quantities.

Further, with the continuous updating of the map, it may also be necessary to simplify the pose graph, delete outdated data, or thin the keyframe nodes again to reduce the amount of data that needs to be stored and calculated; When the time difference between the pose graph node and the newly added pose graph node is greater than 1 year, the data is removed from the pose graph.

In the above embodiment, by identifying the nodes to be updated in the new pose graph, it is beneficial to update the map area corresponding to the nodes to be updated in the to-be-updated map, and comprehensively consider the to-be-updated nodes in the new pose graph. Updating the node does not directly use the new node to update the map to be updated, which further improves the accuracy of the map update.

In one embodiment, in the above step S204, updating the map area corresponding to the node to be updated in the map to be updated includes: acquiring map data corresponding to the node to be updated; determining the map area covered by the map data; the map area in the map to be updated to update.

Among them, there are many types of map data, which can be point cloud data, two-dimensional color grid maps represented by RGB three colors, or two-dimensional grayscale maps; of course, map data can also be Other types of data are not specifically limited here.

Specifically, the server obtains the map data corresponding to the node to be updated, performs statistical analysis on the map area covered by the map data corresponding to the node to be updated, and obtains the map area covered by the map data; reuses the map data and the pose track, and performs a statistical analysis on the map area covered by the map data corresponding to the node to be updated. The updated map area is updated to obtain the updated map as a high-precision map for autonomous driving. For example, using the optimized pose trajectory, the pose of each frame of point cloud data is interpolated to obtain the updated 3D point cloud map.

The above embodiment, by updating the map area corresponding to the node to be updated in the map to be updated, avoids directly matching the new map data to the old map data, resulting in the defect that the accuracy of the updated map data is low, thereby improving the accuracy of the updated map data. accuracy of map updates.

In one embodiment, as shown in FIG. 4 , the above step S201, acquiring a new key frame of the map to be updated, specifically includes the following steps:

Step S401 , acquiring sensor data collected by sensors installed on the carrier device on the target route of the map to be updated.

Specifically, by installing various types of sensors (such as cameras, GPS receivers, lidars, IMUs, wheel speedometers, etc.) carrier devices (such as cars, drones, etc.), drive along the target route of the map to be updated and Collect the sensor data along the road, and send the sensor data to the corresponding server through the network; or save the collected sensor data in the memory of the carrier device in a specific format, and then read the data stored in the specific format from the memory through the server. sensor data.

Of course, it is also possible to measure by a sensor installed at a fixed position on the target route, and send the measured sensor data to the corresponding server through the network; the sensor data can include the data collection time in addition to the data output by the sensor itself. The timeline of individual sensor data should remain consistent.

For example, use the car as the carrier device, use the lidar and GPS receiver installed on the car as the sensor, carry out driving and data collection on the target route, and transmit the sensor data obtained by the sensor through the network in real time during the driving process. The server in the cloud reads the sensor data collected this time from the server after the collection.

It should be noted that, by using high-precision and low-precision map acquisition vehicles, the acquisition cost is reduced in the case of high coverage.

Step S402, according to the sensor data, determine the initial carrier state quantity of the carrier device at each moment.

Among them, the carrier state quantity can include the position, attitude, speed, acceleration and other information of the carrier device at a specific time in a specific reference coordinate system (such as the ENU global coordinate system, or a local coordinate system with the initial position of the carrier as the origin, etc.), It can also contain the status information of the sensor at different times (such as IMU zero offset, wheel radius, etc.). The mileage calculation method used in this step is selected according to the actual application scene and sensor configuration as needed; based on the camera sensor, visual odometry calculation methods such as feature point method, direct method, and optical flow method can be used; based on laser sensor, ICP can be used , NDT and other point cloud matching algorithms to calculate the relative position relationship of point clouds at adjacent times; GPS sensors can also be used to directly use the sensor position information output by GPS to obtain the initial position value of the download tool at each time through interpolation.

Specifically, based on the sensor data, the server estimates the change relationship of the carrier state quantities at adjacent moments; and determines the initial carrier state quantities of the carrier device at each time according to the change relationship of the carrier state quantities at adjacent times.

For example, using a car as a carrier device, the carrier coordinate system is defined as a Cartesian coordinate system with the center of the four wheels of the car as the origin of the coordinate system, and the xyz axes point to the front, left, and top of the car, respectively. The carrier coordinate system at time t is denoted by Ft. The carrier coordinate system F0 at the initial time t=0 is used as the reference coordinate system. Use Trans_mn=[p_mn, R_mn] to represent the coordinate conversion relationship from the coordinate system Fn to the coordinate system Fm, p_mn is the position of the coordinate origin of the coordinate system Fn in the coordinate system Fm, and R_mn is the rotation from the coordinate system Fn to the coordinate system Fm. rotation matrix. Then the coordinates of a point P_Fn=(x, y, z) in Fm in the carrier coordinate system Fn are P_Fm=p_mn+R_mn·P_Fn. Set the carrier state value state_t at time t as the coordinate conversion relationship from Ft to F0, that is, state_t=Trans_0t, then p_0t is the coordinate of the center of the four wheels of the car in the reference frame F0 at time t, and R_0t represents the rotation from Ft to F0. p_00=[0,0,0], R_00=I. At t=0, 1, 2, the laser point clouds pcd0, pcd1, pcd2 under the carrier coordinate systems F0, F1, and F2 are obtained by scanning with a laser scanner, and the coordinate transformations Trans_01 and pcd2 from pcd1 to pcd0 are obtained by using the ICP algorithm Coordinate transformation Trans12 to pcd1. Then state_1=Trans01, state_2=Trans02=[p_02, R_02]=[p_01+R_01·p_12, R_01·R_12]. And so on state_k=[p_0k, R_0k]=[p_01+R_01·p_12+...+R_0,k-1·p_k-1,k, R_01·R_12...·R_k-1,k]. This completes the initialization of state_0 to state_k.

It should be noted that for camera and lidar data, each time a data is obtained, a frame of state quantity is established at the time corresponding to the data. For data with high input frequency such as Imu, another frame can be added at intervals such as 0.01s. state quantity.

Step S403: From the initial carrier state quantities of the carrier device at each moment, an initial carrier state quantity that satisfies a preset condition is screened out as a new key frame of the map to be updated.

Specifically, based on the graph optimization theory, the server selects the initial carrier state quantity that satisfies the preset conditions from the initial carrier state quantity of the carrier device as a new key frame of the map to be updated; in addition, the server can also use the key frame as the graph The nodes in the node, use sensor data to construct constraints between key frames as edges between nodes; add key frames to the historical pose graph in a certain order, select the nodes to be optimized to optimize the update of the carrier state quantity, and follow the Certain rules move the keyframes that no longer need to be optimized out of the historical pose graph, so as to obtain a new pose graph of the map to be updated.

For example, in order to reduce the amount of calculation during optimization, in the optimization process, according to certain rules, from the initial carrier state quantities at all times, select a part of the initial carrier state quantities as key frames, and input the pose graph for optimization. The specific selection method can use a fixed time interval, such as selecting a key frame every 0.1s from a series of initial carrier state quantities with a time interval of 0.01s; it can also be obtained by setting the pose change threshold and other methods, such as with When the Euclidean distance of the previous key frame is greater than 0.1m, a new key frame is established; when the total initial carrier state quantity is small, the initial carrier state quantity at all times can also be directly used as the key frame.

It should be noted that, in addition to selecting key frames, it is also necessary to move point cloud data and camera data from non-key frames to key frames; the specific method is to find the key frame Frame_key with the smallest time difference from the point cloud frame Frame_pcd, according to their The initial state meter is converted from Frame_pcd to the spatial transformation relationship of Frame_key, Trans_key_pcd=Trans_0_key-1·Trans_0_pcd; then the point cloud is converted from the carrier coordinate system of the point cloud frame to the carrier coordinate system of the key frame. Similarly, the value in the key frame can be obtained. Camera external parameters.

It should be noted that the selection method of keyframes in the historical pose graph is the same as the above process; in addition, if the pose graph is initially empty, after the keyframe is obtained, the keyframe can be added as a node to the pose graph , as the historical pose graph, and store it; the next time the mapping operation is performed, the existing historical pose graph can be loaded and updated based on it.

In the above embodiment, by acquiring a new key frame of the map to be updated, it is beneficial to subsequently add the new key frame as a new node to the historical pose graph to obtain a new pose graph.

In one embodiment, the above step S204, after updating the map area corresponding to the node to be updated in the map to be updated, further includes: acquiring the updated map; converting the updated map into a preset type of automatic driving map; Send the autopilot map to the autopilot vehicle terminal, and trigger the autopilot vehicle terminal to control the corresponding autopilot vehicle to drive.

Among them, automatic driving maps refer to high-precision maps that can be used for automatic driving, including various types.

Specifically, the server obtains the updated map, and according to the preset map conversion instruction, converts the updated map into a preset type of automatic driving map, such as adding traffic signs, road signs, etc.; and sends the automatic driving map to the corresponding The self-driving vehicle terminal; the self-driving vehicle terminal displays the self-driving map through the terminal interface, and controls the corresponding self-driving vehicle to drive safely, such as stop at red light, go at green light, etc.

In the above embodiment, by converting the updated map into a high-precision automatic driving map and sending it to the terminal of the automatic driving vehicle, it is beneficial to improve the automatic driving safety of the automatic driving vehicle.

In one embodiment, as shown in FIG. 5 , another method for updating a map is provided, which is described by taking the method applied to the server in FIG. 1 as an example, including the following steps:

Step S501, acquiring sensor data collected by sensors installed on the carrier device on the target route of the map to be updated.

Step S502, according to the sensor data, determine the initial carrier state quantity of the carrier device at each moment.

Step S503: From the initial carrier state quantities of the carrier device at each moment, an initial carrier state quantity that satisfies a preset condition is screened out as a new key frame of the map to be updated.

Step S504, obtaining the historical pose graph of the map to be updated, and using the new key frame as a new node.

In step S505, loopback detection is performed between the new node and the historical nodes in the historical pose graph to obtain historical nodes adjacent to the new node in time or position.

Step S506 , obtaining the constraint relationship between the new node and the historical node in the historical pose graph according to the historical node adjacent to the new node in time or position.

Step S507, according to the constraint relationship, add a new node to the corresponding position in the historical pose graph to obtain a new pose graph.

Step S508, when the number of new nodes in the new pose graph reaches a first preset number, construct a first node set, a second node set and a third node set.

Step S509, optimize the carrier state quantities in the first node set, the second node set and the third node set by using the pre-trained optimization model, until the number of nodes in the first node set reaches a second preset number.

Step S510, if the number of nodes in the first node set reaches the second preset number, the nodes in the first node set are used as nodes to be updated.

Step S511: Obtain map data corresponding to the node to be updated; determine a map area covered by the map data; and update the map area in the map to be updated.

The above map update method realizes the purpose of updating the map to be updated according to the new key frame of the map to be updated and the historical pose graph of the map to be updated, and avoids directly matching the new map data to the old map data, resulting in The updated map data has a low accuracy defect, thereby improving the accuracy of the map update.

In one embodiment, as shown in FIG. 6 , the present application further provides an application scenario where the above-mentioned map update method is applied. Specifically, the application steps of the map update method in this application scenario are as follows:

Step S601, acquiring sensor data collected by sensors installed on the carrier device on the target route of the map to be updated.

Step S602, according to the sensor data, perform an initial estimation on the state quantity of the carrier at each moment.

For example, based on the sensor data, the server estimates the change relationship of the carrier state quantities at adjacent times, and performs initial estimation of the carrier states at different times.

Step S603 , selecting a carrier state quantity that meets the preset condition as a key frame; using the key frame as a node, a pose graph is constructed, and the pose that needs to be updated is optimized.

For example, based on graph optimization theory, the server selects key frames from the initial state set of the carrier as nodes in the pose graph, and uses sensor data to construct constraints between key frames as edges between nodes; in a certain order Add keyframes to the pose graph, select the nodes that need to be optimized to optimize and update the state quantity, and move the keyframes that no longer need to be optimized out of the pose graph according to certain rules.

Step S604, using the optimized key frame pose to construct or update map data.

It should be noted that for the specific description of steps S601 to S604, reference may be made to the above-mentioned related embodiments, and details are not repeated here.

Step S605, save the mapping data to the storage in a certain format, or read the data from the storage for back-end optimization and update.

Specifically, the server is responsible for saving and reading mapping data; mapping data refers to various data required for map construction and pose graph optimization, including pose graph data and related sensor data, and pose graph data includes nodes The state quantity of the edge, the node information connected to the edge, and the constraint information contained in the edge, the sensor data can include the photos taken by the camera, the point cloud scanned by the laser scanner, etc.

For example, for each node NodeM of the pose graph, use the node to collect the vehicle license plate number and the acquisition time accurate to microseconds to set the node's unique id, and use the node id as the file name. For example, the key frame collected by the car with the license plate number "Guangdong A12345" at 13:15:17.1866 on January 1, 2020 can be set as YUE_A12345_20200101131517186600. Save the state quantity and time of the node in the file in text form. For each constraint EdgeMN related to NodeM (i.e. edges in the pose graph) is also saved in this file as text. It is necessary to record the id of the NodeN connected to the NodeM through the EdgeMN, the type of the constraint, and the data contained in the constraint. For example, NodeM and NodeN each contain a set of point cloud data, use the ICP algorithm to obtain the coordinate system transformation relationship between the two point clouds, and obtain the carrier coordinate system transformation relationship TransMN and covariance matrix CovIcpMN of NodeM and NodeN, then record the constraint. The type is ICP coordinate transformation, and the parameters p_MN and R_MN of the coordinate system transformation TransMN are recorded, and the covariance matrix CovIcpMN of the transformation relationship is recorded. In addition, it is necessary to save the laser point cloud data contained in NodeM and NodeN, each with the node id as the file name, and save it as point cloud data in pcd format. When reading sensor data, just load the file with the corresponding name through the data id number. When reading a pose graph node, you can get a node by inputting the node id, or you can read several nodes by specifying the node coordinate range. When reading nodes by coordinate range, read each node in the database, and calculate whether its coordinate range is within the specified range to decide whether to return the node. When the range of the map is large, the mapping data can be divided into blocks according to certain rules, and the data block id can be used as the folder name to save in blocks to speed up the processing speed of reading nodes by coordinates. Blocking is carried out based on the coordinates of the data, and the id of each block obtained is related to the coordinates. The id range of the block can be determined by the input coordinate range, and only the nodes that meet the coordinate range to be read are searched in these blocks.

For example, divide into blocks according to the precision of 2 degrees and the latitude of 1 degree, divide the grid according to the integer longitude and latitude, and name the grid with the minimum longitude and latitude. Then for a key frame whose longitude and latitude are longitude and latitude respectively, the precision value range is from 0 to 360 degrees, and when the latitude value range is -90 to 90 degrees, the grid number is determined by the precision grid number and the latitude grid number. The calculation method is: precision grid number grid_id_longitude=floor(longitude/2.0), latitude grid number grid_id_latitude=floor(latitude+90). floor is a round-down function. The longitude grid number ranges from 0 to 179, and the latitude grid number ranges from 0 to 180. For a keyframe with a longitude of 36.3 degrees and a latitude of 66.1 degrees, its grid number is (longitude grid number 18, latitude grid number 66). There are eight adjacent grids in the 8-neighborhood of the grid to which it belongs. ), (19, 65), (19, 66), (19, 67). When searching for key frames with an accuracy range of 35.7-36.5 and a latitude range of 65.8-66.8, first calculate the average of the latitude and longitude ranges, which is the accuracy of 36.1 and the latitude of 66.3. The number of grid A corresponding to this coordinate is accuracy 18, latitude 66, Grid A is added to the grid set G1 to be searched, and eight neighboring grids of A are added to the grid set G2 to be checked. When G2 is not an empty set, take out and remove grid B from G2, check whether B overlaps with the target latitude and longitude range, if so, add the eight neighbor grids of grid B to G2 set, and add the grid Lattice B joins set G1. When the G2 set is cleared, the grids contained in G1 are the grids that may contain the target keyframe. Traverse the keyframes in all grids contained in G1, and output the coordinate range that meets the requirements.

It should be noted that, after the update of the pose graph is performed in step S604, the updated nodes and the constraints between the nodes need to be updated. Yes; if the block id to which the node belongs changes, you need to delete the node from the old block folder first, and then add the file corresponding to the node in the new block folder.

In step S606, a constraint relationship is established between relevant key frames that are discontinuous in time through loop closure detection.

Specifically, the server searches for possible loops for each initialized key frame, and the search for loops can be based on bag-of-words method, feature point matching, and geographic proximity. After finding the loopback, use point cloud or camera data, use algorithms such as ICP, NDT, and feature points to estimate the relative positional relationship between the two frames, and use the relative positional relationship as a constraint to add an edge between the two nodes.

In addition, if the map range is large, the initial geographic location of the node is used to calculate the id of the mapping data block where the node is located, and the id of the adjacent data block is obtained, and the loopback search is performed in the map construction data blocks with close distances.

The above-mentioned map update method can achieve the following technical effects: (1) Make full use of the collected data, realize high-precision and high-efficiency map construction and incremental update, and reduce equipment and labor in the process of making maps for autonomous driving. (2) Reuse historical data to improve the accuracy and consistency of map construction, reduce manual inspection and adjustment by map producers in the map construction step, reduce the configuration requirements for map carts and internal production computers, and speed up map construction. Production speed; (3) Save the pose graph and sensor information for back-end optimization, loopback detection and map update; (4) Optimize both the new pose and the pose in the old map to fully Use historical data; (5) The optimization model uses local graph optimization of variable scale to reduce the configuration requirements for internal production computers.

It should be understood that although the steps in the flowcharts of FIGS. 2-6 are shown in sequence according to the arrows, these steps are not necessarily executed in the sequence shown by the arrows. Unless explicitly stated herein, the execution of these steps is not strictly limited to the order, and these steps may be performed in other orders. Moreover, at least a part of the steps in FIGS. 2-6 may include multiple steps or multiple stages. These steps or stages are not necessarily executed and completed at the same time, but may be executed at different times. The execution of these steps or stages The order is also not necessarily sequential, but may be performed alternately or alternately with other steps or at least a portion of the steps or phases within the other steps.

In one embodiment, as shown in FIG. 7 , a map updating apparatus is provided, including: an acquiring module 710, an adding module 720, an identifying module 730 and an updating module 740, wherein:

The obtaining module 710 is configured to obtain a new key frame of the map to be updated and a historical pose graph of the map to be updated.

The adding module 720 is configured to add the new key frame as a new node to the historical pose graph to obtain a new pose graph.

The identification module 730 is configured to identify the node to be updated in the new pose graph.

The updating module 740 is configured to update the map area corresponding to the node to be updated in the map to be updated.

In one embodiment, the above-mentioned adding module 720 is further configured to use the new key frame as a new node; perform loopback detection between the new node and the historical node in the historical pose graph to obtain the new node and historical position The constraint relationship between the historical nodes in the pose graph; according to the constraint relationship, new nodes are added to the corresponding positions in the historical pose graph to obtain a new pose graph.

In one embodiment, the above-mentioned adding module 720 is further configured to perform loopback detection between the new node and the historical nodes in the historical pose graph to obtain historical nodes adjacent to the new node in time or position; The node time or position is adjacent to the historical node, and the constraint relationship between the new node and the historical node in the historical pose graph is obtained.

In one embodiment, the above identification module 730 is further configured to construct a first node set, a second node set and a third node set when the number of new nodes in the new pose graph reaches a first preset number ; The first node set is a set composed of new nodes, the second node set is a set of historical nodes that have a constraint relationship with the new node and do not belong to the first node set, and the third node set is a set of historical nodes with the second node set. The historical nodes in the node set have a constraint relationship and do not belong to the set formed by the historical nodes of the first node set and the second node set; through the pre-trained optimization model, the first node set, the second node set and the third node set are The carrier state quantity in the node set is optimized until the number of nodes in the first node set reaches the second preset number; if the number of nodes in the first node set reaches the second preset number, the first node set is The nodes in are the nodes to be updated.

In one embodiment, the above updating module 740 is further configured to acquire map data corresponding to the node to be updated; determine the map area covered by the map data; and update the map area in the map to be updated.

In one embodiment, the above acquisition module 710 is further configured to acquire sensor data collected by sensors installed on the carrier device on the target route of the map to be updated; according to the sensor data, determine the initial carrier state quantity of the carrier device at each moment; From the initial carrier state quantities of the carrier device at each moment, the initial carrier state quantities that meet the preset conditions are screened out as new key frames of the map to be updated.

In one embodiment, the map update device of the present application further includes a sending module for acquiring the updated map; converting the updated map into a preset type of automatic driving map; sending the automatic driving map to the terminal of the automatic driving vehicle , triggering the terminal of the autonomous driving vehicle to control the driving of the corresponding autonomous driving vehicle.

For the specific limitation of the map updating apparatus, please refer to the limitation on the map updating method above, which will not be repeated here. Each module in the above-mentioned map updating apparatus may be implemented in whole or in part by software, hardware and combinations thereof. The above modules can be embedded in or independent of the processor in the computer device in the form of hardware, or stored in the memory in the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

In one embodiment, a computer device is provided, and the computer device may be a server, and its internal structure diagram may be as shown in FIG. 8 . The computer device includes a processor, memory, and a network interface connected by a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium, an internal memory. The nonvolatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the execution of the operating system and computer programs in the non-volatile storage medium. The database of the computer device is used to store data such as historical pose graphs, new pose graphs, and the like. The network interface of the computer device is used to communicate with an external terminal through a network connection. The computer program, when executed by the processor, implements a map update method.

Those skilled in the art can understand that the structure shown in FIG. 8 is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer equipment to which the solution of the present application is applied. Include more or fewer components than shown in the figures, or combine certain components, or have a different arrangement of components.

In one embodiment, a computer device is also provided, including a memory and a processor, where a computer program is stored in the memory, and the processor implements the steps in the foregoing method embodiments when the processor executes the computer program.

In one embodiment, a computer-readable storage medium is provided, which stores a computer program, and when the computer program is executed by a processor, implements the steps in the foregoing method embodiments.

Those skilled in the art can understand that all or part of the processes in the methods of the above embodiments can be implemented by instructing relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium , when the computer program is executed, it may include the processes of the above-mentioned method embodiments. Wherein, any reference to memory, storage, database or other media used in the various embodiments provided in this application may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (Read-Only Memory, ROM), magnetic tape, floppy disk, flash memory, or optical memory, and the like. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, the RAM may be in various forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

The technical features of the above embodiments can be combined arbitrarily. In order to make the description simple, all possible combinations of the technical features in the above embodiments are not described. However, as long as there is no contradiction in the combination of these technical features It is considered to be the range described in this specification.

The above examples only represent several embodiments of the present application, and the descriptions thereof are relatively specific and detailed, but should not be construed as a limitation on the scope of the invention patent. It should be pointed out that for those skilled in the art, without departing from the concept of the present application, several modifications and improvements can be made, which all belong to the protection scope of the present application. Therefore, the scope of protection of the patent of the present application shall be subject to the appended claims.

Claims

A map update method, characterized in that the method comprises:

Obtain the new key frame of the map to be updated and the historical pose graph of the map to be updated;

adding the new key frame as a new node to the historical pose graph to obtain a new pose graph;

Identifying nodes to be updated in the new pose graph;

The map area corresponding to the node to be updated in the map to be updated is updated.
The method according to claim 1, wherein, adding the new key frame as a new node to the historical pose graph to obtain a new pose graph, comprising:

Use the new key frame as a new node;

Performing loop closure detection between the new node and the historical nodes in the historical pose graph to obtain the constraint relationship between the new node and the historical nodes in the historical pose graph;

According to the constraint relationship, the new node is added to the corresponding position in the historical pose graph to obtain a new pose graph.
The method according to claim 2, characterized in that, performing loop closure detection between the new node and the historical node in the historical pose graph to obtain the new node and the historical pose Constraints between historical nodes in the graph, including:

Perform loopback detection between the new node and the historical node in the historical pose graph to obtain a historical node adjacent to the new node in time or position;

The constraint relationship between the new node and the historical node in the historical pose graph is obtained according to the historical nodes adjacent to the new node in time or position.
The method according to claim 1, wherein the identifying the node to be updated in the new pose graph comprises:

When the number of the new nodes in the new pose graph reaches a first preset number, a first node set, a second node set and a third node set are constructed; the first node set is the A set composed of new nodes, the second node set is a set composed of historical nodes that have a constraint relationship with the new node and do not belong to the first node set, and the third node set is a set of The historical nodes in the second node set have a constraint relationship, and do not belong to the set formed by the historical nodes of the first node set and the second node set;

By using the pre-trained optimization model, the carrier state quantities in the first node set, the second node set and the third node set are optimized until the number of nodes in the first node set reaches a second preset number;

If the number of nodes in the first node set reaches the second preset number, the nodes in the first node set are used as nodes to be updated.
The method according to claim 1, wherein the updating the map area corresponding to the node to be updated in the map to be updated comprises:

obtaining the map data corresponding to the node to be updated;

determining the map area covered by the map data;

The map area in the to-be-updated map is updated.
The method according to any one of claims 1 to 5, wherein the acquiring a new key frame of the map to be updated comprises:

Obtain the sensor data collected by the sensor installed on the carrier device on the target route of the map to be updated;

According to the sensor data, determine the initial carrier state quantity of the carrier device at each moment;

From the initial carrier state quantities of the carrier device at each moment, an initial carrier state quantity that satisfies a preset condition is screened out as a new key frame of the map to be updated.
The method according to claim 6, wherein after updating the map area corresponding to the node to be updated in the map to be updated, the method further comprises:

Get the updated map;

converting the updated map into a preset type of autopilot map;

The automatic driving map is sent to the automatic driving vehicle terminal, and the automatic driving vehicle terminal is triggered to control the driving of the corresponding automatic driving vehicle.
A map updating device, characterized in that the device comprises:

an acquisition module for acquiring a new key frame of the map to be updated and a historical pose graph of the map to be updated;

An adding module is used to add the new key frame as a new node to the historical pose graph to obtain a new pose graph;

an identification module for identifying the node to be updated in the new pose graph;

An update module, configured to update the map area corresponding to the node to be updated in the map to be updated.
A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein the processor implements the steps of the method according to any one of claims 1 to 7 when the processor executes the computer program.
A computer-readable storage medium on which a computer program is stored, characterized in that, when the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.