Trajectory optimization method and electronic device

Abstract

The application provides a trajectory optimization method and an electronic device, and relates to the technical field of high-precision maps. The trajectory optimization method corrects the pose through multi-scale laser point cloud frame matching, corrects the pose using adjacent point cloud frame matching for abnormal trajectory points, corrects the pose using interval point cloud frame matching for abnormal sections with weak positioning signals, and optimizes the vehicle motion trajectory through pose graph optimization, so that the trajectory of local jumping and slow drift can be effectively recovered, the relative accuracy of the vehicle motion trajectory is improved, and the production quality of the high-precision map is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of high-definition map, and particularly relates to a trajectory optimization method and an electronic device. BACKGROUND

[0002] High-definition map (HD Map) is crucial for autonomous driving. The production of high-definition map relies on accurate vehicle motion trajectory. Generally, a high-precision integrated navigation system can stably output a vehicle pose meeting accuracy requirements in an open scene with good GNSS (Global Navigation Satellite System) signals, but in a city canyon with high-rise buildings or under an elevated road, due to problems such as building obstruction and multipath effect, the GNSS signal is lost or even lost, and the vehicle pose calculated by the high-precision integrated navigation system relying on the absolute pose correction provided by the GNSS system is prone to some abnormal jumps and slow drifts, and the relative accuracy and absolute accuracy of the trajectory decrease. SUMMARY

[0003] In view of the above problems, a trajectory optimization method and an electronic device are provided to overcome the above problems or at least partially solve the above problems.

[0004] An object of the present application is to provide a trajectory optimization method capable of improving the relative accuracy of a vehicle motion trajectory.

[0005] A further object of the present application is to effectively repair local jumps and slow drifts of a vehicle motion trajectory.

[0006] Another object of the present application is to provide an electronic device for executing the trajectory optimization method.

[0007] In particular, according to an aspect of an embodiment of the present application, a trajectory optimization method is provided, comprising:

[0008] acquiring trajectory data of a vehicle and laser point cloud data corresponding to the trajectory, wherein the trajectory data contains time stamps and pose information of each trajectory point in the trajectory, and the laser point cloud data is data collected by a vehicle-mounted laser radar at different times in the trajectory;

[0009] selecting multiple key frame laser point clouds from the laser point cloud data, and taking the pose information of a trajectory point with the same time stamp as the key frame laser point cloud as a key frame pose;

[0010] iteratively checking all trajectory points in the trajectory to find abnormal trajectory points and determine abnormal key frame poses corresponding to the abnormal trajectory points;

[0011] performing point cloud matching on the key frame laser point cloud corresponding to each abnormal key frame pose and the next frame key frame laser point cloud adjacent to the abnormal key frame pose, to obtain adjacent frame matching results;

[0012] performing first pose graph optimization on all the key frame poses according to the adjacent frame matching results, to obtain first optimized key frame poses, and obtaining a first optimized trajectory according to the first optimized key frame poses and the trajectory data;

[0013] detecting a section with weak positioning signals in the first optimized trajectory, and determining key frame poses falling into the section as weak signal key frame poses;

[0014] determining two key frame poses with a second specified distance as a pair of interval frames, to obtain multiple pairs of interval frames;

[0015] for each pair of interval frames, if the weak signal key frame poses exist in an interval defined by the two key frame poses of the pair of interval frames as end points, performing point cloud matching on the key frame laser point clouds corresponding to the two key frame poses of the pair of interval frames, to obtain interval frame matching results;

[0016] performing second pose graph optimization on the first optimized key frame poses according to the adjacent frame matching results and the interval frame matching results, to obtain second optimized key frame poses, and obtaining a final optimized trajectory according to the second optimized key frame poses and the first optimized trajectory.

[0017] Optionally, the step of traversing all trajectory points in the trajectory to find abnormal trajectory points comprises:

[0018] traversing each trajectory point in the trajectory, and establishing a set of the center point and a specified number of trajectory points adjacent to the center point on both sides of the center point;

[0019] detecting trajectory smoothness of the set;

[0020] determining whether the center point is abnormal according to the detection result of the trajectory smoothness, wherein the abnormality includes jump and / or discontinuity;

[0021] if yes, determining that the center point is an abnormal trajectory point.

[0022] Optionally, the step of determining abnormal key frame poses corresponding to the abnormal trajectory points comprises:

[0023] aggregating abnormal trajectory points with a distance between every two adjacent abnormal trajectory points less than a first specified threshold into an abnormal section to obtain at least one abnormal section, and determining key frame poses falling into the at least one abnormal section as abnormal key frame poses.

[0024] Optionally, the step of performing point cloud matching between the key frame laser point cloud corresponding to each abnormal key frame pose and the next frame key frame laser point cloud adjacent thereto respectively to obtain an adjacent frame matching result comprises:

[0025] performing point cloud matching between the key frame laser point cloud corresponding to each abnormal key frame pose and the next frame key frame laser point cloud adjacent thereto respectively;

[0026] if the matching is successful, a matching post-relative pose between the abnormal key frame pose and the next frame key frame pose adjacent thereto is obtained.

[0027] Optionally, the key frame pose is a key frame pose matrix, and the step of performing first pose graph optimization on all the key frame poses according to the adjacent frame matching result to obtain a first optimized key frame pose and obtaining a first optimized trajectory according to the first optimized key frame pose and the trajectory data comprises:

[0028] taking all the key frame pose matrices as vertices of a pose graph to be optimized;

[0029] constructing a single-sided constraint condition with all the key frame pose matrices and their inverse matrices;

[0030] constructing a double-sided constraint condition with the matching post-relative poses of the abnormal key frame poses whose matching is successful and the initial relative poses of the abnormal key frame poses and the non-abnormal key frame poses whose matching is unsuccessful;

[0031] optimizing and solving the pose graph by a nonlinear optimization algorithm to obtain a first optimized key frame pose matrix, and replacing trajectory points in the trajectory corresponding to the trajectory points determined by the first optimized key frame pose matrix to generate a first optimized trajectory.

[0032] Optionally, when constructing the single-sided constraint condition, the weight of the single-sided constraint condition composed of the abnormal key frame pose matrix and its inverse matrix is reduced.

[0033] Optionally, the step of detecting a section with weak positioning signals in the first optimized trajectory comprises:

[0034] obtaining a signal parameter of a combined navigation system, the signal parameter comprising at least one of a current received satellite number, a position precision factor of a satellite system and a standard deviation of a solved position and attitude;

[0035] determining a section with weak positioning signals in the first optimized trajectory according to the signal parameter.

[0036] Optionally, the step of performing point cloud matching on the keyframe laser point clouds corresponding to the poses of the two keyframes in the interval frame to obtain the interval frame matching result includes:

[0037] Point cloud matching is performed on the key frame laser point clouds corresponding to the poses of the two key frames in the interval frame.

[0038] If the match is successful, the relative pose after matching between the two keyframe poses is obtained.

[0039] Optionally, the step of performing a second pose graph optimization on the first-optimized keyframe pose based on the adjacent frame matching result and the interval frame matching result to obtain the second-optimized keyframe pose, and obtaining the final optimized trajectory based on the second-optimized keyframe pose and the first-optimized trajectory, includes:

[0040] The vertices of the pose graph are constructed using all the keyframe pose matrices of the first optimization as variables to be optimized.

[0041] Construct one-sided constraints using all the keyframe pose matrices and their inverses of the first-order optimization;

[0042] The first bilateral constraint condition is constructed by the matched relative pose of the successfully matched abnormal keyframe pose and the initial relative pose of the unmatched abnormal keyframe pose and the non-abnormal keyframe pose.

[0043] A second bilateral constraint condition is constructed by matching the relative poses of the two keyframes of each pair of interval frames that have been successfully matched.

[0044] The pose graph is optimized using a nonlinear optimization algorithm to obtain a secondary optimized keyframe pose matrix. The trajectory points determined by the secondary optimized keyframe pose matrix are then used to replace the corresponding trajectory points in the primary optimized trajectory to generate the final optimized trajectory.

[0045] According to another aspect of the present invention, an electronic device is also provided, including a memory, a processor, and a machine-executable program stored in the memory and running on the processor, wherein the processor executes the machine-executable program to implement any of the aforementioned trajectory optimization methods.

[0046] The trajectory optimization method of the present invention performs pose correction by multi-scale laser point cloud frame matching and optimizes the vehicle motion trajectory by pose graph optimization, thereby improving the relative accuracy of the vehicle motion trajectory and thus improving the production quality of high-precision maps.

[0047] Furthermore, the trajectory optimization method of the present invention uses adjacent point cloud frame matching correction for abnormal trajectory points and interval point cloud frame matching correction for abnormal sections with weak positioning signals, which can effectively recover trajectories with local jumps and slow drifts, and improve the relative accuracy of the output trajectory.

