Trajectory Alignment Method, Device, Electronic Device, and Computer Storage Medium

Abstract

An embodiment of the present application provides a trajectory alignment method, apparatus, electronic device, and computer storage medium. The trajectory alignment method includes: obtaining the pose errors of more than two driving trajectories on the same road and the corresponding homologous point clouds in the overlapping part of the driving trajectories; obtaining the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories and the pose errors of the homologous point clouds, where the bias matrix is used to express the pose errors of the homologous point clouds, and the sparse coefficient matrix is used to express the conversion matrix for converting the pose errors of the homologous point clouds into driving trajectory errors; optimizing the adjustment parameters in the trajectory correction expression based on the position information of the trajectory points corresponding to the homologous point clouds in the driving trajectories, the sparse coefficient matrix, and the bias matrix until the change rate of the adjustment parameters is less than a set threshold; and performing alignment processing on the driving trajectories based on the trajectory correction values obtained from the adjustment parameters. This trajectory alignment method has a small calculation burden for alignment processing.

Description

Technical Field

[0001] Embodiments of the present application relate to the field of geographic information technology, and in particular, to a trajectory alignment method, apparatus, electronic device, and computer storage medium. Background Art

[0002] In the field of data processing technology for high-precision maps (hereinafter referred to as high-precision maps), a high-precision map production data collection vehicle travels along a road, and uses the devices mounted thereon to collect environmental information of the road and its surroundings. For example, in one collection process, the collection vehicle collects point cloud data through the lidar mounted thereon. At the same time, the driving trajectory data of the collection vehicle during driving is obtained through a positioning device. Based on the driving trajectory data, point cloud data, and other data, a high-precision map can be produced.

[0003] The inventors found that for the same road, there is more than one driving trajectory data and corresponding point cloud data. These data may be collected when the same collection vehicle travels on the road at different times, or may be collected by different collection vehicles on the road at the same or different times. When producing a high-precision map, it is necessary to align different driving trajectory data of the same road to ensure that there is no ghosting after the point cloud data corresponding to these driving trajectory data is stitched together. An existing trajectory alignment method requires calculating the Jacobian matrix J and the coefficient matrix N of the corresponding Normal equation N = J T J, and then solving the Normal equation to obtain the iterative optimization step size. When the scale of the driving trajectory data increases sharply, the J matrix will become very large, and the computational complexity becomes O(n 2 ) of N = J T The computational amount of J will also increase greatly, resulting in a sharp increase in the computing resources and computing costs required for the solution, and ultimately affecting the production efficiency and cost of high-precision maps. Summary of the Invention

[0004] In view of this, embodiments of the present application provide a trajectory alignment solution to at least partially solve the above problems.

[0005] According to the first aspect of the embodiments of the present application, a trajectory alignment method is provided, including: obtaining the pose errors of more than two driving trajectories of the same road and the corresponding homologous point clouds of the overlapping parts of the driving trajectories; obtaining the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories and the pose errors of the homologous point clouds, where the bias matrix is used to represent the pose errors of the homologous point clouds, and the sparse coefficient matrix is used to represent the transformation matrix for converting the pose errors of the homologous point clouds into driving trajectory errors; optimizing the adjustment parameters in the trajectory correction expression based on the position information of the trajectory points corresponding to the homologous point clouds in the driving trajectories, the sparse coefficient matrix, and the bias matrix until the change rate of the adjustment parameters is less than a set threshold; and performing alignment processing on the driving trajectories based on the trajectory correction values obtained from the adjustment parameters.

[0006] According to the second aspect of the embodiments of the present application, a trajectory alignment device is provided, including: a first acquisition module for obtaining the pose errors of more than two driving trajectories of the same road and the corresponding homologous point clouds of the overlapping parts of the driving trajectories; a second acquisition module for obtaining the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories and the pose errors of the homologous point clouds, where the bias matrix is used to represent the pose errors of the homologous point clouds, and the sparse coefficient matrix is used to represent the transformation matrix for converting the pose errors of the homologous point clouds into driving trajectory errors; an optimization module for optimizing the adjustment parameters in the trajectory correction expression based on the position information of the trajectory points corresponding to the homologous point clouds in the driving trajectories, the sparse coefficient matrix, and the bias matrix until the change rate of the adjustment parameters is less than a set threshold; and an alignment module for performing alignment processing on the driving trajectories based on the trajectory correction values obtained from the adjustment parameters.

[0007] According to the third aspect of the embodiments of the present application, an electronic device is provided, including: a processor, a memory, a communication interface, and a communication bus, where the processor, the memory, and the communication interface complete communication with each other through the communication bus; the memory is used to store at least one executable instruction, and the executable instruction causes the processor to perform the operations corresponding to the trajectory alignment method described in the first aspect.

[0008] According to the fourth aspect of the embodiments of the present application, a computer storage medium is provided, on which a computer program is stored, and when the program is executed by a processor, it implements the trajectory alignment method described in the first aspect.

[0009] According to the trajectory alignment scheme provided by the embodiments of the present application, the pose error of homologous points is obtained. Based on the driving trajectory and the pose error of the homologous point cloud, a bias matrix and a sparse coefficient matrix are obtained. Then, based on the position information of the trajectory points corresponding to the homologous point cloud, the sparse coefficient matrix, and the bias matrix, the adjustment parameters are optimized until the optimized adjustment parameters are obtained. Furthermore, based on the trajectory correction value obtained from the adjustment parameters, the driving trajectory is aligned. During the optimization of the adjustment parameters, the sparse coefficient matrix can be calculated and stored in the form of a sparse matrix, reducing the memory occupied during calculation and the computational load. And correcting the driving trajectory according to the trajectory correction value can ensure that the pose deviation of homologous points in the aligned driving trajectory is small, thus solving the ghosting problem. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings described below are only some embodiments recorded in the embodiments of the present application. For those of ordinary skill in the art, other drawings can also be obtained based on these drawings.

[0011] Figure 1A It is a flowchart of the steps of a trajectory alignment method according to Embodiment 1 of the present application;

[0012] Figure 1B is Figure 1A a schematic diagram of the trajectory of a scenario example in the illustrated embodiment;

[0013] Figure 1C is Figure 1A a schematic diagram of the error curve of a scenario example in the illustrated embodiment;

[0014] Figure 2 It is a flowchart of the steps of a trajectory alignment method according to Embodiment 2 of the present application;

[0015] Figure 3 It is a structural block diagram of a trajectory alignment device according to Embodiment 3 of the present application;

[0016] Figure 4 It is a schematic structural diagram of an electronic device according to Embodiment 4 of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0017] To enable those skilled in the art to better understand the technical solutions in the embodiments of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art shall fall within the protection scope of the embodiments of the present application.

[0018] The following further illustrates the specific implementation of the embodiments of the present application in conjunction with the accompanying drawings of the embodiments of the present application.

[0019] Embodiment 1

[0020] Referring to Figure 1A , a flowchart showing the trajectory alignment method according to Embodiment 1 of the present application is shown.

