Pose map optimization method and device, equipment and storage medium
By obtaining and constructing error sides in the position pose map and optimizing the position pose map with navigation data, the position deviation problem caused by map error in the autonomous driving system is solved, and more accurate vehicle control and navigation is achieved.
Patent Information
- Application Number
- CN202411392664.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-08
- Publication Date
- 2024-12-27
AI Technical Summary
In the process of optimizing position maps of autonomous driving systems, the 6-dimensional error amount introduced by the prior art causes deviations from the bicycle position and map position. How to reduce positioning deviations caused by map errors has become an urgent problem to be solved.
By obtaining pose estimation data, high-precision map data and front-and-back perception data, we determine the lateral displacement error, heading angle error and longitudinal displacement error, construct the lateral error edge and longitudinal error edge, and when these error edges are detected, based on the combined navigation The data determines the vertical displacement error, roll angle error and pitch angle error, and constructs the constraint relationship of the pose map to optimize the pose map.
Effectively reduce positioning deviations due to map errors, achieving more accurate vehicle control and navigation.
Smart Images
Figure CN119197557A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of map positioning technology, and in particular to a pose graph optimization method, device, equipment and storage medium. Background Art
[0002] In the pose graph optimization process of the autonomous driving system, the error edge is the edge connecting the vertices (i.e., poses) in the pose graph, which is used to represent the difference between the expected relationship between two poses and the actual measured value. Defining the error edge is a key step in constructing the pose graph, and the construction of the error edge directly affects the accuracy and reliability of the pose graph optimization results.
[0003] The existing technology will introduce six dimensions of error in the edge setting, namely err_x (lateral displacement error), err_y (longitudinal displacement error), err_z (vertical displacement error), err_roll (roll angle error), err_pitch (pitch angle error), and err_yaw (heading angle error). That is, regardless of whether there is a map error, the vehicle's position will trust the position given by the combined navigation more, and the vehicle's position will be closer to the combined navigation posture. After the introduction of high-precision maps, since the high-precision maps themselves also have errors, the use of six dimensions of error will cause the vehicle's position to deviate from the map position. How to reduce the positioning deviation caused by map errors in the process of optimizing the posture graph has become an urgent problem to be solved.
[0004] The above contents are only used to assist in understanding the technical solution of the present application and do not constitute an admission that the above contents are prior art. Summary of the invention
[0005] The main purpose of this application is to provide a pose graph optimization method, device, equipment and storage medium, aiming to solve the technical problem of how to reduce positioning deviation caused by map errors in the process of optimizing the pose graph.
[0006] To achieve the above objectives, the present application proposes a pose graph optimization method, the method comprising:
[0007] Obtain posture estimation data, high-precision map data, front and rear perception data, and combined navigation data;
[0008] Determine a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture calculation data, the high-precision map data, and the front and rear perception data, and construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error;
[0009] When the lateral error edge and the longitudinal error edge are detected, determining a vertical displacement error, a roll angle error, and a pitch angle error based on the integrated navigation data;
[0010] A constraint relationship of a pose graph is constructed based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error, and the pitch angle error to optimize the pose graph.
[0011] In one embodiment, the step of determining a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the pose calculation data, the high-precision map data, and the front and rear perception data, and constructing a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error includes:
[0012] Determine the current position of the vehicle based on the posture estimation data;
[0013] Based on the current vehicle position, the high-precision map data, and the front and rear perception data, a lateral displacement error, a heading angle error, and a longitudinal displacement error are determined, and a lateral error edge and a longitudinal error edge are constructed based on the lateral displacement error, the heading angle error, and the longitudinal displacement error.
[0014] In one embodiment, the step of determining the current vehicle position based on the posture estimation data includes:
[0015] Determine the relative poses of the previous and next frames based on the pose estimation data;
[0016] The historical ego vehicle position is obtained, and the current ego vehicle position is determined based on the historical ego vehicle position and the relative posture of the previous and next frames.
[0017] In one embodiment, the high-precision map data includes map lane line data and map sign data, and the front and rear perception data includes perception lane line data and perception sign data;
[0018] The step of determining the lateral displacement error, the heading angle error and the longitudinal displacement error based on the current vehicle position, the high-precision map data and the front and rear perception data, and constructing the lateral error edge and the longitudinal error edge based on the lateral displacement error, the heading angle error and the longitudinal displacement error includes:
[0019] Determine a lateral displacement error and a heading angle error based on the current vehicle position, the map lane line data, and the sensed lane line data, and construct a lateral error edge based on the lateral displacement error and the heading angle error;
[0020] A longitudinal displacement error is determined based on the map sign data and the sensory sign data, and a longitudinal error edge is constructed based on the longitudinal displacement error.
[0021] In one embodiment, the step of determining a lateral displacement error and a heading angle error based on the current vehicle position, the map lane line data, and the sensed lane line data, and constructing a lateral error edge based on the lateral displacement error and the heading angle error includes:
[0022] Determine a first lateral distance and a first relative heading of a lane line based on the current vehicle position and the map lane line data;
[0023] Determining a second lateral distance and a second relative heading based on the sensed lane line data;
[0024] A lateral displacement error and a heading angle error are determined based on the first lateral distance, the first relative heading, the second lateral distance, and the second relative heading, and a lateral error edge is constructed based on the lateral displacement error and the heading angle error.
[0025] In one embodiment, the step of determining the longitudinal displacement error based on the map sign data and the perception sign data, and constructing the longitudinal error edge based on the longitudinal displacement error comprises:
[0026] Determining a relative position difference between a position of a map sign and a position of a perceived sign based on the map sign data and the perceived sign data;
[0027] A longitudinal displacement error is determined based on the relative position and posture, and a longitudinal error edge is constructed based on the longitudinal displacement error.
[0028] In one embodiment, before the step of determining the vertical displacement error, the roll angle error and the pitch angle error based on the integrated navigation data when the lateral error edge and the longitudinal error edge are detected, the step further includes:
[0029] When the lateral error edge and the longitudinal error edge are not detected, the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error, and the pitch angle error are determined based on the integrated navigation data.
[0030] In addition, to achieve the above purpose, the present application also proposes a posture graph optimization device, the posture graph optimization device comprising:
[0031] Optimize the data acquisition module to obtain posture estimation data, high-precision map data, front and rear perception data, and integrated navigation data;
[0032] A first error construction module, configured to determine a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture estimation data, the high-precision map data, and the front and rear perception data, and to construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error;
[0033] a second error construction module, configured to determine a vertical displacement error, a roll angle error, and a pitch angle error based on the integrated navigation data when the lateral error edge and the longitudinal error edge are detected;
[0034] The optimization execution module is used to construct a constraint relationship of a pose graph based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error to complete the optimization of the pose graph.
[0035] In addition, to achieve the above-mentioned purpose, the present application also proposes a pose graph optimization device, which includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, and the computer program is configured to implement the steps of the pose graph optimization method described above.
[0036] In addition, to achieve the above-mentioned purpose, the present application also proposes a storage medium, which is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, the steps of the pose graph optimization method described above are implemented.
[0037] In addition, to achieve the above-mentioned purpose, the present application also provides a computer program product, which includes a computer program, and when the computer program is executed by a processor, it implements the steps of the pose graph optimization method described above.
[0038] One or more technical solutions proposed in this application have at least the following technical effects:
[0039] Acquire posture calculation data, high-precision map data, front and rear perception data and combined navigation data; determine the lateral displacement error, heading angle error and longitudinal displacement error based on the posture calculation data, high-precision map data and front and rear perception data, and construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, heading angle error and longitudinal displacement error, decompose the posture error into more specific longitudinal and lateral errors, and make the actual posture of the vehicle consistent with the position on the high-precision map by decomposing the errors and optimizing the longitudinal and lateral errors in a targeted manner; when the lateral error edge and the longitudinal error edge are detected, determine the vertical displacement error, the roll angle error and the pitch angle error based on the combined navigation data; construct a constraint relationship of the posture graph based on the lateral displacement error, heading angle error, longitudinal displacement error, vertical displacement error, roll angle error and pitch angle error to complete the optimization of the posture graph, which can effectively reduce the positioning deviation caused by the map error, thereby achieving more accurate vehicle control and navigation. BRIEF DESCRIPTION OF THE DRAWINGS
[0040] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.
[0041] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.
[0042] Figure 1 A schematic diagram of a process flow provided for Embodiment 1 of the pose graph optimization method of the present application;
[0043] Figure 2 A constraint flow chart based on pose estimation data provided in Example 1 of the pose graph optimization method of the present application;
[0044] Figure 3 A flow chart of constructing a lateral error edge provided in Example 1 of the pose graph optimization method of this application;
[0045] Figure 4 A flowchart of constructing a longitudinal error edge provided in Example 1 of the pose graph optimization method of this application;
[0046] Figure 5 A schematic diagram of a flow chart provided for Embodiment 2 of the pose graph optimization method of the present application;
[0047] Figure 6 A constraint flow chart based on combined navigation data provided in Example 1 of the pose graph optimization method of the present application;
[0048] Figure 7 This is a schematic diagram of the module structure of the pose graph optimization device according to an embodiment of the present application;
[0049] Figure 8 Schematic diagram of the device structure of the hardware operating environment involved in the pose graph optimization method in the embodiment of the present application.
[0050] The purpose, features and advantages of this application will be further described in conjunction with the embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION
[0051] It should be understood that the specific embodiments described herein are only used to explain the technical solutions of the present application and are not used to limit the present application.
[0052] In order to better understand the technical solution of the present application, a detailed description will be given below in conjunction with the accompanying drawings and specific implementation methods.
[0053] The main solution of the embodiment of the present application is: obtaining posture calculation data, high-precision map data, front and rear perception data and combined navigation data; determining the lateral displacement error, heading angle error and longitudinal displacement error based on the posture calculation data, the high-precision map data and the front and rear perception data, and constructing a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error and the longitudinal displacement error; when the lateral error edge and the longitudinal error edge are detected, determining the vertical displacement error, the roll angle error and the pitch angle error based on the combined navigation data; constructing the constraint relationship of the posture graph based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error to complete the optimization of the posture graph.
[0054] In this embodiment, for ease of description, the following description is made using the identification posture graph optimization device as the execution subject.
[0055] The existing technology will introduce six dimensions of error in the edge setting, namely err_x (lateral displacement error), err_y (longitudinal displacement error), err_z (vertical displacement error), err_roll (roll angle error), err_pitch (pitch angle error), and err_yaw (heading angle error). That is, regardless of whether there is a map error, the vehicle's position will trust the position given by the combined navigation more, and the vehicle's position will be closer to the combined navigation posture. After the introduction of high-precision maps, since the high-precision maps themselves also have errors, the use of six dimensions of error will cause the vehicle's position to deviate from the map position. How to reduce the positioning deviation caused by map errors in the process of optimizing the posture graph has become an urgent problem to be solved.
[0056] This application provides a solution. After the introduction of high-precision maps, in order to achieve map positioning, the posture is decomposed into longitudinal error and lateral error. Therefore, the error amount in 6 dimensions is not used, but the error edge is decomposed, and finally the actual situation is consistent with the map mapping, so as to effectively reduce the positioning deviation caused by map error, thereby achieving more accurate vehicle control and navigation.
[0057] It should be noted that the execution subject of this embodiment can be a computing service device with data processing, network communication and program running functions, such as a tablet computer, a personal computer, a mobile phone, etc., or an electronic device capable of realizing the above functions, a posture graph optimization device, etc. The following takes the posture graph optimization device as an example to illustrate this embodiment and the following embodiments.
[0058] Based on this, the present application embodiment provides a posture graph optimization method, referring to Figure 1 , Figure 1 This is a flow chart of the first embodiment of the pose graph optimization method of the present application.
[0059] In this embodiment, the pose graph optimization method includes steps S10 to S40:
[0060] Step S10, acquiring posture estimation data, high-precision map data, front and rear perception data, and combined navigation data;
[0061] It should be noted that the map positioning module is an indispensable part of the high-level autonomous driving system. From the functional point of view, it can be roughly divided into two modules: front-end odometer and back-end optimization. Back-end optimization is based on the prediction and correction optimization of the odometer generated by the front-end. At present, there are two common solutions for back-end optimization, namely filter-based back-end optimization and graph-based back-end optimization. The filter-based back-end optimization predicts the state of the next frame based on the state of the previous frame, so it is impossible to achieve global optimization. The graph-based back-end optimization achieves overall optimization based on the constraint relationship of the entire graph. In the graph-based back-end optimization solution, a very important step is to construct the constraint relationship between the graph elements. The constraint relationship can be the error between the predicted value and the measured value. The error value is minimized through continuous iteration, thereby achieving graph optimization. Therefore, how to set the vertex elements and error edges is particularly important.
[0062] Specifically, the graph-based backend optimization constructs a pose graph, representing the poses of the vehicle as vertices in the graph, and the relationships between poses as edges. Pose graph optimization takes into account the entire driving path of the vehicle and the constraints in the environment. These constraints are based on the error between the predicted value and the measured value, such as the relative position of the vehicle to landmarks such as lane lines, road signs, and traffic signals. By minimizing the error in the graph, global optimization can be achieved, thereby improving the accuracy of vehicle positioning.
[0063] It should be understood that the pose estimation data refers to the pose (position and direction) calculated based on the vehicle's motion model, which can provide an initial estimate of the vehicle's motion for subsequent pose graph construction and optimization; high-precision map data contains data on high-precision geographic information such as lane lines, road signs, and traffic signals, which can be used as a reference for vehicle positioning and help determine the vehicle's precise position on the road; front and rear perception data includes forward perception data and rear perception data, which refers to the environmental information detected by the vehicle's forward and rearward sensors (such as cameras and lidars), which can provide real-time information about the vehicle's surroundings and help with vehicle positioning and decision-making; combined navigation data refers to navigation information that integrates multiple sensor data such as GPS (Global Positioning System), IMU (Inertial Measurement Unit), and wheel speed encoders, which can provide global positioning information for the vehicle and enhance the accuracy and robustness of positioning. In the construction of the pose graph, these types of data will be introduced for fusion positioning.
[0064] Step S20, determining a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture calculation data, the high-precision map data, and the front and rear perception data, and constructing a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error;
[0065] It should be noted that this embodiment is more versatile in the selection of backend optimization libraries and is applicable to a variety of different application scenarios and requirements. Specifically, general optimization libraries such as g2o, ceres, and gtsam can be used to process pose graph optimization problems. The pose is represented in the form of 6-dimensional parameters, including three position coordinates x, z, y and three rotation angles roll, pitch, and yaw.
[0066] In addition, it should be noted that the pose graph will introduce six dimensions of error in the edge setting, namely the lateral displacement error err_y, the longitudinal displacement error err_x, the vertical displacement error err_z, the roll angle error err_roll, the pitch angle error err_pitch, and the heading angle error err_yaw. Among them, err_x is used to represent the error in the x direction of the vehicle coordinate system; err_y is used to represent the error in the y direction of the vehicle coordinate system; err_z is used to represent the error in the z direction of the vehicle coordinate system; err_roll: the roll angle error around the x direction in the vehicle coordinate system; err_pitch is used to represent the pitch angle error around the y direction in the vehicle coordinate system; err_yaw is used to represent the yaw angle error around the z direction in the vehicle coordinate system.
[0067] It should be understood that the preliminary posture estimate of the vehicle can be obtained based on the vehicle's posture inference data. Combined with the actual information of the vehicle's surrounding environment provided by the high-precision map data and the front and rear perception data, the lateral displacement error err_y, the heading angle error err_yaw and the longitudinal displacement error err_x can be calculated. By adding the lateral displacement error err_y, the heading angle error err_yaw and the longitudinal displacement error err_x to the posture graph, the construction of the lateral error edge and the longitudinal error edge can be completed.
[0068] In addition, it should be understood that the lateral error describes the error of the vehicle in the lane width direction (i.e., the left and right direction of the vehicle), and mainly focuses on the accuracy of the left and right position of the vehicle in the lane. The longitudinal error describes the error of the vehicle in the lane length direction (i.e., the front and rear direction of the vehicle), and mainly focuses on the accuracy of the front and rear position of the vehicle in the lane. Decomposing the posture error into more specific longitudinal and lateral errors can more accurately describe the position of the vehicle in the lane, thereby improving the accuracy of positioning.
[0069] In a feasible implementation, step S20 may include steps S21 to S22:
[0070] Step S21, determining the current vehicle position based on the posture estimation data;
[0071] It should be noted that based on the pose estimation data, the error edge of the front and rear frames can be constructed. This edge is a binary edge. The six-dimensional constraints of lateral displacement error err_y, vertical displacement error err_z, longitudinal displacement error err_x, roll angle error err_roll, pitch angle error err_pitch and heading angle error err_yaw can be used to constrain the poses at both ends and then determine the current position of the vehicle.
[0072] In a feasible implementation, step S21 may include: determining the relative posture of the previous and next frames based on the posture estimation data; obtaining the historical ego vehicle position, and determining the current ego vehicle position based on the historical ego vehicle position and the relative posture of the previous and next frames.
[0073] It should be noted that the pose estimation data provides the vehicle's motion information, such as acceleration, angular velocity, wheel rotation, etc. The pose estimation result T of the previous frame is obtained based on the pose estimation data. k-1 And the pose estimation result of the current frame T k , according to the pose calculation result of the previous frame T k-1 And the pose estimation result of the current frame T k The relative pose T of the previous and next frames can be determined r , the calculation formula is as follows:
[0074]
[0075] Among them, T is a 4*4 matrix, representing the posture, is the pose matrix T k-1 The inverse matrix of .
[0076] After obtaining the relative pose T of the previous and next frames r After that, the historical vehicle position C can be obtained k-1 , and according to the relative posture T r Predict the current frame’s ego vehicle position, i.e., the current ego vehicle position C k , the calculation formula is as follows:
[0077] C k =C k-1 *T r
[0078] After that, the vehicle pose of the previous frame and the predicted pose of the current frame are added to the pose graph as vertices, and the relative pose is added to the pose graph as a binary edge for constraint. Figure 2 , Figure 2 The constraint flow chart based on the pose estimation data is provided for the first embodiment of the pose graph optimization method of the present application. When optimizing the pose graph, it is determined whether there is pose estimation data for the previous and next frames. When the pose estimation data for the previous and next frames is detected, the relative pose (i.e., the relative pose T of the previous and next frames) is calculated based on the pose estimation data for the previous and next frames. r ), the pose of the vehicle in the previous and next frames (the pose calculation result of the previous frame T k-1 And the pose estimation result of the previous frame T k ) as the vertex, and the relative pose (i.e. the relative pose of the previous and next frames T r ) as a constraint and added to the pose graph.
[0079] Step S22, determining the lateral displacement error, the heading angle error and the longitudinal displacement error based on the current vehicle position, the high-precision map data and the front and rear perception data, and constructing a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error and the longitudinal displacement error.
[0080] It should be understood that after determining the current position of the vehicle based on the posture estimation data, the environmental map information and environmental perception information can be obtained based on the high-precision map data and the front and rear perception data. According to the current position of the vehicle, the relative errors of the high-precision map data and the front and rear perception data in the vehicle coordinate system are determined in combination with the environmental map information and the environmental perception information, including the lateral displacement error err_y, the heading angle error err_yaw and the longitudinal displacement error err_x, which can more accurately describe the position of the vehicle in the lane, thereby improving the positioning accuracy.
[0081] In a feasible implementation manner, the high-precision map data includes map lane line data and map sign data, and the front and rear perception data includes perception lane line data and perception sign data; step S22 may include steps A11 to A12:
[0082] Step A11, determining a lateral displacement error and a heading angle error based on the current vehicle position, the map lane line data, and the sensed lane line data, and constructing a lateral error edge based on the lateral displacement error and the heading angle error;
[0083] It should be noted that map lane line data and map sign data can be separated from the high-precision map. Map lane line data is used for lateral positioning constraints, and map signs are used for longitudinal positioning constraints. Specifically, in the process of obtaining map lane line data and map sign data, the map data around the vehicle will be parsed from the map engine first, and then the 4 map lane line data around the vehicle including the left lane line, left-left lane line, right lane line, right-right lane line and the map sign data around the vehicle will be parsed, including the speed limit sign position, and the parsed results will be converted to the vehicle body coordinate system. Among them, the left lane line refers to the lane line on the left side of the lane where the vehicle is currently located; the left-left lane line refers to the second lane line on the left side of the lane where the vehicle is currently located; the right lane line refers to the lane line on the right side of the lane where the vehicle is currently located; the right-right lane line refers to the second lane line on the right side of the lane where the vehicle is currently located.
[0084] In addition, it should be noted that the forward perception and rearward perception will separate the perceived lane line data and the perceived sign data respectively. The perceived lane line data is used for lateral positioning constraints, and the perceived sign data is used for longitudinal positioning constraints. Specifically, in the process of obtaining map sign data and perceived sign data, the perception data around the vehicle will be obtained from the perception module first, and then the 4 perceived lane line data around the vehicle will be parsed, including the left lane line, the left-left lane line, the right lane line, the right-right lane line, and the perceived sign data around the vehicle including the speed limit sign position.
[0085] It should be understood that based on the current vehicle position, combined with the map lane line data and the perceived lane line data, the distance error between the vehicle and the lane line and the relative heading of the vehicle and the lane line can be determined, and then the lateral displacement error and heading angle error can be determined, and the lateral error edge can be constructed based on the lateral displacement error and heading angle error.
[0086] In a feasible implementation, step A11 may include steps B11 to B13:
[0087] Step B11, determining a first lateral distance and a first relative heading of a lane line based on the current vehicle position and the lane line data of the map;
[0088] Please refer to Figure 3 , Figure 3 A flowchart for constructing the lateral error edge provided in Example 1 of the pose graph optimization method of this application.
[0089] It should be noted that when constructing the lateral error edge, the corresponding map lane line data and perceived lane line data will be traversed (for example, the left lane line on the map corresponds to the perceived left lane line, and the right lane line on the map corresponds to the perceived right lane line). Based on the traversed map lane line data and perceived lane line data, the lateral displacement error and heading angle error are determined, and the lateral displacement error and heading angle error are added to the pose graph to construct the lateral error edge.
[0090] Specifically, according to the current vehicle position and the map lane line data, the lateral distance from the vehicle to the map lane line, i.e., the first lateral distance dist_line_m, and the relative heading between the vehicle and the map lane line, i.e., the first relative heading ang_yaw_m, can be calculated. Among them, according to the vehicle position, the two points closest to the map lane line are obtained, and combined with the vehicle heading, the first relative heading ang_yaw_m can be calculated.
[0091] Step B12, determining a second lateral distance and a second relative heading based on the sensed lane line data;
[0092] It should be noted that the lane line polynomial can be determined based on the lane line data, and then the constant term C0 and the coefficient of the first term C1 of the lane line polynomial can be obtained. C0 represents the lateral distance between the vehicle and the lane line, and C1 represents the orientation angle of the vehicle relative to the lane line. Record the value of C0 to get the second lateral distance dist_line_p; record the value of C1, and the second relative heading ang_yaw_p can be calculated by the following calculation formula:
[0093] ang_yaw_p=arctan(C1)
[0094] Step B13, determining a lateral displacement error and a heading angle error based on the first lateral distance, the first relative heading, the second lateral distance, and the second relative heading, and constructing a lateral error edge based on the lateral displacement error and the heading angle error.
[0095] It should be noted that the calculation formula of the lateral displacement error err_y is as follows:
[0096] err_y=dist_line_m-dist_line_p
[0097] The calculation formula of heading angle error err_yaw is as follows:
[0098] err_yaw=ang_yaw_m-ang_yaw_p
[0099] Step A12: determining a longitudinal displacement error based on the map sign data and the perception sign data, and constructing a longitudinal error edge based on the longitudinal displacement error.
[0100] It should be understood that the error of the sign position can be determined based on the map sign data and the perceived sign data, and then the longitudinal displacement error can be determined, and the longitudinal error edge can be constructed based on the longitudinal displacement error.
[0101] In a feasible implementation, step A12 may include: determining a relative posture difference between a map sign position and a perceived sign position based on the map sign data and the perceived sign data; determining a longitudinal displacement error based on the relative posture difference, and constructing a longitudinal error edge based on the longitudinal displacement error.
[0102] Please refer to Figure 4 , Figure 4 A flowchart for constructing the longitudinal error edge provided in Example 1 of the pose graph optimization method of the present application.
[0103] It should be noted that when constructing the longitudinal error edge, the corresponding map sign data and perception sign data will be traversed, and the longitudinal displacement error will be determined based on the traversed map sign data and perception sign data, and the longitudinal displacement error will be added to the pose graph to construct the longitudinal error edge.
[0104] In addition, it should be noted that the position of the map sign is recorded as dist_sign_m, and the position of the sensed sign is recorded as dist_sign_p. According to the map sign data and the sensed sign data, the map sign pose (global pose) Tdist_sign_m and the sensed sign pose (relative pose) Tdist_sign_p can be determined. Then, according to the pose Tpose of the vehicle and the following formula, the relative pose difference err_pose between the map sign position and the sensed sign position can be determined:
[0105]
[0106] in, is the inverse matrix of Tpose; err_pose is the difference between the global pose of the map sign converted into the relative pose by the pose of the vehicle and the perceived relative pose; T is a 4*4 matrix used to represent the pose, and the general form is as follows:
[0107]
[0108] Among them, R is a 3*3 matrix, indicating the posture, and t is a 3*1 matrix, indicating the position.
[0109] According to the relative pose difference err_pose, the displacement difference in the x direction in the relative pose in the vehicle coordinate system, that is, the longitudinal displacement error err_x, can be calculated. The formula is as follows:
[0110] err_x=err_pose(0,3)
[0111] Among them, err_pose(0,3) refers to the number in the first row and fourth column of the 4*4 matrix, which represents the displacement difference in the x direction, that is, the longitudinal displacement error.
[0112] Step S30, when the lateral error edge and the longitudinal error edge are detected, determining a vertical displacement error, a roll angle error and a pitch angle error based on the integrated navigation data;
[0113] It should be understood that when optimizing the pose graph, the pose is expressed in the form of 6-dimensional parameters, including three position coordinates x, z, y and three rotation angles roll, pitch, yaw, and the corresponding 6-dimensional error amounts are lateral displacement error err_y, vertical displacement error err_z, longitudinal displacement error err_x, roll angle error err_roll, pitch angle error err_pitch and heading angle error err_yaw. In general, regardless of whether there is a map error, the position of the vehicle will trust the position given by the combined navigation data more, and the position of the vehicle will be closer to the combined navigation pose. However, due to the errors in the high-precision map data itself, the use of 6-dimensional error amounts will cause the position of the vehicle to deviate from the map position.
[0114] Therefore, this solution first combines the pose estimation data, high-precision map data, and front and rear perception data to construct the lateral error edge and the longitudinal error edge, and realizes the error edge construction under different data conditions. If the lateral error edge and the longitudinal error edge are successfully constructed, that is, when the lateral error edge and the longitudinal error edge are detected, the combined navigation data only needs to constrain the remaining three dimensions of the vertical displacement error err_z, the roll angle error err_roll, and the pitch angle error err_pitch. Adding these errors to the pose graph can ensure that all dimensions of the pose graph are constrained for optimization.
[0115] Step S40, constructing a constraint relationship of a pose graph based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error, and the pitch angle error to optimize the pose graph.
[0116] It should be understood that all errors determined based on the pose estimation data, high-precision map data, front and rear perception data, and combined navigation data, including the lateral displacement error err_y, vertical displacement error err_z, longitudinal displacement error err_x, roll angle error err_roll, pitch angle error err_pitch, and heading angle error err_yaw, are integrated into the pose graph as constraints, and the error value is minimized through continuous iteration of the graph optimization algorithm to adjust the vehicle's pose estimation, thereby achieving the global optimal pose solution and completing the optimization of the pose graph.
[0117] The present embodiment provides a pose graph optimization method, which obtains pose estimation data, high-precision map data, front and rear perception data, and combined navigation data; determines a lateral displacement error, a heading error, and a longitudinal displacement error based on the pose estimation data, the high-precision map data, and the front and rear perception data, and constructs a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading error, and the longitudinal displacement error, decomposes the pose error into more specific longitudinal and lateral errors, and optimizes the longitudinal and lateral errors in a targeted manner so that the actual pose of the vehicle is consistent with the position on the high-precision map; when the lateral error edge and the longitudinal error edge are detected, determines a vertical displacement error, a roll angle error, and a pitch angle error based on the combined navigation data; constructs a constraint relationship of the pose graph based on the lateral displacement error, the heading error, the longitudinal displacement error, the vertical displacement error, the roll angle error, and the pitch angle error to complete the optimization of the pose graph, which can effectively reduce the positioning deviation caused by the map error, thereby achieving more accurate vehicle control and navigation.
[0118] Based on the first embodiment of the present application, in the second embodiment of the present application, the same or similar contents as those in the above-mentioned embodiment 1 can be referred to the above introduction, and will not be repeated in the following. Figure 5 Before step S30, the pose graph optimization method further includes step S201:
[0119] Step S201, when the lateral error edge and the longitudinal error edge are not detected, determining the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error based on the integrated navigation data.
[0120] Please refer to Figure 6 , Figure 6 A constraint flow chart based on combined navigation data is provided for Example 1 of the pose graph optimization method of the present application.
[0121] It should be noted that after calculating the relative position of the combined navigation based on the combined navigation data, it will be detected whether there are lateral error edges and longitudinal error edges constructed based on the high-precision map data and the front and rear perception data. The error determined based on the combined navigation data will be selected according to the construction of the lateral error edge and the longitudinal error edge.
[0122] It should be understood that in order to ensure that all dimensions of the pose graph are constrained for optimization, if the lateral error edge and the longitudinal error edge fail to be constructed, that is, the lateral error edge and the longitudinal error edge are not detected, then the combined navigation data needs to constrain the lateral displacement error err_y, vertical displacement error err_z, longitudinal displacement error err_x, roll angle error err_roll, pitch angle error err_pitch and heading angle error err_yaw. The error of the combined navigation constraints is the difference between each dimension of the combined navigation and each dimension of the vehicle.
[0123] Exemplarily, the error construction formula of 6 dimensions is as follows:
[0124]
[0125] err_x=err_pose(0,3)
[0126] err_y=err_pose(1,3)
[0127] err_z=err_pose(2,3)
[0128] err_roll = tan -1 (err_pose(2,1),err_pose(2,2))
[0129]
[0130] err_yaw=tan -1 (err_pose(1,0),err_pose(0,0))
[0131] Among them, T is a 4*4 matrix, representing the posture; T pose Used to represent the position of the vehicle; Used to represent T pose Inverse of the matrix; T 组合导航 Used to represent the combined navigation pose; err_pose is used to represent the relative pose of the combined navigation pose converted by the pose of the vehicle; err_pose(m,n) is used to represent the number in the (m+1)th row and (n+1)th column in the 4*4 matrix.
[0132] In this embodiment, when the lateral error edge and the longitudinal error edge are not detected, the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error are determined based on the combined navigation data to ensure that all dimensions of the pose graph are constrained for optimization.
[0133] It should be noted that the above examples are only used to understand the present application and do not constitute a limitation on the pose graph optimization method of the present application. More simple transformations based on this technical concept are all within the scope of protection of the present application.
[0134] This application also provides a posture graph optimization device, please refer to Figure 7 , the pose graph optimization device comprises:
[0135] Optimize the data acquisition module 10, which is used to obtain posture estimation data, high-precision map data, front and rear perception data, and integrated navigation data;
[0136] A first error construction module 20, configured to determine a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture estimation data, the high-precision map data, and the front and rear perception data, and to construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error;
[0137] A second error construction module 30, configured to determine a vertical displacement error, a roll angle error and a pitch angle error based on the integrated navigation data when the lateral error edge and the longitudinal error edge are detected;
[0138] The optimization execution module 40 is used to construct the constraint relationship of the pose graph based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error to complete the optimization of the pose graph.
[0139] The pose graph optimization device provided by the present application adopts the pose graph optimization method in the above embodiment, which can solve the technical problem of how to reduce the positioning deviation caused by map error in the process of optimizing the pose graph. Compared with the prior art, the beneficial effects of the pose graph optimization device provided by the present application are the same as the beneficial effects of the pose graph optimization method provided by the above embodiment, and the other technical features in the pose graph optimization device are the same as the features disclosed in the above embodiment method, which will not be repeated here.
[0140] In one embodiment, the first error construction module 20 is further used to determine the current vehicle position based on the posture estimation data; determine the lateral displacement error, the heading angle error and the longitudinal displacement error based on the current vehicle position, the high-precision map data and the front and rear perception data, and construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error and the longitudinal displacement error.
[0141] In one embodiment, the first error construction module 20 is further used to determine the relative posture of the previous and next frames based on the posture estimation data; obtain the historical ego vehicle position, and determine the current ego vehicle position based on the historical ego vehicle position and the relative posture of the previous and next frames.
[0142] In one embodiment, the first error construction module 20 is further used to determine a lateral displacement error and a heading angle error based on the current vehicle position, the map lane line data and the perceived lane line data, and to construct a lateral error edge based on the lateral displacement error and the heading angle error; determine a longitudinal displacement error based on the map sign data and the perceived sign data, and to construct a longitudinal error edge based on the longitudinal displacement error.
[0143] In one embodiment, the first error construction module 20 is also used to determine a first lateral distance and a first relative heading of the lane line based on the current vehicle position and the map lane line data; determine a second lateral distance and a second relative heading based on the perceived lane line data; determine a lateral displacement error and a heading angle error based on the first lateral distance, the first relative heading, the second lateral distance and the second relative heading, and construct a lateral error edge based on the lateral displacement error and the heading angle error.
[0144] In one embodiment, the first error construction module 20 is further used to determine the relative posture difference between the map sign position and the perceived sign position based on the map sign data and the perceived sign data; determine the longitudinal displacement error based on the relative posture, and construct the longitudinal error edge based on the longitudinal displacement error.
[0145] In one embodiment, the second error construction module 30 is further used to determine the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error based on the combined navigation data when the lateral error edge and the longitudinal error edge are not detected.
[0146] The present application provides a pose graph optimization device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the pose graph optimization method in the above-mentioned embodiment one.
[0147] Reference below Figure 8 , which shows a schematic diagram of the structure of a pose graph optimization device suitable for implementing the embodiment of the present application. The pose graph optimization device in the embodiment of the present application may include but is not limited to mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 8 The pose graph optimization device shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.
[0148] like Figure 8 As shown, the pose graph optimization device may include a processing device 1001 (e.g., a central processing unit, a graphics processor, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM: Read Only Memory) 1002 or a program loaded from a storage device 1003 to a random access memory (RAM: Random Access Memory) 1004. In RAM1004, various programs and data required for the operation of the pose graph optimization device are also stored. The processing device 1001, ROM1002, and RAM1004 are connected to each other via a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Generally, the following systems can be connected to the I / O interface 1006: an input device 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; an output device 1008 including, for example, a liquid crystal display (LCD: Liquid Crystal Display), a speaker, a vibrator, etc.; a storage device 1003 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 1009. The communication device 1009 can allow the pose graph optimization device to communicate wirelessly or wired with other devices to exchange data. Although the pose graph optimization device with various systems is shown in the figure, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems can be implemented or provided alternatively.
[0149] In particular, according to the embodiments disclosed in the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present application include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication device, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present application are executed.
[0150] The pose graph optimization device provided by the present application adopts the pose graph optimization method in the above embodiment, which can solve the technical problem of how to reduce the positioning deviation caused by map error in the process of optimizing the pose graph. Compared with the prior art, the beneficial effects of the pose graph optimization device provided by the present application are the same as the beneficial effects of the pose graph optimization method provided by the above embodiment, and the other technical features in the pose graph optimization device are the same as the features disclosed in the method of the previous embodiment, which will not be repeated here.
[0151] It should be understood that the various parts disclosed in this application can be implemented by hardware, software, firmware or a combination thereof. In the description of the above embodiments, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.
[0152] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.
[0153] The present application provides a computer-readable storage medium having computer-readable program instructions (i.e., computer programs) stored thereon, and the computer-readable program instructions are used to execute the pose graph optimization method in the above-mentioned embodiment.
[0154] The computer-readable storage medium provided in the present application may be, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, systems or devices, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, system or device. The program code contained on the computer-readable storage medium may be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination of the above.
[0155] The above-mentioned computer-readable storage medium may be included in the pose graph optimization device; or it may exist independently without being assembled into the pose graph optimization device.
[0156] The above-mentioned computer-readable storage medium carries one or more programs. When the above-mentioned one or more programs are executed by a pose graph optimization device, the pose graph optimization device: obtains pose inference data, high-precision map data, front and rear perception data and combined navigation data; determines the lateral displacement error, heading angle error and longitudinal displacement error based on the pose inference data, the high-precision map data and the front and rear perception data, and constructs a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error and the longitudinal displacement error; when the lateral error edge and the longitudinal error edge are detected, determines the vertical displacement error, the roll angle error and the pitch angle error based on the combined navigation data; constructs the constraint relationship of the pose graph based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error to complete the optimization of the pose graph.
[0157] Computer program code for performing the operations of the present application may be written in one or more programming languages or a combination thereof, including object-oriented programming languages such as Java, Smalltalk, C++, and conventional procedural programming languages such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider).
[0158] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present application. In this regard, each square box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two square boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.
[0159] The modules involved in the embodiments described in this application may be implemented by software or hardware, wherein the name of the module does not constitute a limitation on the unit itself in some cases.
[0160] The readable storage medium provided in the present application is a computer-readable storage medium, which stores computer-readable program instructions (i.e., computer programs) for executing the above-mentioned pose graph optimization method, and can solve the technical problem of how to reduce the positioning deviation caused by map errors in the process of optimizing the pose graph. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in the present application are the same as the beneficial effects of the pose graph optimization method provided in the above-mentioned embodiment, and will not be repeated here.
[0161] The present application also provides a computer program product, including a computer program, which implements the steps of the pose graph optimization method as described above when executed by a processor.
[0162] The computer program product provided by the present application can solve the technical problem of how to reduce the positioning deviation caused by map error in the process of optimizing the pose graph. Compared with the prior art, the beneficial effects of the computer program product provided by the present application are the same as the beneficial effects of the pose graph optimization method provided by the above embodiment, which will not be repeated here.
[0163] The above descriptions are only some embodiments of the present application, and are not intended to limit the patent scope of the present application. All equivalent structural changes made using the contents of the present application specification and drawings under the technical concept of the present application, or direct / indirect applications in other related technical fields are included in the patent protection scope of the present application.
Claims
1. A pose graph optimization method, characterized in that: The pose graph optimization method comprises: Obtain posture estimation data, high-precision map data, front and rear perception data, and combined navigation data; Determine a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture calculation data, the high-precision map data, and the front and rear perception data, and construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error; When the lateral error edge and the longitudinal error edge are detected, determining a vertical displacement error, a roll angle error, and a pitch angle error based on the integrated navigation data; A constraint relationship of a pose graph is constructed based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error, and the pitch angle error to optimize the pose graph.
2. The method according to claim 1, characterized in that The step of determining a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture calculation data, the high-precision map data, and the front and rear perception data, and constructing a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error includes: Determine the current position of the vehicle based on the posture estimation data; Based on the current vehicle position, the high-precision map data, and the front and rear perception data, a lateral displacement error, a heading angle error, and a longitudinal displacement error are determined, and a lateral error edge and a longitudinal error edge are constructed based on the lateral displacement error, the heading angle error, and the longitudinal displacement error.
3. The method according to claim 2, characterized in that The step of determining the current vehicle position based on the posture estimation data comprises: Determine the relative poses of the previous and next frames based on the pose estimation data; The historical ego vehicle position is obtained, and the current ego vehicle position is determined based on the historical ego vehicle position and the relative posture of the previous and next frames.
4. The method according to claim 2, characterized in that The high-precision map data includes map lane line data and map sign data, and the front and rear perception data includes perception lane line data and perception sign data; The step of determining the lateral displacement error, the heading angle error and the longitudinal displacement error based on the current vehicle position, the high-precision map data and the front and rear perception data, and constructing the lateral error edge and the longitudinal error edge based on the lateral displacement error, the heading angle error and the longitudinal displacement error includes: Determine a lateral displacement error and a heading angle error based on the current vehicle position, the map lane line data, and the sensed lane line data, and construct a lateral error edge based on the lateral displacement error and the heading angle error; A longitudinal displacement error is determined based on the map sign data and the sensory sign data, and a longitudinal error edge is constructed based on the longitudinal displacement error.
5. The method according to claim 4, characterized in that The step of determining a lateral displacement error and a heading angle error based on the current vehicle position, the map lane line data, and the sensed lane line data, and constructing a lateral error edge based on the lateral displacement error and the heading angle error includes: Determine a first lateral distance and a first relative heading of a lane line based on the current vehicle position and the map lane line data; Determining a second lateral distance and a second relative heading based on the sensed lane line data; A lateral displacement error and a heading angle error are determined based on the first lateral distance, the first relative heading, the second lateral distance, and the second relative heading, and a lateral error edge is constructed based on the lateral displacement error and the heading angle error.
6. The method according to claim 4, characterized in that The step of determining the longitudinal displacement error based on the map sign data and the perception sign data, and constructing the longitudinal error edge based on the longitudinal displacement error comprises: Determining a relative position difference between a position of a map sign and a position of a perceived sign based on the map sign data and the perceived sign data; A longitudinal displacement error is determined based on the relative position and posture, and a longitudinal error edge is constructed based on the longitudinal displacement error.
7. The method according to any one of claims 1 to 6, characterized in that Before the step of determining the vertical displacement error, the rolling angle error and the pitch angle error based on the integrated navigation data when the lateral error edge and the longitudinal error edge are detected, the method further comprises: When the lateral error edge and the longitudinal error edge are not detected, the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error, and the pitch angle error are determined based on the integrated navigation data.
8. A pose graph optimization device, characterized in that: The device comprises: Optimize the data acquisition module to obtain posture estimation data, high-precision map data, front and rear perception data, and integrated navigation data; A first error construction module, configured to determine a lateral displacement error, a heading angle error, and a longitudinal displacement error based on the posture estimation data, the high-precision map data, and the front and rear perception data, and to construct a lateral error edge and a longitudinal error edge based on the lateral displacement error, the heading angle error, and the longitudinal displacement error; a second error construction module, configured to determine a vertical displacement error, a roll angle error, and a pitch angle error based on the integrated navigation data when the lateral error edge and the longitudinal error edge are detected; The optimization execution module is used to construct a constraint relationship of a pose graph based on the lateral displacement error, the heading angle error, the longitudinal displacement error, the vertical displacement error, the roll angle error and the pitch angle error to complete the optimization of the pose graph.
9. A pose graph optimization device, characterized in that: The device comprises: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program is configured to implement the steps of the pose graph optimization method according to any one of claims 1 to 7.
10. A storage medium, characterized in that: The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, the steps of the pose graph optimization method according to any one of claims 1 to 7 are implemented.