[0048] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, and in order to make the above and other objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention are described below.

[0049] The above and other objects, advantages and features of the present invention will become more apparent to those skilled in the art from the following detailed description of specific embodiments of the invention in conjunction with the accompanying drawings. Attached Figure Description

[0050] Various other advantages and benefits will become apparent to those skilled in the art upon reading the following detailed description of preferred embodiments. The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Furthermore, the same reference numerals denote the same parts throughout the drawings. In the drawings:

[0051] Figure 1 A schematic flowchart of a trajectory optimization method according to an embodiment of the present invention is shown;

[0052] Figure 2 This diagram illustrates the effect of optimizing an abnormal trajectory segment according to an embodiment of the present invention.

[0053] Figure 3 A schematic diagram illustrating the effect of secondary optimization on a segment with weak positioning signal according to an embodiment of the present invention is shown.

[0054] Figure 4 A schematic diagram of the structure of an electronic device according to an embodiment of the present invention is shown. Detailed Implementation

[0055] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.

[0056] To solve, or at least partially solve, the above-mentioned technical problems, this invention proposes a trajectory optimization method. Figure 1 A schematic flowchart of a trajectory optimization method according to an embodiment of the present invention is shown. See alsoFigure 1 The method may include at least the following steps S102 to S118.

[0057] Step S102: Obtain vehicle trajectory data and corresponding laser point cloud data. The trajectory data includes the timestamp and pose information of each trajectory point in the trajectory, and the laser point cloud data is the data collected by the vehicle-mounted LiDAR at different times in the trajectory.

[0058] Step S104: Select multiple key frame laser point clouds from the laser point cloud data, and use the pose information of trajectory points with the same timestamp as the key frame laser point cloud as the key frame pose.

[0059] Step S106: Traverse all trajectory points in the inspection trajectory, find abnormal trajectory points, and determine the abnormal keyframe poses corresponding to the abnormal trajectory points.

[0060] Step S108: Perform point cloud matching between the laser point cloud of each abnormal key frame pose and the laser point cloud of the next adjacent key frame to obtain the adjacent frame matching result.

[0061] Step S110: Perform the first pose graph optimization on all keyframe poses based on the adjacent frame matching results to obtain the first optimized keyframe poses, and obtain the first optimized trajectory based on the first optimized keyframe poses and trajectory data.

[0062] Step S112: Detect the weak signal segment in the optimized trajectory and take the key frame pose that falls into the segment as the weak signal key frame pose.

[0063] Step S114: Take two keyframe poses that are spaced apart by a second specified distance in the keyframe pose as a pair of interval frames to obtain multiple pairs of interval frames.

[0064] Step S116: For each pair of interval frames, if there is a weak signal key frame pose in the interval defined by the two key frame poses of the pair of interval frames as endpoints, then perform point cloud matching on the key frame laser point cloud corresponding to the two key frame poses of the pair of interval frames to obtain the interval frame matching result.

[0065] Step S118: Perform a second pose graph optimization on the keyframe pose of the first optimization based on the matching results of adjacent frames and the matching results of interval frames to obtain the second optimized keyframe pose, and obtain the final optimized trajectory based on the second optimized keyframe pose and the first optimized trajectory.

[0066] The trajectory optimization method provided in this invention corrects the pose by matching multi-scale laser point cloud frames and optimizes the vehicle's motion trajectory by optimizing the pose graph, thereby improving the relative accuracy of the vehicle's motion trajectory and thus improving the production quality of high-precision maps.

[0067] Furthermore, the trajectory optimization method of the present invention uses adjacent point cloud frame matching correction for abnormal trajectory points and interval point cloud frame matching correction for abnormal sections with weak positioning signals, which can effectively recover trajectories with local jumps and slow drifts, and improve the relative accuracy of the output trajectory.

