Positioning mapping method and device for GNSS limited degraded environment and mobile carrier

Abstract

The application discloses a positioning and mapping method and device for a GNSS limited degenerative environment and a mobile carrier, and relates to the technical field of navigation and positioning. The method comprises the following steps: constructing a map matching constraint factor and an inertial motion constraint factor based on multi-source sensor data and an envelope parameter outside a protective structure, and extracting a degenerative degree and a degenerative direction of a local information matrix; when a collision event is detected, determining a contact deformation amount according to an impact residual and the envelope parameter; constructing a collision plane constraint factor by taking the complementarity of a collision normal and the degenerative direction as a constraint; incorporating the three types of constraint factors into a factor graph for joint optimization to obtain an optimized first trajectory pose and a posterior residual and a covariance matrix; verifying the first trajectory pose and / or the collision plane constraint factor, and outputting a positioning trajectory and a fused map based on a verification result. The method can effectively suppress pose drift and improve positioning robustness and mapping accuracy when collision causes abnormal sensor data.

Description

Technical Field

[0001] This application relates to the field of positioning technology, and in particular to a positioning and mapping method, apparatus and mobile carrier for GNSS-constrained and degraded environments. Background Technology

[0002] Currently, in GNSS-denied environments, mobile vehicles typically rely on onboard LiDAR (Light Detection and Ranging) and inertial measurement units (IMUs) or visual odometry for autonomous positioning. However, single SLAM algorithms are prone to degradation in narrow, repetitive, dusty, dark, and metallic reflective environments. When a mobile vehicle enters long straight passages, flat walls, or areas with repetitive structures, the scan-to-map matching objective function lacks sufficient curvature along some pose directions, resulting in weakly observable degrees of freedom. If fixed weights are still used to receive such map matching results, erroneous constraints may propagate during global optimization, reducing trajectory reliability. Summary of the Invention

[0003] In view of this, this application provides a positioning and mapping method, apparatus and mobile vehicle for GNSS-restricted and degraded environments, which solves the problems of low positioning accuracy, poor continuity and insufficient reliability in degraded GNSS denial scenarios where physical collisions may occur.

[0004] According to a first aspect of this application, a positioning and mapping method for GNSS-constrained and degraded environments is provided, the method comprising: Based on the multi-source sensor data of the mobile carrier and the outer envelope parameters of the protective structure, a map matching constraint factor and an inertial motion constraint factor are constructed. A local information matrix is ​​constructed based on the map matching constraint factor and the inertial motion constraint factor, and eigenvalue decomposition is performed to extract the degree and direction of degradation of the state variables of the local information matrix as the environment dynamically changes. When a collision event is detected involving the mobile carrier, the contact deformation of the protective structure is determined based on the impact residual of the mobile carrier within the time window before and after the collision and the outer envelope parameter. Using the complementarity between the collision normal and the degradation direction as a constraint, and based on the collision normal, the contact deformation, the outer envelope parameter, and the collision plane reference point, a collision plane constraint factor is constructed to characterize the distance from the center of the moving carrier to the contact plane. The map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor are incorporated into the factor graph in the same frame for joint optimization to obtain the optimized first trajectory pose. Verify the first trajectory pose and / or the collision plane constraint factor; Based on the verification results, the positioning trajectory of the mobile vehicle is output and a fused map is constructed.

[0005] According to a second aspect of this application, a positioning and mapping device for GNSS-constrained and degraded environments is provided, the device comprising: The constraint construction module is used to construct map matching constraint factors and inertial motion constraint factors based on the multi-source sensor data of the mobile carrier and the outer envelope parameters of the protective structure; and to construct a local information matrix based on the map matching constraint factors and the inertial motion constraint factors, and perform eigenvalue decomposition to extract the degradation degree and degradation direction of the state variables of the local information matrix as the environment dynamically changes; and when a collision event is detected in the mobile carrier, to determine the contact deformation of the protective structure based on the impact residual of the mobile carrier within the time window before and after the collision and the outer envelope parameters; and to construct a collision plane constraint factor to characterize the distance from the center of the mobile carrier to the contact plane, based on the collision normal, the contact deformation, the outer envelope parameters, and the collision plane reference point, with the complementarity between the collision normal and the degradation direction as constraints. The positioning module is used to incorporate the map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor into the factor map in the same frame to perform joint optimization and obtain the optimized first trajectory pose. The verification module is used to verify the first trajectory pose and / or the collision plane constraint factor; and to output the positioning trajectory of the mobile vehicle and construct a fused map based on the verification results.

[0006] According to a third 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 positioning and mapping method for GNSS-constrained and degraded environments.

[0007] According to a fourth aspect of this application, a mobile carrier is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, it implements the steps of the above-described positioning and mapping method for GNSS-constrained and degraded environments.

[0008] Using the above technical solution, map matching constraint factors and inertial motion constraint factors are constructed based on multi-source sensor data and the outer envelope parameters of the protective structure. The degree and direction of environmental degradation are evaluated in real time through eigenvalue decomposition of the local information matrix. When a collision occurs, the contact deformation is determined using the impact residual and the outer envelope parameters. Then, the collision plane constraint factor is constructed using the complementarity between the collision normal and the degradation direction as constraints. Subsequently, the three types of constraint factors are incorporated into the factor map in the same keyframe for joint optimization to obtain the first trajectory pose and its posterior residual and covariance matrix. The collision constraints are then rigorously verified through the chi-square consistency statistical test of the normalized posterior residual and the observability gain test of the local information matrix. The trajectory is also double-verified by combining the quality level consistency of consecutive frames. Finally, the positioning trajectory is output and a map integrating the environmental degradation state and collision attributes is constructed. Firstly, by introducing a collision plane constraint factor and ensuring its access direction complements the system degradation direction, collision events, originally considered interference, are transformed into effective observations that accurately fill the weakest direction in the information matrix. This allows collision information to specifically compensate for information loss in the degradation direction, improving system observability and pose estimation accuracy under extreme degradation environments such as GNSS denial. This effectively solves the problems of low positioning accuracy, poor continuity, and insufficient reliability of related technologies in complex scenarios where degradation and potential physical collisions are possible. Secondly, while performing successive quality checks on collision constraints using chi-square tests of posterior residuals and observability gain tests, a secondary quality inspection link at the trajectory level is constructed through continuous frame quality level consistency verification. This effectively prevents abnormal or low-quality collision observations from contaminating the optimization results, ensuring that the final output positioning trajectory has high confidence in both local smoothness and global consistency. This helps improve the long-term robustness and mission safety of mobile vehicles in complex dynamic scenarios.

[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 positioning and mapping method for GNSS-constrained and degraded environments provided in an embodiment of this application is shown. Figure 2 This illustration shows a frame-by-frame quality classification and effective range provided in an embodiment of this application; Figure 3This paper shows a structural block diagram of a positioning and mapping device for GNSS-constrained and degraded environments provided in an embodiment of this application; Figure 4 A schematic diagram of the electronic structure of the mobile carrier 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 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 present. Furthermore, the term “and / or” as used herein includes all or any units 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] Before introducing the solutions of the embodiments of this application, the names of the technical terms that may be involved in the embodiments of this application will be explained first.

[0016] Mobile carrier: refers to a carrier capable of autonomous movement in space, including but not limited to drones, mobile robots, and autonomous vehicles. In this embodiment, the mobile carrier is externally enclosed by a protective structure to allow it to continue performing its mission in the event of a collision.

[0017] Protective structure: refers to the physical structure that surrounds the outside of a moving vehicle, used to absorb impact energy and protect the vehicle body when it collides with the environment. Protective structures can be constructed as spheres, ellipsoids, cylinders, convex polyhedra, or pre-defined three-dimensional envelope structures.

[0018] Collision normal: refers to the unit direction vector from the center of the carrier to the contact surface when a collision occurs, defined in the world coordinate system.

[0019] Contact deformation: refers to the maximum compression or contact depth of the protective structure during a collision.

[0020] Factor graph: refers to an undirected probabilistic graphical model in which nodes represent variables to be estimated (e.g., pose, velocity, bias, etc.) and edges represent constraint factors (e.g., IMU pre-integration factor, map matching factor, loop closure factor, collision plane constraint factor, etc.). The optimal state estimate is obtained by jointly optimizing the residuals of all factors.

[0021] After introducing the technical terms that may be involved in the embodiments of this application, the application scenarios of the embodiments of this application will be introduced below.

[0022] The positioning and mapping method for GNSS-constrained and degraded environments provided in this application can be applied to autonomous navigation and positioning scenarios for mobile vehicles in environments with GNSS (global navigation satellite system) signal rejection, severe signal attenuation, or local geometric degradation, including but not limited to: pipe gallery inspection, tunnel mapping, boiler internal inspection, mine exploration, water diversion tunnel inspection, and underground pipeline network survey. In these scenarios, the mobile vehicle is usually enclosed by a spherical protective cage, and may collide with environmental surfaces (e.g., pipe gallery sidewalls, tunnel roofs, boiler inner walls) during flight or travel.

[0023] Currently, in GNSS-denied environments, mobile vehicles typically rely on inertial navigation systems, lidar-based SLAM (Simultaneous Localization and Mapping), or visual odometry for autonomous localization. However, single SLAM algorithms are prone to degradation in narrow, repetitive, dusty, dark, and metallic reflective environments. When a mobile vehicle enters long straight passages, flat walls, or areas with repetitive structures, the scan-to-map matching objective function lacks sufficient curvature along some pose directions, resulting in weakly observable degrees of freedom. If fixed weights are still used to receive such map matching results, erroneous constraints may propagate in the global optimization process, reducing trajectory reliability. Simultaneously, collision-permissible protective vehicles frequently come into contact with protective structures when operating in confined spaces. Existing systems treat collisions as abnormal noise or safety incidents, only handling them with emergency stops, bounces, or data removal, without utilizing the geometric information inherent in the collisions. Degradation perception and collision processing are executed independently, failing to form a closed loop within the same keyframe, and collision information cannot supplement the currently weakest observable direction.