[0021] This trajectory alignment method can be applied to the production scenario of a high-precision map (which can be used in scenarios such as autonomous driving, intelligent driving, and assisted driving of vehicles or robots). Through this method, the driving trajectories with at least partial overlap can be corrected to reduce the trajectory point error in the driving trajectory, thereby solving the problem of ghosting of the same-name points (i.e., the same ground object, such as the same building, the same traffic sign, etc.) when multiple driving trajectories are aligned, because the errors of different driving trajectories are different, resulting in too large a position deviation of the same-name points determined according to different driving trajectories.

[0022] Of course, in other embodiments, this trajectory alignment method can be applied to other appropriate scenarios, and this embodiment does not limit this.

[0023] This method includes the following steps:

[0024] Step S102: Obtain the pose errors of two or more driving trajectories on the same road and the same-name point cloud corresponding to the overlapping part of the driving trajectories.

[0025] The driving trajectory can be the trajectory formed by the corresponding data acquisition device (such as a high-precision mapping vehicle) during movement. The driving trajectory includes information such as the position information of the trajectory points and the time of the trajectory points. According to needs, the driving trajectory can also include the relative pose of the same-name point cloud corresponding to the trajectory points. Based on the relative pose of the same-name point cloud and the position information of the trajectory points, the position of the same-name points in the world coordinate system in the same-name point cloud can be deduced.

[0026] Since the errors contained in different driving trajectories are different, the positions of the same-name points estimated from the position information of the trajectory points in different driving trajectories are different. As a result, when different driving trajectories with overlapping parts of the driving trajectories are aligned, the same-name points will appear ghosted, affecting the alignment effect.

[0027] To solve this problem, the pose error of the corresponding homologous point clouds in the overlapping part of the driving trajectories is obtained. The pose error of the homologous point clouds is used to indicate the deviation degree of the overlapping part of the driving trajectories. Subsequently, the driving trajectories are corrected according to the pose error of the homologous point clouds, so as to reduce the error and improve the alignment effect.

[0028] For example, as Figure 1B shown, among two or more driving trajectories corresponding to Road 1, driving trajectory A includes trajectory points A1 to A3, and driving trajectory B includes driving trajectory points B1 to B4. Among them, the part of trajectory points A2 to A3 in driving trajectory A and the part of trajectory points B1 to B2 in driving trajectory B constitute the overlapping part of the driving trajectories. Trajectory point A2 and trajectory point B1 correspond to homologous points 1 and 2 in the homologous point cloud, and trajectory point A3 and trajectory point B2 correspond to homologous point 3 in the homologous point cloud.

[0029] For homologous point 1, according to the position of trajectory point A2, the pose information of a homologous point 1 can be estimated, and according to trajectory point B1, the pose information of another homologous point 1 can be estimated. The difference between the pose information of these two homologous points 1 can be used as the pose error of homologous point 1. For each homologous point, the pose error of the homologous point can be determined according to the position information of the trajectory points in at least two different overlapping parts of the driving trajectories, so as to determine the pose error of the homologous point cloud based on the pose errors of multiple homologous points.

[0030] Step S104: Obtain the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories and the pose error of the homologous point cloud.

[0031] Among them, the bias matrix is used to express the pose error of the homologous point cloud, and the sparse coefficient matrix is used to express the transformation matrix that converts the pose error of the homologous point cloud into a driving trajectory error.

[0032] In this embodiment, the trajectory correction expression can be expressed as: Among them, f is an adjustment parameter, is the sparse coefficient matrix, and h is the bias matrix. Subsequently, by optimizing the adjustment parameter f in the trajectory correction expression, an adjustment parameter f (that is, the optimized adjustment parameter f) with a sufficiently small matching error of the corresponding homologous points (which can be represented by the value of the trajectory correction expression) when matching the overlapping part of the driving trajectories can be determined. Subsequently, the driving trajectories are corrected according to the optimized adjustment parameter f to achieve the alignment of different driving trajectories.

[0033] Since the value of the trajectory correction expression needs to be calculated when optimizing the adjustment parameter f, the computational complexity, computational load, and computational time consumption of the trajectory correction expression directly affect the efficiency of driving trajectory alignment. To improve efficiency, reduce computational and spatial complexity, reduce computational time consumption, and reduce computing power requirements, in this embodiment, the trajectory correction expression is reconstructed so that the coefficient matrix therein is a sparse coefficient matrix, and the linear equation solving method can be used to complete the optimization.

[0034] It should be noted that the sparse coefficient matrix in this embodiment does not necessarily require the entire matrix to be a sparse matrix. As long as it can be split into two or more sparse matrix blocks for calculation, the computational efficiency can be improved, and the problem of calculating the Jacobian matrix J and the coefficient matrix N = J of the corresponding Normal equation in the prior art can be solved. T After J, solving the Normal equation to obtain the iterative optimization step size. When the scale of the driving trajectory data increases sharply, the J matrix will become very large, and the computational complexity becomes O(n 2 ) of N = J T The computational amount of J will also increase greatly, resulting in a sharp increase in the computing resources and computational costs required for solving, and ultimately affecting the production efficiency and cost of the high-precision map.

[0035] Step S106: Based on the position information of the trajectory points corresponding to the same-name point cloud in the driving trajectory, the sparse coefficient matrix, and the bias matrix, optimize the adjustment parameter in the trajectory correction expression until the change rate of the adjustment parameter is less than the set threshold.

[0036] In the process of optimizing the adjustment parameter f in the trajectory correction expression, the bias matrix is calculated according to the pose error of the same-name point cloud, and the sparse coefficient matrix is calculated according to the conversion relationship between the pose error of the same-name point cloud and the driving trajectory error, as well as the position information of the trajectory points corresponding to the same-name point cloud in the driving trajectory. Based on the calculated sparse coefficient matrix, bias matrix, and the initial value of the adjustment parameter f, the expression value of the trajectory correction expression can be calculated. According to the expression value, the adjustment parameter f is adjusted, and the expression value is calculated again. This cycle continues until the set number of cycles is satisfied or the change rate of the adjustment parameter is less than the set threshold. The set threshold can be determined as needed. For example, it can be 0.1 or 0.01.

[0037] Step S108: Based on the trajectory correction value obtained from the adjustment parameter, perform alignment processing on the driving trajectory.

[0038] In a feasible manner, when the adjustment parameter f is represented in matrix form, it can be represented in the following form:

[0039]

[0040] Among them, f1 to f K+1 are all elements in the adjustment parameter f, corresponding to each trajectory point in the corresponding driving trajectory. Based on this, the elements corresponding to each trajectory point in the driving trajectory can be determined, and the value of the corresponding element is used as the trajectory correction value of this trajectory point. Furthermore, according to the trajectory correction value, the position information of each trajectory point in the driving trajectory is corrected, so as to realize the alignment process of at least two driving trajectories with overlapping driving trajectories in the same road.

[0041] The trajectory alignment method is described below with a specific usage scenario as follows:

[0042] Such as Figure 1B shows a schematic diagram including 3 driving trajectories. Figure 1B The driving trajectory A in it includes trajectory points A1 to A3, the driving trajectory B includes trajectory points B1 to B5, and the driving trajectory C includes trajectory points C1 to C4. Among them, trajectory point A2 and trajectory point B1 correspond to the same points 1 and 2 in the same-name point cloud, trajectory point A3 and trajectory point B2 correspond to the same point 3 in the same-name point cloud, and trajectory point B4 and trajectory point C2 correspond to the same point 4 in the same-name point cloud.

[0043] Figure 1C shows an error curve of M driving trajectories with overlapping driving trajectories. This error curve can be determined according to the optimized adjustment parameters. The value corresponding to each moment (t) in the error curve can be used as the trajectory correction value of the corresponding trajectory point. This trajectory alignment method is used to calculate the value of the trajectory correction expression based on the pose error of the same-name point cloud, the position information of the trajectory points corresponding to the same-name point cloud, the offset matrix, the sparse coefficient matrix, and the adjustment parameters in the trajectory correction expression, and then optimize the adjustment parameters according to the calculated value, so as to obtain the optimized adjustment parameters, and obtain Figure 1C the error curve shown in. Based on this error curve, the corresponding driving trajectory can be corrected, and then the trajectory alignment process can be realized.

[0044] The following combines Figure 1B and Figure 1C to specifically describe the implementation process of this method:

[0045] For Road 1, a high-precision mapping vehicle drives on Road 1 and collects the environmental information of Road 1 and its surroundings to obtain 3 driving trajectories and the same-name point cloud as shown in Figure 1B . For each same-name point in the same-name point cloud, its pose can be determined according to the position information of the trajectory points in the different driving trajectories corresponding to it, and then the pose error of the same-name point can be determined.

[0046] For example, if the corresponding points with the same name 1 corresponds to the trajectory point A2 and the trajectory point B1, then according to the pose of the corresponding point with the same name 1, the relative pose between the corresponding point with the same name 1 and the trajectory point A2, and the position information of the trajectory point A2, the first pose of the corresponding point with the same name 1 is determined. According to the pose of the corresponding point with the same name 1, the pose of the corresponding point with the same name 1, the relative pose between the corresponding point with the same name 1 and the trajectory point B1, and the position information of the trajectory point B1, the second pose of the corresponding point with the same name 1 is determined. The difference between these two poses of the corresponding point with the same name 1 can be used as the pose error of the corresponding point with the same name 1.

[0047] The pose errors of each corresponding point in the corresponding point cloud can be determined in a similar manner, and then the pose error of the corresponding point cloud can be determined.

[0048] Based on the pose errors of the driving trajectory and the corresponding point cloud, the trajectory correction expression can be reconstructed so that the coefficient matrix in the trajectory correction expression is a sparse coefficient matrix that can be split into multiple different sparse matrix blocks, thereby improving the solution efficiency of the subsequent trajectory correction expression. In the usage scenario, the trajectory correction expression is denoted as:

[0049]

[0050] Among them, is a sparse coefficient matrix, which is a conversion matrix used to express the conversion of the pose error of the corresponding point cloud into the driving trajectory error. h is a bias matrix, which is used to express the pose error of the corresponding point cloud. f is an adjustment parameter, which is used to represent the driving trajectory error and can also be understood as the trajectory correction value.

[0051] For the trajectory correction expression, it can be solved according to the pose error of the corresponding point cloud, the position information of the corresponding trajectory points in the driving trajectory, etc. Then, based on the solution result, the adjustment parameter f in it can be optimized, so as to obtain a more accurate trajectory correction value, and accordingly, the driving trajectory can be aligned.

[0052] Specifically, the process of optimizing the adjustment parameter f can be as follows:

[0053] Based on the pose error of the corresponding point cloud, the bias matrix h in the trajectory correction expression can be calculated. According to the position information of the trajectory points corresponding to the corresponding points with the same name in the driving trajectory, the position of the corresponding points with the same name can be determined, and then the sparse coefficient matrix can be determined according to the position of the corresponding points with the same name, the conversion matrix, etc. In the first optimization, according to the initialized adjustment parameter f and the calculated sparse coefficient matrix and the bias matrix h, calculate the value of the trajectory correction expression. Adjust the adjustment parameter f according to the calculated value. In the subsequent optimization process, based on the adjustment parameter f optimized in the previous time and the sparse coefficient matrix And the bias matrix h, etc., calculate the value of the trajectory correction expression again until the change rate of the adjustment parameter is less than the set threshold, indicating that the optimized adjustment parameter is obtained.

[0054] According to the adjustment parameter, the trajectory correction value of the trajectory points corresponding to the same-name points in each driving trajectory can be obtained, and the corresponding driving trajectory is corrected using this trajectory correction value, that is, the driving trajectory alignment process is realized, so that when different aligned driving trajectories are merged into a larger point cloud map, the same-name points will not appear ghosted.

[0055] Since the coefficient matrix in the trajectory correction expression is reconstructed in this usage scenario to make it a sparse coefficient matrix, it can be stored and calculated in the way of a sparse matrix, thereby reducing the memory occupation and calculation load during calculation. Moreover, the calculation process is a linear equation solving process, making the calculation complexity a linear complexity, greatly reducing the calculation complexity.

[0056] Through this embodiment, the pose error of the same-name points is obtained, the bias matrix and the sparse coefficient matrix are obtained according to the pose error of the driving trajectory and the same-name point cloud, and the adjustment parameter is optimized based on the position information of the trajectory points corresponding to the same-name point cloud, the sparse coefficient matrix and the bias matrix until the optimized adjustment parameter is obtained. Furthermore, based on the trajectory correction value obtained from the adjustment parameter, the driving trajectory is aligned. During the optimization process of the adjustment parameter, and the sparse coefficient matrix can be calculated and stored in the way of a sparse matrix, reducing the memory occupied and calculation load during calculation. And correcting the driving trajectory according to the trajectory correction value can ensure that the pose deviation of the same-name points in the aligned driving trajectories is small, thus solving the ghosting problem.

[0057] The trajectory alignment method of this embodiment can be executed by any suitable electronic device with data processing capabilities, including but not limited to: servers, mobile terminals (such as mobile phones, PADs, etc.) and PC machines, etc.

[0058] Embodiment 2

[0059] Refer to Figure 2 , which shows the schematic flow chart of the steps of the trajectory alignment method of the second embodiment of the present application.

[0060] In this embodiment, the method includes:

[0061] Step S202: Obtain the pose errors of two or more driving trajectories on the same road and the same-name point cloud corresponding to the overlapping part of the driving trajectories.

[0062] In a feasible manner, step S202 is implemented through the following sub-steps:

[0063] Sub-step S2021: Obtain two or more driving trajectories on the same road.

[0064] The driving trajectory includes the position information of the trajectory points, the time of the trajectory points, etc. As needed, it also includes information such as the relative pose of the corresponding homologous points of the trajectory points.

[0065] Sub-step S2022: Determine the overlapping part of the driving trajectories of different driving trajectories according to the position information of the trajectory points included in at least two of the driving trajectories.

[0066] In a feasible manner, determine the overlapping part of the driving trajectories by comparing the position information of the starting trajectory points and the ending trajectory points of at least two driving trajectories.