[0068] The trajectory data obtained in step S102 above can be the trajectory data of vehicle motion calculated by a high-precision integrated navigation system. Each line of the trajectory data (provided in the form of a trajectory file) contains the following information: timestamp and position / attitude in the WGS84 coordinate system. Specifically, the trajectory data can be represented as T = {(s i γ i θ i , φ i lon i lat i h i The position and pose information (referred to as pose information) in the trajectory is stored sequentially according to the timestamp order (i = 1, 2, ..., n). i γ i θ i , φ i lon i lat i h i These represent the timestamp, roll angle, pitch angle, heading angle, longitude, latitude, and altitude corresponding to the i-th point, respectively. The laser point cloud data is data collected by the vehicle-mounted LiDAR at certain moments along the trajectory; distortion correction processing using trajectory information is required when using it.

[0069] Since the data volume of each frame of laser point cloud is generally large, key frame laser point clouds are selected for subsequent processing in step S104, thereby reducing the workload of data processing and improving processing efficiency. The selection method of key frame laser point clouds can be determined according to the actual application needs. In a specific implementation, corresponding laser point clouds can be selected from the laser point cloud data at fixed intervals as key frame laser point clouds. The pose information of trajectory points with the same timestamp as each key frame laser point cloud is used as the key frame pose. Since the original acquired trajectory data contains the pose information of each trajectory point in the WGS84 coordinate system, the pose information of each trajectory point in the WGS84 coordinate system can be converted into pose information in the local coordinate system (represented by a pose matrix) before use. At this time, the key frame pose refers to the key frame pose matrix.

[0070] In step S106 above, all trajectory points are traversed to check for anomalies.

[0071] In some specific embodiments, the step of traversing and examining all trajectory points in the trajectory to identify abnormal trajectory points may include:

[0072] Iterate through each trajectory point in the trajectory, and with each trajectory point as the center point, establish a set of the center point and a specified number of trajectory points on both sides of it.

[0073] Detect the smoothness of the trajectory of the set;

[0074] Determine whether there is an anomaly at the center point based on the results of trajectory smoothness detection;

[0075] If so, then the center point is determined to be an abnormal trajectory point.

[0076] Of course, if it is determined that the center point is not abnormal, then the center point is determined to be a non-abnormal trajectory point.

[0077] Specifically, anomalies can include jumps, disconnections, etc. Existing methods can be used to detect trajectory smoothness; for example, the smoothness can be detected by measuring the angles formed between the lines connecting the center point and its two nearest neighbors.

[0078] In some specific embodiments, after finding the abnormal trajectory points, the step of determining the abnormal keyframe pose corresponding to the abnormal trajectory points in step S106 may include: aggregating the abnormal trajectory points whose distance between any two adjacent abnormal trajectory points is less than a first specified threshold into an abnormal segment to obtain at least one abnormal segment, and determining the keyframe pose falling into at least one abnormal segment as the abnormal keyframe pose.

[0079] In practical applications, all trajectory points can be traversed and checked in a predetermined order. Taking the i-th trajectory point as the center point, a set is created of the center point and its two adjacent points (e.g., left and right). A trajectory smoothness detection method is used to determine whether the center point is an abnormal trajectory point. The same abnormal point detection is then performed on the next trajectory point. After detecting abnormal trajectory points, those with an adjacent distance less than a first specified threshold are aggregated into an abnormal segment s, resulting in an abnormal segment set S = {s...} i |i=1,2,...,m}, where m is the number of anomalous segments. The set of anomalous segments is associated with keyframe poses, and keyframe poses belonging to anomalous segments (i.e., falling into anomalous segments) can be marked as anomalous keyframe poses.

[0080] In step S108, adjacent frame laser point cloud matching is performed on the pose of the abnormal keyframe. Specifically, step S108 may include:

[0081] The laser point cloud corresponding to each abnormal keyframe pose is matched with the laser point cloud of the next adjacent keyframe.

[0082] If the match is successful, the relative pose after matching between the abnormal keyframe pose and the pose of the next adjacent keyframe is obtained.

[0083] That is, assuming the initial relative pose between the pose of the i-th abnormal keyframe and the pose of the (i+1)-th keyframe is: If the laser point cloud of the i-th keyframe corresponding to the abnormal keyframe pose in frame i is successfully matched with the laser point cloud of the (i+1)-th keyframe, then the relative pose after matching between the abnormal keyframe pose in frame i and the (i+1)-th keyframe pose is obtained. Therefore, the relative pose between the pose of the abnormal keyframe in frame i and the pose of the keyframe in frame (i+1) can be updated as follows: Of course, if the match fails, the relative pose between the abnormal keyframe pose of frame i and the keyframe pose of frame i+1 remains at the initial value. It should be noted that the keyframe pose of frame i+1 may or may not be abnormal.

[0084] After matching and updating the poses of all abnormal keyframes, step S110 constructs the first pose graph optimization problem based on the aforementioned adjacent frame matching results. By solving the pose graph optimization problem, the trajectory is optimized in one step. Graph optimization uses a graph model to express a nonlinear least squares optimization problem, where vertices represent variables to be optimized and edges represent error terms.

[0085] In some specific embodiments, step S110 may specifically include the following steps (1)-(4).

[0086] (1) The pose matrices of all keyframes are used as variables to be optimized to form the vertices of the pose graph. That is, each keyframe pose matrix constitutes a vertex of the pose graph, and the set of vertices is denoted as . in, This represents the pose matrix of the i-th keyframe to be optimized.

[0087] (2) Construct unilateral constraints using all keyframe pose matrices and their inverse matrices.

[0088] Due to the lack of means to measure absolute pose information, unilateral constraints are still given by the trajectory data before matching and updating. Let the set of unilateral constraints be denoted as . in It represents the inverse matrix of the pose matrix of the i-th keyframe in the trajectory.

[0089] Furthermore, since the pose matrix corresponding to the abnormal segment (i.e., the abnormal keyframe pose matrix) may be inaccurate, constructing unilateral constraints with it inevitably introduces errors to some extent. Therefore, in some embodiments, when constructing unilateral constraints, the weight of the unilateral constraints composed of the abnormal keyframe pose matrix and its inverse matrix can be reduced. For example, assuming the weight of the unilateral constraint composed of the non-abnormal keyframe pose matrix and its inverse matrix is ​​'a', the weight of the unilateral constraint composed of the abnormal keyframe pose matrix and its inverse matrix can be reduced to less than 'a', for example, to 10-50% of 'a'. In this way, the error introduced by the inaccurate abnormal keyframe pose matrix can be minimized, thereby further improving the relative accuracy of the optimized trajectory.

[0090] (3) The bilateral constraint conditions are constructed by using the matched relative poses of the successfully matched abnormal keyframe poses and the initial relative poses of the unmatched abnormal keyframe poses and the non-abnormal keyframe poses.

[0091] Bilateral constraints consist of two parts. For anomalous keyframe poses that have been successfully matched and updated in relative pose within anomaly segments, these keyframes are denoted as set A. Bilateral constraints are constructed using the updated relative poses, and the set of bilateral constraints corresponding to A is... For the keyframe poses in the normal segment (i.e., non-abnormal) and the anomalous keyframe poses in the abnormal segment where matching fails, relative frames are constructed using their initial relative poses. These keyframes are denoted as set B, and the bilateral constraint set corresponding to B is...

[0092] After adding vertices (1), adding unilateral constraints (2), and adding bilateral constraints (3), the pose graph is constructed.

[0093] (4) The pose graph is optimized by a nonlinear optimization algorithm to obtain the first-optimized keyframe pose matrix. The trajectory points determined by the first-optimized keyframe pose matrix are used to replace the corresponding trajectory points in the trajectory to generate the first-optimized trajectory.

[0094] Specifically, nonlinear optimization algorithms can include Gauss-Newton algorithm, Levenberg-Marquardt algorithm, DogLeg algorithm, etc.

[0095] After optimization, abnormal jumps and disconnections in the trajectory can be repaired. Figure 2 This diagram illustrates the effect of optimizing an abnormal trajectory segment according to an embodiment of the present invention. Figure 2 The enlarged view within the dashed box shows two trajectories with abnormal jumps, while the repaired trajectory (represented by a solid line) has regained its smoothness.

[0096] After step S110, a relatively smooth trajectory is obtained, correcting anomalies such as jumps. However, some slow-drift segments of the trajectory cannot be detected by trajectory smoothness testing. Therefore, in step S112, segments with weak positioning signals in the optimized trajectory can be detected, thereby narrowing down the possible range of slow-drift problem trajectory segments for subsequent repair. Specifically, the positioning signal can be a GNSS signal.

[0097] In some specific embodiments, weak positioning signal segments in an optimized trajectory can be detected by: acquiring signal parameters of an integrated navigation system (such as a high-precision integrated navigation system) and determining the weak positioning signal segments in the optimized trajectory based on these signal parameters.

[0098] Signal parameters can include the number of currently received satellites, the Position Dilution of Precision (PDOP) of the satellite system, and the standard deviation of the calculated position and attitude. Specifically, it can be determined whether the signal parameters acquired in a certain segment meet preset judgment conditions. If at least one signal parameter meets the preset judgment condition, the positioning signal in that segment is determined to be weak. For example, if the number of currently received satellites in a certain segment meets the preset judgment condition of less than 4, the positioning signal in that segment is determined to be weak.

[0099] After identifying the weak signal segments (hereinafter referred to as weak signal segments), the set of weak signal segments is associated with the key frame poses, and the key frame poses falling into the weak signal segments are determined as weak signal key frame poses.

[0100] Since the goal is to address the slow drift problem, step S114 first uses the poses of two keyframes spaced two times apart as a pair of interval frames to obtain multiple pairs of interval frames. The second specified distance can be set according to actual needs and data quality, such as 5m, 7m, 10m, etc.

[0101] In some embodiments, different pairs of interval frames do not overlap at all or have no overlapping portions except at the endpoints. For example, for keyframe poses A, B, C, D, E, F, A and C are considered as one pair of interval frames, and D and F as another pair of interval frames; or, A and C are considered as one pair of interval frames, and C and E as another pair of interval frames.

[0102] In other embodiments, there may be partial overlap between different pairs of interval frames. For example, for keyframe poses A, B, C, D, E, and F, A and C, B and D, C and E, and D and F are respectively regarded as four pairs of interval frames.

[0103] In step S116, for a pair of interval frames with weak signal keyframe poses between them, point cloud matching (referred to as interval frame matching) is performed on the keyframe laser point clouds corresponding to the two keyframe poses of the pair of interval frames to obtain the interval frame matching result.

[0104] Specifically, the step of performing point cloud matching on the keyframe laser point clouds corresponding to the poses of the two keyframes in the pair of interval frames to obtain the interval frame matching result may include:

[0105] Perform point cloud matching on the keyframe laser point clouds corresponding to the poses of the two keyframes in the pair of interval frames.

[0106] If the match is successful, the relative pose after matching between the two keyframe poses is obtained.

[0107] Specifically, a pair of interval frames is denoted as T. p and T q Then, the interval defined by the poses of the two keyframes of the pair of interval frames as endpoints is represented as [T]. p T q If [T] p T q If there is a weak signal keyframe pose, then the laser point cloud of the p-th keyframe and the laser point cloud of the q-th keyframe are matched. If the match is successful, the relative pose relationship is updated. Assume the keyframe pose T p With T q The initial values ​​of the relative poses between them are If the laser point cloud of the p-th keyframe matches the laser point cloud of the q-th keyframe, then the keyframe pose T is obtained. p With T q Relative poses after matching Therefore, the keyframe pose T can be determined. p With T q The relative pose between them is updated to Of course, if the match fails, the keyframe pose T p With T q The relative poses between them are maintained at their initial values.

[0108] After matching and updating the interval frames of all weak signal segments, in step S118, a second pose graph optimization problem is constructed based on the aforementioned adjacent frame matching results and interval frame matching results. By performing pose graph optimization, the trajectory is optimized in two stages.

[0109] In some specific embodiments, step S118 may specifically include the following steps (1)-(5).

[0110] (1) Use all the keyframe pose matrices of the first optimization as variables to be optimized to form the vertices of the pose graph.

[0111] (2) Construct unilateral constraints using the pose matrices of all first-order keyframes and their inverse matrices.

[0112] (3) The first bilateral constraint condition is constructed by using the matched relative pose of the successfully matched abnormal keyframe pose and the initial relative pose of the unmatched abnormal keyframe pose and the non-abnormal keyframe pose.

[0113] In the construction of the second pose graph, steps (1)-(3) are similar to steps (1)-(3) in the construction of the first pose graph, and the representation of the constraint set is also similar, so they will not be described again here.

[0114] (4) Construct a second bilateral constraint condition based on the relative poses of the two keyframes of each pair of successfully matched interval frames after matching.

[0115] In step (4), new bilateral constraints are constructed for the interval frames. Specifically, for keyframes in the weak signal segment that have successfully matched and updated the relative pose between two keyframes of the interval frame, the set of these keyframes is denoted as C. The updated relative poses of these keyframes are used to construct the second bilateral constraint, and the set of bilateral constraints corresponding to C is denoted as...

[0116] After adding vertices (1), adding unilateral constraints (2), adding the first bilateral constraint (3), and adding the second bilateral constraint (4), the pose graph is constructed.

[0117] (5) The pose graph is optimized using a nonlinear optimization algorithm to obtain the keyframe pose matrix of the secondary optimization. The trajectory points determined by the keyframe pose matrix of the secondary optimization are used to replace the corresponding trajectory points in the trajectory of the primary optimization to generate the final optimized trajectory. The final optimized trajectory is output as a trajectory file.

[0118] Specifically, nonlinear optimization algorithms can include Gauss-Newton algorithm, Levenberg-Marquardt algorithm, DogLeg algorithm, etc.

[0119] After secondary optimization, slow drift in the trajectory can be repaired. Figure 3 This diagram illustrates the effect of secondary optimization on a segment with weak positioning signals according to an embodiment of the present invention. Figure 3 A comparison of the effects before and after optimization shows that, due to the slow drift of the original trajectory, obvious ghosting and blurring occurred after the point cloud was stitched together. After the second optimization of the trajectory, the ghosting disappeared, and the consistency of the stitched point cloud was significantly improved.

[0120] The embodiments of the present invention provide a method for optimizing trajectories based on graph optimization and multi-scale laser point cloud frame matching. The method uses adjacent point cloud frame matching to correct trajectory jumps and interval point cloud frame matching to correct weak GNSS signal segments. Finally, a graph optimization problem is constructed to optimize the trajectory as a whole. This method can effectively repair trajectories with local jumps and slow drifts, improve the relative accuracy of the output vehicle motion trajectory, and thus improve the quality of high-precision map production.

[0121] Based on the same inventive concept, embodiments of the present invention also provide an electronic device 200. See also Figure 4 As shown, the electronic device 200 includes a memory 201, a processor 202, and a machine-executable program 203 stored in the memory 201 and running on the processor 202. When the processor 202 executes the machine-executable program 203, it implements the trajectory optimization method for road boundary lines of any of the foregoing embodiments or combinations of embodiments.

[0122] Those skilled in the art will clearly understand that the specific working process of the systems, devices and units described above can be referred to the corresponding process in the foregoing method embodiments, and for the sake of brevity, it will not be repeated here.

[0123] Furthermore, the functional units in the various embodiments of the present invention can be physically independent of each other, or two or more functional units can be integrated together, or all functional units can be integrated into one processing unit. The integrated functional units described above can be implemented in hardware, or in software or firmware.

[0124] Those skilled in the art will understand that if the integrated functional unit is implemented in software and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or all or part of it, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computing device (e.g., a personal computer, server, or network device) to execute all or part of the steps of the methods described in the embodiments of the present invention when running the instructions. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0125] Alternatively, all or part of the steps of the foregoing method embodiments can be implemented by hardware (such as a computing device, personal computer, server, or network device) related to program instructions. The program instructions can be stored in a computer-readable storage medium. When the program instructions are executed by the processor of the computing device, the computing device executes all or part of the steps of the methods described in the various embodiments of the present invention.

