Method and device for evaluating the quality of a trajectory of a mobile carrier

Abstract

The application discloses a kind of mobile carrier motion trajectory quality evaluation method and device, it is related to positioning technical field.The method comprises: obtaining the motion trajectory to be evaluated based on factor graph optimization, state variable after convergence of factor graph optimization is substituted into the residual error function of each factor, and the posterior residual error of each factor is calculated;Determine the observable index based on the local Fisher information matrix corresponding to the key frame time;According to the posterior residual error and the observable index, determine the comprehensive quality score corresponding to the key frame time;Based on the measured position of independent control point, determine the prediction position error based on the comprehensive quality score;According to the prediction position error, determine the quality level corresponding to the key frame time, and divide the interval according to the quality level to the motion trajectory to be evaluated, obtain effective interval, suspicious interval and invalid interval.The application can realize frame-by-frame quality evaluation and interval quality labeling of motion trajectory, improve the accuracy and explainability of trajectory quality evaluation.

Description

Technical Field

[0001] This application relates to the field of positioning technology, and in particular to a method and apparatus for evaluating the quality of motion trajectory of a mobile carrier. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) algorithms typically evaluate their localization and mapping performance by comparing the error between the estimated trajectory and the reference trajectory. In large-scale environments such as underground parking lots, underground pipe networks, tunnels, and industrial plants where GNSS (Global Navigation Satellite System) is blocked, satellite signals are obstructed, and RTK-GNSS (Real-time Kinematic Global Navigation Satellite System) becomes directly ineffective. In such cases, only the INS (Inertial Navigation System) operates alone, and errors accumulate rapidly, making it impossible to continuously provide high-precision absolute position references. Motion capture systems and laser trackers also suffer from high deployment costs, limited field of view, and insufficient spatial coverage. Related technologies can utilize high-precision static prior maps, LiDAR point clouds, and IMU (inertial measurement unit) data to generate continuous motion trajectories through point cloud map matching, IMU pre-integration, and factor graph joint optimization. However, they typically only output the overall trajectory or additional theoretical covariance information, lacking mechanisms for frame-by-frame verification, hierarchical labeling, and cause analysis of the quality of different segments within the trajectory. When low-quality segments exist in the trajectory, such as observation degradation, point cloud occlusion, matching anomalies, sudden motion changes, or residual anomalies, directly using the entire trajectory as the evaluation reference trajectory may contaminate the SLAM algorithm's evaluation results due to these low-quality segments, reducing the accuracy and reliability of the evaluation conclusions. Summary of the Invention

[0003] In view of this, this application provides a method and apparatus for evaluating the quality of motion trajectory of a mobile vehicle, which solves the technical problem of how to perform frame-by-frame quality evaluation of motion trajectories generated based on prior maps and factor graph optimization in large-scale environments where GNSS is denied.

[0004] According to one aspect of this application, a method for evaluating the motion trajectory quality of a mobile carrier is provided, comprising: The motion trajectory to be evaluated of the mobile vehicle is obtained; the motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile vehicle, the factor graph including factors associated with the state variables of the mobile vehicle at each key frame time during the motion. Substitute the state variables after factor graph optimization convergence into the residual function of each factor to calculate the posterior residual of the factor; Based on the local Fischer information matrix corresponding to the key frame moment after factor graph optimization convergence, the observable index corresponding to the key frame moment is determined. Based on the posterior residual and the observable index, the comprehensive quality score corresponding to the key frame moment is determined; Based on the comprehensive quality score and the measured position of the independent control point, the predicted position error corresponding to the key frame moment is determined. The independent control point is a spatial reference point with a fixed position in the target space of the moving vehicle. Based on the predicted position error, the quality level corresponding to the key frame moment is determined, and based on the quality level, the motion trajectory to be evaluated is divided into intervals to obtain the trajectory quality interval of the motion trajectory to be evaluated.

[0005] According to another aspect of this application, a mobile carrier motion trajectory quality evaluation device is provided, comprising: The acquisition module is used to acquire the motion trajectory to be evaluated of the mobile carrier; the motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile carrier, and the factor graph includes factors associated with the state variables of the mobile carrier at each key frame time during the motion process; The prediction module is used to substitute the state variables after factor graph optimization convergence into the residual function of each factor to calculate the posterior residual of the factor; and to determine the observable index corresponding to the key frame time based on the local Fischer information matrix corresponding to the key frame time after factor graph optimization convergence; and to determine the comprehensive quality score corresponding to the key frame time based on the posterior residual and the observable index; and to determine the prediction position error corresponding to the key frame time based on the comprehensive quality score and the measured position of the independent control point, wherein the independent control point is a spatial reference point with a fixed position in the target space of the mobile carrier's motion. The evaluation module is used to determine the quality level corresponding to the key frame moment based on the predicted position error, and to divide the motion trajectory to be evaluated into intervals based on the quality level to obtain the trajectory quality interval of the motion trajectory to be evaluated.

[0006] According to another aspect of this application, a readable storage medium is provided, on which a program or instructions are stored, which, when executed by a processor, implement the steps of the above-described method for evaluating the motion trajectory quality of a mobile carrier.

[0007] According to another aspect of this application, a computer device is provided, including a storage medium, a processor, and a computer program stored on the storage medium and executable on the processor, wherein the processor executes the program to implement the steps of the above-described method for evaluating the motion trajectory quality of a mobile carrier.

[0008] By employing the above technical solution, this application provides a method and apparatus for evaluating the motion trajectory quality of a mobile carrier. It normalizes the posterior residuals of each factor after factor map optimization and performs a chi-square consistency test, and combines this with observability indicators determined by the local Fischer information matrix to perform frame-by-frame quality scoring and quality level determination of the motion trajectory. Simultaneously, it uses the measured positions of independent control points for external calibration of the quality score, establishing a correspondence between the quality level and the actual position error. Therefore, it can identify valid, suspicious, and invalid intervals in the motion trajectory, thereby reducing the impact of low-quality trajectory segments on SLAM evaluation results and improving the accuracy, interpretability, and engineering usability of trajectory quality evaluation.

[0009] The above description is only an overview of the technical solution of this application. In order to better understand the technical means of this application and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of this application more obvious and understandable, the following are specific embodiments of this application. Attached Figure Description

[0010] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings: Figure 1 A flowchart illustrating the mobile carrier motion trajectory quality evaluation method provided in an embodiment of this application is shown. Figure 2 A flowchart illustrating a method for evaluating the motion trajectory quality of a mobile carrier according to another embodiment of this application is shown. Figure 3 A structural block diagram of the mobile carrier motion trajectory quality evaluation device provided in an embodiment of this application is shown. Detailed Implementation

[0011] The present application will be described in detail below with reference to the accompanying drawings and embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in the embodiments of the present application can be combined with each other.

[0012] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain this application, and should not be construed as limiting this application.

[0013] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this application’s specification means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when we say an element is “connected” or “attached” to another element, it can be directly connected or attached to the other element, or there may be intermediate elements. Furthermore, “connected” or “attached” as used herein can include wireless connections or wireless coupling. The term “and / or” as used herein includes all or any unit and all combinations of one or more associated listed items.

[0014] Exemplary embodiments according to this application will now be described in more detail with reference to the accompanying drawings. However, these exemplary embodiments may be implemented in many different forms and should not be construed as being limited to the embodiments set forth herein. It should be understood that these embodiments are provided so that the disclosure of this application is thorough and complete, and that the concept of these exemplary embodiments is fully conveyed to those skilled in the art.

[0015] This embodiment provides a method for evaluating the quality of a mobile carrier's motion trajectory, such as... Figure 1 As shown, the method includes: Step S101: Obtain the motion trajectory of the mobile carrier to be evaluated.

[0016] The motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile carrier. The factor graph includes factors associated with the state variables of the mobile carrier at each key frame time during the motion.

[0017] Step S102: Substitute the state variables after factor graph optimization convergence into the residual function of each factor, and calculate the posterior residual of the factor.

[0018] Step S103: Based on the factor graph optimization, the local Fischer information matrix corresponding to the key frame time after convergence is used to determine the observable index corresponding to the key frame time.

[0019] Step S104: Based on the posterior residuals and observable indicators, determine the comprehensive quality score corresponding to the key frame moment.

[0020] Step S105: Based on the comprehensive quality score and the measured positions of independent control points, determine the predicted position error corresponding to the key frame moment.

[0021] Among them, the independent control point is a spatial reference point with a fixed position in the target space of the moving vehicle.

[0022] Step S106: Determine the quality level corresponding to the key frame time based on the predicted position error, and divide the motion trajectory to be evaluated into intervals based on the quality level to obtain the trajectory quality interval of the motion trajectory to be evaluated.

[0023] In this embodiment, the motion trajectory of the mobile vehicle to be evaluated, obtained based on factor graph optimization, is acquired. The state variables after factor graph optimization convergence are substituted into the residual functions of the factors associated with each state variable to calculate the posterior residuals corresponding to each factor. Simultaneously, based on the local Fisher information matrix corresponding to the keyframe moments after factor graph optimization convergence, observable indices corresponding to each keyframe moment are determined to characterize the sufficiency of observation constraints, the balance of constraint directions, and the theoretical uncertainty level of the state variables at the corresponding keyframe moments. Further, a comprehensive quality score corresponding to each keyframe moment is determined based on the posterior residuals and observable indices. The comprehensive quality score is externally calibrated using the measured positions of independent control points in the target space, which are fixed and independent of the trajectory generation process, to determine the predicted position error corresponding to each keyframe moment. Therefore, based on the relationship between the predicted position error and the preset error threshold, the quality level corresponding to each key frame moment is determined, and the motion trajectory to be evaluated is divided into continuous intervals according to the quality levels of adjacent key frame moments, resulting in at least one trajectory quality interval including valid interval, suspicious interval and invalid interval, thereby realizing key frame-by-key frame quality evaluation, error quantification and interval quality labeling of the motion trajectory to be evaluated.