[0067] Taking the Figure 1B driving trajectory A and driving trajectory B in the example shown above as an example, according to the positions of the trajectory points A1 to A3 in driving trajectory A and the positions of the trajectory points B1 to B4 in driving trajectory B, determine that the part of the trajectory points A2 to A3 and the part of the trajectory points B1 to B2 are the overlapping parts of the driving trajectories.

[0068] Sub-step S2023: Determine the pose error of the homologous point cloud according to the pose information of the homologous point cloud corresponding to the overlapping part of the driving trajectories.

[0069] As Figure 1C shown, the error curve is segmented into N curve segments according to the time of the trajectory points. The value of N corresponding to different driving trajectories may be different. According to the trajectory points included in the curve segment, the corresponding homologous points of the curve segment can be determined.

[0070] Taking the l-th homologous point in the homologous point cloud as an example, assume that the l-th homologous point corresponds to the n-th curve segment in driving trajectory m and the j-th curve segment in driving trajectory i. Determine the pose information of the l-th homologous point according to the position information of the trajectory point corresponding to the l-th homologous point in the n-th curve segment of driving trajectory m and the relative pose between the trajectory point and the l-th homologous point (which can be represented in world coordinates). Determine the pose information of the l-th homologous point according to the position information of the trajectory point corresponding to the j-th curve segment of driving trajectory i corresponding to the l-th homologous point and the relative pose between the trajectory point and the l-th homologous point The pose error of the l-th homologous point can be expressed by the following expression:

[0071]

[0072] where, v l (x m,n ,x i,j ) is the pose error (which can also be called the matching error) of the l-th homologous point.

[0073] Assume that the corresponding point cloud includes L corresponding points, then the pose error of the corresponding point cloud includes the pose errors of the L corresponding points. represents the pose (i.e., world coordinates) of the l-th corresponding point determined based on the n-th curve segment of the driving trajectory m, represents the pose (i.e., world coordinates) of the l-th corresponding point determined based on the j-th curve segment of the driving trajectory i. x m,n represents the error correction parameter for the n-th curve segment of the driving trajectory m, x i,j represents the error correction parameter for the j-th curve segment of the driving trajectory i. There can be a mapping between the error correction parameter and the adjustment parameter through a transformation matrix. represents the error of the l-th corresponding point on the n-th curve segment of the driving trajectory m, represents the error of the l-th corresponding point on the j-th curve segment of the driving trajectory i.

[0074] Since the position information of the trajectory points is known, the pose error of the corresponding points can be expressed in terms of the error correction parameter, that is, in terms of the adjustment parameter.

[0075] Assume that the corresponding point cloud includes L corresponding points, then the corresponding points and the pose error include the pose errors of the L corresponding points.

[0076] Step S204: Obtain the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectory and the pose error of the corresponding point cloud.

[0077] Before explaining the bias matrix and the sparse coefficient matrix, for the sake of easy understanding, the derivation process of the trajectory correction expression, as well as the reconstruction process and reconstruction principle of the sparse coefficient matrix are explained as follows:

[0078] Taking Figure 1C the error curve shown as an example, the pose error (also called the matching error) of the l-th corresponding point is expressed as:

[0079]

[0080] The meanings of the parameters therein have been explained in the description of sub-step S2023, so they will not be elaborated here.

[0081] Among them, represents the pose (i.e., world coordinates) of the l-th corresponding point determined based on the n-th curve segment of the driving trajectory m, which can be determined according to the position information (world coordinates) of the corresponding trajectory point on the n-th curve segment of the driving trajectory m and the relative pose with respect to this trajectory point.

[0082] The position information of the trajectory point can be expressed as:

[0083]

[0084] Among them, p W is the world coordinate in the world coordinate system, that is, the position information. is the observation trajectory angle The corresponding rotation matrix. is the translation parameter. p I is the trajectory coordinate of the trajectory point on the trajectory coordinate system.

[0085] The calculation method of is the same as that of

[0086] It represents the error of the l-th homologous point of the n-th curve segment of the driving trajectory m. It can be obtained by the first-order approximation of Taylor expansion. It can be expressed as:

[0087]

[0088] Among them, Δ Wc (x) is the error of the homologous point. p I is the trajectory coordinate of the trajectory point corresponding to the homologous point. is the observation trajectory angle The corresponding rotation matrix. dφ is the rotation and translation error term of the trajectory, and its expansion is:

[0089] dφ = (dθ x , dθ y , dθ z , dt x , dt y , dt z ) T

[0090] The rotation and translation error term of the trajectory can be modeled by a cubic spline function to obtain the corresponding cubic spline error curve. The cubic spline error curve has better smoothness, so it can make the corrected driving trajectory smooth and avoid sudden changes in the pose of the trajectory points. The rotation and translation error term can be expressed as:

[0091] dφ k (t) = s k (t) = a k + b k (t - t k ) + c k (t - t k ) 2 + d k (t - t k ) 3

[0092] The corresponding matrix representation form is:

[0093]

[0094] Among them, a k , b k , c k and d k are parameters to be obtained. Since each parameter has 6 adjustment dimensions (i.e., displacements along the x, y, and z axes and angles around the x, y, and z axes), for each error point corresponding to a trajectory point on the cubic spline error curve (this error point corresponds to a moment), the value of this error point is determined based on 24 parameters. If the driving trajectory corresponding to the error curve includes K trajectory points, the cubic spline error curve is determined based on 24K parameters. Such adjustments and calculations are huge. Moreover, to further protect the usage effect of the error curve, some regularization terms can be added to avoid drastic fluctuations of the error curve. The regularization terms are, for example, the rigid body constraint R(x) and the amplitude constraint C(x).

[0095] Among them, the rigid body constraint R(x) can be expressed as:

[0096]

[0097] r can be determined as needed, and I is the identity matrix.

[0098] The amplitude constraint C(x) can be expressed as:

[0099]

[0100] Δt m,n is the time difference corresponding to the nth curve segment of the error curve corresponding to the driving trajectory m.

[0101] Based on the above regularization terms, when the error curve is represented in a parametric form, the objective expression to be optimized can be expressed as:

[0102]

[0103] Among them, x is the parameter in the parametric form of the error curve.

[0104] is the homonymous matching error term, and L is the total number of homonymous point pairs. is the selection function, which indicates that the homonymous point l corresponds to the nth curve segment of the driving trajectory m and the jth curve segment of the driving trajectory i respectively, takes a value of 0 or 1. If the homonymous point l is related to the parameter x, it takes a value of 1; otherwise, it takes a value of 0. w l is the weight of the observed homonymous point, which can be determined as needed.

[0105] is a rigid constraint term, β is a weight, which can be determined as needed. M is the total number of driving trajectories with overlapping driving trajectories. m is the current driving trajectory, and its value ranges from 1 to M. N m is the total number of curve segments corresponding to the driving trajectory m. n is the current curve segment, and its value ranges from 1 to N m .