[0024] To address the aforementioned problems, this embodiment provides a positioning and mapping method for GNSS-constrained and degraded environments, such as... Figure 1 As shown, the method includes: Step 101: Based on the multi-source sensor data of the mobile carrier and the outer envelope parameters of the protective structure, construct the map matching constraint factor and the inertial motion constraint factor.

[0025] The multi-source sensor data includes lidar point cloud data, inertial measurement data, visual data, and prior map data. The outer envelope parameters of the protective structure include its geometric and mechanical parameters.

[0026] Map matching constraint factors are constraint terms constructed from the geometric registration relationship between point cloud data and prior maps or local submaps. They are used to characterize the spatial consistency between the observed pose of the mobile vehicle in the global coordinate system and the map environment. They are usually measured by the nearest neighbor matching residual from point to plane or the residual of normal distribution transformation, and are used as absolute observation constraints in the factor map to suppress long-term drift.

[0027] The inertial motion constraint factor refers to the constraint term constructed by the pre-integrated recursive quantity of the inertial measurement unit (IMU) on the mobile vehicle between adjacent keyframes. It is used to characterize the dynamic continuity relationship of the mobile vehicle in the three motion state quantities of rotation, velocity and position. Its residual is composed of the deviation between the pre-integrated measurement value and the state transfer prediction value, and is weighted by the inverse of its covariance matrix as the information matrix. It serves as the relative motion constraint in the factor graph to ensure the smoothness and high-frequency continuity of the local trajectory.

[0028] In practical application scenarios, step 101 specifically includes the following steps: Step 101-1: Within the time window before and after the collision of the mobile carrier, acquire point cloud data and inertial measurement data of the space where the mobile carrier is located, and synchronize and align them using the clock of the inertial measurement data as the time reference.

[0029] For example, the lidar mounted on the mobile carrier is a non-repeating scanning lidar, and the inertial measurement unit (IMU) consists of dual redundant accelerometers and gyroscopes. Point cloud and inertial measurement data are aligned to a unified time base using hardware timestamps, with the master clock of the inertial measurement data as the time base, and the point cloud timestamps aligned via linear interpolation. The extrinsic parameters (including spatial extrinsic participation time offset) of the lidar and IMU are pre-calibrated and determined. Simultaneously, a high-precision static prior map is acquired, generated by multi-station 3D laser scanning registration, establishing a unified world coordinate system.

[0030] Step 101-2: Perform pre-integration calculation on the inertial data between adjacent keyframes to calculate the state increment, and construct the inertial motion constraint factor based on the state increment.

[0031] Specifically, the system state is defined as a state vector containing rotation, position, velocity, gyroscope bias, and accelerometer bias. Pre-integration is performed on the inertial measurement data between adjacent keyframes, recursively deriving the pre-integrated values ​​according to the propagation formula for rotation, velocity, and position. Gravity, accelerometer bias, and noise are included in the propagation process. The predicted state is obtained from the pre-integrated values ​​and the previous state, and this predicted state serves as the initial value for subsequent optimization steps. The pre-integrated residuals of the inertial measurement unit are used to construct norms for rotation, velocity, and position components, and Mahalanobis distances are taken. The corresponding covariance matrices are obtained by propagating the pre-integrated covariance. The pre-integrated residuals of the inertial measurement unit are assembled into inertial motion constraint factors (IMU factors), which are then connected to adjacent keyframes to provide prior information about the carrier motion.

[0032] Step 101-3: Through backpropagation of inertial measurement data, distortion correction is performed on each measurement point in the point cloud data.

[0033] Step 101-4: Perform point-to-plane matching between the distortion-free point cloud data and the prior map to construct map matching constraint factors.

[0034] Specifically, for each point, the nearest plane is retrieved in the prior map, and the point-to-plane residual is calculated using the following formula. All matching residuals are then assembled into a map matching factor. ; ; In the formula, Match residuals to the map; Let be the unit normal vector of the a priori plane in the world coordinate system G; Let the nearest point on the prior map be any point on the plane. This is the extrinsic transformation matrix from the IMU coordinate system to the lidar coordinate system L; This is the pose transformation matrix from the world coordinate system G to the IMU coordinate system; These are the original laser observation point coordinates at time k in the lidar coordinate system L; This is the observation noise vector for the laser point.

[0035] In this embodiment, the lidar point cloud data and inertial measurement data are first synchronized and aligned using the IMU clock as a reference. Then, for each measurement point within the scan frame, the inertial measurement data is iteratively propagated from the sampling time to the frame reference time, with point-by-point compensation based on position, velocity, and attitude. This transforms each measurement point to a unified reference time, eliminating the influence of platform motion on the point cloud geometry and obtaining distortion-free point cloud data. The backward propagation of IMU data is used to correct motion distortion at each measurement point in the point cloud, effectively eliminating point cloud blurring and stretching caused by continuous carrier motion within the scanning cycle. The distortion-free, high-quality point cloud is then precisely matched from point to plane with a priori map to construct map matching constraint factors, providing global absolute pose observation. Finally, both types of factors are incorporated into a complementary structure formed by the factor graph framework, achieving a unity of local continuity and global consistency. This provides a stable and reliable baseline pose estimation for the collision plane constraint factors introduced during subsequent collisions.

[0036] Step 102: Construct a local information matrix based on map matching constraint factors and inertial motion constraint factors, and perform eigenvalue decomposition to extract the degree and direction of degradation of the state variables of the local information matrix as the environment dynamically changes.

[0037] The local information matrix is ​​constructed by linearizing all constraint factors associated with the current state variable in the factor graph at the state estimation point. The eigenvalues ​​of the local information matrix reflect the amount of information available in each state direction: the larger the eigenvalue, the stronger the constraint and the more reliable the estimation in that direction; the smaller the eigenvalue, the weaker the constraint and the less certain the estimation in that direction.

[0038] In this embodiment, a local information matrix is ​​constructed by linearizing the pre-built map matching constraint factors and inertial motion constraint factors at the current state estimation point. Eigenvalue decomposition is then performed on this matrix to extract the information distribution in each feature direction. This allows the system to perceive the impact of environmental geometric changes on positioning observability in real time and quantitatively. When the mobile vehicle operates in environments with GNSS rejection and degraded geometry, such as long corridors, large open areas, or tunnels, the eigenvalue decomposition results clearly show that the information content in a certain direction is significantly lower, and the corresponding feature vector direction constraint is weaker. This provides clear directional guidance for the selective access of subsequent collision plane constraint factors. Thus, under limited resources, the system can prioritize accurately supplementing the external contact geometry information provided by collision events to the degraded direction with the weakest observability, avoiding computational redundancy caused by blindly introducing collision constraints into already fully constrained directions.

[0039] In practical applications, step 102 specifically includes the following steps: Step 102-1: Calculate the Jacobian matrix based on the map matching constraint factor and the inertial motion constraint factor, and construct the local information matrix.

[0040] Specifically, using the predicted state of the inertial measurement data as the linearization point, a first-order Taylor expansion is performed on the map matching constraint factor residuals and the inertial motion constraint factor residuals at the current state estimation point, and the Jacobian matrix of each factor residual with respect to the state variables is calculated. A local Fisher information matrix is ​​constructed from the Jacobian matrix and the information matrix (i.e., the inverse of the covariance matrix). This information matrix is ​​a symmetric positive semi-definite matrix with the same dimension as the state variables. The Jacobian matrix can be jointly constructed from the map matching Jacobian and the inertial measurement unit pre-integrated Jacobian, or the local geometric information matrix can be constructed solely using the map matching Jacobian; both methods fall within the scope of this application.

[0041] This local information matrix can be represented as: ; In the formula, For measuring time k The set of all factors corresponding to the lower state variable; Let be the Jacobian matrix of the factor residuals with respect to the state variables; for The information matrix of the factor's edges, i.e., the covariance matrix of that factor. The inverse matrix; The local information matrix is ​​a symmetric positive semi-definite matrix with the same dimension as the state variables.

[0042] Step 102-2: Divide the local information matrix into multiple sub-matrices based on the state variables.

[0043] The state variables include position variables and attitude variables.

[0044] Step 102-3: Perform eigenvalue decomposition on different submatrices to obtain the eigenvalues ​​of the submatrices.

[0045] For example, eigenvalue decomposition is performed on the position submatrix and the attitude submatrix respectively. Taking the position submatrix as an example, the eigenvalues ​​obtained after decomposition are arranged in descending order, and each eigenvalue corresponds to an eigenvector. The magnitude of the eigenvalue reflects the strength of the constraint in that direction, and the eigenvector represents the direction of that constraint in space. The eigenvalue decomposition of the attitude submatrix is ​​similar. Thus, the eigenvalue decomposition of the position submatrix is ​​obtained. The position submatrix corresponding to each data frame at any given time with attitude submatrix Eigenvalue decomposition is performed on both, and the positional submatrix is ​​represented as follows: The attitude submatrix is ​​represented as: .in, This indicates that the three elements within the parentheses are used as diagonal elements to form a diagonal matrix, and the off-diagonal elements are all 0; The eigenvalues ​​of the position submatrix are... ; The eigenvalues ​​of the attitude submatrix are... ; , Let G be the eigenvector matrices of the position submatrix and the attitude submatrix, respectively, defined in the map frame G. Here, the column vectors of the eigenvector matrix of the position submatrix are eigenvectors, corresponding to the three directions of the position variable.

[0046] Step 102-4: Calculate the degree of degradation of the submatrix based on the ratio of the maximum to the minimum eigenvalues ​​of the submatrix.

[0047] Specifically, the degree of degradation can be calculated from the square root of the maximum ratio of eigenvalues.

[0048] Taking the positional submatrix as an example, the degree of degradation is determined by the square root of the ratio of the smallest eigenvalue to the largest eigenvalue, with a value range of (0,1). A degradation degree closer to 1 indicates no degradation and uniform constraints in all directions. A degradation degree closer to 0 indicates severe degradation and extremely weak constraints in the corresponding directions. The degradation degree corresponding to the positional submatrix can be expressed as: The degradation degree of the attitude submatrix is ​​calculated similarly.

