Posture degradation detection and optimization method, device, equipment and storage medium

Abstract

The present application discloses a posture degradation detection and optimization method, device, equipment and storage medium, which relates to the field of posture measurement technology, and discloses a posture degradation detection and optimization method, including: obtaining laser point cloud information and vehicle inertial information, and performing Kalman filter fusion according to the laser point cloud information and the inertial information to obtain the posture and posture covariance matrix; calculating the trace of the posture covariance matrix according to the posture covariance matrix, and detecting posture degradation according to the trace of the posture covariance matrix; when posture degradation is detected, obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix, and obtaining the degradation direction according to the eigenvalues ​​and eigenvectors; determining the posture according to the degradation direction and the posture to confirm degradation; when confirming that the posture is degraded, optimizing the posture according to the degradation direction and the posture covariance matrix. By decomposing the eigenvalues ​​of the posture covariance matrix and adjusting the eigenvalues ​​in the degradation direction, the positioning accuracy and the robustness of the system are improved.

Description

Technical Field

[0001] The present application relates to the field of posture measurement technology, and in particular to posture degradation detection and optimization methods, devices, equipment and storage media. Background Art

[0002] In autonomous driving and high-precision navigation, the fusion method of LiDAR and inertial measurement unit is widely used for vehicle pose estimation to achieve accurate measurement of vehicle position and attitude. However, in environments with single geometric features or lack of significant geometric structures, such as highways or country roads, the feature point matching ability of LiDAR is significantly reduced, resulting in the degradation of the covariance matrix in the pose estimation process. The degradation will significantly increase the uncertainty of the vehicle's pose estimation in certain directions, thereby affecting the accuracy and stability of the overall positioning.

[0003] Traditional methods lack versatility in different environments. There are large differences in the posture constraints obtained by the lidar posture observation in different environments. Therefore, different degradation detection thresholds are often required to detect degradation in different environments. If the surrounding environmental structure changes significantly during the vehicle's driving, such as from urban roads to country roads, using the same degradation detection threshold in different environments will lead to waste of posture constraint information. At the same time, this method only analyzes the possibility of posture degradation in the direction without constraints through posture constraints, and performs corresponding optimization processing without confirming whether the final output posture is actually degraded. Due to the various reasons for posture degradation, the optimization processing method may not be able to effectively correct the posture.

[0004] The above contents are only used to assist in understanding the technical solution of the present application and do not constitute an admission that the above contents are prior art. Summary of the invention

[0005] The main purpose of this application is to provide a posture degradation detection and optimization method, device, equipment and storage medium, aiming to solve the technical problems that the degradation detection threshold is not universal enough in different environments, and the optimization processing is performed without confirming the posture degradation, resulting in limited posture optimization effect.

[0006] To achieve the above objectives, the present application proposes a posture degradation detection and optimization method, the method comprising:

[0007] Acquire laser point cloud information and inertial information of the vehicle, and perform Kalman filter fusion according to the laser point cloud information and the inertial information to obtain a posture and a posture covariance matrix;

[0008] Calculating a trace of a pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix;

[0009] When posture degradation is detected, eigenvalues ​​and eigenvectors are obtained according to the posture covariance matrix, and degradation directions are obtained according to the eigenvalues ​​and the eigenvectors;

[0010] Determine the posture according to the degradation direction and the posture to perform degradation confirmation;

[0011] When it is confirmed that the posture is degraded, posture optimization is performed according to the degradation direction and the posture covariance matrix.

[0012] In one embodiment, when it is confirmed that the posture is degraded, the step of performing posture optimization according to the degradation direction and the posture covariance matrix includes:

[0013] When it is confirmed that the posture is degraded, a first posture covariance matrix and a second posture covariance matrix are obtained according to the posture covariance matrix;

[0014] Calculating a first projection value according to the first pose covariance matrix and the degenerate direction;

[0015] Calculating a second projection value according to the second pose covariance matrix and the degenerate direction;

[0016] Comparing the first projection value and the second projection value to obtain a projection value difference;

[0017] Perform posture optimization according to the projection value difference, the first posture covariance matrix, the second posture covariance matrix, the degenerate direction, the first projection value and the second projection value.

[0018] In one embodiment, the step of performing posture optimization according to the projection value difference, the first posture covariance matrix, the second posture covariance matrix, the degradation direction, the first projection value and the second projection value includes:

[0019] Get the preset projection threshold;

[0020] When the projection value difference is less than or equal to the preset projection threshold, calculating the target eigenvalue according to the first posture covariance matrix, the second posture covariance matrix, the degradation direction, the first projection value and the second projection value;

[0021] The eigenvalues ​​of the second posture covariance matrix are adjusted according to the target eigenvalues ​​to complete posture optimization.

[0022] In one embodiment, when posture degradation is detected, the steps of obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix, and obtaining degradation directions according to the eigenvalues ​​and the eigenvectors include:

[0023] When posture degradation is detected, eigenvalue decomposition is performed according to the posture covariance matrix to obtain eigenvalues ​​and eigenvectors;

[0024] Sorting the eigenvalues ​​according to a preset arrangement strategy to obtain an eigenvalue sorting;

[0025] Calculating, according to the eigenvalues ​​and the eigenvectors, eigenvalue ratios of adjacent eigenvalues ​​in the eigenvalue sorting;

[0026] The posture degradation direction is calculated according to the eigenvalue ratio.

[0027] In one embodiment, the step of determining the posture according to the degradation direction and the posture to perform degradation confirmation includes:

[0028] Get the preset posture degradation angle threshold;

[0029] Obtaining a first posture and a second posture according to the posture;

[0030] Calculating a posture degradation angle according to the degradation direction, the first posture and the second posture;

[0031] When the posture degradation angle is less than the preset posture degradation angle threshold, it is determined that the posture is degraded.

[0032] In one embodiment, the step of calculating the trace of the pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix includes:

[0033] Obtain a first pose covariance matrix and a second pose covariance matrix according to the pose covariance matrix;