[0106] is the amplitude constraint, λ is the weight, which can be determined as needed. M, m, N m and n have the same meanings as the rigid constraint term, so they will not be elaborated here.

[0107] The error curve represented by this parametric form requires each parameter x to be determined according to a k , b k , c k and d k Therefore, it is not the expression form with the minimum number of parameters. To reduce the number of parameters, the error curve can be segmented, and the endpoint value f of each curve segment can be taken k to describe the error curve in this way, which can simplify the number of parameters, and the endpoint value f k is the minimum parameter set f of the cubic spline error curve, that is, the adjustment parameter.

[0108] The conversion relationship between these two parameters can be expressed as:

[0109]

[0110] Among them, x is the parameter corresponding to the parametric form, f is the adjustment parameter, which can reduce the number of parameters to be adjusted. After being converted into matrix form, the number of adjustment parameters is reduced from 24K to 6(K + 1). is the conversion matrix.

[0111] Based on the aforementioned conversion matrix and adjustment parameter f, the target expression to be optimized is represented in matrix form as the trajectory correction expression, which is expressed as:

[0112]

[0113] The reconstruction of the sparse coefficient matrix is:

[0114]

[0115] Among them, V 3L×12N is the homonymous matching error term, C 3N×12N is the amplitude constraint term, R 12N×12N is the rigid constraint term. is the conversion matrix. h is the bias matrix.

[0116] In the above - mentioned manner, the trajectory correction expression can be represented in matrix form, and the reconstructed sparse coefficient matrix can be calculated in the way of a sparse matrix, thereby reducing the storage space and computing power requirements.

[0117] The acquisition of the bias matrix and the sparse coefficient matrix will be described below:

[0118] In step S204, obtaining the bias matrix in the trajectory correction expression according to the pose error of the corresponding point clouds can be realized as follows: determining the bias matrix in the trajectory correction expression according to the product of the pose error of the corresponding point clouds and the weights of the corresponding point clouds.

[0119] In this embodiment, the bias matrix h can be expressed as:

[0120]

[0121] Among them, L is the number of corresponding points in the corresponding point clouds. N is the number of the maximum curve segments of the corresponding driving trajectory. It can be expressed as:

[0122]

[0123] where, w 1，1，1 is the weight of the first corresponding point corresponding to the first curve segment of the driving trajectory 1. is the world coordinate of the first corresponding point determined according to the first curve segment of the driving trajectory 1. is the world coordinate of the first corresponding point determined according to the first curve segment of the driving trajectory i. is the pose error between the first corresponding point of the first curve segment of the driving trajectory 1 and the first corresponding point of the j - th curve segment of the driving trajectory i. The meanings of the remaining parameters are similar, only the driving trajectories, curve segments, and corresponding points are different, so they will not be elaborated.

[0124] The remaining elements in the bias matrix are all 0, so they will not be elaborated.

[0125] In step S204, obtaining the sparse coefficient matrix in the trajectory correction expression according to the driving trajectory can be realized according to the following sub - steps:

[0126] Sub - step S2041: Determine the transformation matrix part in the sparse coefficient matrix according to the time of the trajectory points included in the overlapping part of the driving trajectories.

[0127] As mentioned above, the transformation matrix is used to realize the conversion between the parameter x in parametric form and the minimum parameter set f.

[0128] Since the conversion matrix as a whole is a dense matrix, it results in a large load during calculation and requires a large amount of memory. In this embodiment, the conversion matrix is partitioned to form 4 sparse partitions, so that each sparse partition can be calculated separately during calculation, thereby reducing the amount of calculation and memory occupation.

[0129] For example, the conversion matrix is cut into four sparse partitions, and the conversion matrix forms a matrix after the row transformation position.

[0130] It is represented by 4 sparse partitions as:

[0131]

[0132] The calculation of the 4 sparse partitions is realized through the following process:

[0133] Process A: Determine the first sparse partition according to the identity matrix.

[0134] The first sparse partition can be expressed as: I is the identity matrix.

[0135] The remaining second, third, and fourth sparse partitions all need to be determined according to M.

[0136] Therefore, first, the solution process of M is described. In this embodiment, M is solved through Process B and Process C.

[0137] Process B: Determine the first tridiagonal matrix and the second tridiagonal matrix according to the identity matrix, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and the reciprocal of the time difference.

[0138] The first tridiagonal matrix is determined according to the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and it can be expressed as:

[0139]

[0140] Among them, Δt1 is the time difference between the first adjustment parameter and the second adjustment parameter. is the time difference between the last adjustment parameter and the penultimate adjustment parameter in the corresponding driving trajectory. The first adjustment parameter corresponds to the first trajectory point in the corresponding driving trajectory, and the second adjustment parameter corresponds to the second trajectory point in the corresponding driving trajectory. In other words, Δt1 is the duration of the first curve segment in the corresponding error curve.

[0141] The second tridiagonal matrix is determined according to the reciprocal of the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and it can be expressed as:

[0142]

[0143] Process C: Use the Thomas algorithm to solve the first tridiagonal matrix and the second tridiagonal matrix.

[0144] In this embodiment, Γ3M = Γ4, and based on this, the Thomas algorithm is used to solve and find the first tridiagonal matrix and the second tridiagonal matrix, thereby determining M.

[0145] Process D: Determine the second sparse sub-block according to the solution result, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, the reciprocal of the time difference, and the identity matrix.

[0146] In this embodiment, the second sparse sub-block is expressed as: Γ1 + Γ2M. Wherein:

[0147]

[0148] It is determined according to the reciprocal of the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories and the identity matrix.

[0149]

[0150] It is determined according to the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories and the identity matrix. In this way, the second sparse sub-block can be solved.

[0151] Process E: Determine the third sparse sub-block according to the solution result and the identity matrix.

[0152] The third sparse sub-block is expressed as The third sparse sub-block can be calculated according to the solved M and Γ0.

[0153] Process F: Determine the fourth sparse sub-block according to the solution result, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and the identity matrix.

[0154] The fourth sparse sub-block The fourth sparse sub-block can be calculated according to the solved M and Γ1.

[0155] Sub-step S2042: Obtain the set constraint value and the weight of the homologous point cloud, and determine the error matrix part in the sparse coefficient matrix according to the position information, attitude information, time of the trajectory points, the weight of the homologous point cloud, and the set constraint value included in the overlapping part of the driving trajectories.

[0156] In a feasible manner, sub-step S2042 can be implemented through the following process:

[0157] Process G: Obtain the set constraint value and the weights of the homologous point clouds.

[0158] The set constraint value r can be determined as needed, and it affects the rigid constraint on the error curve.

[0159] The weights of the homologous point clouds can be determined according to requirements.

[0160] Process H: Determine the homologous matching error sub-blocks in the error matrix part according to the position information, attitude information of the trajectory points included in the overlapping part of the driving trajectories and the weights of the homologous point clouds. The homologous matching error sub-blocks are used to express the matching errors of the homologous point clouds after correcting the corresponding overlapping part of the driving trajectories using the trajectory correction expression.

[0161] The matching error term V can be expressed as:

[0162]