[0126] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that within the spirit and principles of the present invention, modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the corresponding technical solutions to depart from the protection scope of the present invention.

Claims

1. A trajectory optimization method, comprising: The vehicle's trajectory data and the corresponding laser point cloud data are acquired. The trajectory data includes the timestamp and pose information of each trajectory point in the trajectory, and the laser point cloud data is data collected by the vehicle-mounted LiDAR at different times in the trajectory. Multiple keyframe laser point clouds are selected from the laser point cloud data, and the pose information of trajectory points with the same timestamp as the keyframe laser point cloud is used as the keyframe pose. Traverse and examine all trajectory points in the trajectory, find abnormal trajectory points, and determine the abnormal keyframe pose corresponding to the abnormal trajectory points; The laser point cloud of each abnormal keyframe pose is matched with the laser point cloud of the next adjacent keyframe to obtain the adjacent frame matching result. Based on the adjacent frame matching results, the pose of all keyframes is optimized for the first time to obtain the optimized keyframe pose, and the optimized trajectory is obtained based on the optimized keyframe pose and the trajectory data. Detect the weak signal segment in the optimized trajectory and take the key frame pose that falls in the segment as the weak signal key frame pose. Two keyframe poses spaced apart by a second specified distance in the keyframe pose are taken as a pair of interval frames to obtain multiple pairs of interval frames. For each pair of interval frames, if the weak signal key frame pose exists in the interval defined by the two key frame poses of the pair of interval frames as endpoints, then point cloud matching is performed on the key frame laser point cloud corresponding to the two key frame poses of the pair of interval frames to obtain the interval frame matching result. Based on the adjacent frame matching results and the interval frame matching results, the keyframe pose of the first optimization is optimized a second time to obtain the second-optimized keyframe pose, and the final optimized trajectory is obtained based on the second-optimized keyframe pose and the first-optimized trajectory.