[0024] Furthermore, as a refinement and extension of the specific implementation methods of the above embodiments, and to fully illustrate the specific implementation process of this embodiment, another embodiment of this application provides a method for evaluating the motion trajectory quality of a mobile carrier, such as... Figure 2 As shown, this method is applied to evaluate the quality of the motion trajectory of a mobile vehicle, and determines the valid, suspicious, and invalid intervals within the trajectory based on the evaluation results. A mobile vehicle refers to a vehicle capable of autonomous movement in space, including but not limited to drones, mobile robots, and autonomous vehicles. The method includes: Step S201: Obtain the motion trajectory of the mobile vehicle to be evaluated; substitute the state variables after factor graph optimization convergence into the residual function of each factor, and calculate the posterior residual of the factor; linearize the residual function of the factor according to the state variables after factor graph optimization convergence, and obtain the Jacobian matrix corresponding to the state variables of the factor after factor graph optimization convergence; based on the Jacobian matrix and the information matrix determined by the factor when constructing the factor graph, determine the local Fischer information matrix corresponding to the key frame time after factor graph optimization convergence.

[0025] The motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile carrier. The factor graph includes factors associated with the state variables of the mobile carrier at each key frame time during the motion.

[0026] In this embodiment, after the mobile vehicle moves within the target space, its motion trajectory to be evaluated can be obtained. The motion trajectory to be evaluated includes the target pose of the mobile vehicle at multiple keyframe moments during its movement. It can be obtained by combining point cloud data scanned by the lidar onboard the mobile vehicle and motion data collected by the inertial measurement unit (IMU), along with a high-precision prior map of the target space, through factor graph optimization. Specifically, the target pose includes the optimal attitude and position variables estimated by the mobile vehicle within the map system; the map system is the coordinate system of the high-precision prior map; the attitude variable is the rotation matrix from the vehicle system to the map system; the position variable represents the coordinates of the mobile vehicle within the map system; the vehicle system is the coordinate system of the mobile vehicle itself where the IMU is located; the keyframe moment is the scanning moment of the keyframe point cloud by the lidar; and the keyframe point cloud is a representative lidar point cloud from the point cloud data scanned by the lidar.

[0027] Specifically, to obtain the motion trajectory of the mobile vehicle to be evaluated, the state variables of the mobile vehicle at each key frame are used as nodes in the factor graph. Based on the point cloud data, motion data, and high-precision prior map of the mobile vehicle, factors associated with the state variables are determined, thereby constructing the factor graph. The state variables include attitude variables and position variables; factors connect one or more state variable nodes in the factor graph, and a residual function describes the constraints that these state variables should satisfy.

[0028] The factor graph is used to describe the relationship between the state variables of the mobile vehicle at each keyframe time and different observation constraints. The factor graph includes at least one of the following: map matching factor, inertial measurement unit (IMU) factor, and loop closure factor. Specifically, the map matching factor describes the geometric consistency constraints between the distorted point cloud of the mobile vehicle at each keyframe time and the high-precision prior map. The IMU factor describes the relative motion constraints of the mobile vehicle between adjacent keyframe times. The loop closure factor describes the global consistency constraints between non-adjacent keyframe times that have a revisit relationship. The distorted point cloud is obtained by distorting the keyframe point cloud corresponding to each keyframe time.

[0029] When constructing the factor map, the covariance matrix of each factor is determined based on the observed data and observation noise corresponding to each factor, and the information matrix of each factor is determined based on the covariance matrix. Specifically, the covariance matrix of the map matching factor is determined based on the observation noise corresponding to the matching of the distortion-free point cloud with the high-precision prior map; the covariance matrix of the inertial measurement unit factor is determined based on the motion data collected by the inertial measurement unit and the observation noise corresponding to the motion data; and the covariance matrix of the loop closure factor is determined based on the observation noise corresponding to loop closure matching. The covariance matrix of each factor represents the uncertainty of the observed data corresponding to each factor. The information matrix of each factor represents the constraint information provided by the observed data to the state variables.

[0030] After constructing the factor graph, the predicted values ​​of the state variables at each key frame time, the factors associated with the state variables, the covariance matrix of each factor, and the information matrix of each factor are used to optimize the factor graph, resulting in the optimized and converged state variables. The predicted values ​​of the state variables at each key frame time are determined based on the motion data collected by the IMU.

[0031] After the factor graph optimization converges, the target pose of the mobile vehicle at each key frame time is determined based on the attitude and position variables in the state variables after factor graph optimization convergence. Based on the target pose at multiple key frame times, the motion trajectory to be evaluated of the mobile vehicle is determined.

[0032] Furthermore, after the factor graph optimization converges, the values ​​of the converged state variables are substituted into the residual functions of each factor to calculate the posterior residuals of each factor. The posterior residuals of each factor represent the deviation between the observed constraints corresponding to each factor and the converged state variables after factor graph optimization. Simultaneously, based on the values ​​of the converged state variables, the residual functions of each factor are linearized to obtain the Jacobian matrix corresponding to each factor at the converged state variables.

[0033] Furthermore, based on the Jacobian matrix corresponding to the state variables of each factor after factor graph optimization convergence and the information matrix determined by each factor when constructing the factor graph, the local Fischer information matrix corresponding to each key frame time after factor graph optimization convergence is determined.

[0034] In this embodiment, one or more independent control points are set within the target space. Each independent control point is a spatial reference point in the target space that is fixed in position and can be identified or measured. The independent control points can be arranged according to the evaluation range of the motion trajectory to be evaluated, distributing them at different locations within the target space or near the area traversed by the motion trajectory. Specifically, the independent control points can be set at fixed structures, fixed facilities, or preset locations within the target space. To facilitate determining the spatial relationship between the moving vehicle and the independent control points, the independent control points can have features recognizable by the sensors of the moving vehicle, or they can have identifying features recognizable by the independent measurement system. The independent control points remain fixed in position after being set up, or remain fixed relative to the target space during the evaluation period.

[0035] Therefore, the factor graph, the covariance matrix and information matrix of each factor when constructing the factor graph, the motion trajectory of the mobile vehicle to be evaluated, the posterior residuals and Jacobian matrices of each factor after factor graph optimization convergence, the local Fischer information matrix corresponding to each key frame moment after factor graph optimization convergence, and the independent control points are determined as the input data of this embodiment.

[0036] Step S202: Each keyframe moment in the motion trajectory to be evaluated is taken as an evaluation unit, and each factor is assigned to the corresponding evaluation unit according to the preset factor assignment rules; the normalized posterior residual of the factor is calculated based on the posterior residual of the factor and the covariance matrix corresponding to the factor when constructing the factor graph; the degree of freedom of the factor is determined according to the residual dimension of the factor; the normalized posterior residuals of the factors in the evaluation unit are aggregated, and the degree of freedom of the factors in the evaluation unit is accumulated to obtain the aggregated statistics and aggregated degrees of freedom of the evaluation unit respectively.

[0037] In this step, each keyframe moment in the motion trajectory to be evaluated is taken as an evaluation unit, and normalized posterior residuals and chi-square consistency statistics are performed on each evaluation unit and each factor within each evaluation unit.

[0038] In this system, each keyframe corresponds to one evaluation unit. For a factor associated with only one keyframe, it is assigned to the evaluation unit corresponding to that keyframe. For a factor connecting two adjacent keyframes, it is assigned to one of the evaluation units corresponding to that keyframe according to a preset factor assignment rule, ensuring that each factor belongs to only one evaluation unit to avoid duplicate statistics for the same factor. Specifically, for a factor associated with only one keyframe, such as a map matching factor corresponding to that keyframe, it is assigned to the evaluation unit corresponding to that keyframe. For a factor connecting two adjacent keyframes, such as an IMU factor between adjacent keyframes, it is assigned to the evaluation unit corresponding to the later keyframe. For loopback factors connecting non-adjacent keyframes, it is assigned to the evaluation unit corresponding to the keyframe at which the loopback factor was established. This method ensures that each factor belongs to only one evaluation unit, thereby guaranteeing that the residual statistics for each keyframe are independent.

[0039] Specifically, for any factor within any evaluation unit, the normalized posterior residual of the factor is calculated based on the posterior residual of the factor after the factor graph optimization converges and the covariance matrix of the factor when constructing the factor graph. The normalized posterior residual is used to eliminate the influence caused by differences in residual dimensions, residual dimensions, and observation noise between different factors.

[0040] For example, for the first evaluation unit within any evaluation unit The normalized posterior residuals of the factors are expressed as follows: ; in, Indicates the first Normalized posterior residuals of each factor; Indicates the first The posterior residuals of each factor after convergence of factor graph optimization; Representation matrix Transpose of; Indicates the first The covariance matrix of each factor when constructing the factor plot; Indicates the first The inverse of the covariance matrix corresponding to each factor when constructing the factor graph.

[0041] Furthermore, for any factor, its degrees of freedom are determined based on the residual dimension of that factor. The IMU factor includes rotational residuals, velocity residuals, and position residuals, each corresponding to three scalar residuals, thus its degrees of freedom are 9. For the map matching factor, after distortion correction of the keyframe point cloud, it is transformed to the map coordinate system based on the current pose, and a point-to-plane matching relationship is established with the corresponding local plane in the high-precision prior map. Each effective point-to-plane matching relationship, filtered by distance, normal, and matching quality and actually used for optimization, generates a point-to-plane scalar residual; therefore, the degrees of freedom of the map matching factor are equal to the number of effective point-to-plane matching relationships. The loop closure factor contains 3D rotational residuals and 3D translational residuals, thus its degrees of freedom are 6.