[0034] Calculating the trace of the first pose covariance matrix and the second pose covariance matrix to obtain the trace of the first pose covariance matrix and the trace of the second pose covariance matrix;

[0035] Pose degradation is detected based on the trace of the first pose covariance matrix and the trace of the second pose covariance matrix.

[0036] In one embodiment, the step of detecting posture degradation according to the trace of the first posture covariance matrix and the trace of the second posture covariance matrix comprises:

[0037] Obtaining a preset degradation threshold;

[0038] Calculating a degradation index based on a trace of the first pose covariance matrix and a trace of the second pose covariance matrix;

[0039] When the degradation index is greater than the preset degradation threshold, posture degradation is detected.

[0040] In addition, to achieve the above purpose, the present application also proposes a posture degradation detection and optimization device, which includes: an acquisition module, used to acquire laser point cloud information and inertial information of the vehicle, and perform Kalman filter fusion according to the laser point cloud information and the inertial information to obtain the posture and posture covariance matrix;

[0041] A calculation module, used for calculating the trace of the pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix;

[0042] A detection module, for obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix when posture degradation is detected, and obtaining a degradation direction according to the eigenvalues ​​and the eigenvectors;

[0043] A determination module, used for determining whether the posture is degraded according to the degradation direction and the posture;

[0044] An optimization module is used to optimize the posture according to the degradation direction and the posture covariance matrix when it is determined that the posture is degraded.

[0045] In addition, to achieve the above-mentioned objectives, the present application also proposes a posture degradation detection and optimization device, which includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, and the computer program is configured to implement the steps of the posture degradation detection and optimization method as described above.

[0046] In addition, to achieve the above-mentioned purpose, the present application also proposes a storage medium, which is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by the processor, the steps of the posture degradation detection and optimization method described above are implemented.

[0047] In addition, to achieve the above-mentioned purpose, the present application also provides a computer program product, which includes a computer program, and when the computer program is executed by a processor, it implements the steps of the posture degradation detection and optimization method described above.

[0048] One or more technical solutions proposed in this application have at least the following technical effects:

[0049] By adopting the Kalman filter fusion technology of laser point cloud information and vehicle inertial information, the posture and posture covariance matrix are obtained, and the trace value of the covariance matrix is ​​used as a unified indicator for degradation detection, without setting different degradation detection thresholds for different environments. In addition, by performing eigenvalue decomposition on the posture covariance matrix, analyzing the degradation direction in combination with the eigenvalue and eigenvector, and optimizing the constraints on the degradation direction, the technical problems of insufficient universality of degradation detection thresholds in different environments and limited posture optimization effect caused by optimizing without confirming posture degradation are solved. Compared with the prior art, the present invention improves the accuracy and efficiency of posture degradation detection and optimization in complex environments, and achieves the technical effects of unified environment adaptation and precise optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0051] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0052] Figure 1 A flow chart of the first embodiment of the posture degradation detection and optimization method of the present application is provided;

[0053] Figure 2 A flow chart of the second embodiment of the posture degradation detection and optimization method of the present application is provided;

[0054] Figure 3 A brief flowchart of the posture degradation detection and optimization method provided in Example 2 of the present application;

[0055] Figure 4 This is a schematic diagram of the module structure of the posture degradation detection and optimization device according to an embodiment of the present application;

[0056] Figure 5 Schematic diagram of the device structure of the hardware operating environment involved in the posture degradation detection and optimization method in the embodiment of the present application.

[0057] The purpose, features and advantages of this application will be further described in conjunction with the embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0058] It should be understood that the specific embodiments described herein are only used to explain the technical solutions of the present application and are not used to limit the present application.

[0059] In order to better understand the technical solution of the present application, a detailed description will be given below in conjunction with the accompanying drawings and specific implementation methods.

[0060] The main solution of the embodiment of the present application is: obtaining laser point cloud information and inertial information of the vehicle, and performing Kalman filtering fusion based on the laser point cloud information and the inertial information to obtain posture and posture covariance matrix; calculating the trace of the posture covariance matrix based on the posture covariance matrix, and detecting posture degradation based on the trace of the posture covariance matrix; when posture degradation is detected, obtaining eigenvalues ​​and eigenvectors based on the posture covariance matrix, and obtaining the degradation direction based on the eigenvalues ​​and the eigenvectors; determining the posture based on the degradation direction and the posture to confirm degradation; when it is confirmed that the posture has degraded, optimizing the posture based on the degradation direction and the posture covariance matrix.

[0061] In this embodiment, for ease of description, the following description is made using the posture degradation detection and optimization device as the execution subject.

[0062] Since the prior art lacks universality of degradation detection thresholds in different environments, and optimization processing is performed without confirming posture degradation, resulting in limited posture optimization effects, the present application provides a solution, which uses the Kalman filter fusion technology of laser point cloud information and vehicle inertial information to obtain posture and posture covariance matrix, and uses the trace value of the covariance matrix as a unified indicator for degradation detection, without setting different degradation detection thresholds for different environments. In addition, by performing eigenvalue decomposition on the posture covariance matrix, analyzing the degradation direction in combination with eigenvalues ​​and eigenvectors, and optimizing the constraints of the degradation direction, the technical problems of insufficient universality of degradation detection thresholds in different environments and limited posture optimization effects due to optimization processing without confirming posture degradation are solved. Compared with the prior art, the present invention improves the accuracy and efficiency of posture degradation detection and optimization in complex environments, and achieves the technical effects of unified environment adaptation and precise optimization.

[0063] It should be noted that the execution subject of this embodiment can be a computing service device with data processing, network communication and program running functions, such as a tablet computer, a personal computer, a mobile phone, etc., or an electronic device capable of realizing the above functions, a posture degradation detection and optimization device, etc. The following takes the posture degradation detection and optimization device as an example to illustrate this embodiment and the following embodiments.

[0064] Based on this, the present application embodiment provides a posture degradation detection and optimization method, referring to Figure 1 , Figure 1 This is a flow chart of the first embodiment of the posture degradation detection and optimization method of the present application.

[0065] In this embodiment, the posture degradation detection and optimization method includes steps S10 to S50:

[0066] Step S10, acquiring laser point cloud information and inertial information of the vehicle, and performing Kalman filter fusion according to the laser point cloud information and the inertial information to obtain a posture and a posture covariance matrix;

[0067] It should be noted that laser point cloud information is the three-dimensional data of the environment collected by laser radar. Laser radar can determine the distance and shape of surrounding objects by emitting lasers and measuring the time and angle of reflected light, thereby generating a point cloud. The three-dimensional data of the environment includes the spatial coordinates of each point in the environment. These points and their coordinate information constitute a point cloud set. Point cloud information provides the geometric features of the environment around the vehicle and is often used for map construction and feature matching in pose estimation.

[0068] In addition, it should be noted that inertial information is provided by an inertial measurement unit (IMU), including but not limited to information such as the vehicle's linear acceleration and angular velocity, which is used to estimate the vehicle's motion state. The IMU can measure the vehicle's motion parameters through a built-in accelerometer and gyroscope. For example, the IMU can measure the acceleration and angular velocity of the vehicle during acceleration, deceleration or turning. These data are used to infer the vehicle's instantaneous position and direction in a short period of time.

[0069] It is understood that the Kalman filter is a recursive algorithm for extracting state information from multi-source sensor data while reducing the impact of noise. In this embodiment, an extended Kalman filter (EKF) can be used for data extraction and fusion. EKF is a nonlinear system state estimation method that is typically used to fuse multiple sensor data for precise positioning in complex or noisy environments. It is an extended version of the standard linear Kalman filter that can effectively process nonlinear data. EKF is often used to fuse different information from sensors such as IMU and lidar to achieve accurate position and attitude estimation of vehicles or robots.

[0070] In addition, it should be noted that the pose refers to a comprehensive description of the position and attitude of an object in space. The position usually indicates the coordinates of the object in space, which corresponds to the position of the object in the front, back, left, right, and up and down directions. The pose indicates the orientation or rotation state of an object. For example, when a car is driving on the road, its pose describes the specific position of the vehicle on the road and the orientation of the front of the vehicle. The pose covariance matrix is ​​a mathematical tool used to quantify the uncertainty in pose estimation and the correlation between different components. The uncertainty can reflect the accuracy of the pose estimation in each direction, and the correlation can reflect the degree of association between different directions or angles. For example, the position change in the front and back direction may be related to the change in the pitch angle. A larger value indicates that the estimation uncertainty in this direction is high, and a smaller value indicates that the estimation accuracy is high. The pose covariance matrix is ​​usually a 6×6 symmetric matrix. By analyzing the pose covariance matrix, the accuracy and reliability of the pose estimation can be evaluated, so that corresponding adjustments and optimizations can be made in applications such as navigation and positioning.

[0071] In addition, it can be understood that the laser point cloud information provides low-frequency but high-precision environmental feature data, and the inertial information provides high-frequency but short-term reliable motion prediction data. The Kalman filter combines the two and can generate accurate posture through prediction and updating.

[0072] Step S20, calculating a trace of a pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix;

[0073] It should be noted that the trace of the matrix refers to the sum of the diagonal elements of the matrix, and the diagonal elements of the pose covariance matrix are the variances in each direction (position and attitude), indicating the uncertainty in these directions.

[0074] It can be understood that posture degradation refers to the lack of constraints on the vehicle in certain directions, resulting in a decrease in the accuracy of posture estimation. In the posture covariance matrix, it can be manifested as an increase in the trace value of the covariance matrix.

[0075] Step S30, when posture degradation is detected, obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix, and obtaining degradation directions according to the eigenvalues ​​and the eigenvectors;

[0076] It should be noted that the eigenvalue is a scalar that represents the uncertainty strength of the covariance matrix in a specific direction. The larger the eigenvalue, the higher the uncertainty in that direction, that is, the weaker the constraint. The smaller the eigenvalue, the lower the uncertainty in that direction, that is, the stronger the constraint. The eigenvector is a direction vector that represents the main change direction of the covariance matrix corresponding to a certain eigenvalue in space.

[0077] In addition, it should be noted that the degenerate direction is the direction in which the positioning system needs to strengthen constraints most in space, providing a target for subsequent optimization. It is usually the eigenvector corresponding to the maximum eigenvalue in the covariance matrix, indicating the direction with the weakest posture constraint and the greatest uncertainty.

[0078] In a feasible implementation, step S30 may include steps S31 to S34:

[0079] Step S31, when posture degradation is detected, eigenvalue decomposition is performed according to the posture covariance matrix to obtain eigenvalues ​​and eigenvectors;

[0080] It should be noted that eigenvalue decomposition is a linear algebraic operation that decomposes the covariance matrix into eigenvalues ​​and eigenvectors. The eigenvalue represents the uncertainty intensity of the covariance matrix in the corresponding direction. A larger eigenvalue indicates that the constraint in that direction is weaker and the uncertainty is higher. The eigenvector describes the direction corresponding to the eigenvalue, which is the main axis direction of the covariance matrix in space. Each eigenvector corresponds to an eigenvalue, which is used to clarify the degradation direction.

[0081] It is understandable that eigenvalue decomposition can be performed through principal component analysis (PCA). PCA is a statistical method for dimensionality reduction and data feature extraction. Its purpose is to find the main change directions of data in high-dimensional data and project the data onto these directions to reduce the dimension of the data while retaining the main information of the original data as much as possible. The eigenvalues ​​and eigenvectors are obtained as follows:

[0082] (Formula 1)

[0083] In the formula, is the pose covariance matrix, is the eigenvector matrix, is the eigenvalue matrix, , as well as are the eigenvectors, , as well as are the eigenvalues ​​respectively.

[0084] Step S32, sorting the eigenvalues ​​according to a preset arrangement strategy to obtain an eigenvalue sorting;