2. The trajectory optimization method according to claim 1, wherein, The step of traversing and examining all trajectory points in the trajectory to identify abnormal trajectory points includes: Traverse each trajectory point in the trajectory, and establish a set of a specified number of trajectory points on both sides of each trajectory point as the center point; Detect the trajectory smoothness of the set; Based on the detection results of trajectory smoothness, it is determined whether the center point is abnormal, including jumps and / or disconnections; If so, then the center point is determined to be an abnormal trajectory point.

3. The trajectory optimization method according to claim 1, wherein, The step of determining the abnormal keyframe pose corresponding to the abnormal trajectory point includes: Abnormal trajectory points whose distance between any two adjacent abnormal trajectory points is less than a first specified threshold are aggregated into an abnormal segment to obtain at least one abnormal segment, and the keyframe pose that falls into the at least one abnormal segment is determined as an abnormal keyframe pose.

4. The trajectory optimization method according to claim 1, wherein, The step of matching the laser point cloud of each abnormal keyframe pose with the laser point cloud of the next adjacent keyframe to obtain the adjacent frame matching result includes: The laser point cloud corresponding to each abnormal keyframe pose is matched with the laser point cloud of the next adjacent keyframe. If the match is successful, the relative pose after matching between the abnormal keyframe pose and the pose of the next adjacent keyframe is obtained.