[0042] For any evaluation unit, the normalized posterior residuals of all factors within that unit are aggregated, and the degrees of freedom of each factor are summed to obtain the aggregated statistic and aggregated degrees of freedom for that evaluation unit. Furthermore, a chi-square consistency test is performed based on the aggregated statistic and aggregated degrees of freedom of that evaluation unit to determine whether the posterior residuals within that unit meet the consistency requirements.

[0043] For example, the first Aggregate statistics of each evaluation unit and aggregation degrees of freedom Represented as: ; ; in, Indicates the first The set of factors contained in each evaluation unit; Indicates the first The degrees of freedom of each factor.

[0044] The chi-square consistency test for the evaluation units is expressed as follows: ; in, Describing the degrees of freedom as The 95th percentile of the chi-square distribution.

[0045] If the aggregate statistics and aggregate degrees of freedom of the evaluation unit satisfy the chi-square consistency test above, then the posterior residuals within the evaluation unit are determined to meet the consistency requirements, and the key frame moments corresponding to the evaluation units that pass the chi-square consistency test are marked with unit-level consistency flags.

[0046] In addition, chi-square consistency tests can be performed on each factor within the evaluation unit. The normalized posterior residuals of the factor can be compared with the 95th quantile of the chi-square distribution of its degrees of freedom to determine whether the posterior residuals of the factor meet the consistency requirements.

[0047] For example, the chi-square consistency test of the factors is expressed as: ; in, Describing the degrees of freedom as The 95th percentile of the chi-square distribution.

[0048] If the normalized posterior residuals of a factor satisfy the chi-square consistency test above, then the factor is determined to meet the consistency requirements, and a factor-level consistency flag is marked for factors that pass the chi-square consistency test.

[0049] It should be noted that the confidence level corresponding to the 95th quantile of the chi-square distribution used in the chi-square consistency test is 95%. Assuming the factor model and the observed noise statistics for each factor are correct, the probability that the normalized posterior residual of a factor or the aggregate statistic of an evaluation unit exceeds the corresponding 95th quantile of the chi-square distribution is 5%. Therefore, the chi-square consistency test allows for approximately 5% statistical out-of-limit probability, which is used to characterize the false alarm rate when both the model and noise statistics are correct.

[0050] In this embodiment, by performing a chi-square consistency test on the evaluation unit and the normalized posterior residuals of each factor within the evaluation unit, it can be determined whether the actual residuals after factor map optimization match the pre-set factor model and observation noise model. When the test results do not meet the consistency requirements, it indicates that the corresponding observation constraints may be abnormal, or there may be a large deviation between the factor model, observation noise, and state estimation results, thus providing a basis for identifying abnormal regions in the motion trajectory to be evaluated.

[0051] Step S203: Perform eigenvalue decomposition on the local Fischer information matrix corresponding to the key frame time to obtain the eigenvalues ​​of the local Fischer information matrix; truncate the local Fischer information matrix according to a preset eigenvalue threshold; invert the eigenvalues ​​greater than or equal to the eigenvalue threshold of the local Fischer information matrix to obtain the inverse matrix of the truncated and regularized local Fischer information matrix; determine the total theoretical uncertainty index corresponding to the key frame time based on the trace of the inverse matrix of the truncated and regularized local Fischer information matrix; determine the theoretical uncertainty index corresponding to the key frame time based on the maximum and minimum eigenvalues ​​of the local Fischer information matrix corresponding to the key frame time. The condition number index is determined as follows: The local Fischer information matrix corresponding to the key frame time is divided into blocks according to the position and attitude variables in the state variables, to obtain the position submatrix corresponding to the position variable and the attitude submatrix corresponding to the attitude variable at the key frame time; Eigenvalue decomposition is performed on the position submatrix and the attitude submatrix respectively to obtain the eigenvalues ​​of the position submatrix and the attitude submatrix; Based on the ratio of the minimum eigenvalue to the maximum eigenvalue of the position submatrix, the position normalization degradation index of the position variable at the key frame time is determined; Based on the ratio of the minimum eigenvalue to the maximum eigenvalue of the attitude submatrix, the attitude normalization degradation index of the attitude variable at the key frame time is determined.

[0052] In this step, the local Fisher information matrix corresponding to each key frame time is optimized and converged based on the factor graph to determine the observability index corresponding to each key frame time, so as to evaluate the observation constraint strength and theoretical state estimation accuracy of each key frame time.

[0053] Specifically, based on the local Fisher information matrix corresponding to any key frame moment after factor graph optimization convergence, the total theoretical uncertainty index corresponding to that key frame moment is determined.

[0054] For example, the first The total theoretical uncertainty index corresponding to each key frame moment is expressed as follows: ; in, The factor graph optimization convergence result is represented by the th... Local Fischer information matrix corresponding to each key frame moment; Represents the trace of a matrix; The factor graph optimization convergence result is represented by the th... The trace of the inverse of the local Fischer information matrix corresponding to the nth keyframe time is used as the 1st keyframe time. The total theoretical uncertainty index corresponding to each key frame moment.

[0055] Here, the local Fischer information matrix is ​​used to represent the ability of each observation constraint to constrain the state variables at the keyframe time. The inverse of the local Fischer information matrix can be used to characterize the theoretical covariance level of the state variables. Therefore, the smaller the total theoretical uncertainty index, the more sufficient the observation constraints at the corresponding keyframe time, and the higher the theoretical estimation accuracy of the state variables; the larger the total theoretical uncertainty index, the weaker the observation constraints at the corresponding keyframe time, and the larger the theoretical estimation uncertainty.

[0056] Furthermore, when the local Fischer information matrix has a minimum eigenvalue close to zero, it indicates that there may be observation degradation at the corresponding key frame time. In this case, truncation regularization is used to calculate the total theoretical uncertainty index to avoid index anomalies caused by matrix inversion instability.

[0057] For example, the factor graph optimization converges after the first convergence. The eigenvalues ​​and eigenvectors of the local Fischer information matrix corresponding to each keyframe time are obtained by performing eigenvalue decomposition, as follows: ; in, Represents the eigenvector matrix; Represents the eigenvalues ​​of the local Fischer information matrix. ; This represents a function that extracts diagonal elements from a matrix or creates a diagonal matrix.

[0058] Furthermore, when the local Fischer information matrix has small eigenvalues, to avoid numerical instability caused by direct inversion, the local Fischer information matrix is ​​truncated according to a preset eigenvalue threshold. Only eigenvalues ​​greater than or equal to the threshold are inverted, and the directions corresponding to eigenvalues ​​less than the threshold are set to zero, resulting in the inverse matrix of the truncated and regularized local Fischer information matrix. : ; in, This represents the preset feature value threshold used to determine whether a feature value is too small.

[0059] Therefore, based on the inverse of the truncated and regularized local Fisher information matrix, the total theoretical uncertainty index is determined, expressed as: .

[0060] Here, the eigenvalue threshold can be set based on the numerical precision of the computing device and the range of values ​​in the local Fischer information matrix. For example, the eigenvalue threshold can be set to 10 of the largest eigenvalue. 6 times.

[0061] It should be noted that the total theoretical uncertainty index is used to characterize the observability and theoretical uncertainty level under the current observation structure, and does not directly represent the upper limit of the actual position error or attitude error of the mobile vehicle at the corresponding key frame moment. In other words, even if the total theoretical uncertainty index is small, it cannot be concluded that the actual error is necessarily less than this index; the actual error may also be affected by factors such as observation anomalies, inaccurate factor models, unreasonable noise statistical parameter settings, and unmodeled errors.

[0062] Furthermore, based on the maximum and minimum eigenvalues ​​of the local Fischer information matrix corresponding to each key frame moment after factor graph optimization convergence, the condition number index for that key frame moment is determined.

[0063] For example, the first Condition number index for each keyframe moment Represented as: ; in, The factor graph optimization convergence result is represented by the th... The maximum eigenvalue of the local Fischer information matrix corresponding to each key frame time; The factor graph optimization convergence result is represented by the th... The minimum eigenvalue of the local Fischer information matrix corresponding to each key frame time; This indicates a preset small quantity used to avoid denominators that are zero or too small. This represents the function that takes the maximum value.

[0064] The condition number index describes whether the observation constraints are balanced across different directions. A large condition number index indicates that the eigenvalues ​​of the local Fischer information matrix differ significantly across directions, suggesting that some directions may have weaker constraints at the corresponding keyframe moments. Therefore, the presence of multipath degradation at keyframe moments can be determined based on the condition number index and the overall magnitude of the eigenvalues ​​of the local Fischer information matrix.

[0065] Specifically, when the condition number index at a keyframe moment exceeds a preset condition number judgment threshold, and the overall eigenvalues ​​of the local Fischer information matrix corresponding to that keyframe moment after factor graph optimization convergence are relatively small, it can be determined that multipath degradation exists at that keyframe moment. The condition number judgment threshold can be determined based on calibration data from a non-degradation scenario. For example, the corresponding quantile can be selected as the judgment threshold based on the statistical distribution of the condition number index in a non-degradation scenario.

[0066] It should be noted that the dimensions of the local Fischer information matrix are the same as those of the state variables, including the rows and columns corresponding to the attitude and position variables.