[0085] It should be noted that the preset arrangement strategy is a rule for the arrangement order. In this embodiment, the preset arrangement strategy is to arrange the three eigenvalues ​​in order from large to small. Eigenvalue sorting means sorting the eigenvalues ​​according to the preset arrangement strategy. The sorted eigenvalues ​​help to quickly identify the direction with the strongest uncertainty, that is, the maximum eigenvalue, and the direction with the weakest uncertainty, that is, the minimum eigenvalue, in order.

[0086] Step S33, calculating the eigenvalue ratio of adjacent eigenvalues ​​in the eigenvalue sorting according to the eigenvalue and the eigenvector;

[0087] It should be noted that the eigenvalue ratio is the ratio of adjacent eigenvalues ​​after sorting, which indicates the degree of difference in uncertainty in different directions.

[0088] Step S34, calculating the posture degradation direction according to the eigenvalue ratio.

[0089] It can be understood that the calculation of the degradation direction is as follows:

[0090] (Formula 2)

[0091] In the formula, is the degradation direction, is the threshold value, , as well as are the eigenvectors, , as well as are the eigenvalues ​​respectively. It is to regularize the vector.

[0092] Step S40, determining the posture according to the degradation direction and the posture to perform degradation confirmation;

[0093] It is understandable that degradation confirmation is to verify whether the actual performance of the posture in the degradation direction meets the characteristics of degradation, so as to avoid misjudgment. It is necessary to further verify whether the motion state of the posture is affected by this direction, and to optimize after confirming that degradation has occurred, thereby reducing unnecessary calculations and improving resource utilization efficiency.

[0094] In a feasible implementation, step S40 may include steps S41 to S44:

[0095] Step S41, obtaining a preset posture degradation angle threshold;

[0096] It should be noted that the preset posture degradation angle threshold is a preset angle value, which is used to determine whether the angle between the posture change direction and the degradation direction is small enough so that the posture can be considered to have degraded. The preset posture degradation angle threshold can be determined based on experience and experiments to balance false positives and false negatives.

[0097] Step S42, obtaining a first posture and a second posture according to the posture;

[0098] It should be noted that the first pose is the pose calculated by the IMU, and the second pose is the pose obtained after the state vector is updated. These two poses represent the initial value and the optimized value of the pose estimation respectively.

[0099] Step S43, calculating a posture degradation angle according to the degradation direction, the first posture and the second posture;

[0100] It should be noted that the degradation angle is the angle between the posture change vector and the degradation direction, and the angle can be calculated by the dot product and modulus length between the vectors.

[0101] It can be understood that the calculation of the degradation angle is as follows:

[0102] (Formula 3)

[0103] In the formula, For the first position, For the second position, is the degradation direction, To find the magnitude of a vector.

[0104] Step S44, when the posture degradation angle is less than the preset posture degradation angle threshold, it is determined that the posture is degraded.

[0105] It can be understood that if the posture degradation angle calculated in step S43 is smaller than the posture degradation angle threshold preset in step S41, it means that the actual change direction of the posture is relatively consistent with the calculated degradation direction. At this time, it can be confirmed that the posture degradation is caused by the uneven distribution of posture constraints, and then subsequent posture optimization is performed.

[0106] Step S50: When it is confirmed that the posture is degraded, the posture is optimized according to the degradation direction and the posture covariance matrix.

[0107] It can be understood that the accuracy of pose estimation can be improved by adjusting the eigenvalues ​​of the pose covariance matrix in the degenerate direction to reduce the uncertainty in that direction.

[0108] In a feasible implementation, step S50 may include steps S51 to S55:

[0109] Step S51, when it is confirmed that the posture is degraded, obtaining a first posture covariance matrix and a second posture covariance matrix according to the posture covariance matrix;

[0110] It should be noted that the first pose covariance matrix refers to the pose covariance matrix output by the Kalman filter module in the iteration before the current pose estimation (i.e., the k-1th iteration). This matrix reflects the uncertainty of the pose estimation in the previous iteration. The second pose covariance matrix refers to the pose covariance matrix output by the Kalman filter module in the iteration of the current pose estimation (i.e., the kth iteration). This matrix reflects the uncertainty of the pose estimation in the current iteration.

[0111] It is understandable that the first pose covariance matrix and the second pose covariance matrix need to be decomposed into eigenvalues ​​and eigenvectors. The characteristic vector decomposition is as follows:

[0112] (Formula 4)

[0113] In the formula, and are the second pose covariance matrix and the first pose covariance matrix respectively, and are the eigenvector matrices of the second pose covariance matrix and the first pose covariance matrix, respectively. and are the eigenvalue matrices of the second pose covariance matrix and the first pose covariance matrix, respectively.

[0114] Step S52, calculating a first projection value according to the first posture covariance matrix and the degenerate direction;

[0115] Step S53, calculating a second projection value according to the second posture covariance matrix and the degenerate direction;

[0116] It should be noted that steps S52 and S53 are respectively for calculating the projection value of the first pose covariance matrix in the degenerate direction and the projection value of the second pose covariance matrix in the degenerate direction. The first projection value is the projection of the first pose covariance matrix in the degenerate direction, which can be calculated by multiplying the first pose covariance matrix with the outer product of the degenerate direction vector. The second projection value is the projection of the second pose covariance matrix in the degenerate direction, which can be calculated by multiplying the second pose covariance matrix with the outer product of the degenerate direction vector.

[0117] It can be understood that the calculation of the first projection value and the second projection value is as follows:

[0118] (Formula 5)

[0119] In the formula, and are the second pose covariance matrix and the first pose covariance matrix respectively, and are the eigenvector matrices of the second pose covariance matrix and the first pose covariance matrix, respectively. and are the eigenvalue matrices of the second pose covariance matrix and the first pose covariance matrix, respectively. and are the degenerate directions at time k and k-1 respectively, It means to find the sum of all elements in a vector.

[0120] Step S54, comparing the first projection value and the second projection value to obtain a projection value difference;

[0121] It can be understood that when the degradation is confirmed, the projection of the covariance of the pose in the degradation direction should increase, so by comparing the above two projection values, the projection value difference can be obtained.

[0122] Step S55, performing posture optimization according to the projection value difference, the first posture covariance matrix, the second posture covariance matrix, the degenerate direction, the first projection value and the second projection value.