[0163] where w 1，1，1 is the weight of the first homologous point of the first curve segment of the error curve corresponding to driving trajectory 1, and B 1，1，1 is the matching error of the first homologous point of the first curve segment of the error curve corresponding to driving trajectory 1. It can be determined according to the following formula:

[0164]

[0165]

[0166] where, is the trajectory coordinate of the trajectory point corresponding to the l-th homologous point of the n-th curve segment in driving trajectory m. When the adjustment parameter to be optimized is only the translation amount, is the identity matrix.

[0167]

[0168] where, is the time corresponding to the l-th homologous point in the error curve, and t k is the time corresponding to the k-th trajectory point on the driving trajectory corresponding to the l-th homologous point.

[0169] Process I: Determine the rigid constraint sub-blocks in the error matrix part according to the set constraint value. The rigid constraint sub-blocks are used to perform rigid constraints on the trajectory correction expression.

[0170] The matrix representation of the rigid constraint sub-block is:

[0171]

[0172] Among them, β is the weight, which can be determined as needed.

[0173] Process J: Determine the amplitude constraint block of the error matrix part according to the time difference between two adjacent trajectory points indicated by the time of the trajectory points included in the overlapping part of the driving trajectories, and the amplitude constraint block is used to perform amplitude constraint on the trajectory correction expression.

[0174] The matrix representation of the amplitude constraint block is:

[0175]

[0176] Among them, λ is the weight, which can be determined as needed. Δt m,n represents the duration corresponding to the nth curve segment in the error curve corresponding to the driving trajectory m, that is, the time difference between two adjacent trajectory points.

[0177] In this way, a trajectory correction expression in the form of a cubic spline function can be obtained. Subsequently, the adjustment parameter f can be optimized based on this trajectory correction expression and the driving trajectory.

[0178] Step S206: Optimize the adjustment parameter in the trajectory correction expression based on the position information of the trajectory points corresponding to the homologous point cloud in the driving trajectory, the sparse coefficient matrix, and the offset matrix until the change rate of the adjustment parameter is less than the set threshold.

[0179] In an example, when performing optimization, the following initial settings are made:

[0180] r0 = -h, where h is the offset matrix, which is determined according to the pose error of the homologous point cloud, and r0 represents the negative value of the offset matrix.

[0181] s0 = -G T h, G are sparse coefficient matrices, which are determined by the position information of the trajectory points, and G is conjugate to, G T is the transpose of G, and by assigning -G T h to s0, it is convenient for subsequent iteration.

[0182] p1 = s0, and by assigning s0 to p1, it is convenient for subsequent iteration.

[0183] e0 = |s0| 2 , and by assigning |s0| 2 to e0, it is convenient for subsequent iteration

[0184] f0 = 0. Among them, f0 is initialized to 0 for subsequent optimization and iteration.

[0185] Let the value of the loop count \(i\) range from 1 to \(N\). The value of \(N\) can be determined according to requirements, such as 100, 200, etc.

[0186] In one loop, let \(q_i = Gp\) i ; \(i\) is the loop count, \(q_i\) is the residual, and \(p_i\) is the parameter adjustment direction.

[0187] d i =e i-1 / |q i | 2 ; d i is the adjustment step size.

[0188] f i =f i-1 +d i p i ; where \(f\) i is the adjustment parameter for this loop.

[0189] r i =r i-1 -d i q i ; \(r_i\) is the parameter error vector.

[0190] s i =G T r i ; \(s_i\) is the parameter error iteration inertia coefficient.

[0191] e i =|s i | 2 ; \(e_i\) is the parameter error amplitude.

[0192] y i =e i / e i-1 ;

[0193] p i =s i +y i p i ;

[0194] If the change rate of \(f_i\) obtained in this loop and \(f_i\) of the previous time is less than the set threshold, then terminate the loop; otherwise, make \(i = i + 1\) and execute the loop again until the change rate of \(f_i\) obtained in this loop and \(f_i\) of the previous time is less than the set threshold or the maximum loop count \(N\) is satisfied.

[0195] Through the above loop, the adjustment parameter \(f\) can be optimized, and the error curve determined according to the adjustment parameter \(f\) is a cubic spline curve, thus ensuring smoothness.

[0196] Step S208: Perform alignment processing on the driving trajectory based on the trajectory correction value obtained from the adjustment parameter.

[0197] According to the elements included in the adjustment parameter f, determine the trajectory correction value corresponding to the trajectory point of each driving trajectory. Furthermore, the driving trajectory can be corrected by summing the trajectory point and the corresponding trajectory correction value, thereby achieving trajectory alignment. Since the optimized adjustment parameter f is the parameter that minimizes the matching error value of the corresponding points, after trajectory alignment based on it, the problem of corresponding point ghosting can be avoided.

[0198] Since the adjustment parameter f forms a cubic spline error curve, the cubic spline error curve is used to correct the driving trajectory, so that the error has 3 - order continuity in continuous time, which can ensure that the corrected driving trajectory also has good smoothness.

[0199] By adjusting the adjustment parameter, the cubic spline error curve can be changed, and then the position of the driving trajectory and the corresponding points determined according to the driving trajectory can be changed. The adjustment parameter that minimizes the matching error of the corresponding points is the optimized adjustment parameter, and the cubic spline error curve determined accordingly is the cubic spline error curve matching the driving trajectory.

[0200] In the process of optimizing the adjustment parameter, the calculation of the sparse coefficient matrix and the bias matrix can be carried out through the calculation method and storage method of the sparse matrix to reduce the storage and calculation load during the solution, and solve the problem of large memory occupation and large calculation load existing in the multiplication of the J matrix related to calculating Jacobian in the prior art. T The problem of large memory occupation and large calculation load existing in the multiplication of the J matrix related to calculating Jacobian.

[0201] In this embodiment, optimization is carried out through the conjugate gradient least squares method (CGLS), which can avoid the multiplication calculation of the J matrix related to the Jacobian algorithm, making the calculation and space complexity of the optimization process both linear complexity, suitable for large - scale driving trajectory alignment optimization. T The problem of large memory occupation and large calculation load existing in the multiplication of the J matrix related to calculating Jacobian.

[0202] By reconstructing the sparse coefficient matrix of the trajectory alignment optimization, the target optimization can be completed by using the linear equation solving method; based on the sparse coefficient matrix expression and the conjugate gradient least squares (CGLS) optimization method, the large - scale trajectory alignment objective function is optimized, and the calculation and space complexity of the optimization can be made linear complexity.

[0203] The trajectory alignment method solves the problems in the prior art that in the optimization process, the matrix uses a dense matrix, which multiplies the computational complexity and space storage complexity, and is not conducive to large-scale optimization calculations. In addition, it can also solve the problem that the automatic gradient calculation method will multiply the calculation of automatically solving the gradient, and for most optimization libraries, the automatic gradient optimization will use a dense matrix for storage, increasing the memory occupancy, and is not suitable for large-scale trajectory alignment optimization with high computational complexity requirements.