[0067] Furthermore, the local Fischer information matrices corresponding to each keyframe time after factor graph optimization convergence are divided into blocks according to position and attitude variables, obtaining the position submatrices corresponding to the position variables and the attitude submatrices corresponding to the attitude variables at that keyframe time. Eigenvalue decomposition is then performed on the position submatrices to obtain their eigenvalues ​​and eigenvectors. Similarly, eigenvalue decomposition is performed on the attitude submatrices to obtain their eigenvalues ​​and eigenvectors.

[0068] For example, the first The position submatrix corresponding to each keyframe time Represented as: ; in, The eigenvector matrix representing the position submatrix; Let represent the eigenvalues ​​of the positional submatrix, and satisfy . .

[0069] Furthermore, based on the maximum and minimum eigenvalues ​​of the position submatrix and the attitude submatrix, the position normalization degradation index of the position variable and the attitude normalization degradation index of the attitude variable are determined at the key frame time, respectively.

[0070] For example, the first Position-normalized degradation index at each key frame moment With posture normalization degradation index Represented as: ; ; in, , Let represent the largest and smallest eigenvalues ​​of the pose submatrix, respectively.

[0071] Here, the location-normalized degradation index With posture normalization degradation index The range of values ​​is .

[0072] The smaller the position normalization degradation index, the greater the difference in observation constraints of the position variable in different directions, and the more severe the degradation of the position variable. Similarly, the smaller the attitude normalization degradation index, the greater the difference in observation constraints of the attitude variable in different directions, and the more severe the degradation of the attitude variable. When the corresponding normalization degradation index is close to 1, it indicates that the observation constraints of the corresponding position or attitude variable are relatively balanced in all directions.

[0073] Furthermore, the position normalization degradation index and the attitude normalization degradation index are compared with preset degradation judgment thresholds to determine whether the position or attitude variables at the corresponding key frame time have degraded.

[0074] For example, if the position normalization degradation index at a keyframe is less than the degradation judgment threshold, the position variable at that keyframe is determined to be degraded; if the pose normalization degradation index at a keyframe is less than the degradation judgment threshold, the pose variable at that keyframe is determined to be degraded. The degradation judgment threshold can be adjusted based on calibration data from a non-degraded scenario, for example, determined based on the low quantile of the normalization degradation in the non-degraded calibration data.

[0075] Therefore, for each keyframe moment, an observable index vector can be obtained, consisting of observable indices including the total theoretical uncertainty index, the condition number index, the position normalization degradation index, and the attitude normalization degradation index.

[0076] For example, the first Observable index vector at each key frame time Represented as: .

[0077] In this embodiment, by determining the observability index vector at each key frame moment, regions with high theoretical uncertainty, position degradation, and attitude degradation in the trajectory to be evaluated can be identified from the perspective of the observation constraint structure. These indices reflect the strength of constraints on the factor graph optimization results in different directions and, combined with the chi-square consistency test results obtained in the above steps, are used to distinguish between anomalies caused by observation structure degradation and anomalies caused by observation residual anomalies.

[0078] Step S204: Calculate the cumulative probability of the aggregate statistics of the evaluation unit under the chi-square distribution of the aggregate degrees of freedom of the evaluation unit, and perform an inverse transformation on the cumulative probability to obtain the chi-square consistency score of the key frame moment corresponding to the evaluation unit; perform logarithmic compression and inverse normalization on the condition number index corresponding to the key frame moment to obtain the condition number score corresponding to the key frame moment; normalize and invert the total theoretical uncertainty index corresponding to the key frame moment to obtain the observable score corresponding to the key frame moment; perform weighted summation on the position normalized degradation index and the attitude normalized degradation index corresponding to the key frame moment to obtain the degradation score corresponding to the key frame moment; and perform weighted combination on the chi-square consistency score, condition number score, observable score, and degradation score of the key frame moment to obtain the comprehensive quality score corresponding to the key frame moment.

[0079] In this step, based on the aggregated statistics and aggregated degrees of freedom within the evaluation unit corresponding to each key frame moment, and the observable indicators corresponding to each key frame moment, a frame-by-frame quality score is performed on each key frame moment in the motion trajectory to be evaluated.

[0080] Specifically, for any key frame moment, the aggregate statistics and aggregate degrees of freedom within the evaluation unit corresponding to that key frame moment, as well as the observable indicators corresponding to each key frame moment, are normalized to determine multiple indicator scores. The indicator scores may include chi-square consistency score, condition number score, observable score, and degradation score.

[0081] Specifically, for any keyframe moment, the chi-square consistency score corresponding to that keyframe moment is determined based on the aggregated statistic and aggregated degrees of freedom of the evaluation unit corresponding to that keyframe moment. Specifically, the aggregated degrees of freedom of the evaluation unit are used as the degree of freedom parameter of the chi-square distribution. The cumulative probability of the aggregated statistic under the chi-square distribution of the aggregated degrees of freedom is calculated, and the cumulative probability is inversely transformed to obtain the value located in... Chi-square consistency score within the interval.

[0082] Furthermore, since a smaller condition number indicates smaller eigenvalue differences and more balanced observation constraints in different directions of the local Fischer information matrix, the condition number index is logarithmically compressed and inversely normalized, mapping it to... The condition number score is obtained by dividing the interval. Here, when the condition number index equals 1, it means that the largest and smallest eigenvalues ​​are equal, and the observation constraints in all directions are completely balanced. Therefore, the closer the condition number index is to 1, the higher the condition number score. As the condition number index increases, the condition number score decreases.

[0083] Furthermore, since a smaller total theoretical uncertainty index indicates more sufficient observational constraints on the state variables at the corresponding keyframe moment, and a smaller theoretical uncertainty in position and attitude estimation, there is a negative correlation between the total theoretical uncertainty index and trajectory quality. To facilitate its unified combination with other indices, it is necessary to first determine the effective range of the total theoretical uncertainty index based on the lower and upper limits determined by the non-degradable scenario or historical calibration data. After determining the effective range, the total theoretical uncertainty index corresponding to each keyframe moment is compared with the effective range, and linear normalization is performed according to its position within the effective range to obtain a normalized value. Since the subsequent combined scoring stipulates that a higher score indicates better quality, the normalized value needs to be inverted, for example, by subtracting the normalized value from 1 to obtain the observable score. This ensures that keyframe moments with lower theoretical uncertainty receive higher observable scores, while keyframe moments with higher theoretical uncertainty receive lower observable scores. For the total theoretical uncertainty index that exceeds the effective range, it can be truncated according to the boundary of the range to ensure that the observable scores between different keyframes are comparable.

[0084] Furthermore, the degradation score is determined based on the values ​​of the position-normalized degradation index and the attitude-normalized degradation index. The closer both the position-normalized degradation index and the attitude-normalized degradation index are to 1, the more balanced the observation constraints of the corresponding position or attitude variable are in different directions, and the higher the degradation score. For example, the degradation score can be determined by weighted summation of the position-normalized degradation index and the attitude-normalized degradation index.

[0085] Furthermore, the scores of each indicator at the same keyframe moment are weighted and combined to obtain the comprehensive quality score corresponding to that keyframe moment. The comprehensive quality score is used to comprehensively characterize the consistency of observation residuals, the strength of observation constraints, and the degree of observation degradation at the corresponding keyframe moment.

[0086] For example, the first Overall quality score for each keyframe moment Represented as: ; in, Indicates the first The first keyframe moment Each indicator score Indicates the first The weights of the scores for each indicator, and .

[0087] For a specific example, the scores of each indicator are combined using equal weights, as shown below: .

[0088] Step S205: Based on the key frame timestamps associated with the independent control points, determine the trajectory positions of the independent control points in the motion trajectory to be evaluated; based on the trajectory positions and measured positions of the independent control points, determine the actual position errors corresponding to the independent control points; generate calibration samples based on the comprehensive quality scores and actual position errors of the independent control points, and fit the mapping relationship between the comprehensive quality scores and actual position errors in the calibration samples to obtain the mapping function; based on the comprehensive quality scores and mapping function of the key frame timestamps, determine the predicted position errors of the key frame timestamps.

[0089] The overall quality score of an independent control point is determined based on the observable indicators of the key frame moments associated with the timestamp of the independent control point.

[0090] It should be noted that independent control points are spatial reference points in the target space that are fixed in position and can be identified by the moving vehicle or measured by an independent measurement system. For example, independent control points can be targets pre-positioned in the target space. Independent control points do not participate in the coordinate alignment, factor map construction, or factor map optimization of the trajectory to be evaluated; they are only used to independently calibrate the trajectory quality after the trajectory is generated.

[0091] Specifically, during the movement of the mobile platform within the target space, the LiDAR scans or identifies independent control points. When an independent control point is scanned or identified by the LiDAR, a timestamp corresponding to that scan or identification is obtained. This timestamp is the scan time in the LiDAR point cloud data containing the independent control point. Since the timestamp of the independent control point and the scan time are on the same time base, the independent control point can be associated with the corresponding keyframe time based on the timestamp.

[0092] Furthermore, for any independent control point, the trajectory position corresponding to the timestamp of that independent control point is determined in the motion trajectory to be evaluated. Specifically, if the timestamp is the same as a keyframe moment, the position variable in the target pose corresponding to that keyframe moment is used as the trajectory position; when the timestamp is between two adjacent keyframe moments, the position variables corresponding to the adjacent keyframes are interpolated based on the position of the timestamp between the two adjacent keyframe moments to obtain the trajectory position corresponding to the timestamp. Thus, the trajectory position represents the position determined by the lidar mounted on the mobile vehicle based on the motion trajectory to be evaluated when scanning or identifying an independent control point.