[0123] It should be noted that the purpose of pose optimization is to reduce the uncertainty of pose estimation in the degraded direction, thereby improving the accuracy and robustness of pose estimation. By adjusting the pose covariance matrix, the pose estimation can be made more reliable, especially in environments with insufficient pose constraints.

[0124] In a feasible implementation, step S55 may include steps S551 to S553:

[0125] Step S551, obtaining a preset projection threshold;

[0126] It should be noted that the preset projection threshold is a predefined value used to determine when the pose covariance matrix needs to be adjusted. This threshold is determined based on experience and experiments to balance the sensitivity and stability of pose optimization. In this embodiment, the preset projection threshold is 0.

[0127] Step S552, when the projection value difference is less than or equal to the preset projection threshold, calculating the target eigenvalue according to the first posture covariance matrix, the second posture covariance matrix, the degradation direction, the first projection value and the second projection value;

[0128] It can be understood that if the projection value difference is less than or equal to the preset projection threshold, it means that no growth occurs after comparing the first projection value with the second projection value, and the target feature value needs to be calculated for posture optimization.

[0129] It should be noted that the target eigenvalue is calculated based on the first pose covariance matrix, the second pose covariance matrix, the degradation direction, the first projection value and the second projection value. These values ​​are used to adjust the second pose covariance matrix to increase the uncertainty in the degradation direction. The target eigenvalue is calculated as follows:

[0130] (Formula 6)

[0131] In the formula, represents the target feature value, represents the eigenvalue of the second pose covariance matrix, represents the function that diagonalizes a vector, and denote the first projection value and the second projection value respectively, is the second pose covariance matrix, is the eigenvector matrix of the second pose covariance matrix.

[0132] Step S553, adjusting the eigenvalues ​​of the second posture covariance matrix according to the target eigenvalues ​​to complete posture optimization.

[0133] It can be understood that the eigenvalues ​​of the second pose covariance matrix are adjusted according to the target eigenvalues ​​calculated in step S552. After adjusting the second pose covariance matrix, the state vector update step of the Kalman filter can be re-executed to complete the pose optimization.

[0134] This embodiment provides a posture degradation detection and optimization method. Through a technical means of a posture degradation detection and optimization method combining laser point cloud information and vehicle inertial information, the technical problem of reduced positioning accuracy caused by the degradation of the posture estimation of the laser radar odometer in a specific environment in an autonomous driving vehicle is solved, and the accuracy and robustness of the posture estimation are improved, ensuring that high-precision positioning can be achieved in an environment with insufficient posture constraints. The method first obtains the laser point cloud information and the inertial information of the vehicle, and uses Kalman filtering to fuse these information to obtain the posture and posture covariance matrix. Then, the posture degradation is detected by analyzing the trace of the posture covariance matrix, and when the posture degradation is detected, the degradation direction is further determined by eigenvalue decomposition. The posture degradation confirmation module calculates the degradation direction and determines whether degradation has occurred in the degradation direction. Combined with the subsequent degradation posture optimization method, it ensures that each degradation posture processing is effective and accurate, and improves the efficiency of the degradation processing and the robustness of the laser radar odometer. Finally, the pose is optimized according to the degraded direction and the pose covariance matrix, and the uncertainty in the degraded direction is reduced by adjusting the eigenvalues ​​in the pose covariance matrix, thereby improving the accuracy and robustness of the pose estimation. This series of steps ensures that the positioning performance of the lidar odometer can be improved by optimizing the pose covariance matrix even in an environment with insufficient pose constraints.

[0135] Based on the first embodiment of the present application, in the second embodiment of the present application, the same or similar contents as those in the above-mentioned embodiment 1 can be referred to the above introduction, and will not be repeated in the following. Figure 2 , step S20 of the posture degradation detection and optimization method includes steps S21 to S23:

[0136] Step S21, obtaining a first pose covariance matrix and a second pose covariance matrix according to the pose covariance matrix;

[0137] It should be noted that the first pose covariance matrix usually refers to the pose covariance matrix output by the Kalman filter module in the iteration before the current pose estimation, that is, the k-1th iteration. This matrix reflects the uncertainty of the pose estimation in the previous iteration. The second pose covariance matrix refers to the pose covariance matrix output by the Kalman filter module in the iteration of the current pose estimation (that is, the kth iteration). This matrix reflects the uncertainty of the pose estimation in the current iteration.

[0138] Step S22, calculating the trace of the first pose covariance matrix and the second pose covariance matrix to obtain the trace of the first pose covariance matrix and the trace of the second pose covariance matrix;

[0139] Step S23: Detecting posture degradation according to the trace of the first posture covariance matrix and the trace of the second posture covariance matrix.

[0140] It should be noted that step S22 and step S23 indicate that after calculating the trace of the matrix, posture degradation detection is performed based on the trace of the matrix.

[0141] It can be understood that by comparing the trace of the first pose covariance matrix and the trace of the second pose covariance matrix, it is possible to detect whether the pose has degraded. If the trace of the second pose covariance matrix is ​​significantly larger than the trace of the first pose covariance matrix, this may indicate that the uncertainty of the pose estimate has increased, that is, pose degradation has occurred.

[0142] In a feasible implementation, step S23 may include steps S231 to S233:

[0143] Step S231, obtaining a preset degradation threshold;

[0144] It should be noted that the preset degradation threshold is a predefined value used to determine whether the change in the trace of the pose covariance matrix is ​​large enough to consider that the pose estimation has degraded. This threshold is set based on system performance requirements, experimental data and experience.

[0145] Step S232, calculating a degradation index according to the trace of the first pose covariance matrix and the trace of the second pose covariance matrix;

[0146] It should be noted that the degradation index is a numerical index used to quantify the degree of degradation of pose estimation. The calculation of the degradation index is as follows:

[0147] (Formula 7)

[0148] In the formula, and Represent the first pose covariance matrix and the second pose covariance matrix respectively, and denote the trace of the first pose covariance matrix and the trace of the second pose covariance matrix, respectively.