5. The trajectory optimization method according to claim 4, wherein, The keyframe pose is a keyframe pose matrix. The step of performing a first pose graph optimization on all keyframe poses based on the adjacent frame matching results to obtain a first-optimized keyframe pose, and obtaining a first-optimized trajectory based on the first-optimized keyframe pose and the trajectory data includes: The vertices of the pose graph are constructed using all the keyframe pose matrices as variables to be optimized. Construct one-sided constraints using all the keyframe pose matrices and their inverses; Bilateral constraints are constructed by combining the matched relative poses of successfully matched anomalous keyframe poses and the initial relative poses of unmatched anomalous and non-nominal keyframe poses. The pose graph is optimized using a nonlinear optimization algorithm to obtain a first-optimized keyframe pose matrix. The trajectory points determined by the first-optimized keyframe pose matrix are then used to replace the corresponding trajectory points in the trajectory to generate a first-optimized trajectory.

6. The trajectory optimization method according to claim 5, wherein, When constructing the unilateral constraint conditions, the weight of the unilateral constraint conditions composed of the abnormal keyframe pose matrix and its inverse matrix is ​​reduced.

7. The trajectory optimization method according to claim 1, wherein, The step of detecting the weak localization signal segment in the optimized trajectory includes: Obtain signal parameters of the integrated navigation system, wherein the signal parameters include at least one of the following: the number of currently received satellites, the position accuracy factor of the satellite system, and the calculated position and attitude standard deviation; Based on the signal parameters, the segments with weak positioning signals in the optimized trajectory are determined.