[0093] On the other hand, before the mobile carrier operates, the independent control points are independently measured using a total station or other independent surveying system to obtain the fixed spatial position of each independent control point. This fixed spatial position is used as the measured position of the independent control point.

[0094] Furthermore, the measured location is transformed or represented in the same map system as the trajectory to be evaluated. This coordinate transformation is not performed through the trajectory to be evaluated, nor are independent control points used as constraints in the factor map optimization. Here, the measured location serves as an external reference location independent of the trajectory generation process, used for comparison with the trajectory location.

[0095] Therefore, based on the trajectory position and measured position of the same independent control point, the actual position error corresponding to that independent control point is calculated. The actual position error characterizes the actual deviation between the corresponding trajectory position in the motion trajectory being evaluated and the measured position of the independent control point.

[0096] For example, the first Actual position error of each independent control point Represented as: ; in, Indicates the first The trajectory position of each independent control point; Indicates the first The measured positions of each independent control point.

[0097] Furthermore, following the same scoring method for keyframe moments, the scores of the four indicators for each independent control point are calculated based on the target aggregation statistics, target aggregation degrees of freedom, and target observable indicators corresponding to the independent control point. These scores are then weighted and summed to obtain the overall quality score of the independent control point.

[0098] Here, based on the timestamp of the independent control point, the keyframe time or adjacent keyframe time corresponding to that timestamp is determined, and the target aggregate statistics, target aggregate degrees of freedom, and target observable index corresponding to the independent control point are obtained accordingly. Specifically, if the timestamp is the same as a keyframe time, the aggregate statistics, aggregate degrees of freedom, and observable index corresponding to that keyframe time are determined as the target aggregate statistics, target aggregate degrees of freedom, and target observable index of the independent control point. If the timestamp is between two adjacent keyframe times, the aggregate statistics and observable index of the two adjacent keyframe times can be linearly interpolated, and the interpolation results are used as the target aggregate statistics and target observable index of the independent control point, respectively; at the same time, the aggregate degrees of freedom corresponding to the keyframe time that is closest to the timestamp in time is used as the target aggregate degrees of freedom of the independent control point. Alternatively, the aggregate statistics, aggregate degrees of freedom, and observable index corresponding to the keyframe time that is closest to the timestamp in time can be directly selected as the target aggregate statistics, target aggregate degrees of freedom, and target observable index of the independent control point.

[0099] Therefore, for each independent control point, a set of calibration samples can be formed based on the comprehensive quality score and actual position error of that independent control point.

[0100] For example, the first The calibration sample corresponding to each independent control point is represented as follows: ; in, Indicates the first The overall quality score of each independent control point.

[0101] After obtaining calibration samples corresponding to multiple independent control points, the mapping relationship between the overall quality score and the actual position error in the calibration samples is fitted to obtain the mapping function between the overall quality score and the actual position error.

[0102] Here, by calibrating the comprehensive quality score using the measured positions of independent control points, a correspondence can be established between the dimensionless comprehensive quality score calculated internally by the factor graph and the positional error with actual physical meaning. Therefore, the actual positional error at the corresponding keyframe moment can be predicted based on the comprehensive quality score, and the quality of the motion trajectory to be evaluated can be graded based on the predicted positional error.

[0103] For example, the mapping function between the overall quality score and the actual location error is expressed as: ; in, This represents the mapping function between the overall quality score and the actual location error; This represents the predicted location error obtained using a fitted mapping function based on the comprehensive quality score.

[0104] It should be noted that the overall quality score is used to characterize the overall quality of the motion trajectory being evaluated at the corresponding keyframe time. A higher overall quality score indicates better consistency of the observation residuals at the corresponding keyframe time, more sufficient observation constraints, and lower degradation, thus typically resulting in a smaller predicted position error. Since a higher overall quality score indicates better trajectory quality, and a smaller predicted position error indicates higher trajectory accuracy, a monotonicity constraint can be imposed on the mapping function when fitting it, ensuring that the mapping function satisfies a monotonically non-increasing relationship.

[0105] The monotonically non-increasing relationship described above indicates that when the overall quality score of one keyframe is higher than that of another keyframe, the predicted position error corresponding to that keyframe is less than or equal to the predicted position error corresponding to the other keyframe. This monotonically non-increasing constraint ensures that the predicted position error corresponding to a keyframe with a higher overall quality score is generally no greater than the predicted position error corresponding to a keyframe with a lower overall quality score, thus aligning the evaluation direction of the overall quality score with the evaluation direction of the actual position error. Due to the influence of the number of independent control points, measurement noise, and unmodeled errors, local fluctuations may exist between individual keyframes.

[0106] The mapping function can be fitted using a regression model with monotonic constraints, such as ordinal-preserving regression, monotonic piecewise linear regression, monotonic regression tree, or nonlinear regression model with monotonic constraints. This embodiment does not impose any specific limitations on this.

[0107] Furthermore, based on the comprehensive quality score and mapping function corresponding to each keyframe moment in the motion trajectory to be evaluated, the predicted position error of the motion trajectory to be evaluated at each keyframe moment is determined. Here, the predicted position error refers to the error between the trajectory position corresponding to that keyframe moment and the actual position, predicted based on the comprehensive quality score of the keyframe moment and through the mapping function calibrated using independent control points.

[0108] It is worth mentioning that by determining the predicted position error at each keyframe moment, the overall quality score can be converted into an error quantity with actual physical meaning. Compared to judging quality solely based on the overall quality score, the predicted position error can directly reflect the expected position accuracy at the corresponding keyframe moment and can serve as the basis for determining valid, suspicious, and invalid intervals.

[0109] Step S206: Match the predicted position error at the key frame moment with preset position error thresholds for different quality levels; determine the quality level of the key frame moment based on the quality level of the position error thresholds matched with the predicted position error at the key frame moment.

[0110] In this step, based on the application scenario, allowable error range, and usage requirements corresponding to different quality levels of the motion trajectory to be evaluated, the maximum permissible position error for each quality level is determined, resulting in position error thresholds for each quality level. For example, when the first quality level corresponds to a trajectory range that can be directly used for high-precision positioning, map evaluation, or trajectory accuracy statistics, the position error threshold for the first quality level can be determined according to the corresponding high-precision usage requirements. When the second quality level corresponds to a trajectory range that can be used for limited reference, auxiliary analysis, or manual verification, the position error threshold for the second quality level can be determined according to the corresponding acceptable error requirements.

[0111] In this embodiment, by determining the position error threshold based on trajectory usage requirements, statistical results of actual position errors of independent control points, or reliability requirements for quality levels, the comprehensive quality score can be converted into a position error level with actual physical meaning. The position error threshold not only enables the grading of the motion trajectory to be evaluated but also establishes a clear correspondence between quality levels and position errors, avoiding subjective judgments based solely on the quality score output by the system.

[0112] Therefore, the predicted position error at each key frame moment can be compared with the position error thresholds for different quality levels to determine the quality level corresponding to each key frame moment.

[0113] For example, position error thresholds for a first quality level and a second quality level are set. For any keyframe moment, if the predicted position error of the keyframe moment is less than or equal to the position error threshold of the first quality level, the keyframe moment is determined to be of the first quality level; if the predicted position error of the keyframe moment is greater than the position error threshold of the first quality level but less than or equal to the position error threshold of the second quality level, the keyframe moment is determined to be of the second quality level; if the predicted position error of the keyframe moment is greater than the position error threshold of the second quality level, the keyframe moment is determined to be of the third quality level. Here, the first quality level corresponds to the valid interval in the motion trajectory to be evaluated, the second quality level corresponds to the suspicious interval in the motion trajectory to be evaluated, and the third quality level corresponds to the invalid interval in the motion trajectory to be evaluated.

[0114] Furthermore, based on the unit-level consistency flag, aggregate statistics, aggregate degrees of freedom, and observable indicators corresponding to each key frame moment, a quality level reason code is generated to explain why each key frame moment is determined to the corresponding quality level.

[0115] For example, the quality grade reason code includes at least one of the following: The first reason code is used to indicate that the aggregate statistic of the evaluation unit failed the chi-square consistency test, indicating that the posterior residuals in the corresponding evaluation unit are inconsistent with the preset factor model or observation noise model. The second reason code is used to indicate that there are factors in the evaluation unit that have not passed the chi-square consistency test, indicating that the corresponding factor has observational or constraint anomalies. The third reason code is used to indicate that the total theoretical uncertainty index is large, indicating that the observation constraints at the corresponding key frame time are insufficient or the theoretical estimation uncertainty is large. The fourth reason code is used to indicate that a large condition number index indicates that the eigenvalues ​​of the local Fischer information matrix differ significantly in different directions, and the observation constraints in some directions may be weak. The fifth reason code is used to indicate that the location normalization degradation index is less than the preset degradation judgment threshold, indicating that the location variable has degraded; The sixth reason code is used to indicate that the attitude normalization degradation index is less than the preset degradation judgment threshold, indicating that the attitude variable has degradation. The seventh reason code is used to indicate that the predicted position error at the key frame time is greater than the position error threshold. The eighth reason code is used to indicate that multiple reasons exist simultaneously, or that the cause of quality degradation cannot be determined based on a single reason.

[0116] In this embodiment, by establishing a mapping function between the comprehensive quality score and the actual position error using independent control points, the comprehensive quality score at each key frame moment can be converted into a predicted position error. By comparing the predicted position error with the position error threshold, frame-by-frame quality grading can be achieved at each key frame moment. By combining the chi-square consistency test results and observability indicators, the specific reasons for quality degradation can be determined, and the corresponding quality degradation reason code can be output, thereby improving the interpretability of the quality evaluation results of the motion trajectory to be evaluated.