[0149] Step S233: when the degradation index is greater than the preset degradation threshold, posture degradation is detected.

[0150] It is understandable that when the calculated index value is greater than a certain threshold, it is considered that the posture output by the Kalman filter module has the potential to degrade. Since the degradation detection index is calculated based on the change of posture uncertainty, a large change in posture uncertainty in any link may cause posture degradation. Therefore, a unified threshold can be set to detect posture degradation in different types of environments. The establishment of the threshold size will only affect the sensitivity of degradation detection.

[0151] This embodiment provides a posture degradation detection and optimization method. By comparing and analyzing the traces of two consecutive iterations of the posture covariance matrix and using the covariance of the posture to design the degradation detection index, the degradation detection problem caused by the increase in the uncertainty of the laser radar odometer posture estimation in a dynamically changing environment is solved. The posture covariance is a relatively general uncertainty description method. This method can be used to set the same threshold for degradation detection in different environments, so that the posture degradation can be effectively detected and identified under different environmental conditions, thereby improving the positioning accuracy of the autonomous driving vehicle and the system robustness. Through this method, timely detection can be performed when the posture uncertainty increases significantly, providing a basis for subsequent posture optimization, improving the versatility of degradation detection in different environments, and ensuring the navigation and positioning performance of the vehicle in various environments.

[0152] For example, in order to help understand the implementation process of the posture degradation detection and optimization method obtained by combining this embodiment with the above-mentioned embodiment 1, please refer to Figure 3 , Figure 3 A brief flowchart of a posture degradation detection and optimization method is provided, specifically:

[0153] First, in the extended Kalman filter module, the laser point cloud data and inertial information, i.e., IMU data, are received, and preliminary posture observation is performed through the laser point cloud. At the same time, the state vector is estimated using the IMU data. The two are fused through the extended Kalman filter to update the state vector, thereby generating the initial posture and covariance matrix of the vehicle. Next, the posture information enters the posture degradation processing module, which first performs posture degradation detection. If no degradation is detected, the vehicle posture is directly output; if degradation is detected, the posture degradation confirmation link is entered, and further judgment is made as to whether degradation actually occurs. If degradation is not confirmed, the posture is also output; if degradation is confirmed, the posture optimization phase is entered, and the constraint strength of the degradation direction is optimized by adjusting the covariance matrix, and finally the optimized posture information is output. The entire process ensures that the posture degradation problem can be dynamically detected, confirmed, and processed in complex scenarios, thereby providing more reliable vehicle positioning results.

[0154] It should be noted that the above examples are only used to understand the present application and do not constitute a limitation on the posture degradation detection and optimization method of the present application. More simple transformations based on this technical concept are all within the scope of protection of the present application.

[0155] This application also provides a posture degradation detection and optimization device, please refer to Figure 4 , the posture degradation detection and optimization device comprises:

[0156] An acquisition module 10 is used to acquire laser point cloud information and inertial information of the vehicle, and perform Kalman filter fusion according to the laser point cloud information and the inertial information to obtain a posture and a posture covariance matrix;

[0157] A calculation module 20, configured to calculate a trace of a pose covariance matrix according to the pose covariance matrix, and detect pose degradation according to the trace of the pose covariance matrix;

[0158] A detection module 30, for obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix when posture degradation is detected, and obtaining a degradation direction according to the eigenvalues ​​and the eigenvectors;

[0159] A determination module 40, configured to determine whether a posture is degraded according to the degradation direction and the posture;

[0160] The optimization module 50 is used to optimize the posture according to the degradation direction and the posture covariance matrix when it is determined that the posture is degraded.

[0161] The posture degradation detection and optimization device provided by the present application adopts the posture degradation detection and optimization method in the above-mentioned embodiment, which can solve the technical problems that the degradation detection threshold is not universal enough in different environments, and the optimization processing is performed without confirming the posture degradation, resulting in limited posture optimization effect. Compared with the prior art, the beneficial effects of the posture degradation detection and optimization device provided by the present application are the same as the beneficial effects of the posture degradation detection and optimization method provided by the above-mentioned embodiment, and the other technical features in the posture degradation detection and optimization device are the same as the features disclosed in the above-mentioned embodiment method, which will not be repeated here.

[0162] In one embodiment, the optimization module 50 is also used to obtain a first pose covariance matrix and a second pose covariance matrix according to the pose covariance matrix when it is confirmed that the pose has degenerated; calculate a first projection value according to the first pose covariance matrix and the degradation direction; calculate a second projection value according to the second pose covariance matrix and the degradation direction; compare the first projection value and the second projection value to obtain a projection value difference; and perform pose optimization according to the projection value difference, the first pose covariance matrix, the second pose covariance matrix, the degradation direction, the first projection value, and the second projection value.

[0163] In one embodiment, the optimization module 50 is also used to obtain a preset projection threshold; when the projection value difference is less than or equal to the preset projection threshold, the target eigenvalue is calculated according to the first pose covariance matrix, the second pose covariance matrix, the degradation direction, the first projection value and the second projection value; the eigenvalue of the second pose covariance matrix is ​​adjusted according to the target eigenvalue to complete the pose optimization.

[0164] In one embodiment, the detection module 30 is further used to perform eigenvalue decomposition according to the posture covariance matrix to obtain eigenvalues ​​and eigenvectors when posture degradation is detected; sort the eigenvalues ​​according to a preset arrangement strategy to obtain an eigenvalue sorting; calculate the eigenvalue ratio of adjacent eigenvalues ​​in the eigenvalue sorting according to the eigenvalues ​​and the eigenvectors; and calculate the posture degradation direction according to the eigenvalue ratio.

[0165] In one embodiment, the determination module 40 is also used to obtain a preset posture degradation angle threshold; obtain a first posture and a second posture according to the posture; calculate a posture degradation angle according to the degradation direction, the first posture and the second posture; when the posture degradation angle is less than the preset posture degradation angle threshold, determine that the posture has degenerated.

