A backend optimization method based on goal-directed bias search

Abstract

The present application relates to a kind of target guide-based deviation search back-end optimization method.By experiment demonstration, it is superior to traditional brute-force enumeration matching algorithm in matching quality and matching speed.Secondly, the present application also proposes a kind of corner feature loop based on slope difference, the slope of corner point and the slope of the adjacent two points of corner point are described as a corner point, and in two observations, the above three factors can be matched successfully as the mark of loop success, reduce the wrong loop.Then based on the sparsity of hessian matrix in back-end optimization method, the sparse pose adjustment method (SPA) optimization is carried out, so as to simplify the process of back-end nonlinear optimization, reduce the back-end optimization time.Finally, through the simulation of gazebo platform, the effectiveness of the method is verified.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of simultaneous localization and mapping (SLAM), in particular to a target-guided partial search-based backend optimization method. BACKGROUND

[0002] In recent years, with the development of intelligent driving technology, the automobile industry is gradually developing towards unmanned driving. Simultaneous localization and mapping (SLAM) can provide real-time high-precision positioning for vehicles and also complete environment mapping operations, which is a prerequisite for realizing unmanned driving. Therefore, the performance of the SLAM algorithm is related to the safety and effectiveness of the autonomous operation of unmanned vehicles. In the front end of SLAM, the data collected by various sensors is not very accurate, and after a long period of accumulation, errors will continue to accumulate. Therefore, the SLAM backend is needed to optimize the data of the front end. SLAM backend optimization is an important link in SLAM technology, mainly dealing with data fusion, pose matching and loop detection in the SLAM process. In order to ensure the real-time performance of the vehicle, it is usually required to minimize the optimization time of the backend. However, most of the current matching algorithms are through a brute-force matching method between the current information and the local map, that is, all the translational and rotational poses within a certain range are traversed, and the pose with the highest score is selected. This will consume a lot of computing time, making it difficult to ensure real-time performance. SUMMARY

[0003] The purpose of the present application is to ensure the real-time performance of the algorithm, and to provide a target-guided partial search-based backend optimization method.

[0004] To achieve the above purpose, the technical scheme of the present application is as follows: a target-guided partial search-based backend optimization method, comprising the following steps:

[0005] Step one, collecting unmanned vehicle data through sensors;

[0006] Step two, time and space calibration of sensor data information;

[0007] Step three, fusion of unified sensor data;

[0008] Step four, determining the search area to be performed by the matching algorithm;

[0009] Step five, using a loop strategy based on corner features to determine whether to loop, if the loop fails, returning to step three, otherwise continuing to execute;

[0010] Step six, using a sparse pose adjustment method to optimize the pose nodes of the unmanned vehicle;

[0011] Step seven, comparing brute-force enumeration matching and target area matching, and analyzing the advantages of target area matching;

[0012] Step eight, visualize the unmanned vehicle backend optimization structure on gazebo and conduct error analysis.

[0013] Compared with the prior art, the present application has the following beneficial effects: the present application is a kind of target guide based on the bias search of rear-end optimization method, a kind of target guide based on the bias search strategy is proposed, and the bias search strategy is error analyzed, and the experiment proves that the method has the advantage of the original brute force enumeration matching method.And use the corner feature based on the loop strategy to loop.In the loop is successful, a kind of sparse pose adjustment (SPA) optimization is used, that is, only the pose node of unmanned vehicle is optimized, and the road mark point is no longer optimized in real time, so as to achieve a simplified model of graph optimization, and the effectiveness of the method is verified by using gazebo to carry out simulation experiment. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 Data nodes are issued for each sensor.

[0015] Figure 2 It is a linear interpolation diagram.

[0016] Figure 3 It is a coordinate transformation relationship diagram.

[0017] Figure 4 It is a target area diagram.

[0018] Figure 5 It is a Gaussian distribution area diagram.

[0019] Figure 6 It is a laser radar scanning point diagram.

[0020] Figure 7 It is a slope mutation diagram.

[0021] Figure 8 It is a slope distribution diagram.

[0022] Figure 9 It is a graph optimization pose constraint diagram.

[0023] Figure 10 It is a nonlinear optimization flow chart.

[0024] Figure 11 It is a Jacobian matrix structure.

[0025] Figure 12 It is a hessian matrix structure.

[0026] Figure 13 It is a hessian matrix sparsity instance diagram.

[0027] Figure 14For experimental result distribution chart.

[0028] Figure 15 For SPA constraint relationship chart.

[0029] Figure 16 For back-end optimization result comparison chart.

[0030] Figure 17 For the method flow chart of the application. DETAILED DESCRIPTION

[0031] The technical solutions of the application will be specifically described below with reference to the drawings.

[0032] As Figure 17 shown, the application provides a target-guided bias search back-end optimization method, which comprises the following steps:

[0033] Step one, collecting unmanned vehicle data through sensors;

[0034] Step two, time and space calibration of sensor data information;

[0035] Step three, fusion of good unified sensor data;

[0036] Step four, determination of the search area to be performed by the matching algorithm;

[0037] Step five, use of a loop-back strategy based on corner features to determine whether to loop back, and if loop back fails, return to step three, otherwise continue;

[0038] Step six, use of a sparse pose adjustment method to optimize the unmanned vehicle pose node;

[0039] Step seven, comparison of brute force enumeration matching and target area matching, and analysis of the advantages of target area matching;

[0040] Step eight, visualization of the unmanned vehicle back-end optimization structure on gazebo and error analysis.