[0204] The trajectory correction expression of this method is expressed by a sparse coefficient matrix and a bias matrix, and can solve linear equations. This method has the performance of linear complexity in terms of computational time and space, and can be applied to large-scale trajectory alignment optimization in high-speed and ordinary road networks; improving the consistency of large-scale point cloud maps.

[0205] Embodiment III

[0206] Refer to Figure 3 , which shows the structural block diagram of the trajectory alignment device according to Embodiment III of the present application.

[0207] The trajectory alignment device of this embodiment includes:

[0208] The first acquisition module 302 is used to acquire the pose errors of two or more driving trajectories on the same road and the corresponding homologous point clouds of the overlapping part of the driving trajectories;

[0209] The second acquisition module 304 is used to obtain the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories and the pose errors of the homologous point clouds. The bias matrix is used to express the pose errors of the homologous point clouds, and the sparse coefficient matrix is used to express the conversion matrix for converting the pose errors of the homologous point clouds into driving trajectory errors;

[0210] The optimization module 306 is used to optimize the adjustment parameters in the trajectory correction expression based on the position information of the trajectory points corresponding to the homologous point clouds in the driving trajectories, the sparse coefficient matrix, and the bias matrix until the change rate of the adjustment parameters is less than a set threshold;

[0211] The alignment module 308 performs alignment processing on the driving trajectories based on the trajectory correction values obtained from the adjustment parameters.

[0212] Optionally, the first acquisition module 302 is used to acquire two or more driving trajectories on the same road; determine the overlapping part of the driving trajectories of different driving trajectories according to the position information of the trajectory points included in at least two of the driving trajectories; and determine the pose errors of the homologous point clouds according to the pose information of the homologous point clouds corresponding to the overlapping part of the driving trajectories.

[0213] Optionally, when obtaining the bias matrix in the trajectory correction expression according to the pose error of the homologous point cloud, the second acquisition module 304 is configured to determine the bias matrix in the trajectory correction expression according to the product of the pose error of the homologous point cloud and the weight of the homologous point cloud.

[0214] Optionally, when obtaining the sparse coefficient matrix in the trajectory correction expression according to the driving trajectory, the second acquisition module 304 is configured to determine the transformation matrix in the sparse coefficient matrix according to the time of the trajectory points included in the overlapping part of the driving trajectories; obtain the set constraint value and the weight of the homologous point cloud, and determine the error matrix part in the sparse coefficient matrix according to the position information, attitude information, time of the trajectory points, the weight of the homologous point cloud, and the set constraint value included in the overlapping part of the driving trajectories.

[0215] Optionally, the transformation matrix part includes at least four sparse blocks. When determining the transformation matrix in the sparse coefficient matrix according to the time of the trajectory points included in the overlapping part of the driving trajectories, the second acquisition module 304 is configured to determine the first sparse block according to the identity matrix; determine the first tridiagonal matrix and the second tridiagonal matrix according to the identity matrix, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and the reciprocal of the time difference; solve the first tridiagonal matrix and the second tridiagonal matrix using the Thomas algorithm; determine the second sparse block according to the solution result, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, the reciprocal of the time difference, and the identity matrix; determine the third sparse block according to the solution result and the identity matrix; determine the fourth sparse block according to the solution result, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and the identity matrix.

[0216] Optionally, the second acquisition module 304 is configured to, when acquiring a set constraint value and the weight of the homologous point cloud, and determining the error matrix part in the sparse coefficient matrix according to the position information, attitude information, time of the trajectory points included in the overlapping part of the driving trajectories, the weight of the homologous point cloud, and the set constraint value, acquire the set constraint value and the weight of the homologous point cloud; determine the homologous matching error block in the error matrix part according to the position information, attitude information, and the weight of the homologous point cloud of the trajectory points included in the overlapping part of the driving trajectories, where the homologous matching error block is used to represent the matching error of the homologous point cloud after correcting the corresponding overlapping part of the driving trajectory using the trajectory correction expression; determine the rigid constraint block in the error matrix part according to the set constraint value, where the rigid constraint block is used to perform a rigid constraint on the trajectory correction expression; and determine the amplitude constraint block in the error matrix part according to the time difference between two adjacent trajectory points indicated by the time of the trajectory points included in the overlapping part of the driving trajectories, where the amplitude constraint block is used to perform an amplitude constraint on the trajectory correction expression.

[0217] Optionally, the trajectory correction expression is a cubic spline function expression.

[0218] The trajectory alignment device in this embodiment is used to implement the corresponding trajectory alignment method in the foregoing multiple method embodiments, and has the beneficial effects of the corresponding method embodiments, which will not be elaborated herein. In addition, the function implementation of each module in the trajectory alignment device in this embodiment can be referred to the description of the corresponding part in the foregoing method embodiments, which will not be elaborated herein either.

[0219] Embodiment IV

[0220] Referring to Figure 4 , a schematic structural diagram of an electronic device according to Embodiment IV of the present application is shown. The specific implementation of the electronic device in the specific embodiment of the present application is not limited.

[0221] As Figure 4 shown, the electronic device may include: a processor 402, a communications interface 404, a memory 406, and a communication bus 408.

[0222] Wherein:

[0223] The processor 402, the communications interface 404, and the memory 406 communicate with each other through the communication bus 408.

[0224] The communications interface 404 is used to communicate with other electronic devices or servers.

[0225] A processor 402 is configured to execute a program 410, and specifically, can execute the relevant steps in the above-described embodiments of the trajectory alignment method.

[0226] Specifically, the program 410 may include program code, and the program code includes computer operation instructions.

[0227] The processor 402 may be a central processing unit (CPU), or a specific integrated circuit (ASIC) (Application Specific Integrated Circuit), or one or more integrated circuits configured to implement the embodiments of the present application. One or more processors included in the intelligent device may be of the same type of processor, such as one or more CPUs; or may be of different types of processors, such as one or more CPUs and one or more ASICs.

[0228] A memory 406 is configured to store the program 410. The memory 406 may include a high-speed RAM memory, and may also include a non-volatile memory, such as at least one disk memory.

[0229] The program 410 is specifically configured to cause the processor 402 to execute the corresponding steps of the foregoing trajectory alignment method.

[0230] For the specific implementation of each step in the program 410, reference may be made to the corresponding steps and descriptions in the corresponding units in the above-described embodiments of the trajectory alignment method, which will not be elaborated herein. Those skilled in the art can clearly understand that for the convenience and conciseness of description, the specific working processes of the above-described devices and modules may refer to the corresponding process descriptions in the foregoing method embodiments, which will not be elaborated herein.

[0231] It should be noted that according to the needs of implementation, each component / step described in the embodiments of the present application may be split into more components / steps, or two or more components / steps or partial operations of the components / steps may be combined into new components / steps to achieve the objectives of the embodiments of the present application.