[0166] In one embodiment, the calculation module 20 is also used to obtain a first pose covariance matrix and a second pose covariance matrix based on the pose covariance matrix; calculate the traces of the first pose covariance matrix and the second pose covariance matrix to obtain the trace of the first pose covariance matrix and the trace of the second pose covariance matrix; detect pose degradation based on the trace of the first pose covariance matrix and the trace of the second pose covariance matrix.

[0167] In one embodiment, the calculation module 20 is also used to obtain a preset degradation threshold; calculate a degradation index based on the trace of the first posture covariance matrix and the trace of the second posture covariance matrix; when the degradation index is greater than the preset degradation threshold, posture degradation is detected.

[0168] The present application provides a posture degradation detection and optimization device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the posture degradation detection and optimization method in the above-mentioned embodiment one.

[0169] Reference below Figure 5 , which shows a schematic diagram of the structure of a posture degradation detection and optimization device suitable for implementing the embodiment of the present application. The posture degradation detection and optimization device in the embodiment of the present application may include but is not limited to mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 5 The posture degradation detection and optimization device shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0170] like Figure 5As shown, the posture degradation detection and optimization device may include a processing device 1001 (such as a central processing unit, a graphics processing unit, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM: Read Only Memory) 1002 or a program loaded from a storage device 1003 to a random access memory (RAM: Random Access Memory) 1004. In RAM1004, various programs and data required for the operation of the posture degradation detection and optimization device are also stored. The processing device 1001, ROM1002 and RAM1004 are connected to each other through a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to the I / O interface 1006: input devices 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; storage devices 1003 including, for example, a magnetic tape, a hard disk, etc.; and communication devices 1009. The communication device 1009 can allow the posture degradation detection and optimization device to communicate with other devices wirelessly or by wire to exchange data. Although the figure shows a posture degradation detection and optimization device with various systems, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems may be implemented or have instead.

[0171] In particular, according to the embodiments disclosed in the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present application include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication device, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present application are executed.

[0172] The posture degradation detection and optimization device provided by the present application adopts the posture degradation detection and optimization method in the above-mentioned embodiment, which can solve the technical problems that the degradation detection threshold is not universal enough in different environments, and the optimization processing is performed without confirming the posture degradation, resulting in limited posture optimization effect. Compared with the prior art, the beneficial effects of the posture degradation detection and optimization device provided by the present application are the same as the beneficial effects of the posture degradation detection and optimization method provided by the above-mentioned embodiment, and the other technical features in the posture degradation detection and optimization device are the same as the features disclosed in the method of the previous embodiment, which will not be repeated here.

[0173] It should be understood that the various parts disclosed in this application can be implemented by hardware, software, firmware or a combination thereof. In the description of the above embodiments, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0174] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

[0175] The present application provides a computer-readable storage medium having computer-readable program instructions (ie, computer programs) stored thereon, and the computer-readable program instructions are used to execute the posture degradation detection and optimization method in the above-mentioned embodiment.

[0176] The computer-readable storage medium provided in the present application may be, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, systems or devices, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM: Random Access Memory), a read-only memory (ROM: Read Only Memory), an erasable programmable read-only memory (EPROM: Erasable Programmable Read Only Memory or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM: CD-Read Only Memory), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program, which may be used by or in combination with an instruction execution system, system or device. The program code contained on the computer-readable storage medium may be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (Radio Frequency: Radio Frequency), etc., or any suitable combination of the above.

[0177] The above-mentioned computer-readable storage medium may be included in the posture degradation detection and optimization device; or it may exist independently without being assembled into the posture degradation detection and optimization device.

[0178] The above-mentioned computer-readable storage medium carries one or more programs. When the above-mentioned one or more programs are executed by the posture degradation detection and optimization device, the posture degradation detection and optimization device: obtains laser point cloud information and inertial information of the vehicle, and performs Kalman filtering fusion based on the laser point cloud information and the inertial information to obtain posture and posture covariance matrix; calculates the trace of the posture covariance matrix based on the posture covariance matrix, and detects posture degradation based on the trace of the posture covariance matrix; when posture degradation is detected, obtains eigenvalues ​​and eigenvectors based on the posture covariance matrix, and obtains the degradation direction based on the eigenvalues ​​and the eigenvectors; determines the posture according to the degradation direction and the posture to confirm degradation; when it is confirmed that the posture has been degraded, performs posture optimization according to the degradation direction and the posture covariance matrix.

[0179] Computer program code for performing the operations of the present application may be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0180] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present application. In this regard, each square box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two square boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0181] The modules involved in the embodiments described in this application may be implemented by software or hardware, wherein the name of the module does not constitute a limitation on the unit itself in some cases.

[0182] The readable storage medium provided by the present application is a computer-readable storage medium, which stores computer-readable program instructions (i.e., computer programs) for executing the above-mentioned posture degradation detection and optimization method, and can solve the technical problems that the degradation detection threshold is not universal enough in different environments, and the optimization process is performed without confirming the posture degradation, resulting in limited posture optimization effect. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided by the present application are the same as the beneficial effects of the posture degradation detection and optimization method provided by the above-mentioned embodiment, and will not be repeated here.

[0183] The present application also provides a computer program product, including a computer program, which, when executed by a processor, implements the steps of the above-mentioned posture degradation detection and optimization method.

[0184] The computer program product provided by the present application can solve the technical problems that the degradation detection threshold is not universal enough in different environments, and the optimization process is performed without confirming the posture degradation, resulting in limited posture optimization effect. Compared with the prior art, the beneficial effects of the computer program product provided by the present application are the same as the beneficial effects of the posture degradation detection and optimization method provided by the above-mentioned embodiment, which will not be repeated here.

[0185] The above descriptions are only some embodiments of the present application, and are not intended to limit the patent scope of the present application. All equivalent structural changes made using the contents of the present application specification and drawings under the technical concept of the present application, or direct / indirect applications in other related technical fields are included in the patent protection scope of the present application.

Claims