[0041] The following is the specific implementation process of the method of the application.

[0042] Step one, collecting unmanned vehicle data through sensors;

[0043] Read sensor information such as laser radar, inertial measurement unit (IMU), and wheel encoder sensor information from the vehicle-mounted sensor.

[0044] Step two, time and space unification of sensor data information;

[0045] After reading the sensor information, unify the data information to the same time node and the same coordinate system. Since the data frequency published by each sensor is different, as shown in the attachedFigure 1 As shown, each sensor data needs to be time-unified, and the laser radar is the core sensor for mapping, so the time stamp of the laser radar is taken as the reference for time-unification.

[0046] As shown in the figure, the laser radar, IMU and wheel encoder may encounter different time nodes of each sensor, in order to align the time of each data, a linear interpolation method can be adopted, assuming that t1 and t2 are the nearest two time points of the sensor IMU when the laser radar data arrives at time t, t1 is the maximum number smaller than t, t2 is the minimum number larger than t, the state corresponding to t1 is x1, the state corresponding to t2 is x2, and the data time of the laser radar is t, as shown in the attached Figure 2 .

[0047] Therefore, the linear interpolation formula of the state X of the sensor to be aligned at time t is:

[0048]

[0049] The odometer of the application is derived from the wheel encoder and IMU pre-integration, and from t to t+1, the formula of the wheel encoder for deriving the odometer is:

[0050]

[0051] Where x and y represent the position of the unmanned vehicle, and theta is the heading angle.

[0052] The IMU pre-integration is to change the reference frame from the world coordinate system w to the coordinate system b of the kth frame data k That is, on this basis, multiply the rotation matrix to obtain:

[0053]

[0054] Where p, v and q represent the pose, speed and quaternion of the unmanned vehicle, g represents the gravity acceleration, represents the quaternion multiplication. The and in the above formula are:

[0055]

[0056] Where a is the actual acceleration, delta t represents the time difference, w is the actual measured angular velocity, and The result of and is the relative pose change of i+1 relative to b k , the initial state and is 0, is a unit quaternion, i represents a certain time between the kth moment and the k+1th moment, and δt is the time interval between i and i+1.

[0057] The state of the sensor can be directly the observation of the sensor or the pose after data processing of the sensor. The present application adopts post fusion, so the corresponding state is the pose, that is, X [x, y, z, roll, pitch, yaw]. The time alignment of the wheel encoder is similar to the IMU, and the linear interpolation method of the above formula is also adopted, and thus the time calibration of the wheel encoder, the IMU and the laser radar is completed.

[0058] The motion between two three-dimensional space coordinate systems is rigid body motion, which can be represented by a three-dimensional column vector for translation and a 3x3 rotation matrix for rotation. The rotation change is left multiplication of the rotation matrix, and the translation change is matrix addition. In order to facilitate multiple calculations, a 4x4 augmented matrix can also be used to uniformly describe the position and attitude (pose) of the rigid body.

[0059] The purpose of space calibration is to unify the different installation positions of different sensors into one base coordinate system. According to the installation pose of each sensor, the Euclidean transformation of the coordinates is performed to obtain the translation matrix and the rotation matrix, so that the base coordinate system can be represented.

[0060] As shown in the accompanying drawings, Figure 3 W is the base coordinate system, and C is the coordinate system that needs to be aligned. The motion of the sensor on the trolley is a rigid body motion. During the rigid body motion, the direction angle of the modulus of the same vector in each coordinate system does not change. Assuming that the coordinates of the same vector a in two coordinate systems are [a1, a2, a3] T and [a′1, a′2, a′3] T , the unit orthogonal bases corresponding to the two coordinates are [e1, e2, e3] and [e′1, e′2, e′3], so the following formula is obtained:

[0061]

[0062] In order to reflect the relationship between the two coordinate systems, [e1, e2, e3] T is simultaneously left multiplied on both sides of the formula. According to the properties of the orthogonal matrix, the coefficient matrix on the left side becomes the unit matrix:

[0063]

[0064] In the formula, R is the rotation matrix. In Euclidean transformation, there is translation in addition to rotation. Then a′ can be obtained by one rotation and one translation of the vector a:

[0065] a' = Ra + t

[0066] Therefore, the Euclidean transformation relationship from coordinate system C to coordinate system W is:

[0067]

[0068] For easy calculation, the augmented matrix is changed to:

[0069]

[0070] In the formula, R WC is a 3x3 rotation matrix, representing the rotation from the C coordinate system to the W coordinate system, t WC is a 3x1 translation matrix, representing the translation from the center of the C coordinate system to the center of the W coordinate system, and specifically:

[0071]

[0072] In the formula, R X (α) is the rotation matrix of the C coordinate system rotating around the X axis by an angle α, R Y (β) is the rotation matrix rotating around the Y axis by an angle β, and in the formula, R Z (θ) is the rotation matrix rotating around the Z axis by an angle θ. From this, the time and space calibration before multi-sensor data fusion is completed, and a unified time node and space coordinate system representation method is obtained.

[0073] Step three, fuse the unified sensor data;

[0074] In the initial state, the predicted pose of the unmanned vehicle is set to 0, then one of the wheel encoder and IMU observation data arrives, and the other is linearly interpolated to obtain time-aligned pose observation data, then the data of the two are fused, and finally the optimal pose estimation is output.

[0075] Step four, determine the search area of the matching algorithm;