[0049] Step 102-5: When the degree of degradation is less than the degradation judgment threshold, the eigenvector corresponding to the smallest eigenvalue in the submatrix is ​​taken as the degradation direction of the corresponding state variable of the submatrix.

[0050] For example, when the position degradation degree is lower than the degradation decision threshold, geometric degradation is determined (e.g., lack of constraint on axial displacement of long straight pipe sections); when the attitude degradation degree is lower than the degradation decision threshold, texture degradation is determined (e.g., lack of tangential features on smooth concrete walls); when the condition number of the position submatrix exceeds the condition number threshold and the overall eigenvalues ​​are small, multipath degradation is determined (e.g., false echoes from metal reflections). Both the degradation decision threshold and the condition number threshold can be determined based on the statistical distribution of the non-degradable calibration set.

[0051] In this embodiment, by constructing a local information matrix and performing eigenvalue decomposition on it according to position and attitude components, the degradation type (geometric degradation, texture degradation, multipath degradation), degradation degree, and degradation direction can be quantitatively perceived online. This degradation perception result not only provides a basis for subsequent adaptive weight scheduling but also provides a directional benchmark for determining whether collision constraints can supplement the current weakest observable direction. This solves the problem in existing technologies that fuse map matching and inertial constraints with fixed weights and do not address the dynamic adjustment of factor confidence under local geometric degradation conditions.

[0052] Step 103: When a collision event is detected on the mobile carrier, the contact deformation of the protective structure is determined based on the impact residual and outer envelope parameters of the mobile carrier within the time window before and after the collision.

[0053] The time window refers to a continuous sampling period of a preset duration, including the moment of collision. The start and end points of the time window are located before and after the moment of collision, respectively, in order to fully capture the entire process of steady-state motion before collision and transient impact response after collision.

[0054] A collision event refers to the physical contact between the protective structure of the outer envelope of a mobile vehicle and an obstacle.

[0055] Impact residual is the component of impact acceleration caused by external contact force after the actual inertial measurement data has been transformed by coordinate transformation. When the moving vehicle does not collide, the actual acceleration is basically consistent with the acceleration predicted by normal motion, and the impact residual is close to zero. When a collision occurs, the protective structure comes into contact with the obstacle, generating additional impact acceleration. This impact acceleration cannot be predicted by the normal motion model, resulting in abnormal amplitude of the impact residual.

[0056] In this embodiment, the collision impulse is inverted using the velocity change along the collision normal before and after the collision, and the contact deformation is calculated by combining this with the known stiffness characteristics of the protective structure. Thus, without relying on external ranging sensors or visual odometry, one-dimensional velocity mutation information is transformed into a physical deformation estimate of the protective structure. This ensures that the contact deformation has a clear physical and mechanical meaning and is directly related to the dynamic parameters of the moving vehicle itself and the design parameters of the protective structure. This provides a geometric correction with an objective physical benchmark for the subsequent collision plane constraint factor, further avoiding the problem of constraint factor mismatch caused by inaccurate deformation estimation.

[0057] It is worth mentioning that the inertial forces generated by the vehicle's own acceleration, deceleration, turning, or attitude maneuvering at the moment of collision, as well as the gravity acting on it, can be separated and removed from the raw acceleration data of inertial measurement data to obtain the direction of the impact force purely generated by external physical contact. Specifically, this includes: predicting the normal motion acceleration of the mobile vehicle in the absence of collision based on the current attitude of the mobile vehicle and a preset kinematic model; transforming the inertial measurement data from multi-source sensors to the world coordinate system and filtering out the normal motion acceleration to obtain the impact residual.

[0058] The inertial measurement data is acquired through an inertial measurement unit (IMU) mounted on the mobile vehicle, and includes, but is not limited to, acceleration, angular velocity, and their corresponding sampling timestamps. Normal motion acceleration includes gravitational acceleration, vehicle maneuvering acceleration, noise, and other acceleration signals that the mobile vehicle may generate during normal motion. The motion model includes an IMU forward propagation model, which can reconstruct predicted acceleration time-by-time, reflecting the expected acceleration of the vehicle under normal flight (no collision) conditions.

[0059] Specifically, the impact residual can be calculated for each measurement frame within the time window using the following formula: ; In the formula, For measuring time Impact residuals below; For measuring time The raw acceleration in the inertial measurement data; The accelerometer bias is determined by the stationary segment statistics or Allan variance. For measuring time The predicted normal motion acceleration is reconstructed by the state predictor based on the control command and motion model time by time (including angular velocity, velocity and bias propagation terms). For measuring time The rotation matrix from the lower moving vehicle coordinate system to the world system; This is the world system gravity vector.

[0060] In this embodiment, a clean impact signal source is provided for reliable estimation of the collision normal and collision determination, ensuring that subsequent calculation of contact deformation and construction of collision plane constraint factors have an accurate geometric reference.

[0061] In one embodiment, before step 103, the method for determining whether a collision event has occurred on the mobile carrier includes: when the magnitude of the impact residual is greater than or equal to the magnitude threshold, calculating the normalized impact direction based on the impact residual of consecutive data frames within the time window; determining the number of groups in which the angle between the normalized impact directions of adjacent data frames in the consecutive data frames is less than the direction consistency threshold; and determining that a collision event has occurred on the mobile carrier when the number of groups is greater than the number of groups threshold.

[0062] When the time interval between adjacent measurement moments within the time window is less than or equal to the IMU sampling period, the data frames sampled at these two adjacent measurement moments can be regarded as continuous data frames. A continuous data frame can contain at least two measurement moments arranged in chronological order and their corresponding complete inertial measurement data frames.

[0063] The normalized impact direction can be obtained by normalizing the impact residual. The normalized impact direction can be expressed as: ; The angle between the normalized impact directions of two adjacent frames can be expressed as: ; In the formula, For measuring time The normalized impact direction below; For measuring time Impact residuals below; This is the modulus of the impact residual.

[0064] It is understood that the amplitude threshold can be determined based on the statistical analysis of the motion data of the mobile vehicle when there is no collision. It only needs to represent the acceleration value of the mobile vehicle during normal motion without collision; this embodiment does not impose specific limitations. For example, the amplitude threshold can be set to 2 to 3 times the standard deviation of acceleration noise to ensure that the false alarm rate is controlled under normal maneuvering and vibration. The directional consistency threshold can be determined based on the statistical characteristics of the impact direction change of the mobile vehicle in typical collision scenarios. For example, it can be based on the maximum angular deviation of the impact direction within a continuous time window in measured or simulated collision data, or adaptively adjusted based on the attitude change rate obtained by integrating the IMU angular velocity to reflect the differences in tolerance for directional consistency under different motion states; this embodiment does not impose specific limitations. For example, the directional consistency threshold can be set to a fixed angle value between 15° and 45°, preferably 30°. It can also be dynamically adjusted according to the current motion speed or angular velocity of the mobile vehicle. The threshold can be appropriately widened when the motion is intense to avoid missed detections, and tightened when the motion is stable to reduce false alarms.

[0065] In this embodiment, comparing the amplitude of the impact residual with its amplitude threshold to determine whether a collision event has occurred effectively avoids false triggering caused by sensor noise or normal carrier maneuvers, while enabling rapid response to real contact events under complex motion conditions. This fully utilizes the inherent characteristic of collision impacts being continuous and persistent along the physical timeline, effectively filtering out isolated false impact signals induced by sensor noise, carrier turbulence and vibration, or short-term attitude maneuvers, achieving high-confidence detection of collision events and effectively distinguishing between real collisions and maneuvering vibration interference.

[0066] In practical applications, step 103 specifically includes: Step 103-1: Calculate the normalized impact direction based on the impact residuals of continuous data frames within the time window.

[0067] Specifically, the agreed direction of the collision normal of a single data frame is from the center of the carrier to the contact surface at the time of collision, opposite to the direction of the impact force, that is: ; In the formula, For measuring time The collision normal direction below; To impact residuals; This is the modulus of the impact residual.

[0068] Step 103-2: Perform a weighted average and / or robust estimation on the reverse direction of the normalized impact direction to obtain the collision normal from the center of the moving carrier to the contact plane.

[0069] Specifically, in weighted average or robust estimation, the normal robust mean is calculated with weights proportional to the impact magnitude, i.e.: ; In the formula, For the collision normal of consecutive data frames; For measuring time The weight of the sampled data frame is proportional to the magnitude of the impact residual in that frame; For measuring time The collision normal below.

[0070] In this embodiment, since the IMU impact residual actually represents the direction of the external thrust on the carrier, while the collision plane normal is geometrically defined as the direction of the supporting reaction force of the contact surface on the carrier, the two are exactly opposite. Based on this principle, the reverse of the normalized impact direction is taken as the collision normal, ensuring that the collision normal is consistent with the geometric direction of the distance from the center of the carrier to the contact plane in the subsequent collision plane constraint factor. Furthermore, the reverse of all normalized impact directions within the time window is fused by using weighted averaging or robust estimation. On the one hand, weighted averaging can assign different weights according to the magnitude or confidence level of the impact residual of each frame, configuring higher contribution for data frames with larger impact residuals, thereby effectively suppressing the interference of low-amplitude noise frames on normal estimation. On the other hand, robust estimation can further eliminate outlier frames caused by occasional sensor spikes or multipath reflections, avoiding the bias of a few erroneous directions on the final normal. Thus, a complete signal processing link from the IMU raw signal to the collision normal is realized, laying the foundation for subsequent collision parameter estimation and constraint factor construction.

[0071] Step 103-3: Project the velocity of the moving carrier before and after the collision onto the collision normal, and calculate the velocity change in the collision normal.

[0072] Step 103-4: Calculate the collision impulse based on the equivalent mass of the carrier and the velocity change in the outer envelope parameters.