[0117] Step S207: If multiple keyframe moments with the same quality level are not less than a preset minimum number of consecutive frames, the multiple consecutive keyframe moments are determined as candidate quality intervals; based on the quality level of the candidate quality intervals, the trajectory quality interval is determined.

[0118] In this step, based on the quality level and quality level reason code corresponding to each key frame moment, the motion trajectory to be evaluated is divided into continuous intervals to determine the valid interval, suspicious interval, and invalid interval in the motion trajectory to be evaluated, i.e., the trajectory quality interval.

[0119] Specifically, each keyframe moment in the motion trajectory to be evaluated is used as an interval determination unit, and the continuity of the quality level of adjacent keyframe moments is judged according to a preset minimum number of consecutive frames. The minimum number of consecutive frames can be determined based on the frame rate of the motion trajectory to be evaluated and the minimum time window required for interval determination. For example, when the frame rate is 10 Hz and the minimum time window is 0.5 seconds, the minimum number of consecutive frames can be set to 5 frames. Generally, the minimum number of consecutive frames can be determined by multiplying the frame rate by the minimum time window, and the calculation result is rounded.

[0120] Furthermore, when multiple keyframe moments with the same quality level are not less than the minimum number of consecutive frames, these multiple consecutive keyframe moments are divided into a candidate quality interval; when the number of keyframe moments with the same quality level is less than the minimum number of consecutive frames, it can be determined that the corresponding quality level is jittering within the time period, and the time period is identified as a suspicious interval; and based on the quality level within the candidate quality interval, the candidate quality interval is determined as a valid interval, a suspicious interval, or an invalid interval, thereby obtaining different trajectory quality intervals.

[0121] For example, if all keyframe moments within a candidate quality interval are at the first quality level, the candidate quality interval is determined as a valid interval. If all keyframe moments within a candidate quality interval are at the second quality level, the candidate quality interval is determined as a suspicious interval. If all keyframe moments within a candidate quality interval are at the third quality level, the candidate quality interval is determined as an invalid interval.

[0122] Furthermore, the start and end times of the corresponding intervals are determined based on the first and last keyframe times within each interval.

[0123] In addition, the occurrence frequency of each quality level cause code within each interval is counted, and the quality level cause code with the highest occurrence frequency is determined as the dominant cause code for that interval. When multiple cause codes with the same occurrence frequency exist within an interval, one of them can be determined as the dominant cause code according to a preset priority, or multiple cause codes can be used together as the dominant cause code for that interval.

[0124] Therefore, a list of valid intervals, a list of suspicious intervals, and a list of invalid intervals corresponding to the motion trajectory to be evaluated can be output. Each interval list can include the interval start and end timestamps, interval type, interval quality level, and interval dominant reason code.

[0125] Step S208: For suspicious and invalid intervals, determine the corresponding evaluation window; re-execute factor graph optimization based on the original observation data within the evaluation window to obtain the re-optimized state variables, posterior residuals, and local Fischer information matrix; re-execute quality evaluation based on the re-optimized state variables, posterior residuals, and local Fischer information matrix; when the re-optimized interval is re-determined as a valid or suspicious interval, retain the re-optimized trajectory results and corresponding quality labels; when the re-optimized interval is still determined as an invalid interval, determine the supplementary sampling area by extending the trajectory bounding box corresponding to the invalid interval along each direction by a preset distance, and generate the corresponding supplementary sampling task order; output the motion trajectory to be evaluated with quality labels.

[0126] The evaluation window includes key frame moments within the corresponding interval and the original observation data associated with the key frame moments; the convergence conditions used for re-executing factor graph optimization are obtained by adjusting the convergence conditions of the first optimization based on a preset multiplier; the supplementary sampling task sheet includes the supplementary sampling area, quality level reason code, and supplementary sampling priority.

[0127] In this step, suspicious and invalid intervals are further processed, and the motion trajectory to be evaluated with quality labels is output.

[0128] Specifically, for suspicious and invalid intervals, corresponding evaluation windows are determined. Each evaluation window can include keyframe moments within the corresponding interval and the raw observation data associated with those keyframe moments. The raw observation data includes inertial measurement unit measurement data, distortion-corrected point clouds, high-precision prior maps, and loop closure detection results within the evaluation window.

[0129] Furthermore, for a given evaluation window, factor graph optimization is re-executed using the original observation data within the evaluation window to obtain the re-optimized state variables, posterior residuals, and local Fischer information matrix. The re-optimization employs more stringent convergence conditions than the initial optimization. For example, the convergence threshold can be tightened by an order of magnitude to 1×10⁻⁶. 5 Furthermore, the maximum number of iterations is increased to 60. The aforementioned convergence threshold and maximum number of iterations can be adjusted according to the convergence conditions of the baseline optimization at a preset multiplier.

[0130] After re-optimization, the mobile carrier trajectory quality evaluation method described in steps S202 to S206 is re-executed based on the re-optimization results. When the re-optimized interval is re-determined as a valid interval or a suspicious interval, the re-optimized trajectory results and corresponding quality labels are retained; when the re-optimized interval is still determined as an invalid interval, supplementary sampling is performed on the invalid interval.

[0131] Specifically, for areas still identified as invalid after re-optimization, the areas requiring re-data collection are marked, and corresponding re-collection task orders are generated. The re-collection area can be determined based on the bounding box of the trajectory corresponding to the invalid interval, and the bounding box can be extended by a preset distance in each direction. For example, the bounding box can be extended outward by 0.5 meters in each direction as the re-collection range. The extension distance can be adjusted according to the ranging range of the LiDAR and the operational coverage requirements. The re-collection task order can include the re-collection area, the corresponding reason code, and the re-collection priority. The trajectory bounding box refers to the spatial enclosure of the trajectory segment determined to be an invalid or suspicious interval within the map system, resulting in a rectangular area that covers the entire trajectory segment; in a 3D scene, this is typically represented as an axis-aligned 3D cuboid bounding box.

[0132] Furthermore, for invalid intervals where supplementary sampling is not feasible, or intervals that are still unqualified after re-evaluation following supplementary sampling, the interval is marked as "cannot be used as a true value" when outputting the trajectory, and the corresponding quality level reason code and handling record are attached.

[0133] Therefore, a quality-annotated motion trajectory can be output. This quality-annotated trajectory can include the target pose, quality level, interval identifier, quality level reason code, and processing record for each keyframe. The quality level can include a first quality level, a second quality level, and a third quality level, or represented as level A, level B, and level C, respectively; the interval identifier can include valid intervals, suspicious intervals, and invalid intervals; the processing record records whether the corresponding interval has undergone re-optimization, re-sampling, or marking retention, as well as the corresponding number of processing steps or the processing result.

[0134] When using quality-annotated motion trajectories for SLAM (Simultaneous Localization and Mapping) evaluation, only valid intervals can be selected for accuracy statistics, or suspicious intervals can be selectively included according to evaluation requirements. Invalid intervals are not used for true accuracy statistics but can be used for result visualization or directly removed. The quality level reason code is used to explain the abnormal reasons corresponding to the suspicious or invalid intervals, and the handling record is used to explain the handling process taken for that interval.

[0135] In another embodiment of this application, a method for generating motion trajectories to be evaluated is provided, which can be applied to GNSS denial scenarios such as underground parking lots, underground pipe networks, mining subsidence areas, hydropower station water diversion tunnels, and thermal power plant boilers.

[0136] In this embodiment, an underground parking lot is used as the target space. The underground parking lot is in a GNSS-covered environment, and the mobile carrier is an unmanned aerial vehicle (UAV) equipped with a 10Hz non-repeating scan LiDAR (light detection and ranging) and a 200Hz IMU. The UAV flies within the underground parking lot and performs mapping.

[0137] Before drone-based aerial mapping, a high-precision 3D scanner was used to perform multi-station static scanning of the underground parking lot. Point cloud data collected from different stations were then registered, and the registered point cloud data was used to create a high-precision prior map of the target space. Simultaneously, a total station was used to measure the coordinates of 72 pre-positioned targets within the underground parking lot, obtaining the coordinates of each target in the target space. The coordinates of these 72 targets were not used in the generation of the trajectory to be evaluated; instead, they served as independent reference benchmarks for subsequent external verification of the generated trajectory.

[0138] After establishing a high-precision prior map, the drone is controlled to move within the underground parking lot, simultaneously acquiring point cloud data collected by LiDAR and motion data measured by IMU, and generating a motion trajectory to be evaluated.

[0139] Specifically, the point cloud data acquired by the LiDAR and the motion data measured by the IMU are synchronized in time, and the extrinsic parameters between the LiDAR and the IMU are calibrated. Specifically, since the LiDAR and IMU each have their own hardware clocks, the scanning time of the LiDAR point cloud data is first converted to the IMU's time reference, ensuring that the LiDAR point cloud data and IMU motion data are on the same time reference. Further, based on the installation position and orientation of the LiDAR and IMU, the extrinsic parameters between the LiDAR coordinate system and the IMU's carrier system are determined. These parameters are used to convert the laser scanning points acquired by the LiDAR to the carrier system or to the map system.

[0140] In this embodiment, the LiDAR scanning frequency is 10Hz, and the IMU measurement frequency is 200Hz. Since the IMU measurement frequency is higher than the LiDAR scanning frequency, multiple sets of IMU motion data exist between adjacent LiDAR point cloud frames. After time synchronization and extrinsic parameter calibration, the LiDAR point cloud and IMU motion data are correlated according to the scanning time of the LiDAR point cloud and the measurement time of the IMU.