[0076] The brute force enumeration matching first inserts the current frame into the grid map, traverses all the poses, and at each pose, the current observation is placed into the map. The observation landmark is converted into a grid corresponding to each grid in the map to obtain a basic score. The score of all corresponding grids is the final score, and the highest score is output. The higher the score value, the more accurate the matching, so the highest score pose is the current optimal pose. The formula for enumerating the maximum score is as follows:

[0077]

[0078] In the formula, ξ * represents the current optimal pose, and T ξThe transformation matrix for transforming the observation data to the map coordinate system is z k The observation of the laser radar at time k is K, and K is the total time.

[0079] The back-end matching is different from the front-end matching. The front-end performs real-time matching within a subgraph, the observation within the subgraph is limited, the calculation amount of brute force enumeration is small, and it is also beneficial to more accurate pose estimation. The back-end matching is performed in all subgraphs. Although the back-end performs matching in the background and does not affect the real-time estimation of the front-end, as the estimation is performed, the subgraph and the observation become more and more, and the calculation amount of the method of traversing the entire map through brute force enumeration will increase exponentially, thereby consuming a large amount of computing power.

[0080] In order to solve the problems of large calculation amount and time consumption caused by brute force enumeration matching, the present application provides a method. That is, the matching is limited within a certain area, and then the matching time is further shortened. According to step three, the multi-source fusion pose information output is X=[x, y, z, q0, q1, q2, q3], wherein x, y, and z respectively represent the coordinate positions in the map coordinate system after coordinate transformation, q0 represents the real part of the quaternion, q1, q2, and q3 represent the imaginary parts of the quaternion, and the conversion into Euler angles is:

[0081]