[0232] The method according to the embodiments of the present application can be implemented in hardware, firmware, or be implemented as software or computer code that can be stored in a recording medium (such as a CD ROM, RAM, floppy disk, hard disk, or magneto-optical disk), or be implemented as computer code that is originally stored in a remote recording medium or a non-transitory machine-readable medium and downloaded through a network and will be stored in a local recording medium. Thus, the method described herein can be stored as such software processing on a recording medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware (such as an ASIC or FPGA). It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component (such as RAM, ROM, flash memory, etc.) that can store or receive software or computer code. When the software or computer code is accessed and executed by the computer, the processor, or the hardware, the trajectory alignment method described herein is implemented. In addition, when a general-purpose computer accesses the code for implementing the trajectory alignment method shown herein, the execution of the code converts the general-purpose computer into a dedicated computer for executing the trajectory alignment method shown herein.

[0233] Those of ordinary skill in the art can realize that the units and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or in a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Skilled professionals can use different methods for each specific application to implement the described functions, but such implementation should not be considered to exceed the scope of the embodiments of the present application.

[0234] The above embodiments are only used to illustrate the embodiments of the present application, rather than to limit the embodiments of the present application. Those of ordinary skill in the relevant technical field can make various changes and modifications without departing from the spirit and scope of the embodiments of the present application. Therefore, all equivalent technical solutions also belong to the scope of the embodiments of the present application. The patent protection scope of the embodiments of the present application shall be defined by the claims.

Claims

1. A trajectory alignment method, wherein, Including: Obtain the pose errors of more than two driving trajectories on the same road and the homologous point clouds corresponding to the overlapping parts of the driving trajectories, where the pose errors of the homologous point clouds are used to indicate the deviation degree of the overlapping parts of the driving trajectories; Based on the driving trajectories and the pose errors of the homologous point clouds, obtain the bias matrix and the sparse coefficient matrix in the trajectory correction expression, where the bias matrix is used to represent the pose errors of the homologous point clouds, and the sparse coefficient matrix is used to represent the conversion matrix for converting the pose errors of the homologous point clouds into driving trajectory errors; Based on the position information of the trajectory points corresponding to the homologous point clouds in the driving trajectories, the sparse coefficient matrix, and the bias matrix, optimize the adjustment parameters in the trajectory correction expression until the change rate of the adjustment parameters is less than a set threshold. Among them, the sparse coefficient matrix is the coefficient of the adjustment parameters in the trajectory correction expression, and the bias matrix is the bias term in the trajectory correction expression; Perform alignment processing on the driving trajectories based on the trajectory correction values obtained from the adjustment parameters.

2. The method according to claim 1, wherein, The obtaining of the pose errors of more than two driving trajectories on the same road and the homologous point clouds corresponding to the overlapping parts of the driving trajectories includes: Obtain more than two driving trajectories on the same road; Determine the overlapping parts of the driving trajectories of different driving trajectories according to the position information of the trajectory points included in at least two of the driving trajectories; Determine the pose errors of the homologous point clouds according to the pose information of the homologous point clouds corresponding to the overlapping parts of the driving trajectories.

3. The method according to claim 1, wherein Obtaining the bias matrix in the trajectory correction expression according to the pose errors of the homologous point clouds includes: Determine the bias matrix in the trajectory correction expression according to the product of the pose errors of the homologous point clouds and the weights of the homologous point clouds.

4. The method according to claim 1, wherein, Obtaining the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories includes: Determine the conversion matrix in the sparse coefficient matrix according to the time of the trajectory points included in the overlapping parts of the driving trajectories; Obtain the set constraint value and the weights of the homologous point clouds, and determine the error matrix part in the sparse coefficient matrix according to the position information, attitude information, time of the trajectory points, weights of the homologous point clouds, and the set constraint value included in the overlapping parts of the driving trajectories.

5. The method according to claim 4, wherein The conversion matrix part includes at least four sparse blocks. The determining of the conversion matrix in the sparse coefficient matrix according to the time of the trajectory points included in the overlapping parts of the driving trajectories includes: Determine the first sparse block according to the identity matrix; Determine the first tridiagonal matrix and the second tridiagonal matrix according to the identity matrix, the time difference between two adjacent trajectory points included in the overlapping parts of the driving trajectories, and the reciprocal of the time difference; Use the Thomas algorithm to solve the first tridiagonal matrix and the second tridiagonal matrix; Determine the second sparse block according to the solution result, the time difference between two adjacent trajectory points included in the overlapping parts of the driving trajectories, the reciprocal of the time difference, and the identity matrix; Determine the third sparse block according to the solution result and the identity matrix; Determine the fourth sparse block according to the solution result, the time difference between two adjacent trajectory points included in the overlapping part of the driving trajectories, and the identity matrix.

6. The method according to claim 4, wherein, Obtain the set constraint value and the weight of the homologous point cloud, and determine the error matrix part in the sparse coefficient matrix according to the position information, attitude information, time of the trajectory points, the weight of the homologous point cloud, and the set constraint value included in the overlapping part of the driving trajectories, including: Obtain the set constraint value and the weight of the homologous point cloud; Determine the homologous matching error block in the error matrix part according to the position information, attitude information, and the weight of the homologous point cloud of the trajectory points included in the overlapping part of the driving trajectories, where the homologous matching error block is used to represent the matching error of the homologous point cloud after correcting the corresponding overlapping part of the driving trajectory using the trajectory correction expression; Determine the rigid constraint block in the error matrix part according to the set constraint value, where the rigid constraint block is used to perform rigid constraint on the trajectory correction expression; Determine the amplitude constraint block of the error matrix part according to the time difference between two adjacent trajectory points indicated by the time of the trajectory points included in the overlapping part of the driving trajectories, where the amplitude constraint block is used to perform amplitude constraint on the trajectory correction expression.

7. The method according to any one of claims 1-6, wherein, The trajectory correction expression is a cubic spline function expression.

8. A trajectory alignment device, comprising: A first acquisition module, configured to acquire the pose errors of two or more driving trajectories on the same road and the homologous point cloud corresponding to the overlapping part of the driving trajectories, where the pose error of the homologous point cloud is used to indicate the deviation degree of the overlapping part of the driving trajectories; A second acquisition module, configured to obtain the bias matrix and the sparse coefficient matrix in the trajectory correction expression according to the driving trajectories and the pose errors of the homologous point cloud, where the bias matrix is used to represent the pose error of the homologous point cloud, and the sparse coefficient matrix is used to represent the conversion matrix that converts the pose error of the homologous point cloud into a driving trajectory error; An optimization module, configured to optimize the adjustment parameters in the trajectory correction expression based on the position information of the trajectory points corresponding to the homologous point cloud in the driving trajectories, the sparse coefficient matrix, and the bias matrix until the change rate of the adjustment parameters is less than a set threshold, where the sparse coefficient matrix is the coefficient of the adjustment parameters in the trajectory correction expression, and the bias matrix is the bias term in the trajectory correction expression; An alignment module, configured to perform alignment processing on the driving trajectories based on the trajectory correction values obtained from the adjustment parameters.

9. An electronic device, comprising: A processor, a memory, a communication interface, and a communication bus, where the processor, the memory, and the communication interface complete communication with each other through the communication bus; The memory is used to store at least one executable instruction, and the executable instruction causes the processor to perform the operations corresponding to the trajectory alignment method according to any one of claims 1-7.

10. A computer storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the trajectory alignment method according to any one of claims 1-7.

Images