[0073] The equivalent mass of the mobile carrier is the sum of the carrier's own mass and the load mass. It can be extracted from the carrier's system parameter storage module or a preset configuration file, or it can be dynamically updated based on the carrier's current actual load state. It is understood that for multi-link or articulated mobile carriers, the equivalent mass is the generalized mass equivalent along the collision normal, and its value can be calculated from the mass distribution of the carrier's components and the kinematic relationship of its attitude at the moment of collision.

[0074] Specifically, the collision impulse can be expressed as: ; In the formula, The impact impulse; The equivalent mass of the mobile carrier; The velocity of the moving carrier before the collision; The velocity of the moving carrier after the collision.

[0075] Step 103-5: Calculate the average contact force based on the duration of the collision impulse and impact residual.

[0076] The duration of the impact residual is the total time from the moment when the impact residual amplitude first exceeds the amplitude threshold until the impact residual amplitude continues to decay to below the amplitude threshold and does not exceed the threshold again. It can also be statistically determined based on the effective pulse width of the impact residual within the time window. This duration reflects the complete process from the external contact force starting to act on the protective structure until it completely detaches or decays to a negligible level.

[0077] Specifically, the average contact force can be expressed as: ; In the formula, This represents the average contact force. The impact impulse; The duration of the collision is used to avoid the inconsistency in dimensions caused by directly dividing the change in velocity by the stiffness.

[0078] Step 103-6: Substitute the average contact force into the mechanical model corresponding to the protective structure and solve in reverse to obtain the contact deformation.

[0079] The mechanical model corresponding to the protective structure can be a piecewise stiffness curve, a linear spring model, a Kelvin-Voigt model, or a data-driven regression model. A piecewise stiffness curve is a nonlinear relationship curve with contact deformation as the independent variable and contact force as the dependent variable. This curve is divided into multiple linear or nonlinear intervals according to the magnitude of the deformation. Each interval corresponds to the stiffness characteristics of the protective structure at different deformation stages (e.g., elastic deformation stage, elastoplastic transition stage, plastic crushing stage, and densification stage). The stiffness is lower in the elastic stage, decreases in the plastic stage, and increases sharply in the densification stage. The shape of this curve depends on the material properties and geometry of the protective structure. It can be determined through material mechanical property testing and structural compression experiments, or pre-calculated using finite element simulation under different loading directions and rates, and stored in the parameter library of the moving vehicle for online querying and retrieval during a collision.

[0080] Specifically, the amount of contact deformation can be expressed as: ; In the formula, This refers to the amount of contact deformation. A mechanical model of the protective structure; This represents the average contact force.

[0081] In this embodiment, the impact impulse is obtained using the momentum theorem based on the changes in mass and velocity, and then the average contact force is calculated by combining the impact residual duration. The contact deformation is then mapped back using a pre-calibrated mechanical model of the protective structure, forming a complete physical calculation chain reflecting the deformation process. This calculation process requires no additional displacement or pressure sensors and can be completed entirely based on existing IMU data and prior system parameters, significantly reducing hardware costs and system complexity. Simultaneously, the introduction of the mechanical model accurately characterizes the force-deformation nonlinear constitutive relationship of the protective structure at different deformation stages, ensuring high physical accuracy in deformation estimation even under extreme scenarios such as large deformations and high-speed collisions. This avoids systematic biases caused by linear assumptions, thereby guaranteeing the accuracy and reliability of subsequent pose correction.

[0082] Step 104: Using the complementarity between the collision normal and the degradation direction as constraints, and based on the collision normal, contact deformation, outer envelope parameters, and collision plane reference point, construct a collision plane constraint factor to characterize the distance from the center of the moving carrier to the contact plane.

[0083] It should be noted that complementarity measures the degree to which collision constraints supplement the currently weakest observable direction. The complementarity can be calculated as the absolute value of the projection of the collision normal onto the degradation direction, expressed as: ; In the formula, As for complementarity, the closer it is to 0, the more orthogonal the collision normal is to the degradation direction of the position variable, and the collision information has almost no supplementary effect on the degradation direction. This is the transpose of the collision normal; This indicates the direction of degradation for positional variables.

[0084] In this embodiment, the angle or directional similarity measure between the collision normal and the eigenvector corresponding to the minimum eigenvalue of the local information matrix is ​​first calculated. This is the angle between the direction where the system's current constraints are weakest and pose estimation is most prone to drift. If the complementarity between the two is higher than a set threshold, the collision event is determined to accurately fill the current system observability. In this case, a collision plane constraint factor is constructed based on the collision normal, contact deformation, outer envelope parameters, and collision plane reference point to characterize the distance from the center of the moving vehicle to the contact plane. This achieves intelligent coupling between degradation perception and collision information, ensuring that the collision constraint factor is not blindly applied but rather directionally reinforces the system's weakest constraint direction. This avoids introducing redundant collision information in constrained directions, thus avoiding wasting computational resources, while ensuring that collision observations can play a maximum role in localization correction in constrained degradation directions.

[0085] It is understandable that the collision plane reference point is a point on the protective structure and the contact plane at the time of collision. Its determination method is not limited to impact estimation by inertial measurement unit, but can also be assisted by sensors such as pressure array, strain gauge, and tactile switch.

[0086] It is worth mentioning that if the complementarity is insufficient, the collision constraint is abandoned or its access priority is reduced, thereby ensuring that each introduced collision observation has a clear geometric gain value.

[0087] In practical applications, step 104 specifically includes the following steps: Step 104-1: Calculate the signal-to-noise ratio of collision observations for the collision event based on the impact residual and acceleration noise variance.

[0088] Specifically, the collision observation signal-to-noise ratio, used to reflect the reliability of the collision signal, can be expressed as: ; In the formula, For collision observation signal-to-noise ratio; To impact residuals; This represents the variance of the acceleration noise.

[0089] Step 104-2: When the complementarity is less than or equal to the complementarity threshold, and the signal-to-noise ratio of the collision observation based on the collision event is greater than the signal-to-noise ratio threshold, a collision plane constraint factor is constructed based on the collision normal, contact deformation, outer envelope parameters, and collision plane reference point.

[0090] Understandably, the complementarity threshold can be determined based on the statistical analysis of the collision normal estimation error in the calibration collision experiment, for example, by setting it to 0.3. The signal-to-noise ratio threshold can be determined based on the statistical analysis of the noise in the collisionless data from the collisionless calibration experiment, for example, by setting it to 10.

[0091] In this embodiment, the collision normal and degradation direction of a collision event are geometrically compared to calculate the complementarity. Simultaneously, the collision observation signal-to-noise ratio (SNR) is calculated based on the collision impact residual energy to assess signal reliability. When both the complementarity and SNR meet a threshold, it indicates that the collision plane constraint can compensate for the current observability gap, triggering the construction of a collision plane constraint factor. This allows each collision observation to improve the overall observability and pose estimation accuracy of the system with a significant geometric complementarity gain and signal reliability. Simultaneously, it eliminates the interference of redundant, low SNR, or mismatched collision events on the optimization process. Under the premise of effectively controlling the computational complexity of the factor graph, it maximizes the auxiliary positioning gain of a limited number of collision events in GNSS rejection or geometric degradation environments.

[0092] For example, the zero-value condition for the scalar residual of the collision plane constraint factor is: the product of the transpose of the collision normal and the difference between the center position of the moving carrier and the reference point of the collision plane is equal to the difference between the radial support distance and the contact deformation in the outer envelope parameters, which can be expressed as: ; In the formula, For collision plane constraint residuals; This is the transpose of the collision normal; The center of the mobile carrier; Used as a reference point for the collision plane; The radial support distance of the protective structure in the collision direction is the radius of the protective cage under the spherical protective structure; This represents the amount of contact deformation.

[0093] The collision plane constraint factor constrains the carrier position along the collision normal direction, which is equivalent to injecting the inverse of the collision residual variance and the collision normal cross product into the local position information matrix. By the Weyl eigenvalue interleaving theorem, when the collision normal and the weakest observable direction (degenerate direction) satisfy the condition that the projection is non-zero, the eigenvalue of the weakest direction strictly increases. That is, the collision factor supplements the missing observability in the degenerate direction. Thus, the physical collision contact event is transformed into a pose constraint with clear geometric meaning and uncertainty, achieving complementary enhancement between degradation and collision.

[0094] It should be noted that for protective structures with circular cross-sections, such as spheres and cylinders, the radial support distance is the same as the radial radius of the protective structure. For irregular protective structures, the radial support distance is the spatial distance along the collision normal direction from the center of the moving carrier along the collision normal ray to the outer surface of the protective structure, which can be pre-defined using discrete triangular meshes or point clouds.

[0095] It is worth noting that when the protective structure is spherical, based on the spherical symmetry, the collision plane constraint factor can be simplified to a scalar residual that only constrains the position component of the carrier center along the collision normal. Because the spherical structure is symmetric, this constraint only constrains the position component of the carrier center along the collision normal and does not depend on the body rotation angle. The Jacobian of the residual with respect to the carrier center position is... The Jacobian for the plane reference point is This facilitates linearization in the factor graph.

[0096] Furthermore, the collision plane reference point is determined by the prior map plane parameters and the local plane fitting parameters of the current point cloud, or it can be provided with an initial value as the plane variable to be estimated by the contact point estimate at the time of collision. Specifically, the collision plane reference point can be determined in one of the following ways: First, take the known plane parameters from the prior map; second, determine it by fitting a local plane in the neighborhood of the contact point from the current laser point cloud; third, use the collision plane reference point as the plane variable to be estimated, with the contact point estimate (carrier center position plus radial support distance multiplied by the collision normal) as the initial value, and jointly estimate it with the pose. When the third method is used, the collision plane constraint factor connects both the pose node and the plane variable node, and can be jointly optimized in the factor graph.

[0097] Step 105: The map matching constraint factor, inertial motion constraint factor and collision plane constraint factor are incorporated into the factor map in the same frame to perform joint optimization, and the optimized first trajectory pose is obtained.