[0141] Furthermore, the initial pose of the UAV in the high-precision prior map is determined. Specifically, during the high-precision prior map establishment process, the attitude information of the high-precision 3D scanner in the map system is recorded to obtain the attitude reference of the scanning device in the target space. After the UAV enters the underground parking lot, based on the relative attitude measured by the UAV's IMU at the acquisition starting point, combined with the attitude reference of the high-precision 3D scanner in the corresponding area, the initial attitude estimate of the UAV in the map system is determined. Then, the first frame of LiDAR point cloud acquired by the UAV at the acquisition starting point is used as the source point cloud, and the high-precision prior map is used as the target point cloud. Using the initial attitude estimate and the translational prior adjacent to the acquisition starting point as the initial registration conditions, coarse registration is performed on the first frame of LiDAR point cloud and the high-precision prior map through G-ICP (Generalized Iterative Closest Point) to obtain the initial pose of the UAV in the map system.

[0142] Furthermore, forward pre-integration and state prediction are performed frame-by-frame on the motion data measured by the IMU to obtain the predicted values ​​of the UAV's state variables at each key frame moment. Specifically, representative point clouds are selected from the multi-frame point cloud data acquired by LiDAR as key frame point clouds, and the scanning time of the key frame point clouds is determined as the corresponding key frame moment. The UAV's state variables at each key frame moment include attitude variables, position variables, velocity variables, gyroscope bias variables, and accelerometer bias variables. For two adjacent key frame moments, motion data including acceleration and angular velocity measurements measured by the IMU within that time period are acquired, and forward pre-integration is performed on the motion data to convert multiple sets of IMU measurement data between two adjacent key frame moments into relative motion increments. The relative motion increments include pre-integrated attitude increments, pre-integrated velocity increments, and pre-integrated position increments. Thus, based on the predicted values ​​of the state variables at the previous key frame moment and the pre-integration results between adjacent key frame moments, the predicted values ​​of the state variables at the current key frame moment are recursively determined. The predicted values ​​of the state variables are used to provide initial values ​​for subsequent factor graph optimization and to construct the IMU factors between adjacent keyframe moments.

[0143] Furthermore, motion data measured by the IMU is used to perform backpropagation distortion correction on the keyframe point cloud, resulting in a distorted point cloud. Specifically, since LiDAR takes a certain amount of time to complete a point cloud scan, different laser scanning points in the same frame correspond to different sampling times. During the drone's movement, the drone's attitude and position differ at different sampling times. If all laser scanning points in the same frame are directly considered as data collected at the same time, it will lead to geometric distortion of the point cloud. Therefore, the scanning time of the keyframe point cloud is used as the frame reference time. Based on the sampling time of each laser scanning point in the keyframe point cloud, motion data measured by the IMU is used to backpropagate each laser scanning point, compensating each laser scanning point to the coordinate system corresponding to the frame reference time. In this way, each laser scanning point in the keyframe point cloud is equivalent to being collected at the same frame reference time, resulting in a distorted point cloud corresponding to the keyframe point cloud.

[0144] Furthermore, point-to-plane matching is performed between the distorted point cloud and the high-precision prior map to construct map matching factors. Specifically, for each distorted point in the distorted point cloud corresponding to the current keyframe point cloud, a corresponding local map point is searched in the high-precision prior map, and the corresponding matching plane is obtained by fitting the local map point. The matching plane includes a reference point and a unit normal vector on the plane. The distorted points are transformed into the map system, and the signed distance from the distorted points to the corresponding matching plane is calculated to obtain the map matching factor residuals. The map matching factor residuals corresponding to each distorted point in the same keyframe point cloud are assembled to obtain the map matching factor at the current keyframe time. Furthermore, the covariance matrix of the map matching factor is determined based on the observation noise when matching the distorted point cloud with the high-precision prior map, and the information matrix of the map matching factor is determined based on the covariance matrix of the map matching factor. At the same time, the pre-integration results between adjacent keyframe times are constructed as IMU factors. The IMU factor is used to describe the relative motion constraints of the UAV between adjacent keyframe moments, and the covariance matrix and information matrix of the IMU factor are determined based on the acceleration noise and angular velocity noise measured by the IMU.

[0145] In this embodiment, a loop closure factor can also be constructed based on the revisit relationships during the actual acquisition process. The loop closure factor is used to describe the global consistency constraints between non-adjacent keyframe moments with revisit relationships. The covariance matrix of the loop closure factor can be determined based on the observation noise corresponding to the loop closure matching.

[0146] Furthermore, a factor graph is constructed based on the map matching factor, IMU factor, and loop closure factor, and the factor graph is jointly optimized to obtain the UAV's trajectory to be evaluated and the input data required for subsequent quality evaluation. Specifically, the UAV's state variables at each keyframe are used as nodes in the factor graph, and the map matching factor, IMU factor, and loop closure factor are used as factors associated with the state variables to construct the factor graph. The factor graph is used to describe the relationship between the UAV's state variables at different keyframes and different observation constraints. After constructing the factor graph, the predicted values ​​of the state variables at each keyframe are used as the initial values ​​for optimization. The factor graph is jointly optimized using the residuals, covariance matrices, and information matrices corresponding to the map matching factor, IMU factor, and loop closure factor. By adjusting the values ​​of the state variables at each keyframe, the weighted sum of squares of the residuals of each factor satisfies the preset optimization objective, and the state variables after factor graph optimization convergence are obtained. Therefore, based on the attitude and position variables in the state variables after factor graph optimization convergence, the target pose of the UAV at each key frame moment is determined, and based on the target pose at multiple key frame moments, the motion trajectory of the UAV to be evaluated in the underground parking lot is determined.

[0147] Furthermore, after the factor graph optimization converges, the converged state variables are substituted into the residual functions of each factor to calculate the posterior residuals of each factor. The residual functions of each factor are then linearized based on the converged state variables to obtain the Jacobian matrix corresponding to each factor. Simultaneously, based on the Jacobian and information matrices of each factor, the local Fischer information matrix corresponding to each keyframe time is determined.

[0148] Therefore, this embodiment can obtain the input data required for subsequent quality evaluation of the motion trajectory to be evaluated. The input data includes: factor graph, covariance matrix and information matrix of each factor, motion trajectory of the UAV to be evaluated, posterior residuals of each factor after factor graph optimization convergence, Jacobian matrix corresponding to each factor, local Fischer information matrix corresponding to each key frame time, and coordinates of 72 total station targets as independent reference benchmarks.

[0149] In this embodiment, the motion trajectory to be evaluated in the underground parking lot was externally verified using the coordinates of 72 targets obtained by total station measurement. The mean target position error was 0.0325m, and the RMSE (Root Mean Square Error) was 0.0345m. Under GNSS full obscuration conditions, the generated motion trajectory to be evaluated can achieve the effect of centimeter-level true value trajectory.

[0150] It should be noted that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0151] Furthermore, such as Figure 3 As shown, as a specific implementation of the above-mentioned mobile carrier motion trajectory quality evaluation method, this application embodiment provides a mobile carrier motion trajectory quality evaluation device 300, which includes: an acquisition module 301, a prediction module 302, and an evaluation module 303.

[0152] The acquisition module 301 is used to acquire the motion trajectory to be evaluated of the mobile carrier. The motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile carrier. The factor graph includes factors associated with the state variables of the mobile carrier at each key frame time during the motion process. The prediction module 302 is used to substitute the state variables after factor graph optimization convergence into the residual function of each factor to calculate the posterior residual of the factor; and, based on the local Fisher information matrix corresponding to the key frame time after factor graph optimization convergence, to determine the observable index corresponding to the key frame time; and, based on the posterior residual and the observable index, to determine the comprehensive quality score corresponding to the key frame time; and, based on the comprehensive quality score and the measured position of the independent control point, to determine the prediction position error corresponding to the key frame time, where the independent control point is a spatial reference point with a fixed position in the target space of the moving vehicle. The evaluation module 303 is used to determine the quality level corresponding to the key frame moment based on the predicted position error, and to divide the motion trajectory to be evaluated into intervals based on the quality level to obtain the trajectory quality interval of the motion trajectory to be evaluated.

[0153] Specific limitations regarding the mobile vehicle trajectory quality evaluation device can be found in the limitations of the mobile vehicle trajectory quality evaluation method described above, and will not be repeated here. Each module in the aforementioned mobile vehicle trajectory quality evaluation device can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the corresponding operations of each module.

[0154] Based on the above, Figures 1 to 2 Accordingly, embodiments of this application also provide a readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method. Figures 1 to 2 The method for evaluating the quality of the motion trajectory of a mobile carrier is shown.

[0155] Based on this understanding, the technical solution of this application can be embodied in the form of a software product. The software product can be stored in a non-volatile storage medium (such as a CD-ROM, USB flash drive, or portable hard drive), and includes several instructions to cause a computer device (such as a personal computer, server, or network device) to execute the methods described in the various implementation scenarios of this application.

[0156] Based on the above, Figures 1 to 2 The method shown, and Figure 3 To achieve the above objectives, the present application also provides a computer device, specifically a personal computer, server, network device, etc., as shown in the virtual device embodiment. This computer device includes a storage medium and a processor; the storage medium stores a computer program; the processor executes the computer program to achieve the above-described objectives. Figures 1 to 2 The method for evaluating the quality of the motion trajectory of a mobile carrier is shown.