8. The trajectory optimization method according to claim 5, wherein, The step of performing point cloud matching on the keyframe laser point clouds corresponding to the poses of the two keyframes in the pair of interval frames to obtain the interval frame matching result includes: Perform point cloud matching on the keyframe laser point clouds corresponding to the poses of the two keyframes in the pair of interval frames. If the match is successful, the relative pose after matching between the two keyframe poses is obtained.

9. The trajectory optimization method according to claim 8, wherein, The step of performing a second pose graph optimization on the first-optimized keyframe pose based on the adjacent frame matching results and the interval frame matching results to obtain the second-optimized keyframe pose, and obtaining the final optimized trajectory based on the second-optimized keyframe pose and the first-optimized trajectory includes: The vertices of the pose graph are constructed using all the keyframe pose matrices of the first optimization as variables to be optimized. Construct one-sided constraints using all the keyframe pose matrices and their inverses of the first-order optimization; The first bilateral constraint condition is constructed by the matched relative pose of the successfully matched abnormal keyframe pose and the initial relative pose of the unmatched abnormal keyframe pose and the non-abnormal keyframe pose. A second bilateral constraint condition is constructed by matching the relative poses of the two keyframes of each pair of successfully matched interval frames. The pose graph is optimized using a nonlinear optimization algorithm to obtain a secondary optimized keyframe pose matrix. The trajectory points determined by the secondary optimized keyframe pose matrix are then used to replace the corresponding trajectory points in the primary optimized trajectory to generate the final optimized trajectory.

10. An electronic device comprising a memory, a processor, and a machine-executable program stored in the memory and running on the processor, wherein the processor, when executing the machine-executable program, implements the trajectory optimization method according to any one of claims 1-9.

Images