[0098] In this embodiment, map matching constraint factors providing global absolute pose observation, inertial motion constraint factors providing local motion continuity and smoothness constraints, and collision plane constraint factors providing instantaneous contact geometry information during collision are all incorporated into the factor graph within the same keyframe, and joint nonlinear least squares optimization is performed. This allows the three heterogeneous constraints to undergo residual equalization and state iterative updates according to their respective information matrix weights within the same optimization framework. The three constraints work synergistically and complement each other, ultimately solving for the optimal trajectory pose sequence that simultaneously satisfies kinematic continuity, global consistency, and collision physical realism. The optimized first trajectory pose retains the high-frequency response characteristics of inertial navigation and possesses the global drift-free advantage of map matching. Furthermore, even in extreme scenarios where GNSS rejection or geometric degradation leads to map matching failure, it can still effectively suppress pose divergence by leveraging the physical contact information provided by the collision event, significantly improving the accuracy, continuity, and robustness of the positioning system in complex dynamic environments.

[0099] In practical applications, step 105 specifically includes the following steps: Step 105-1: Based on the degree of degradation, generate the first weight of the map matching constraint and the second weight of the motion constraint.

[0100] Among them, the first weight of the map matching constraint factor increases with the degree of degradation; that is, the smaller the degree of degradation, the more severe the degradation, the smaller the first weight. The first weight can be expressed as: In the formula, The degree of degradation; This is a preset adjustment coefficient. The second weight of the inertial motion constraint factor decreases as the degree of degradation increases. The second weight can be expressed as: ; It should be noted that this weight allocation satisfies consistency: when the degradation degree approaches the maximum value (no degradation), the first weight approaches 1, degrading to the baseline frame as in the case of no degradation; when the degradation degree approaches the minimum value (severe degradation), the first weight approaches 0, degrading to pure inertial navigation short-time propagation. The second weight is set with a positive lower bound to ensure that the inertial navigation constraint always exists. Optionally, the attitude degradation degree can be used instead of the position degradation degree as the independent variable, or the larger of the two can be taken.

[0101] Step 105-2: Based on complementarity and collision observation signal-to-noise ratio, generate the third weight of the collision plane constraint factor.

[0102] The third weight is positively correlated with complementarity and the signal-to-noise ratio (SNR) of collision observations. Specifically, the third weight of the collision plane constraint factor is determined by the product of the complementarity modulation function and the SNR mapping function. The SNR modulation of the information matrix is ​​achieved through the collision residual variance, while the third weight handles the complementarity modulation; this division of labor avoids redundant weighting. The collision residual variance includes an SNR modulation term. Higher impact signal energy, smaller noise variance, better directional stability, and smaller model residuals result in a smaller collision residual variance and a greater information weight for the collision constraint.

[0103] The third weight can be expressed as: ; In the formula, This is the third weight, which is the reciprocal of the variance of the impact residual; It is the reciprocal of the baseline variance of the collision residuals, determined by the residual statistics of the calibration collision experiment; , , This is a dimensionless adjustment coefficient, determined by data fitting in the calibration collision experiment; The collision normal (unit vector, pointing from the center of the carrier to the contact surface); ε is the transpose of the collision normal; ε is a small value to prevent division by zero. For acceleration noise variance; As a measure of directional stability (variance of the normal angle across multiple frames); To calibrate the normalized (dimensionless) value of the fitting residual of the equivalent stiffness curve in the collision experiment.

[0104] Step 105-3: Based on the first weight, the second weight, and the third weight, the map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor are weighted to obtain the joint optimization objective function.

[0105] For example, the joint optimization objective function is: ; In the formula, , , , These are the residuals of inertial motion constraint factors, map matching constraint factors, collision plane constraint factors, and loop closure detection factors, respectively. , , These are the first weight, the second weight, and the third weight, respectively. , , The covariance matrix for each factor; To impact the residual covariance matrix.

[0106] Step 105-4: Perform iterative solution for the joint optimization objective function.

[0107] Specifically, using the predicted pose from the pre-integrated propagation of the inertial measurement unit as the initial value, the pose, velocity, bias, and optional planar variables are linearized at the current estimation point, and the residuals and Jacobian matrices are calculated. Incremental smoothing (such as Bayesian tree updates in iSAM2) or a sliding window Levenberg-Marquardt solver can be used to iteratively update the state increment until the increment norm is less than the convergence threshold or the maximum number of iterations is reached.

[0108] In this embodiment, the weights of the map matching constraint factor and the inertial motion constraint factor are dynamically adjusted based on the degree of degradation reflected by the eigenvalue decomposition of the local information matrix. Specifically, when the degree of environmental degradation is low, the map matching constraint has high reliability and is given a higher weight to fully utilize the anchoring effect of global absolute observation, while the weight of the inertial constraint is reduced to avoid over-smoothing. When the degree of environmental degradation increases, the reliability of the map matching constraint decreases due to insufficient perception information, so its weight is reduced accordingly to avoid introducing erroneous absolute observations, while the weight of the inertial constraint is increased to maintain short-term accuracy by relying on the continuity and smoothness of relative motion. The complementarity between the collision normal and the degradation direction and the collision observation signal-to-noise ratio can not only serve as a trigger condition to determine whether to construct a factor, but can also be used to jointly calculate the third weight of the collision plane constraint factor. This allows the collision constraint to obtain a high weight when the geometric complementarity is strong and the signal quality is high, so as to accurately fill the information gaps in the weak directions of the system, and automatically reduce the weight when the complementarity is low or the signal-to-noise ratio is low to avoid the contamination of the optimization results by inferior constraints. Finally, the first, second, and third weights are applied to the residual terms of the three types of constraint factors respectively, and summed to form a joint optimization objective function. The objective function is then iteratively solved until it converges to the optimal solution. This achieves online adaptive optimization of the weights of the three types of constraint factors under GNSS rejection or geometric degradation environments. It ensures both high-precision absolute positioning in normal environments and stable continuous tracking in degradation environments, obtaining a globally optimal and physically reasonable corrected trajectory with relatively low computational cost.

[0109] It is worth mentioning that the embodiments of this application can also configure robust kernel functions for the collision plane constraint factors to suppress abnormal residual increments. Specifically, the Huber robust kernel function is used to process the residuals of the collision plane constraint factors: when the collision residuals are small, quadratic weighting is used to retain their complete constraint effect; when the collision residuals are large, the quadratic form is converted to a linear form to reduce the excessive influence of large residual collision factors on the optimization results. This automatically suppresses the impact of false collision events during optimization iterations. The Cauchy kernel is an alternative to the full-range smooth weighting.

[0110] Step 106: Verify the first trajectory pose and / or collision plane constraint factor.

[0111] In this embodiment, dual quality checks at the constraint layer and trajectory layer are achieved by verifying the optimized first trajectory pose and / or collision plane constraint factor. This effectively filters out abnormal observations and infeasible trajectories, ensuring that the final positioning result meets the requirements in terms of statistical consistency and physical rationality, and significantly improving the system's reliability and fault tolerance in degraded environments.

[0112] In practical applications, step 106 involves calculating the normalized posterior residuals of each constraint factor based on the posterior residuals and their covariance matrix. Specifically, this includes calculating the transpose of the posterior residuals, the inverse of the covariance matrix, and the product of the posterior residuals to obtain the square of the Mahalanobis distance as the normalized posterior residuals.

[0113] Specifically, the normalized a posteriori residual can be calculated using the following formula: ; In the formula, To normalize the posterior residuals, when the model and noise statistics are correct, the following degree of freedom is... Chi-square distribution of (the residual dimension of this factor) (theoretical case); The constraint factor is the residual vector after optimization convergence; is the inverse matrix of the covariance matrix corresponding to the constraint factor.

[0114] Furthermore, in step 106, the collision plane constraint factor is verified by the chi-square consistency statistics based on the normalized posterior residuals and the observability gain test results of the local information matrix. Specifically, verifying the collision plane constraint factor includes at least one of the following: First: If the normalized posterior residual corresponding to the collision plane constraint factor satisfies the chi-square threshold with 1 degree of freedom, the collision plane constraint factor is verified.

[0115] For example, a chi-square test is performed on the scalar residuals of the optimized collision plane constraint factors to determine whether the collision observations and state estimates are consistent. When the normalized posterior residuals are less than the 95th quantile of the chi-square distribution with 1 degree of freedom (approximately 3.84), the collision observations are considered consistent with the state estimates; exceeding the threshold indicates that the collision observations and state estimates are inconsistent, which may be caused by false detections, incorrect contact plane reference points, or failure of the contact deformation model.

[0116] The second step: compare the minimum eigenvalues ​​of the position submatrices in the local information matrix before and after the addition of the collision plane constraint factor; if the minimum eigenvalue of the position submatrices after the addition of the collision plane constraint factor is greater than the minimum eigenvalue of the position submatrices before the addition of the collision plane constraint factor, the collision plane constraint factor is verified.

[0117] For example, we compare the changes in the minimum eigenvalues ​​of the local position information matrix before and after adding the collision factor. When the collision normal and the degradation direction satisfy the condition that the projection is not zero, the collision factor supplements observability in the degradation direction, increasing the minimum eigenvalue, i.e., the change in the minimum eigenvalue is greater than zero. This test verifies that the collision factor does indeed supplement the missing observability in the degradation direction.

[0118] The third item: If the deviation between the first trajectory pose and the original predicted pose remains bounded over several consecutive frames, the collision plane constraint factor is verified.

[0119] For example, the deviation between the optimized pose and the pose predicted by the inertial measurement unit is compared. If this deviation remains bounded over several consecutive frames (example threshold 0.2m, adjustable, set according to online position covariance), the collision factor is determined to pass the trajectory continuity verification. This check ensures that the introduction of the collision factor does not cause abrupt changes in the trajectory.

[0120] If all three verifications are satisfied, the collision plane constraint factor is retained; if any verification is not satisfied, the collision plane constraint factor is removed or its weight is reduced, and incremental optimization is re-executed until the number of re-executions exceeds the threshold, at which point the collision plane constraint factor is removed.