[0157] Optionally, the computer device may also include a user interface, a network interface, a camera, radio frequency (RF) circuitry, sensors, audio circuitry, a Wi-Fi module, etc. The user interface may include a display screen, input units such as a keyboard, etc., and optional user interfaces may also include USB ports, card reader ports, etc. The network interface may optionally include standard wired interfaces, wireless interfaces (such as Bluetooth interfaces, Wi-Fi interfaces), etc.

[0158] Those skilled in the art will understand that the computer device structure provided in this embodiment does not constitute a limitation on the computer device, and may include more or fewer components, or combine certain components, or have different component arrangements.

[0159] The storage medium may also include an operating system and a network communication module. The operating system is a program that manages and stores the hardware and software resources of a computer device, supporting the operation of information processing programs and other software and / or programs. The network communication module is used to enable communication between the various components within the storage medium, as well as communication with other hardware and software within the physical device.

[0160] Through the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented by means of software plus necessary general-purpose hardware platform, or the embodiments of this application can be implemented by hardware.

[0161] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of a preferred embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing this application. Those skilled in the art will understand that the modules in the apparatus of the embodiment can be distributed within the apparatus of the embodiment as described, or can be modified to be located in one or more apparatuses different from this embodiment. The modules of the above-described embodiment can be combined into one module, or further divided into multiple sub-modules.

[0162] The serial numbers in this application are for descriptive purposes only and do not represent the superiority or inferiority of any particular implementation scenario. The above disclosures are merely a few specific implementation scenarios of this application; however, this application is not limited thereto, and any variations conceived by those skilled in the art should fall within the protection scope of this application.

Claims

1. A method for evaluating the quality of a mobile carrier's motion trajectory, characterized in that, The method includes: The motion trajectory to be evaluated of the mobile vehicle is obtained; the motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile vehicle, the factor graph including factors associated with the state variables of the mobile vehicle at each key frame time during the motion. Substitute the state variables after factor graph optimization convergence into the residual function of each factor to calculate the posterior residual of the factor; Based on the local Fischer information matrix corresponding to the key frame moment after factor graph optimization convergence, the observable index corresponding to the key frame moment is determined. Based on the posterior residual and the observable index, the comprehensive quality score corresponding to the key frame moment is determined; Based on the comprehensive quality score and the measured position of the independent control point, the predicted position error corresponding to the key frame moment is determined. The independent control point is a spatial reference point with a fixed position in the target space of the moving vehicle. Based on the predicted position error, the quality level corresponding to the key frame moment is determined, and based on the quality level, the motion trajectory to be evaluated is divided into intervals to obtain the trajectory quality interval of the motion trajectory to be evaluated.

2. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 1, characterized in that, The observable indicators include the total theoretical uncertainty indicator and the condition number indicator. The determination of the observable indicators corresponding to the key frame moments based on the local Fisher information matrix after convergence of factor graph optimization specifically includes: Based on the state variables after the factor graph optimization convergence, the residual function of the factor is linearized to obtain the Jacobian matrix of the factor at the state variables after the factor graph optimization convergence. Based on the Jacobian matrix and the information matrix determined by the factors when constructing the factor graph, the local Fischer information matrix corresponding to the key frame time after the factor graph optimization convergence is determined. Based on the inverse matrix of the local Fischer information matrix corresponding to the key frame moment, the total theoretical uncertainty index corresponding to the key frame moment is determined. The eigenvalues ​​of the local Fischer information matrix corresponding to the key frame time are obtained by performing eigenvalue decomposition. Based on the maximum and minimum eigenvalues ​​of the local Fischer information matrix corresponding to the key frame time, the condition number index corresponding to the key frame time is determined.

3. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 2, characterized in that, The determination of the total theoretical uncertainty index corresponding to the key frame time based on the inverse matrix of the local Fischer information matrix corresponding to the key frame time specifically includes: The local Fischer information matrix is ​​truncated according to a preset eigenvalue threshold. The eigenvalues ​​that are greater than or equal to the eigenvalue threshold of the local Fischer information matrix are inverted to obtain the inverse matrix of the truncated and regularized local Fischer information matrix. Based on the trace of the inverse matrix of the truncated and regularized local Fischer information matrix, the total theoretical uncertainty index corresponding to the key frame moment is determined.

4. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 1, characterized in that, The observable metrics include position-normalized degradation metrics and attitude-normalized degradation metrics. The local Fischer information matrix corresponding to the keyframe time after convergence based on factor graph optimization is used to determine the observable metrics corresponding to the keyframe time, specifically including: The local Fischer information matrix corresponding to the key frame time is divided into blocks according to the position variables and attitude variables in the state variables to obtain the position submatrix corresponding to the position variables and the attitude submatrix corresponding to the attitude variables at the key frame time. Eigenvalue decomposition is performed on the position submatrix and the attitude submatrix respectively to obtain the eigenvalues ​​of the position submatrix and the attitude submatrix; Based on the ratio of the minimum eigenvalue to the maximum eigenvalue of the position submatrix, the position normalization degradation index of the position variable at the key frame time is determined. Based on the ratio of the minimum eigenvalue to the maximum eigenvalue of the attitude submatrix, the attitude normalization degradation index of the attitude variables at the key frame time is determined.

5. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 1, characterized in that, Before the step of determining the comprehensive quality score corresponding to the keyframe moment based on the posterior residual and the observable index, the method further includes: Each keyframe moment in the motion trajectory to be evaluated is taken as an evaluation unit, and each factor is assigned to the corresponding evaluation unit according to the preset factor assignment rules. Based on the posterior residuals of the factors and the covariance matrix of the factors when constructing the factor graph, the normalized posterior residuals of the factors are calculated. The degrees of freedom of the factor are determined based on the residual dimension of the factor; The normalized posterior residuals of the factors within the evaluation unit are aggregated, and the degrees of freedom of the factors within the evaluation unit are accumulated to obtain the aggregated statistics and aggregated degrees of freedom of the evaluation unit.

6. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 5, characterized in that, The observable indicators include the total theoretical uncertainty indicator, the condition number indicator, the position normalized degradation indicator, and the attitude normalized degradation indicator. The determination of the comprehensive quality score corresponding to the keyframe moment based on the posterior residual and the observable indicators specifically includes: Calculate the cumulative probability of the aggregate statistics of the evaluation unit under the chi-square distribution of the aggregate degrees of freedom of the evaluation unit, and perform an inverse transformation on the cumulative probability to obtain the chi-square consistency score of the key frame moment corresponding to the evaluation unit. Logarithmic compression and inverse normalization are performed on the condition number index corresponding to the key frame time to obtain the condition number score corresponding to the key frame time. The total theoretical uncertainty index corresponding to the key frame moment is normalized and inverted to obtain the observable score corresponding to the key frame moment. The position-normalized degradation index and the pose-normalized degradation index corresponding to the key frame moment are weighted and summed to obtain the degradation score corresponding to the key frame moment. The chi-square consistency score, condition number score, observability score, and degradation score of the key frame moment are weighted and combined to obtain the comprehensive quality score corresponding to the key frame moment.

7. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 1, characterized in that, The determination of the prediction position error corresponding to the key frame time based on the comprehensive quality score and the measured positions of independent control points specifically includes: Based on the key frame moments associated with the timestamps of independent control points, determine the trajectory positions of the independent control points in the motion trajectory to be evaluated; Based on the trajectory position and measured position of the independent control point, determine the actual position error corresponding to the independent control point; A calibration sample is generated based on the comprehensive quality score and actual position error of the independent control point, and the mapping relationship between the comprehensive quality score and actual position error in the calibration sample is fitted to obtain the mapping function. The comprehensive quality score of the independent control point is determined based on the observable indicators of the key frame time associated with the timestamp of the independent control point. Based on the comprehensive quality score of the key frame moment and the mapping function, the prediction position error of the key frame moment is determined.

8. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 1, characterized in that, The process of determining the quality level corresponding to the keyframe moment based on the predicted position error specifically includes: The predicted position error at the key frame moment is matched with preset position error thresholds for different quality levels; The quality level of the key frame moment is determined based on the quality level of the position error threshold matched with the predicted position error at the key frame moment.

9. The method for evaluating the motion trajectory quality of a mobile carrier according to claim 8, characterized in that, The step of dividing the motion trajectory to be evaluated into intervals based on the quality level to obtain the trajectory quality interval of the motion trajectory to be evaluated specifically includes: If multiple keyframe moments with the same quality level are not less than a preset minimum number of consecutive frames, the multiple consecutive keyframe moments are determined as candidate quality intervals. The trajectory quality interval is determined based on the quality level of the candidate quality interval.

10. A device for evaluating the quality of motion trajectory of a mobile carrier, characterized in that, The device includes: The acquisition module is used to acquire the motion trajectory to be evaluated of the mobile carrier; the motion trajectory to be evaluated is obtained by factor graph optimization of the factor graph corresponding to the mobile carrier, and the factor graph includes factors associated with the state variables of the mobile carrier at each key frame time during the motion process; The prediction module is used to substitute the state variables after factor graph optimization convergence into the residual function of each factor to calculate the posterior residual of the factor; and to determine the observable index corresponding to the key frame time based on the local Fischer information matrix corresponding to the key frame time after factor graph optimization convergence; and to determine the comprehensive quality score corresponding to the key frame time based on the posterior residual and the observable index; and to determine the prediction position error corresponding to the key frame time based on the comprehensive quality score and the measured position of the independent control point, wherein the independent control point is a spatial reference point with a fixed position in the target space of the mobile carrier's motion. The evaluation module is used to determine the quality level corresponding to the key frame moment based on the predicted position error, and to divide the motion trajectory to be evaluated into intervals based on the quality level to obtain the trajectory quality interval of the motion trajectory to be evaluated.

Images