1. A posture degradation detection and optimization method, characterized in that: The method comprises: Acquire laser point cloud information and inertial information of the vehicle, and perform Kalman filter fusion according to the laser point cloud information and the inertial information to obtain a posture and a posture covariance matrix; Calculating a trace of a pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix; When posture degradation is detected, eigenvalues ​​and eigenvectors are obtained according to the posture covariance matrix, and degradation directions are obtained according to the eigenvalues ​​and the eigenvectors; Determining whether the posture is degraded according to the degradation direction and the posture; When it is confirmed that the posture is degraded, optimizing the posture according to the degradation direction and the posture covariance matrix; The step of calculating the trace of the pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix comprises: A first pose covariance matrix and a second pose covariance matrix are obtained according to the pose covariance matrix, wherein the first pose covariance matrix is ​​the pose covariance matrix output by the Kalman filter module in the last iteration of the current pose estimation, and the first pose covariance matrix is ​​used to reflect the uncertainty of the pose estimation in the last iteration, and the second pose covariance matrix is ​​the pose covariance matrix output by the Kalman filter module in the iteration of the current pose estimation, and the second pose covariance matrix is ​​used to reflect the uncertainty of the pose estimation in the current iteration; Calculating the trace of the first pose covariance matrix and the second pose covariance matrix to obtain the trace of the first pose covariance matrix and the trace of the second pose covariance matrix; detecting pose degradation based on a trace of the first pose covariance matrix and a trace of the second pose covariance matrix; The step of detecting posture degradation according to the trace of the first posture covariance matrix and the trace of the second posture covariance matrix comprises: Obtaining a preset degradation threshold; Calculating a degradation indicator based on a trace of the first pose covariance matrix and a trace of the second pose covariance matrix; When the degradation index is greater than the preset degradation threshold, posture degradation is detected.

2. The method according to claim 1, characterized in that When it is confirmed that the posture is degraded, the step of performing posture optimization according to the degradation direction and the posture covariance matrix includes: When it is confirmed that the posture is degraded, a first posture covariance matrix and a second posture covariance matrix are obtained according to the posture covariance matrix; Calculating a first projection value according to the first pose covariance matrix and the degenerate direction; Calculating a second projection value according to the second pose covariance matrix and the degenerate direction; Comparing the first projection value and the second projection value to obtain a projection value difference; Perform posture optimization according to the projection value difference, the first posture covariance matrix, the second posture covariance matrix, the degenerate direction, the first projection value and the second projection value.

3. The method according to claim 2, characterized in that The step of performing posture optimization according to the projection value difference, the first posture covariance matrix, the second posture covariance matrix, the degradation direction, the first projection value and the second projection value comprises: Get the preset projection threshold; When the projection value difference is less than or equal to the preset projection threshold, calculating the target eigenvalue according to the first posture covariance matrix, the second posture covariance matrix, the degradation direction, the first projection value and the second projection value; The eigenvalues ​​of the second posture covariance matrix are adjusted according to the target eigenvalues ​​to complete posture optimization.

4. The method according to claim 1, characterized in that When posture degradation is detected, the steps of obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix, and obtaining degradation directions according to the eigenvalues ​​and the eigenvectors include: When posture degradation is detected, eigenvalue decomposition is performed according to the posture covariance matrix to obtain eigenvalues ​​and eigenvectors; Sorting the eigenvalues ​​according to a preset arrangement strategy to obtain an eigenvalue sorting; Calculating, according to the eigenvalues ​​and the eigenvectors, eigenvalue ratios of adjacent eigenvalues ​​in the eigenvalue sorting; The posture degradation direction is calculated according to the eigenvalue ratio.

5. The method according to claim 1, characterized in that The step of determining whether the posture is degraded according to the degradation direction and the posture comprises: Get the preset posture degradation angle threshold; Obtain a first posture and a second posture according to the posture, wherein the first posture is a posture calculated by an inertial measurement unit, and the second posture is a posture obtained after a state vector is updated; Calculating a posture degradation angle according to the degradation direction, the first posture and the second posture; When the posture degradation angle is less than the preset posture degradation angle threshold, it is determined that the posture is degraded.

6. A posture degradation detection and optimization device, characterized in that: The device comprises: An acquisition module is used to acquire laser point cloud information and inertial information of the vehicle, and perform Kalman filter fusion according to the laser point cloud information and the inertial information to obtain a posture and a posture covariance matrix; A calculation module, used for calculating the trace of the pose covariance matrix according to the pose covariance matrix, and detecting pose degradation according to the trace of the pose covariance matrix; A detection module, for obtaining eigenvalues ​​and eigenvectors according to the posture covariance matrix when posture degradation is detected, and obtaining a degradation direction according to the eigenvalues ​​and the eigenvectors; A determination module, used for determining whether the posture is degraded according to the degradation direction and the posture; An optimization module, configured to optimize the posture according to the degradation direction and the posture covariance matrix when it is determined that the posture is degraded; The calculation module is also used to obtain a first pose covariance matrix and a second pose covariance matrix according to the pose covariance matrix, wherein the first pose covariance matrix is ​​the pose covariance matrix output by the Kalman filter module in the last iteration of the current pose estimation, and the first pose covariance matrix is ​​used to reflect the uncertainty of the pose estimation in the last iteration, and the second pose covariance matrix is ​​the pose covariance matrix output by the Kalman filter module in the iteration of the current pose estimation, and the second pose covariance matrix is ​​used to reflect the uncertainty of the pose estimation in the current iteration; calculate the traces of the first pose covariance matrix and the second pose covariance matrix to obtain the traces of the first pose covariance matrix and the second pose covariance matrix; detect pose degradation according to the traces of the first pose covariance matrix and the second pose covariance matrix; The calculation module is also used to obtain a preset degradation threshold; calculate a degradation index based on the trace of the first posture covariance matrix and the trace of the second posture covariance matrix; when the degradation index is greater than the preset degradation threshold, posture degradation is detected.

7. A posture degradation detection and optimization device, characterized in that: The device comprises: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program is configured to implement the steps of the posture degradation detection and optimization method as described in any one of claims 1 to 5.

8. A storage medium, characterized in that: The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, the steps of the posture degradation detection and optimization method as described in any one of claims 1 to 5 are implemented.

Images