[0121] In this embodiment, a decision-making mechanism for the retention or removal of collision factors is established through three posterior verifications: chi-square test, observability gain test, and trajectory continuity test. When any test condition is not met, the weight of the collision plane constraint factor is actively reduced or even completely removed. This not only prevents erroneous constraints from contaminating the global optimization results of the factor map due to abnormal single collision information, but also ensures that only collision constraints that truly improve system observability and complement, rather than conflict with, existing inertial constraints and map matching constraints are effectively utilized. This helps enhance the fault tolerance and long-term stability of the entire localization and mapping method for abnormal collision situations and sensor failures.

[0122] In practical application scenarios, step 106 verifies the optimized first trajectory pose based on the consistency of quality levels across consecutive frames. Specifically, this includes: mapping the chi-square consistency statistics, the trace of the inverse of the local information matrix, the condition number index, and the normalized degradation degree to a normalized interval, and then weighting and synthesizing the quality scores of each data frame corresponding to the first trajectory pose; assigning quality levels to each data frame based on the quality scores and recording the corresponding degradation reason codes; when all data frames of a preset number of consecutive frames are of qualified quality levels and no chi-square consistency statistics exceed the limit, the first trajectory pose is determined to have passed the verification.

[0123] In this embodiment, a frame-by-frame quality score is synthesized by normalizing and weighting multiple dimensions of indicators, such as chi-square consistency statistics, the trace of the inverse matrix of the local information matrix, condition number index, and normalized degradation degree. The trajectory is then verified based on the consistency of quality levels across consecutive frames, thus achieving a systematic verification and annotation of the reliability of each segment of the trajectory. This avoids low-quality segments contaminating the overall evaluation conclusion, making the output of the positioning and mapping results more reliable and traceable.

[0124] For example, such as Figure 2 As shown, the chi-square consistency score, condition number score, observability score, and degradation score are normalized to the [0,1] interval, and then weighted to synthesize a frame-by-frame quality score. A higher quality score indicates better trajectory quality for that frame. Quality levels are assigned to each data frame based on the quality score, and corresponding degradation reason codes are recorded. The quality levels are divided into A (high quality), B (acceptable quality), and C (low quality), with the grading threshold determined by independent control point calibration or by using a preset empirical threshold. The degradation reason code must distinguish at least the following code bits: position degradation (with degradation direction), attitude degradation, collision event, occlusion or matching anomaly, motion mutation, residual anomaly, insufficient loop closure, and external verification failure. Multiple code bits are allowed to be set simultaneously in each frame.

[0125] The trajectory is divided into valid, questionable, and invalid intervals based on the consistency of quality levels across consecutive frames. For example, the valid interval is six consecutive frames at quality levels A / B with no frames exceeding the chi-square consistency statistic limit; the questionable interval is an interval with quality level jitter or containing frames at quality level C; and the invalid interval is three consecutive frames at low quality levels or with chi-square consistency statistics exceeding the hard upper limit. When all consecutive frames corresponding to the first trajectory pose are within the valid interval, the trajectory is deemed to have passed verification. This application does not impose specific limitations on this aspect.

[0126] Step 107: Output the positioning trajectory of the mobile vehicle based on the verification results and construct a fused map.

[0127] The fused map includes environmental degradation status and collision attributes.

[0128] The positioning and mapping method for GNSS-constrained and degraded environments provided in this application constructs map matching constraint factors and inertial motion constraint factors based on multi-source sensor data and the outer envelope parameters of the protective structure. It evaluates the degree and direction of environmental degradation in real time through eigenvalue decomposition of the local information matrix. When a collision occurs, it uses the impact residual and outer envelope parameters to determine the contact deformation. Then, it constructs a collision plane constraint factor with the complementarity between the collision normal and the degradation direction as a constraint. Subsequently, the three types of constraint factors are incorporated into the factor map in the same keyframe for joint optimization to obtain the first trajectory pose and its posterior residual and covariance matrix. The collision constraints are then rigorously verified through the chi-square consistency statistical test of the normalized posterior residual and the observability gain test of the local information matrix. The trajectory is also double-verified by combining the quality level consistency of consecutive frames. Finally, the positioning trajectory is output and a map integrating the environmental degradation state and collision attributes is constructed. Firstly, by introducing a collision plane constraint factor and ensuring its access direction complements the system degradation direction, collision events, originally considered interference, are transformed into effective observations that accurately fill the weakest direction in the information matrix. This allows collision information to specifically compensate for information loss in the degradation direction, improving system observability and pose estimation accuracy under extreme degradation environments such as GNSS denial. This effectively solves the problems of low positioning accuracy, poor continuity, and insufficient reliability of related technologies in complex scenarios where degradation and potential physical collisions are possible. Secondly, while performing successive quality checks on collision constraints using chi-square tests of posterior residuals and observability gain tests, a secondary quality inspection link at the trajectory level is constructed through continuous frame quality level consistency verification. This effectively prevents abnormal or low-quality collision observations from contaminating the optimization results, ensuring that the final output positioning trajectory has high confidence in both local smoothness and global consistency. This helps improve the long-term robustness and mission safety of mobile vehicles in complex dynamic scenarios.

[0129] In one embodiment, the positioning and mapping for GNSS-constrained and degraded environments further includes: entering a safe propagation mode when the degradation level is less than the safe mode trigger threshold or the collision plane constraint factor fails verification; in the safe propagation mode, abandoning the map matching constraint factor and the collision plane constraint factor, and performing short-term state propagation on the inertial motion constraint factor to generate a second trajectory pose; when the degradation level recovers to a value greater than or equal to the safe mode trigger threshold within a preset number of consecutive frames, exiting the safe propagation mode and progressively reintroducing the map matching constraint factor and the collision plane constraint factor.

[0130] The safety mode trigger threshold is an extreme degradation judgment threshold. When the degradation level is below this threshold, it indicates that the observability of the current environment is severely insufficient, and the map matching constraint factor may introduce systematic errors. The safety propagation mode refers to the operating mode in which, within a frame where degradation or collision factors are rolled back, map matching constraint factors and collision plane constraint factors are abandoned, and short-term state propagation is performed solely based on inertial motion constraint factors.

[0131] In this embodiment, when the environmental degradation level falls below a pre-set safe mode trigger threshold or the collision plane constraint factor fails the posterior residual chi-square consistency test and observability gain test, the system automatically switches to safe propagation mode. In this mode, map matching constraint factors that may introduce erroneous absolute observations and unverified collision plane constraint factors are actively abandoned. Only the inertial motion constraint factor is relied upon to perform short-term state propagation to generate the second trajectory pose, thereby avoiding unreliable external observations contaminating the state estimation. Simultaneously, the system continuously monitors the degradation level changes in subsequent consecutive frames. Once the degradation level recovers to a level greater than or equal to the safe mode trigger threshold, indicating that the environment has once again provided sufficient geometric features for reliable map matching and positioning, the map matching constraint factor and collision plane constraint factor are reintroduced through a smooth transition strategy. This achieves a smooth switch from pure inertial navigation propagation mode to multi-source fusion positioning mode. Even in scenarios with long-term GNSS denial or extreme geometric degradation, it can effectively suppress the risk of pose divergence caused by unreliable observations. At the same time, it can quickly restore the high-precision state of fully constrained fusion positioning when environmental conditions improve. It balances the survivability of the system in extreme environments with the positioning accuracy in normal environments, and significantly improves the robustness and self-recovery capability of the positioning and mapping system for GNSS constrained and degraded environments in complex and long-term tasks.

[0132] Understandably, the second trajectory pose is obtained through pre-integration propagation by the inertial measurement unit without joint optimization of map matching and collision constraints. Therefore, its accuracy may be reduced, and the trajectory quality label of the second trajectory pose can be set to degraded. However, the second trajectory pose can guarantee trajectory continuity under extreme degradation or abnormal collision factors, avoiding complete loss of positioning due to constraint failure.

[0133] It should be noted that the number of consecutive preset frames used to trigger the exit from the safe propagation mode can be the same as or different from the number of consecutive preset frames used to perform collision plane constraint factor verification.

[0134] For example, consider a UAV performing an inspection mission in a hydropower station's water diversion tunnel. In the tunnel environment, GNSS signals are completely blocked, and the UAV relies on inertial measurement unit pre-integration and lidar scan-to-map matching for positioning. Because the tunnel cross-section is approximately circular, lidar observation information in the cross-sectional direction is insufficient, leading to geometric degradation in pose estimation in that direction. When the UAV's protective cage collides with the tunnel sidewall, the collision normal points towards the sidewall, precisely corresponding to the direction of geometric degradation. By converting the collision event into a collision plane constraint factor and incorporating it into factor graph joint optimization, the collision information accurately supplements the observability of the degradation direction. Experimental data shows that in GNSS-blocked scenarios, adding the collision plane constraint factor significantly reduces the positioning error: the error in the hydropower station water diversion tunnel scenario decreased from 0.1770 meters to 0.1390 ​​meters, and in the thermal power plant boiler scenario, it decreased from 0.1892 meters to 0.1671 meters. Actual measurement data from four types of GNSS rejected industrial scenarios (underground pipe networks, mining subsidence areas, hydropower station water diversion tunnels, and thermal power plant boilers) show that, relative to the baseline method, the trajectory errors decreased from 3.1813 meters, 0.3587 meters, 1.3562 meters, and 2.5123 meters to 0.1618 meters, 0.1431 meters, 0.1232 meters, and 0.1544 meters, respectively.

[0135] 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.

[0136] The user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data that have been permitted by the user or fully permitted by all parties. Furthermore, the collection, use and processing of the relevant data must comply with the relevant laws, regulations and standards of the relevant countries and regions, and corresponding operation portals are provided for users to choose to allow or refuse.

[0137] Furthermore, such as Figure 3 As shown, as a specific implementation of the above-mentioned positioning and mapping method for GNSS-constrained and degraded environments, this application provides a positioning and mapping device 300 for GNSS-constrained and degraded environments. The positioning and mapping device 300 for GNSS-constrained and degraded environments includes: a constraint construction module 301, a positioning module 302, and a verification module 303.