[0082] θ=arcsin(-2(q1q3-q0q2)

[0083]

[0084] In the formula is the rotation around the Z axis, that is, the heading angle yaw; θ is the rotation around the Y axis, that is, the pitch angle pitch; and ψ is the rotation around the X axis, that is, the roll angle roll.

[0085] The guide area diagram provided by the present method is shown in the accompanying drawings Figure 4 , wherein the edge length of the discrete grid is represented by r. In the figure, P is the fusion estimated pose P(x', y', yaw), and according to the multi-source fusion output pose X=[x, y, z, q0, q1, q2, q3], the following can be obtained

[0086]

[0087] As shown in the figure, the grid area searched is:

[0088]

[0089] , wherein R is the deviation radius of the target area, δ is the deviation of the heading angle, start represents the starting point of the search, end represents the end point of the search, x represents the horizontal coordinate, y represents the vertical coordinate, and yaw represents the heading angle.

[0090] The pseudo code of the target search is as follows:

[0091]

[0092] In order to prevent the search area from falling into a local optimum, the method of the application also adds a matching hit ratio of the laser radar observation landmark grid and the map, that is, the ratio of the laser radar matching points to the total number of points. If the local matching rate is less than a threshold k, the search radius area is doubled, and the search heading angle is also doubled. Until the matching rate is higher than the threshold k, the highest score pose of the matching is output. By adding this constraint, the algorithm can find the optimal value in the local area faster, and can also prevent falling into a local minimum, thereby increasing the robustness of the algorithm.

[0093] The R of the target area in the code and the heading angle deviation are both taken as 3 times the standard deviation, because the error model satisfies the Gaussian distribution. Taking 3 times the standard deviation, as shown in the accompanying drawings Figure 5 As can be seen from the figure, the 3 times standard deviation range contains 99.73% of the cases. Combined with step 3, the variance can be obtained as:

[0094]

[0095] wherein σ represents the standard deviation, σ 2 represents the variance, represents the variance of the x data distribution, q0 represents the real part of the quaternion, q1, q2, and q3 represent the imaginary parts of the quaternion, represents the mutual influence of q0 on q1.

[0096] According to the conversion relationship of the quaternion to the Euler angle given by the above formula, combined with the known variance of the quaternion, the Jacobian matrix can be obtained by calculating the partial derivative of the Euler angle with respect to the quaternion as:

[0097]

[0098] In the formula represents the partial derivative of ψ with respect to q1, and the quaternion variance is separated from the total variance formula to obtain:

[0099]

[0100] The variance of the Euler angle Eular is obtained by multiplying the quaternion variance by the Jacobian matrix on the left and multiplying the transpose of the Jacobian matrix on the right, so:

[0101]

[0102] Therefore, the deviation radius R of the search target area and the heading angle deviation are:

[0103]

[0104] Although the 3 times of standard deviation covers 99.74% of the cases, in order to prevent other cases from occurring, the application additionally sets a constraint that if the matching rate of the laser radar observation point in this range is lower than the threshold k, the R is doubled until the matching rate is greater than the threshold k, and finally the output with the highest matching score is output, which is equivalent to 99.74% of the matching being searched in a small area only, thereby achieving the purpose of accelerating the search.

[0105] Step five, using the loop-back strategy based on the corner point feature to determine whether to loop back, if the loop back fails, return to step three, otherwise continue to execute;

[0106] After obtaining the optimal pose through step four, the current sensor observation data is matched with the previous observation, and the loop back detection is to determine whether the current observation is similar to the matched observation according to certain features, if the two observations are very similar, it is considered that the current observation and the matched observation have the same observation, so that the same observation can be used to construct constraints and solve the least squares, and then nonlinear optimization can be performed to make the pose of the unmanned vehicle more accurate.

[0107] In order to describe the similarity between the current observation and the previous observation, a mathematical description model needs to be constructed, and according to the corner point feature, the slope difference mutation point in the line is defined as the corner point, as shown in the accompanying Figure 6 Fig. 1, different scanning points of the laser radar for the same obstacle.

[0108] In the figure, point O is the position of the laser radar, δ θ is the angular resolution of the laser radar, Q1, Q2, Q3, Q4, Q5 are the scanning reflection points of the laser radar, d1, d2, d3, d4, d5 are the distances from the emission point of the laser radar to the reflection points, and the perpendicular lines are drawn from the reflection points Q to the adjacent points and the emission point, and the included angles are ω1, ω2, ω3, ω4, ω5 respectively. Because the angular resolution δ θ of the laser radar is small, it can be obtained that:

[0109]

[0110] Let the slope of the i-th point be k i , it can be obtained that:

[0111]

[0112] According to the set relationship of the accompanying Figure 6 , it can be obtained that:

[0113] tanω2-tanω1=tanδ θ (1+tanω2+tanω1)

[0114] Because δ θ is small, tan δ θ ≈0, so:

[0115] k3-k2≈k2-k1=tan ω2-tan ω1≈0

[0116] If these points are on a straight line, the slope difference is 0, but if there are additional Figure 7 line intersection points and the like, the slope difference will have a large mutation.

[0117] Additional Figure 7 slope distribution is shown in the attached Figure 8 , the i-th point is a slope mutation, resulting in a large fluctuation in the slope difference between i-1 and i points, when the difference is kept at 0, it means that the points are on a straight line, when the difference value changes sharply, the point is generally near the intersection point of the two straight lines of the obstacle, this method defines the point with a sharp change in slope difference as a corner point, and the corner point records the slopes of the left and right adjacent points and the slope of the point itself. When a corner point is the same, in addition to having the same slope mutation and its own slope, it also corresponds to the equal slopes of the adjacent two points. When two frames of observations have the same type of corner point to a certain number, the matching score will be higher than the set threshold, then it is considered that the two frames have the same observation, which is considered as a successful loop, otherwise it will continue to optimize from step three.

[0118] Step six, using sparse pose adjustment method to optimize the pose node of the unmanned vehicle;

[0119] Loop success will be optimized in the back end, the present application adopts the method of sparse pose adjustment (SPA), that is, the node is still the pose of the unmanned vehicle, but in addition to the key node (laser radar key frame), the subgraph node is also added, so as to better establish the constraint relationship between the subgraphs, the present application defines the subgraph as the accumulation of 100 frames of laser radar observation; The edge between two nodes is the relative pose constraint between the two nodes, which can be obtained through multi-information fusion, all constraints are constructed between the laser radar key frame node and the subgraph node, as shown in the attached Figure 9 , the actual nodes are on the trajectory of the unmanned vehicle, in order to better represent the constraints between the nodes, the attached Figure 9 shifts the points by a certain amount.

[0120] The positions of each node in the graph are determined sequentially during the graph construction process, and the constraints have a sequential relationship. In the scan matching, each keyframe is scanned and matched with two adjacent subgraphs. Thus, when these two subgraphs are constructed, the relative pose between the keyframe node and the two adjacent subgraphs can be obtained, and constraints between the two nearest subgraph nodes and the keyframe node can be established. Constraints between other subgraphs are implemented in the back-end loop closure detection. After each subgraph is constructed, it is matched with the previous keyframes in the back-end. If the matching score between the keyframe and a certain subgraph is greater than a set threshold, pose constraints are established between the keyframe and the matched subgraph. This completes the bidirectional search from keyframe nodes to subgraph nodes. The keyframe node and subgraph node matching are implemented at the front end of the real-time graph construction, and the subgraph node and keyframe node matching are performed by the back-end loop closure detection.

[0121] As attached Figure 9 In the backend, keyframe nodes and subgraph nodes undergo loop closure detection to obtain the relative pose between the current node and previous subgraphs. Loop closure constraints are then used to calculate the error between the estimated pose and the real-time observation. Therefore, the optimization task is to minimize the overall error. Assume the pose between the i-th subgraph node and the j-th keyframe node is z. ij The error term at this point is e. ij :

[0122] e ij =z ij -h(pose i pose j )

[0123] pose i and pose j Let h(pose) represent the world pose of the subgraph node and the keyframe node, respectively. i pose j () represents the coordinates of the j-th keyframe node after coordinate transformation in the i-th subgraph. During optimization, pose i and pose j As an optimization variable, z continuously approaches the true value during optimization. ij It is a constant. The closer the formula is to 0, the smaller the error, because the current observation is relatively accurate locally, while the estimate is continuously iterated. The existence of observation means that the pose... i and pose j It is not adjusted too much during the optimization process, hence it is called a constraint.

[0124] Let all the nodes be denoted as vector X:

[0125] X = [pose1,...,pose i ,...,pose Ns ,pose1,...,pose j ,...,pose Nn ]

[0126] In the formula, Nn represents the number of key nodes, and Ns represents the number of subgraph nodes. The above formula is recorded as the error of the kth state in all states, that is:

[0127] e k (X) = e ij (X) = e ij

[0128] Because the error is a 7-dimensional vector, that is, a rotation quaternion and a spatial position, the weight of each vector element in the error formula needs to be considered, and an information matrix is used to reflect the weight. The information matrix assumes that all error terms are subject to a Gaussian distribution with a mean of 0, that is, e k (x) ~ N(0, Ω k ), where Ω k represents the information matrix of the Gaussian distribution, also known as the precision matrix, and represents the inverse of the system covariance, which is set to the covariance of the multi-source fusion posterior probability at this time.

[0129] The 2-norm of the error vector is taken to represent the size of the error, so the error can be represented as: Therefore, the optimization problem can be simplified to the minimum of all errors, that is:

[0130]

[0131] From now on, through formula derivation, the optimization problem is changed into a mathematical model, and the optimization process is constructed through the solution of the mathematical formula.

[0132] The method uses an iterative solution method to solve the above mathematical model, takes the multi-source fusion pose as the initial value, then establishes an incremental equation, so that the initial value plus the increment reaches a minimum value, and the iteration stops when the increment is small enough.

[0133] The error term e k is first-order Taylor expanded around x to obtain:

[0134] e k (x+Δx) ≈ e k (x) + J k Δx

[0135] In the formula, Δx is the increment, J k is the Jacobian matrix, that is, e k(x) derivative with respect to x. The expanded formula is brought into the above mathematical model to obtain:

[0136]

[0137] According to the extreme value condition, the derivative is obtained and set to 0 to obtain

[0138]

[0139] The formula is simply denoted as:

[0140] H Δx = g

[0141]

[0142]

[0143] where H is called the Hessian matrix, and the above formula is a linear algebra equation in a regular form, and Δx can be solved by using linear algebra method. In the Gauss-Newton (G-N) optimization method, an increment Δx is solved in each iteration, then x = x + Δx, and one iteration is ended, when Δx is small enough, the iteration can be terminated, and x is the final result of optimization.

[0144] However, the Newton-Gauss optimization is a first-order Taylor expansion in the vicinity, and then the square of the error is solved, which is approximately the second-order Taylor expansion, which requires a higher confidence in the vicinity of the initial value x, that is, a trust region is added, and then

[0145] (H + λ · diagH) Δx = g

[0146] x' = x + Δx

[0147] In the incremental equation, λ is a Lagrange multiplier, set to be positive. Since H is a positive semi-definite matrix, its diagonal matrix diagH is also a positive definite matrix. Thus, as λ increases, Δx decreases, and vice versa. In one iteration, H and g remain constant. The goal of the iteration is to make the new error smaller than the previous error. If the calculated Δx is found to be too large, λ is appropriately increased (here, λ = 2λ), and the incremental equation is recalculated using the new λ to obtain a relatively smaller Δx. Conversely, if the calculated Δx is within a reasonable range, i.e., the overall error is smaller than the previous error, λ is appropriately decreased (here, λ = 0.5λ). The reduced λ will be used for the next iteration, which is equivalent to allowing Δx to have a larger value in the next iteration, in order to speed up the convergence as much as possible. This control method ensures both the stability and speed of convergence. Each iteration yields an increment Δx, which is then set to x = x + Δx to end the iteration. When Δx is sufficiently small, a threshold δ is set; if Δx falls below this threshold, the iteration terminates. The resulting x is the final optimized value. The algorithm flowchart is attached. Figure 10 As shown.

[0148] For the Hessian matrix in the above formula In practice, it is approximated by the square of the Jacobian matrix, which is the square of the first derivative of the error function. Therefore, to sparsify the Hessian matrix, we must first sparsify the Jacobian matrix. Here, the Jacobian matrix represents the derivative of the k-th error with respect to its increment, according to e k (x+Δx)≈e k (Δx)+J k From the definition of Δx, we can obtain e k It is only related to the i-th subgraph node and the j-th keyframe node, and its derivative with other nodes is 0, as shown in the appendix. Figure 11 The diagram shows the structural properties of the Jacobian matrix.

[0149] The Hessian matrix is ​​the sum of all error terms, as shown in the appendix. Figure 12 As shown, this represents the direct superposition of matrices. To better represent the sparsity of the Hessian matrix, this invention assumes a simplified case: assuming the graph optimization has 4 subgraph nodes and 8 keyframe nodes, a subgraph node only has constraints with its 4 nearest consecutive keyframe nodes, and the last keyframe node has a closure constraint with the first subgraph node. In this case, constraint edges are constructed, and the Hessian matrix structure is shown in the attached figure. Figure 13 As shown, the top left node is the subgraph node, and the others are keyframe nodes. Here, the specific formula for constraining the squared Jacobian is simplified and represented by the non-zero region in the figure.

[0150] The dark part in the figure is the constraint. As can be seen from the figure, the Hessian matrix has very good sparsity. In each specific area, there is a constraint only between the diagonals, that is, only in the case of the diagonal, the square of the Jacobian matrix exists. Other areas are 0, so Cholesky decomposition can be performed to express a symmetric positive definite matrix as the product of an upper triangular matrix U and its transpose A = UU T Simplify (H + λ · diagH) Δx = g to A Δx = g, that is:

[0151] U(U T Δx) = g

[0152] Because U is an upper triangular matrix, U T Δx can be quickly solved, and U T is a lower triangular matrix, so Δx can be solved row by row. Thus, the sparsity of the incremental equation in the iterative solution of the optimization problem is completed, the originally complex solving process is simplified to linear function solving, and the process of the back-end global optimization is accelerated.

[0153] Step seven, compare the brute force enumeration matching and the target area matching, and analyze the advantages of the target area matching;

[0154] First, analyze and compare the brute force enumeration matching and the target area matching. The computer configuration used in the experiment is: Intel i7-1180H, RTX3060, 16GB of memory. The experiment records the matching time of 1000 times of the brute force enumeration matching and the biased search scanning matching of the present application, respectively. The experimental results are as follows, the experimental result distribution graph is shown in the accompanying Figure 14 , and the experimental data is shown in Table 1.

[0155] Table 1

[0156]

[0157] From the experimental data result distribution can be seen, the average value of the matching time of the brute force enumeration method is about 0.11s, and the maximum value of the matching is even about 0.18s, and the frequency of the laser radar scanning is 10HZ, which means that every 0.1s will have a frame of laser radar observation data, and the time of the brute force enumeration processing is greater than 0.1s, so the problem of matching lag will appear, and the data distribution of the brute force enumeration algorithm is wide, which is reflected in the data as the standard deviation 0.01065660s. The average value of the matching of the bias search matching proposed in the application is about 0.0044s, and the maximum value of the matching is about 0.008s, which is much smaller than 0.1s, which improves the matching time of the brute force enumeration to a certain extent, and from the data distribution can be seen, the data of the bias search algorithm of the application is concentrated, and the standard deviation is 0.00168748s, that is, the target bias search proposed in the application greatly shortens the matching time in time, and the robustness of the algorithm is also stronger.

[0158] Step eight, visualize the unmanned vehicle back-end optimization structure on gazebo, and perform error analysis.

[0159] The global experiment this time is exactly the same as the experimental environment of step seven, and the constraints in the experiment are shown in the accompanying Figure 15 .

[0160] In the accompanying Figure 15 , the small coordinate axis represents the subgraph node, and it can be seen that there are many key frame nodes between the subgraph nodes, and the constraints between the front-end constructed subgraph nodes and the nearby key frame nodes are constructed, when the subgraph is constructed, a coordinate axis position is published in the graph and labeled, and then loop detection is performed with other key frame nodes, and if the loop detection is successful, the constraint is constructed, from the experimental results, it can be seen that the subgraph node and the key frame node in the subgraph loop more, and the key frame node in other subgraphs loop less, which also conforms to the actual situation, because the observation similarity of the key frame in the subgraph to the same landmark is higher, that is, the observation constraints of the nearby are more, and the observation constraints of the far are less. The optimized trajectory and the constraint between the subgraph node coordinates are shown in the accompanying Figure 16 .

[0161] In the accompanying Figure 16Fig. a is the trajectory and subgraph node coordinates after optimization, Fig. b is the edge constraint constructed after the loop of each subgraph node and key frame node is successful, Fig. c is the superposition experiment result of only the front end subgraph in the same experimental environment, and Fig. d is the global map based on SPA rear-end optimization. It can be obviously seen from the comparison between Fig. c and Fig. d that the global map after the rear-end optimization of the SPA optimization algorithm of the application has no obvious superposition, and the mapping effect is better. However, as can be seen from careful analysis of Fig. b, there is still a certain hysteresis in the optimization process, and it is difficult to achieve real-time optimization of the latest motion trajectory of the vehicle. This is a common problem of rear-end optimization. The optimization can only be completely performed after the subgraph construction is completed.

[0162] The above is the preferred embodiment of the application. Any change made according to the technical solution of the application, as long as the function generated does not exceed the scope of the technical solution of the application, belongs to the protection scope of the application.

Claims

1. A backend optimization method based on goal-guided biased search, characterized in that, Includes the following steps: Step 1: Collect data from the driverless vehicle using sensors; Step 2: Unify the sensor data information in terms of time and space; Step 3: Fuse the unified sensor data; Step 4: Determine the search area for the matching algorithm; Step 5: Use a loop closure strategy based on corner features to determine if a loop closure has occurred. If the loop closure fails, return to step 3; otherwise, continue execution. Step 6: Optimize the pose nodes of the unmanned vehicle using a sparse pose adjustment method; Step 7: Compare brute-force enumeration matching and target region matching, and analyze the advantages of target region matching; Step 8: Visualize the optimized backend structure of the autonomous vehicle on Gazebo and perform error analysis.

2. The backend optimization method based on goal-guided biased search according to claim 1, characterized in that, In step one, the sensor is an on-board sensor, including a lidar, an inertial measurement unit (IMU), and a wheel-side encoder.

3. The backend optimization method based on goal-guided biased search according to claim 1, characterized in that, Step two is implemented as follows: The timestamps of the LiDAR data releases are used as a benchmark for time unification. To ensure time alignment of data from various sensors, a linear interpolation method is employed. Assume t1 and t2 are the two most recent moments t when the LiDAR data arrives for the sensors requiring time alignment. Also, t1 is the largest number less than t, and t2 is the smallest number greater than t. The state at time t1 is x1, and the state at time t2 is x2. The data time of the LiDAR is t. Therefore, the linear interpolation formula for the state X of the sensor requiring alignment at time t is: The state corresponding to the sensor is the pose, namely X[x,y,z,roll,pitch,yaw], where x, y, and z represent the coordinate positions in the map coordinate system after coordinate transformation, and roll, pitch, and yaw are the roll angle, pitch angle, and yaw angle, respectively. The purpose of space calibration is to unify the different installation positions of different sensors to a base coordinate system. According to the installation pose of each sensor, the Euclidean transformation of coordinates is carried out to obtain the translation matrix and rotation matrix of the transformation, so that it can be represented in the base coordinate system; let W be the base coordinate system and C be the coordinate system that needs to be aligned; the motion of the sensor on the unmanned vehicle is a rigid body motion, and during the rigid body motion, the direction angle of the modulus of the same vector in each coordinate system does not change; suppose the coordinates of the same vector a in two coordinate systems are [a1, a2, a3] T and [a'1, a'2, a'3] T The unit orthogonal bases corresponding to the two coordinates are [e1, e2, e3] and [e'1, e'2, e'3], so the following formula is obtained: To reflect the relationship between the two coordinate systems, multiply both sides of the equation by [e1, e2, e3] on the left T According to the properties of orthogonal matrices, the coefficient matrix on the left becomes the identity matrix: In the formula, R is the rotation matrix. In Euclidean transformations, in addition to rotation, there is also translation. Therefore, vector a undergoing one rotation and one translation to obtain a′ can be expressed as: a′=Ra+t Therefore, the Euclidean transformation relationship from coordinate system C to coordinate system W is: For ease of calculation, the augmented matrix is ​​changed to: R WC is a 3x3 rotation matrix, representing the rotation from the C frame to the W frame, t WC is a 3x1 translation matrix, representing the translation from the center of the C frame to the center of the W frame, specifically: R X (α) is a rotation matrix for rotating the C coordinates by an angle a about the X axis, R Y (β) is a rotation matrix for rotating about the Y axis by an angle β, R Z (θ) is a rotation matrix for rotating about the Z axis by an angle θ; This completes the time and space calibration of the sensor.

4. The backend optimization method based on goal-guided biased search according to claim 3, characterized in that, In step three, the predicted pose of the autonomous vehicle is initially set to 0. The system waits for observation data from one of the wheel-side encoders and the inertial measurement unit (IMU) to arrive, while the other is linearly interpolated to obtain time-aligned pose observation data. The two sets of data are then fused together, and the optimal pose estimate is finally output.

5. The backend optimization method based on goal-guided biased search according to claim 1, characterized in that, Step four is implemented as follows: The brute-force enumeration matching first inserts the current frame into the grid map, iterates through all poses, and places the current observation into the map for each pose. The observed landmarks are converted into grids, and each grid in the map corresponds to a base score. The scores of all corresponding grids are the final score. Then, the pose with the highest score is output. The higher the score, the more accurate the match. Therefore, the pose with the highest score is the current optimal pose. The formula for enumerating and calculating the maximum score is as follows: ξ * represents the current optimal pose, T ξ is the transformation matrix that transforms the observation data to the map coordinate system, z k is the observation of the lidar at time k, and K is the total time. The multi-source fused pose information output from step three is X = [x, y, z, q0, q1, q2, q3], where x, y, and z represent the coordinate positions in the map coordinate system after coordinate transformation, q0 represents the real part of the quaternion, and q1, q2, and q3 represent the imaginary parts of the quaternion. Converted to Euler angles, it is: in the formula θ is the rotation about the Z-axis, i.e., the yaw angle; θ is the rotation about the Y-axis, i.e., the pitch angle; ψ is the rotation about the X-axis, i.e., the roll angle. The discrete grid side length is denoted by r. P is used as the fusion estimated pose P(x′,y′,yaw). Based on the multi-source fusion pose information, the output X = [x,y,z,q0,q1,q2,q3] is obtained. Therefore, the search grid area is: Where R is the deviation radius of the target area, δ is the heading angle deviation, the subscript start indicates the search start point, the subscript end indicates the search end point, x represents the x-coordinate, and y represents the y-coordinate; Both R and δ are taken as 3 times the standard deviation. The total variance is: Where σ represents the standard deviation, σ 2 Represents variance. Let x represent the variance of the data distribution, q0 represent the real part of the quaternion, q1, q2, q3 represent the imaginary part of the quaternion, and σ represent the real part of the quaternion. q0 σ q1 This represents the mutual influence of q0 on q1; The above formula provides the conversion relationship from quaternions to Euler angles. Combined with the known variance of the quaternions, the Jacobian matrix is ​​obtained by calculating the partial derivatives of Euler angles with respect to quaternions: in the formula Representing the partial derivative of ψ with respect to q1, we extract the quaternion variance from the total variance formula to obtain: The variance of Euler angles (Eular) is obtained by left-multiplying the quaternion variance by the Jacobian matrix and right-multiplying it by the transpose of the Jacobian matrix, thus: in, Expression Variance of direction Describe the variance in the direction of θ; Therefore, the deviation radius R and heading angle deviation δ of the search target area are: An additional constraint is set: if the matching rate of a lidar observation point is lower than the threshold к within this range, R is doubled until the matching rate is greater than the threshold к, and finally the output with the highest matching score is selected.

6. The backend optimization method based on goal-guided biased search according to claim 1, characterized in that, Step five is implemented as follows: A mathematical description model is constructed, that is, according to the corner point feature, a slope difference mutation point in a line is defined as a corner point, a point O is a position of the laser radar, δ θ is an angle resolution of the laser radar, Q1, Q2, Q3, Q4 and Q5 are scanning reflection points of the laser radar, d1, d2, d3, d4 and d5 are distances from a transmitting point of the laser radar to the reflection points, a perpendicular line is drawn from the reflection point Q to a connecting line of adjacent points and the transmitting point, and angles are ω1, ω2, ω3, ω4 and ω5 respectively, because the angle resolution δ θ of the laser radar is small, so the following can be obtained: Let the slope of the ith point be k i , we get: According to set relations: tan ω2 - tan ω1 = tan δ θ (1 + tan ω2 + tan ω1) Because δ θ is small, tan δ θ ≈0, so: k3-k2≈k2-k1=tanω2-tanω1≈0 If these points are on a straight line, the slope difference will be 0. However, if there are intersections of straight lines, the slope difference will change abruptly. Points with such abrupt changes in slope difference are defined as corner points, and the slope of the left and right adjacent points and the slope of the corner point are recorded. When two corner points are the same, in addition to having the same slope change and its own slope, the slopes of the two adjacent points are also equal. When two frames of observation have the same type of corner points, the matching score will be higher than the set threshold. Then, it is considered that the two frames have the same observation, and the loop closure is successful. Otherwise, return to step three to continue optimization.

7. The backend optimization method based on goal-guided biased search according to claim 1, characterized in that, Step six is ​​implemented as follows: The sparse pose adjustment method involves selecting nodes that represent the pose of the autonomous vehicle, but in addition to key nodes (i.e., LiDAR keyframes), subgraph nodes are added to better establish the constraint relationships between subgraphs. A subgraph is defined as the sum of 100 LiDAR observations. The edge between two nodes represents the relative pose constraint between them, obtained through multivariate information fusion. All constraints are constructed between LiDAR keyframe nodes and subgraph nodes. In reality, each node is on the trajectory of the autonomous vehicle. To better represent the constraints between nodes, each point is offset. Keyframe nodes and subgraph nodes are detected for loop closure in the backend to obtain the relative pose between the current node and the previous subgraph. The error between the estimated pose and the real-time observation is obtained through loop closure constraints. Therefore, the optimization task is to minimize the overall error. Let the pose between the ith subgraph node and the jth keyframe node be z ij The error term at this point is e ij : e ij = z ij - h(pose i , pose j ) where pose i and pose j are the world poses of the subgraph nodes and keyframe nodes respectively, h(pose i , pose j ) is the coordinate of the jth keyframe node in the ith subgraph after coordinate transformation, and pose i and pose j are the optimization variables that are constantly approaching the true values in the optimization, while z ij is a constant, and the closer the formula is to 0, the smaller the error, because the current observation is relatively accurate within the local area, and the estimate is constantly recursively obtained, and the existence of the observation means that pose i and pose j are not adjusted too much in the optimization process, and are therefore referred to as constraints. Let all the nodes be denoted as vector X: X=[pose1,...,pose i ,...,pose Ns ,pose1,...,pose j ,...,pose Nn ] In the formula, Nn represents the number of key nodes, and Ns represents the number of nodes in the subgraph. The above formula is denoted as the error of the k-th state out of all states, i.e.: And k (X)=e ij (X)=e ij Since the error is a 7-dimensional vector, consisting of a rotation quaternion and a spatial position, it's necessary to consider the weights of different elements in the error formula. Therefore, an information matrix is ​​used to represent these weights. The information matrix assumes that all error terms follow a Gaussian distribution with a mean of 0, i.e., e^(-1 / 2)( ... k (x)~N(0,Ω k ), where the information matrix representing the Gaussian distribution is also known as the precision matrix, and the inverse of the system covariance is represented by the covariance set as the covariance of the posterior probability after multi-source fusion. We use the 2-norm of the error vector to represent the magnitude of the error, thus representing the error as: Therefore, the optimization problem can be simplified to minimizing all errors, that is: From then on, the optimization problem was transformed into a mathematical model through formula derivation, and the optimization process was constructed by solving the mathematical formula. The above mathematical model is solved by an iterative solution method. The pose of the multi-source fusion is used as the initial value. Then, an incremental equation is established so that the initial value plus this increment reaches the minimum value. The iteration stops when the increment is small enough. Error term e k A first-order Taylor expansion near x yields: e k (x+Δx)≈e k (x)+J k Δx In the formula, Δx is the increment, J k It is a Jacobian matrix, i.e., e k The derivative of (x) with respect to x; substituting the expanded formula into the mathematical model above, we get: Based on the extremum condition, taking the derivative and setting it to 0 yields... The formula can be simplified as follows: H·Δx=g H is called the Hessian matrix. The above formula uses linear algebra to solve for Δx. In the Gauss-Newton optimization method, each iteration will solve for an increment Δx. Then, let x = x + Δx to end one iteration. When Δx is small enough, the iteration will terminate. At this time, x is the final result of the optimization. However, Newton-Gaussian optimization performs a first-order Taylor expansion in the vicinity and then calculates the square of the error, which approximates a second-order Taylor expansion. This requires a higher confidence level in the vicinity of the initial value x, i.e., adding a confidence region, thus yielding: (H+λ·diagH)Δx=g x′=x+Δx In the incremental equation, λ is a Lagrange multiplier, set to a positive number; since H is a positive semi-definite matrix, its diagonal matrix diagH is a positive definite matrix; thus, when λ increases, Δx decreases, and conversely, when λ decreases, Δx increases; in one iteration, H and g remain unchanged, and the goal of the iteration is to make the new error smaller than the previous error. If the calculated Δx is found to be too large, λ is increased, here set to λ = 2λ; conversely, if the calculated Δx is within a reasonable range, i.e., the overall error is smaller than the previous error, λ is decreased, here set to λ = 0.5λ; each iteration will obtain an incremental Δx, and then let x = x + Δx to end one iteration. When Δx is less than the threshold δ, the iteration terminates, and x at this time is the final result of optimization; For the Hessian matrix in the above formula In practice, it is approximated by the square of the Jacobian matrix, which is the square of the first derivative of the error function. Therefore, to sparsify the Hessian matrix, we must first sparsify the Jacobian matrix. Here, the Jacobian matrix represents the derivative of the k-th error with respect to its increment, according to e k (x+Δx)≈e k (Δx)+J k From the definition of Δx, we can obtain e k It is only related to the i-th subgraph node and the j-th keyframe node, and its derivative with other nodes is 0.

Images