[0138] The constraint construction module 301 is used to construct map matching constraint factors and inertial motion constraint factors based on multi-source sensor data of the mobile carrier and the outer envelope parameters of the protective structure; and to construct a local information matrix based on the map matching constraint factors and inertial motion constraint factors, and to perform eigenvalue decomposition to extract the degradation degree and degradation direction of the state variables of the local information matrix as the environment dynamically changes; and when a collision event is detected in the mobile carrier, to determine the contact deformation of the protective structure based on the impact residual and outer envelope parameters of the mobile carrier within the time window before and after the collision; and to construct a collision plane constraint factor to characterize the distance from the center of the mobile carrier to the contact plane based on the collision normal, contact deformation, outer envelope parameters and collision plane reference point, using the complementarity between the collision normal and the degradation direction as constraints. The localization module 302 is used to incorporate map matching constraint factors, inertial motion constraint factors and collision plane constraint factors into the factor map in the same frame to perform joint optimization and obtain the optimized first trajectory pose. The verification module 303 is used to verify the first trajectory pose and / or collision plane constraint factor; and to output the positioning trajectory of the mobile vehicle and construct a fused map based on the verification results.

[0139] Furthermore, the constraint construction module 301 is specifically used to calculate the Jacobian matrix based on the map matching constraint factor and the inertial motion constraint factor, and construct the local information matrix; divide the local information matrix into multiple sub-matrices based on the state variables; wherein the state variables include position variables and attitude variables; perform eigenvalue decomposition on different sub-matrices to obtain the eigenvalues ​​of the sub-matrices; calculate the degradation degree of the sub-matrices based on the ratio of the maximum value to the minimum value in the eigenvalues ​​of the sub-matrices; when the degradation degree is less than or equal to the degradation judgment threshold, take the eigenvector corresponding to the minimum eigenvalue in the sub-matrices as the degradation direction of the corresponding state variable of the sub-matrices.

[0140] Furthermore, the constraint construction module 301 is specifically used to calculate the normalized impact direction based on the impact residuals of continuous data frames within the time window; to perform weighted averaging and / or robust estimation on the reverse of the normalized impact direction to obtain the collision normal from the center of the moving vehicle to the contact plane; to project the velocity of the moving vehicle before and after the collision onto the collision normal and calculate the velocity change in the collision normal; to calculate the collision impulse based on the equivalent mass and velocity change of the vehicle in the outer envelope parameters; to calculate the average contact force based on the collision impulse and the duration of the impact residual; and to substitute the average contact force into the mechanical model corresponding to the protective structure for inverse solution to obtain the contact deformation.

[0141] Furthermore, the constraint construction module 301 is specifically used to construct a collision plane constraint factor based on the collision normal, contact deformation, outer envelope parameters, and collision plane reference point when the complementarity is less than or equal to the complementarity threshold and the collision observation signal-to-noise ratio based on the collision event is greater than the signal-to-noise ratio threshold. The zero value condition of the scalar residual of the collision plane constraint factor is: the product of the transpose of the collision normal and the difference between the center position of the moving carrier and the collision plane reference point is equal to the difference between the radial support distance and the contact deformation in the outer envelope parameters.

[0142] Further, the positioning module 302 is specifically used to generate a first weight for map matching constraints and a second weight for motion constraints based on the degree of degradation; wherein the first weight is positively correlated with the degree of degradation, and the second weight is negatively correlated with the degree of degradation; and to generate a third weight for collision plane constraint factors based on complementarity and collision observation signal-to-noise ratio; wherein the third weight is positively correlated with complementarity and collision observation signal-to-noise ratio; and to weight the map matching constraint factors, inertial motion constraint factors, and collision plane constraint factors based on the first weight, second weight, and third weight to obtain a joint optimization objective function; and to perform iterative solution on the joint optimization objective function.

[0143] Furthermore, the verification module 303 is specifically used to calculate the normalized posterior residuals of each constraint factor based on the posterior residuals and their covariance matrix obtained from the joint optimization, and to verify the collision plane constraint factors based on the chi-square consistency statistics of the normalized posterior residuals and the observability gain test results of the local information matrix; when the deviation between the first trajectory pose and the original predicted pose remains bounded within several consecutive frames, the collision plane constraint factor is determined to have passed the verification; when the collision plane constraint factor fails the verification, the collision plane constraint factor is removed or its weight is reduced and the incremental optimization solution is re-executed until the number of re-executions exceeds the threshold number, after which the collision plane constraint factor is removed.

[0144] Furthermore, the verification module 303 is specifically used to verify the first trajectory pose based on the consistency of quality level of consecutive frames.

[0145] Furthermore, the verification module 303 is specifically used to calculate the product of the transpose of the posterior residual, the inverse of the covariance matrix, and the posterior residual, and obtain the square of the Mahalanobis distance as the normalized posterior residual.

[0146] Further, the verification module 303 is specifically used to determine that the collision plane constraint factor passes verification when the normalized posterior residual corresponding to the collision plane constraint factor satisfies the chi-square threshold with 1 degree of freedom; compare the minimum eigenvalue of the position submatrix in the local information matrix before and after the addition of the collision plane constraint factor; and determine that the collision plane constraint factor passes verification when the minimum eigenvalue of the position submatrix after the addition of the collision plane constraint factor is greater than the minimum eigenvalue of the position submatrix before the addition of the collision plane constraint factor.

[0147] Furthermore, the verification module 303 is specifically used to map the chi-square consistency statistics, the trace of the inverse matrix of the local information matrix, the condition number index, and the degree of degradation to a normalized interval, and then weight and synthesize the quality score of each data frame corresponding to the first trajectory pose; assign a quality level to each data frame based on the quality score and record the corresponding degradation reason code; when all data frames of a preset consecutive number of frames are of qualified quality level and no chi-square consistency statistics exceed the limit, the first trajectory pose is determined to have passed the verification.

[0148] Furthermore, the constraint construction module 301 is also used to predict the normal motion acceleration of the mobile carrier under no-collision conditions based on the current attitude of the mobile carrier and the preset kinematic model; transform the inertial measurement data in the multi-source sensor data to the world coordinate system, and filter out the normal motion acceleration to obtain the impact residual.

[0149] Furthermore, the constraint construction module 301 is also used to calculate the absolute value of the projection of the collision normal onto the degradation direction, as complementarity.

[0150] Furthermore, the positioning and mapping device 300 for GNSS-constrained and degraded environments also includes: The collision detection module (not shown in the figure) is used to calculate the normalized impact direction based on the impact residual of continuous data frames within a time window when the magnitude of the impact residual is greater than or equal to the magnitude threshold; determine the number of groups in which the angle between the normalized impact directions of adjacent data frames in continuous data frames is less than the direction consistency threshold; and determine that a collision event has occurred on the moving vehicle when the number of groups is greater than the number of groups threshold.

[0151] Furthermore, the constraint construction module 301 is also used to configure a robust kernel function for the collision plane constraint factor to suppress anomalous residual increments.

[0152] Furthermore, the positioning and mapping device 300 for GNSS-constrained and degraded environments also includes: An anomaly handling module (not shown in the figure) is used to enter the safe propagation mode when the degradation level is less than the safe mode trigger threshold or the collision plane constraint factor fails verification. In the safe propagation mode, the map matching constraint factor and the collision plane constraint factor are abandoned, and a short-term state propagation is performed on the inertial motion constraint factor to generate the second trajectory pose. When the degradation level recovers to a value greater than or equal to the safe mode trigger threshold within a preset number of consecutive frames, the safe propagation mode is exited and the map matching constraint factor and the collision plane constraint factor are gradually reintroduced.

[0153] Specific limitations regarding the positioning and mapping device for GNSS-constrained and degraded environments can be found in the limitations of the positioning and mapping method for GNSS-constrained and degraded environments described above, and will not be repeated here. Each module in the aforementioned positioning and mapping device for GNSS-constrained and degraded environments can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in hardware or independently of the processor in the mobile carrier, or stored in software in the memory of the mobile carrier, so that the processor can call and execute the corresponding operations of each module.

[0154] Based on the above, Figure 1 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. Figure 1 The method for positioning and mapping in GNSS-constrained and degraded environments 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, portable hard drive, etc.) and includes several instructions to cause a mobile carrier (such as a personal computer, server, or network device, etc.) to execute the methods described in the various implementation scenarios of this application.

[0156] Based on the above, Figure 1 The method shown, and Figure 3 The virtual device embodiment shown is designed to achieve the above objectives, such as... Figure 4 As shown in the figure, this application embodiment also provides a mobile carrier 400, which includes a processor 401 and a memory 402. The memory 402 stores a program or instructions that can run on the processor 401. When the program or instructions are executed by the processor 401, they implement the above-mentioned... Figure 1 The method for positioning and mapping in GNSS-constrained and degraded environments is shown.

[0157] The memory 402 can be used to store software programs and various data. The memory 402 may primarily include a first storage area for storing programs or instructions and a second storage area for storing data. The first storage area may store the operating system, application programs or instructions required for at least one function (such as sound playback, image playback, etc.). Furthermore, the memory 402 may include volatile memory or non-volatile memory, or both. The non-volatile memory may be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. Volatile memory can be random access memory (RAM), static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct memory bus RAM (DRRAM). The memory 402 in this embodiment includes, but is not limited to, these and any other suitable types of memory.

[0158] Processor 401 may include one or more processing units; optionally, processor 401 integrates an application processor and a modem processor, wherein the application processor mainly handles operations involving the operating system, user interface, and applications, and the modem processor mainly handles wireless communication signals, such as a baseband processor. It is understood that the aforementioned modem processor may also not be integrated into processor 401.

[0159] Mobile carriers can specifically include personal computers, servers, network devices, etc.

[0160] Optionally, the mobile carrier 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 a USB interface, a card reader interface, etc. The network interface may optionally include a standard wired interface, a wireless interface (such as a Bluetooth interface, a Wi-Fi interface), etc.

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

[0162] 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 it can be implemented by hardware.

[0163] 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.

[0164] 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 positioning and mapping method for GNSS-constrained and degraded environments, characterized in that, The method, applied to a mobile carrier with an external protective structure, includes: Based on the multi-source sensor data of the mobile carrier and the outer envelope parameters of the protective structure, a map matching constraint factor and an inertial motion constraint factor are constructed. A local information matrix is ​​constructed based on the map matching constraint factor and the inertial motion constraint factor, and eigenvalue decomposition is performed to extract the degree and direction of degradation of the state variables of the local information matrix as the environment dynamically changes. When a collision event is detected involving the mobile carrier, the contact deformation of the protective structure is determined based on the impact residual of the mobile carrier within the time window before and after the collision and the outer envelope parameter. Using the complementarity between the collision normal and the degradation direction as a constraint, and based on the collision normal, the contact deformation, the outer envelope parameter, and the collision plane reference point, a collision plane constraint factor is constructed to characterize the distance from the center of the moving carrier to the contact plane. The map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor are incorporated into the factor graph in the same frame for joint optimization to obtain the optimized first trajectory pose, the posterior residuals of each constraint factor after optimization convergence, and their covariance matrix. Verify the first trajectory pose and / or the collision plane constraint factor; Based on the verification results, the positioning trajectory of the mobile vehicle is output and a fused map is constructed.

2. The positioning and mapping method for GNSS-constrained and degraded environments according to claim 1, characterized in that, The process of constructing a local information matrix based on the map matching constraint factor and the inertial motion constraint factor, and performing eigenvalue decomposition to extract the degradation degree and direction of the state variables of the local information matrix as the environment dynamically changes, includes: The Jacobian matrix is ​​calculated based on the map matching constraint factor and the inertial motion constraint factor, and the local information matrix is ​​constructed accordingly. The local information matrix is ​​divided into multiple sub-matrices based on the state variables; wherein the state variables include position variables and attitude variables; Eigenvalue decomposition is performed on each of the different submatrices to obtain the eigenvalues ​​of the submatrices; The degree of degradation of the submatrix is ​​calculated based on the square root of the ratio of the maximum to the minimum eigenvalues ​​of the submatrix. When the degree of degradation is less than the degradation judgment threshold, the eigenvector corresponding to the smallest eigenvalue in the submatrix is ​​taken as the degradation direction of the corresponding state variable of the submatrix.

3. The positioning and mapping method for GNSS-constrained and degraded environments according to claim 1, characterized in that, The determination of the contact deformation of the protective structure based on the impact residual of the mobile carrier within a time window before and after the collision and the outer envelope parameters includes: The normalized impact direction is calculated based on the impact residuals of continuous data frames within the time window. By performing a weighted average and / or robust estimation on the reverse of the normalized impact direction, the collision normal from the center of the moving carrier to the contact plane is obtained. Project the velocity of the moving carrier before and after the collision onto the collision normal, and calculate the velocity change in the collision normal. The collision impulse is calculated based on the equivalent mass of the carrier in the outer envelope parameters and the velocity change. The average contact force is calculated based on the duration of the impact impulse and the impact residual. The average contact force is substituted into the mechanical model corresponding to the protective structure and solved in reverse to obtain the contact deformation.

4. The positioning and mapping method for GNSS-constrained and degraded environments according to claim 1, characterized in that, The collision plane constraint factor, constrained by the complementarity between the collision normal and the degradation direction, and based on the collision normal, the contact deformation, the outer envelope parameter, and the collision plane reference point, is constructed to characterize the distance from the center of the moving vehicle to the contact plane, including: When the complementarity is less than or equal to the complementarity threshold, and the signal-to-noise ratio of the collision observation based on the collision event is greater than the signal-to-noise ratio threshold, the collision plane constraint factor is constructed based on the collision normal, the contact deformation, the outer envelope parameter, and the collision plane reference point. The zero-value condition for the scalar residual of the collision plane constraint factor is: the product of the transpose of the collision normal, the difference between the center position of the moving carrier and the reference point of the collision plane, is equal to the difference between the radial support distance and the contact deformation in the outer envelope parameters.

5. The positioning and mapping method for GNSS-constrained and degraded environments according to claim 4, characterized in that, The step of incorporating the map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor into the factor graph within the same frame for joint optimization includes: Based on the degree of degradation, a first weight for the map matching constraint and a second weight for the motion constraint are generated; wherein, the first weight is positively correlated with the degree of degradation, and the second weight is negatively correlated with the degree of degradation; Based on the complementarity and the collision observation signal-to-noise ratio, a third weight is generated for the collision plane constraint factor; wherein the third weight is positively correlated with the complementarity and the collision observation signal-to-noise ratio. The map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor are weighted based on the first weight, the second weight, and the third weight to obtain a joint optimization objective function; The joint optimization objective function is solved iteratively.

6. The positioning and mapping method for GNSS-constrained and degraded environments according to claim 1, characterized in that, The verification of the first trajectory pose and / or the collision plane constraint factor includes: Based on the posterior residuals and their covariance matrices obtained from joint optimization, the normalized posterior residuals of each constraint factor are calculated, and the collision plane constraint factors are verified based on the chi-square consistency statistics of the normalized posterior residuals and the observability gain test results of the local information matrix; and / or, The pose of the first trajectory is verified based on the consistency of the quality level of consecutive frames.

7. The positioning and mapping method for GNSS-constrained and degraded environments according to claim 6, characterized in that, The calculation of the normalized posterior residuals of each constraint factor based on the posterior residuals and their covariance matrix includes: The product of the transpose of the posterior residual, the inverse of the covariance matrix, and the posterior residual is calculated to obtain the square of the Mahalanobis distance as the normalized posterior residual. The collision plane constraint factor is verified by the chi-square consistency statistics based on the normalized posterior residual and the observability gain test results of the local information matrix. When the collision plane constraint factor fails the verification, remove the collision plane constraint factor or reduce its weight and re-execute the incremental optimization solution until the number of re-executions exceeds the threshold, then remove the collision plane constraint factor. The verification of the collision plane constraint factor includes at least one of the following: First item: When the normalized posterior residual corresponding to the collision plane constraint factor satisfies the chi-square threshold with 1 degree of freedom, the collision plane constraint factor is determined to have passed the verification. Second item: Compare the minimum eigenvalues ​​of the position submatrices in the local information matrix before and after the addition of the collision plane constraint factor; if the minimum eigenvalue of the position submatrices after the addition of the collision plane constraint factor is greater than the minimum eigenvalue of the position submatrices before the addition of the collision plane constraint factor, the collision plane constraint factor is determined to have passed the verification. Third item: If the deviation between the first trajectory pose and the original predicted pose remains bounded for several consecutive frames, the collision plane constraint factor is verified. The verification of the optimized trajectory pose based on the consistency of quality levels across consecutive frames includes: After mapping the chi-square consistency statistics, the trace of the inverse matrix of the local information matrix, the condition number index, and the degree of degradation to a normalized interval, the quality scores of each data frame corresponding to the first trajectory pose are weighted and synthesized. Based on the quality score, assign a quality level to each data frame and record the corresponding downgrade reason code; When all data frames for a preset number of consecutive frames are of acceptable quality and no chi-square consistency statistic exceeds the limit, the first trajectory pose is determined to have passed verification.

8. The positioning and mapping method for GNSS-constrained and degraded environments according to any one of claims 1 to 7, characterized in that, The method further includes at least one of the following: First item: Based on the current attitude of the mobile carrier and the preset kinematic model, the normal motion acceleration of the mobile carrier under no-collision conditions is predicted; the inertial measurement data in the multi-source sensor data is transformed to the world coordinate system, and the normal motion acceleration is filtered out to obtain the impact residual; Second item: Calculate the absolute value of the projection of the collision normal onto the degradation direction, as the complementarity. Third item: When the magnitude of the impact residual is greater than or equal to the magnitude threshold, the normalized impact direction is calculated based on the impact residual of continuous data frames within the time window. Determine the number of groups in which the angle between the normalized impact directions of adjacent data frames in the continuous data frames is less than the direction consistency threshold; When the number of groups is greater than the group number threshold, it is determined that the collision event has occurred on the mobile carrier; Fourth item: Configure a robust kernel function for the collision plane constraint factor to suppress anomalous residual increments; Fifth item: When the degradation level is less than the safety mode trigger threshold, or when the collision plane constraint factor fails verification, the system enters the safety propagation mode. In the safe propagation mode, the map matching constraint factor and the collision plane constraint factor are abandoned, and short-time state propagation is performed on the inertial motion constraint factor to generate a second trajectory pose; When the degradation level recovers to a value greater than or equal to the safety mode trigger threshold within a preset number of consecutive frames, the safety propagation mode is exited and the map matching constraint factor and the collision plane constraint factor are gradually reintroduced.

9. A positioning and mapping device for GNSS-constrained and degraded environments, characterized in that, The device, applicable to a mobile carrier with an external protective structure, comprises: A constraint construction module is used to construct map matching constraint factors and inertial motion constraint factors based on the multi-source sensor data of the mobile carrier and the outer envelope parameters of the protective structure; and, A local information matrix is ​​constructed based on the map matching constraint factor and the inertial motion constraint factor, and eigenvalue decomposition is performed to extract the degree and direction of degradation of the state variables of the local information matrix as the environment dynamically changes; and, When a collision event is detected involving the mobile carrier, the contact deformation of the protective structure is determined based on the impact residual of the mobile carrier within a time window before and after the collision and the outer envelope parameters; and... Using the complementarity between the collision normal and the degradation direction as a constraint, and based on the collision normal, the contact deformation, the outer envelope parameter, and the collision plane reference point, a collision plane constraint factor is constructed to characterize the distance from the center of the moving carrier to the contact plane. The positioning module is used to incorporate the map matching constraint factor, the inertial motion constraint factor, and the collision plane constraint factor into the factor map in the same frame to perform joint optimization and obtain the optimized first trajectory pose. The verification module is used to verify the first trajectory pose and / or the collision plane constraint factor; and, Based on the verification results, the positioning trajectory of the mobile vehicle is output and a fused map is constructed.

10. A mobile carrier, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the positioning and mapping method for GNSS-constrained and degraded environments as described in any one of claims 1 to 8.

Images