An underground space-oriented mobile carrier motion state determination method and system

Abstract

The application discloses a kind of underground space-oriented mobile carrier motion state determination method and system, belong to underground space mobile carrier navigation technical field.The method includes: obtaining millimeter wave radar point cloud and inertial measurement data and establishing time correspondence;Inertial prediction state is constructed;The spatial direction of radar observation point, measured doppler velocity and echo intensity are extracted;Doppler motion consistency residual is constructed;Effective observation points are screened and credibility weight is distributed, and reliable radar observation point set is obtained by credibility verification;Self-velocity and its measurement uncertainty are estimated;Self-velocity and inertial prediction state are weighted and fused to update inertial prediction state and form motion trajectory;According to measurement uncertainty and trajectory continuity, the quality of observation is evaluated, and when the quality decreases, the weight of radar measurement is weakened, and the trajectory output is maintained by inertia, and when the quality recovers, the weight of radar measurement is restored.The application improves the robustness and credibility of underground space mobile carrier trajectory determination.

Description

Technical Field

[0001] This application relates to the field of navigation technology for mobile carriers in underground space, and more specifically, to a method and system for determining the motion state of mobile carriers in underground space. Background Technology

[0002] With the increasing demand for intelligent mobile equipment in scenarios such as underground coal mines, tunnel construction, underground rescue, smoke-filled factories, underground utility tunnels, and oil drilling auxiliary spaces, the continuous autonomous navigation capability of mobile carriers in GNSS (Global Navigation Satellite System) restricted or navigation denied environments has become an important foundation for ensuring the safe operation of equipment, environmental perception, and mission execution.

[0003] Current positioning technologies for mobile vehicles in underground spaces primarily rely on visual SLAM (Simultaneous Localization and Mapping), lidar SLAM, or a fusion of both, to estimate pose through image feature extraction and matching or 3D point cloud registration. However, these scenarios often involve complex factors such as dust, smoke, water mist, low light, and occlusion. Visual sensors are easily affected by insufficient lighting and lack of texture, leading to malfunctions. LiDAR also experiences a significant decrease in observation quality under conditions of smoke, water mist, and low reflectivity, making it difficult for mobile vehicles to continuously determine their own trajectory.

[0004] Millimeter-wave radar, with its advantages of being independent of visible light and directly acquiring range, angle, and Doppler velocity observations, has been introduced into the field of mobile vehicle positioning in underground spaces in recent years. Existing schemes extract static scattering points from radar point clouds, construct velocity observations using Doppler velocity equations, and fuse them with inertial navigation data to optimize odometry information. However, low-cost millimeter-wave radar suffers from problems such as sparse point clouds, limited angular resolution, and significant multipath effects in practical applications. Furthermore, dynamic targets such as pedestrians, vehicles, and rotating equipment are common in underground spaces, making it difficult to effectively distinguish between static environmental points and dynamic target points in the radar point cloud. To address these issues, existing radar inertial odometry methods typically employ Doppler residual truncation screening based on empirical thresholds, single-frame random sampling methods such as RANSAC (Random Sample Consensus) for velocity model fitting, or a front-end and back-end unidirectional cascaded architecture to directly pass the front-end velocity estimation results to the back-end for optimization. However, empirical threshold screening methods rely on fixed thresholds, making it difficult to adapt to variations in noise levels, the proportion of dynamic targets, and multipath intensity in different underground space scenarios. When point clouds are sparse or dynamic interference is severe, they may mistakenly remove a large number of valid static points or retain dynamic points incorrectly. Single-frame random sampling methods such as RANSAC significantly reduce the probability of extracting static points when the proportion of static points is low, leading to the velocity estimation model being affected by abnormal observations. Furthermore, they fail to utilize prior information from the previous frame, resulting in significant inter-frame jitter in the estimation results. In a unidirectional cascaded architecture, the pose and velocity information optimized by the backend does not participate in the selection and weight allocation of radar observation points at the frontend. The frontend cannot utilize historical state information to improve its ability to distinguish between static and dynamic points in the current frame. As observations degrade over multiple consecutive frames, errors accumulate frame by frame and propagate to the backend, accelerating trajectory drift. In addition, existing schemes typically set the covariance of radar velocity measurements to a fixed empirical value or simply scale it based on the number of points, failing to comprehensively reflect the impact of observation geometry, residual consistency, and the degree of dynamic interference on the reliability of velocity estimation. This makes it difficult for the backend filter to achieve adaptive fusion weight adjustment.

[0005] In summary, the key technical challenge to improving the ability to determine the trajectory of mobile carriers in visually and laser-constrained environments such as underground spaces lies in how to effectively constrain millimeter-wave radar Doppler observations using motion priors provided by inertial measurement units under low-cost sensor configurations, eliminate dynamic points and noise points that are inconsistent with the carrier's motion, and accurately transfer robust self-velocity observations and their reliability to the fusion estimation framework. Summary of the Invention

[0006] The technical problem to be solved by this application is that it is difficult to continuously determine the motion trajectory of a mobile vehicle in an underground space environment, and thus provides a method and system for determining the motion state of a mobile vehicle in underground space.

[0007] In a first aspect, the technical solution of this application provides a method for determining the motion state of a mobile carrier in underground space, wherein the mobile carrier is equipped with a millimeter-wave radar and an inertial measurement unit, and the method includes: The radar point cloud observation data is acquired through the millimeter-wave radar, and the inertial measurement data is acquired through the inertial measurement unit. A time correspondence between the radar point cloud observation data and the inertial measurement data is established. The inertial prediction state and its covariance information of the mobile carrier are constructed based on the inertial measurement data. The inertial prediction state includes the prior attitude quaternion, prior velocity, and prior angular velocity at the current moment. The radar point cloud observation data is preprocessed to extract the spatial orientation, measured Doppler velocity, and echo intensity of each radar observation point; Based on the prior velocity, the theoretical Doppler velocity of each radar observation point is calculated. The measured Doppler velocity of the same radar observation point is compared with the theoretical Doppler velocity. Based on the comparison results, the Doppler motion consistency residual is constructed. The radar observation points are screened based on the motion consistency residuals, and abnormal observation points with motion consistency residuals exceeding a set threshold are removed. Confidence weights are then assigned to the retained radar observation points based on the motion consistency residuals and the echo intensity. The reliability of the retained radar observation points is verified to obtain a set of reliable radar observation points; Based on the measured Doppler velocity and observation direction in the set of credible radar observation points, the estimated value of the self-velocity of the mobile carrier and its corresponding measurement uncertainty are obtained. The estimated self-velocity of the mobile vehicle is weighted and fused with the inertial prediction state to update the inertial prediction state of the mobile vehicle, and the motion trajectory of the mobile vehicle is formed based on the updated inertial prediction state. The current radar observation quality is evaluated based on the measurement uncertainty and the continuity judgment result of the motion trajectory; when the observation quality decreases, the weight of the mobile vehicle self-velocity estimate during fusion is reduced; when the observation quality recovers, the weight of the mobile vehicle self-velocity estimate during fusion is restored, and then the process returns to the step of constructing the Doppler motion consistency residual.

[0008] Some solutions describe methods for determining the motion state of mobile carriers in underground spaces, including acquiring radar point cloud observation data via millimeter-wave radar, acquiring inertial measurement data via inertial measurement unit, and establishing a time correspondence between the radar point cloud observation data and the inertial measurement data, comprising: Establish the extrinsic parameter transformation relationship between the millimeter-wave radar and the inertial measurement unit: ; in, Represents the coordinate system of the inertial measurement unit. b To radar coordinate system r The extrinsic transformation matrix, Let be the rotation matrix from the carrier coordinate system to the radar coordinate system. The rod arm is the one that represents the phase center of the millimeter-wave radar antenna relative to the center of the inertial measurement unit. This represents the time deviation between the radar timestamp and the inertial measurement unit timestamp. For radar timestamps, For inertial measurement unit timestamps; The radar point cloud observation data acquired by the millimeter-wave radar is represented as follows: ; ; in, This indicates the number of frames in the radar point cloud. Indicates the radar observation point number. For the first Frame number Observation vectors of radar observation points; For distance; It is the azimuth angle; The pitch angle; To measure the Doppler velocity; Echo intensity; For the first The timestamp of the radar point cloud frame; The line-of-sight direction per unit of radar observation point; These are the three-dimensional point coordinates in the radar coordinate system. The inertial measurement data acquired by the inertial measurement unit is represented as follows: ; in, For the first Sampling data from each inertial measurement unit; and These are the measurements from the accelerometer and gyroscope, respectively. and These are the actual specific force and angular velocity, respectively. , The zero bias of the accelerometer and gyroscope are respectively; , These are the corresponding measured noises; The time correspondence between the radar point cloud observation data and the inertial measurement data is expressed as follows: ; in, To align to the first Inertial prediction state of frame radar timestamps; This indicates that the propagation of the inertial measurement unit is performed sequentially in time. Indicates the first The timestamp of the radar point cloud frame; For the discrete propagation function of the inertial measurement unit; This represents the inertial prediction state updated from the previous sampling time. The sampling interval of the inertial measurement unit. and These are the first inertial measurement units. The sampling time and the first sampling time Each sampling time.

[0009] Some solutions describe methods for determining the motion state of mobile vehicles in underground spaces, wherein the step of constructing the inertial predicted state and its covariance information of the mobile vehicle based on the inertial measurement data includes: Based on the angular velocity data output by the inertial measurement unit, predict the prior attitude quaternion of the mobile carrier at the current moment: ; in, This represents the prior attitude quaternion predicted by the inertial measurement unit at the current moment; The attitude quaternion represents the previous time step; Represents quaternion multiplication; This represents the exponential mapping from a small-angle vector to a quaternion; These are gyroscope measurements; The gyroscope was at zero bias at the previous moment; The sampling interval; Based on the acceleration data output by the inertial measurement unit, and combined with the prior attitude quaternion, the prior velocity of the mobile carrier at the current moment is predicted:

[0010]

[0011] in, The predicted acceleration in the navigation coordinate system; This is the rotation matrix from the carrier coordinate system to the navigation coordinate system generated by the attitude quaternion; For accelerometer measurements; The accelerometer bias is the zero bias estimated at the previous moment; This is the gravity vector in the navigation coordinate system; Let be the prior velocity at the current moment in the navigation coordinate system. The velocity at the previous moment in the navigation coordinate system; The prior angular velocity is calculated based on the gyroscope measurement and the gyroscope zero bias at the previous moment: ; Based on the covariance information from the previous time step, the predicted covariance information for the current time step is generated: ; in, For the predicted covariance information; The covariance of the previous time step; This is the error state transition matrix; This is the noise driving matrix; For the process noise covariance of the inertial measurement unit; The inertial prediction state is obtained by processing. and its covariance information .

[0012] Some solutions describe methods for determining the motion state of mobile carriers in underground spaces, including preprocessing the radar point cloud observation data to extract the spatial orientation, measured Doppler velocity, and echo intensity of each radar observation point, comprising: Radar observation points that are outside the measurement range or have abnormal signals are excluded. The measurement range includes the range, the field of view range, and the physical upper limit of the Doppler velocity. The spatial direction is obtained by calculating the unit line-of-sight direction in the radar coordinate system based on the distance, azimuth, and elevation angle of each retained radar observation point; Extract the measured Doppler velocity and echo intensity of the retained radar observation points; Based on the spatial distribution and Doppler velocity distribution of the radar point cloud in the current frame, determine whether there is observation degradation in the radar point cloud in the current frame; When observation degradation exists, the rigor of preprocessing for subsequent radar observation points should be increased.

[0013] Some schemes describe methods for determining the motion state of mobile carriers in underground spaces. The method involves calculating the theoretical Doppler velocity at each radar observation point based on the prior velocity, comparing the measured Doppler velocity at the same radar observation point with the theoretical Doppler velocity, and constructing a Doppler motion consistency residual based on the comparison results. This includes: The theoretical Doppler velocity of the radar observation point is calculated using the following formula: ; in, For the first The theoretical Doppler velocity of each radar observation point; This is the Doppler zero-bias term; The Doppler motion consistency residuals are constructed using the following formula. :

[0014] in, The covariance of the prior velocity in the radar coordinate system; To measure the noise variance of radar Doppler; Also includes: The radar observation point is determined to conform to the overall motion state of the moving vehicle based on the Doppler motion consistency residual. When the Doppler motion consistency residual is less than or equal to a set threshold, the radar observation point is determined to meet the static environment motion consistency constraint and is considered a candidate static environment scattering point. When the Doppler motion consistency residual is greater than the set threshold, the radar observation point is considered a candidate for dynamic target, noise point, or abnormal observation point.

[0015] In some schemes, the method for determining the motion state of a mobile carrier in underground space involves screening radar observation points based on the motion consistency residual, removing abnormal observation points whose motion consistency residual exceeds a set threshold, and assigning confidence weights to the retained radar observation points based on the motion consistency residual and the echo intensity. The confidence weights are calculated as follows: ;

[0016] in, For the first The credibility weight of each radar observation point; This represents the normalization operation; For robust kernel weight functions; The set of radar observation points to be retained; The robust kernel truncation threshold; , These are the intensity and spatial density weighting indices, respectively. For spatial neighborhood set; The number of reference neighborhood points.

[0017] Some schemes describe methods for determining the motion state of mobile carriers in underground spaces, where the estimation of the mobile carrier's self-velocity and its corresponding measurement uncertainty is obtained based on the measured Doppler velocity and observation direction from the set of reliable radar observation points. This includes: Establish the self-velocity estimation equation: ; in, Let be the self-velocity of the moving vehicle to be estimated in the radar coordinate system; For Doppler zero bias; For measurement error; A set of reliable radar observation points; Based on the self-velocity estimation equation, and using the measured Doppler velocity and observation direction in the set of credible radar observation points, the self-velocity estimate is solved by weighted least squares. The objective function of the weighted least squares is expressed as: ; ; ; In the formula, For robust cost function; The observation matrix; for The first in OK; These are a priori regularization terms; This is the prior covariance or regularity intensity matrix; Let be the parameter vector to be estimated; The measurement uncertainty is represented by the velocity measurement covariance, which is determined by the following formula: ; ; in, For velocity measurement covariance, The variance of the velocity-weighted residuals; It is a 3×3 identity matrix; This is the weight matrix; Valid points; This is the estimated value after convergence.

[0018] Some solutions describe methods for determining the motion state of a mobile vehicle in underground space, wherein the step of weightedly fusing the estimated self-velocity of the mobile vehicle with the inertial prediction state to update the inertial prediction state of the mobile vehicle includes: Calculate the velocity measurement residual and the predicted velocity value using the following formulas: ; ; in, For velocity measurement residuals; This is the estimated value of the self-velocity of the moving carrier; The radar coordinate system velocity prediction value is derived from the inertial prediction state. For the prior attitude quaternion; The measurement uncertainty is adaptively adjusted based on the observation geometric condition number and the front-end observation quality factor:

[0019]

[0020] In the formula, For the adaptively adjusted radar velocity measurement covariance; For measurement uncertainty; This is the geometric degradation adjustment coefficient; For observation geometric condition number; To prevent small amounts from being divided by zero; For front-end observation quality factor; To prevent small amounts from being divided by zero; The minimum number of valid points is expected. For reference residual variance; When the measurement uncertainty after adaptive adjustment increases, the weight of the mobile carrier self-velocity estimate in the fusion update decreases accordingly. Calculate the Kalman gain using the following formula. : ; in, For the predicted covariance information, The Jacobian matrix for velocity measurement with respect to error states. For adaptively adjusted radar velocity measurement covariance; Update the status according to the following formula: ; ; in, This is the updated error status. For inertial prediction state, This is the updated inertial prediction state. This indicates that the error state will be injected into the nominal state; Update the covariance using the following formula: ; in, This is the updated covariance information; Based on the velocity in the updated inertial prediction state, calculate the position increment at the current moment; add the position increment to the position at the previous moment to obtain the position at the current moment; accumulate the positions and attitudes at each moment in chronological order to form the motion trajectory of the mobile vehicle.

[0021] Some schemes describe methods for determining the motion state of mobile vehicles in underground spaces, including evaluating the current radar observation quality based on the measurement uncertainty and the continuity judgment result of the motion trajectory; reducing the weight of the mobile vehicle self-velocity estimate during fusion when the observation quality deteriorates, and restoring the weight of the mobile vehicle self-velocity estimate during fusion when the observation quality recovers, including: Based on the changes in position, velocity, and attitude at adjacent moments, determine whether the motion trajectory has any abnormal abrupt changes or discontinuities; The quality of radar observations is evaluated in the following ways: ; ;

[0022] in, For radar observation quality factor, This is the normalized squared innovation statistic; This represents the number of valid points in the set of reliable radar observation points. For innovative covariance; This serves as a reference value for the observed geometric condition number; When the radar observation quality deteriorates, the measurement uncertainty is increased in the following way: ; For measurement uncertainty in degradation mode; When the radar observation quality is below a safety threshold, the mobile vehicle's self-velocity estimate is blocked from participating in the weighted fusion: ;in, This is a radar measurement activation indicator. This indicates that the radar measurements are blocked from participating in the weighted fusion. For a degree of freedom of 3 and a confidence level of β The card threshold, The minimum number of valid points; When the radar measurement is blocked, the state output is maintained only by inertial short-time propagation, and the covariance is increased to reflect the increase in uncertainty: ;in, This is additional process noise in inertial maintenance mode. For time intervals; When the radar observation quality is restored, the weight of the mobile vehicle's self-velocity estimate in the weighted fusion is gradually restored in the following manner: ; ;in, As a recovery factor, To preset the recovery time constant, This refers to the measurement uncertainty during the recovery phase.

[0023] Secondly, the present application provides a system for determining the motion state of a mobile carrier in underground space, including a memory, a processor, and a computer program stored in the memory. The processor executes the computer program to implement the steps of the method for determining the motion state of a mobile carrier in underground space as described in any of the first aspects.

[0024] The technical solution provided in this application has the following technical effects compared with the prior art: This application presents a method and system for determining the motion state of mobile carriers in underground spaces. By establishing a temporal correspondence between radar point cloud observation data and inertial measurement data, and constructing an inertial prediction state including prior attitude quaternions, prior velocity, and prior angular velocity, it provides a physical consistency criterion for radar observation based on the overall motion state of the carrier. Through preprocessing the radar point cloud observation data and extracting the spatial orientation, measured Doppler velocity, and echo intensity of each radar observation point, and combining the comparison between theoretical and actual Doppler velocities to construct a Doppler motion consistency residual, it achieves effective screening and reliability weighting of radar observation points. This approach avoids the limitations of fixed threshold filtering and single-frame random sampling in scenarios with sparse point clouds or dynamic interference. It obtains self-velocity estimates and their measurement uncertainties based on a set of reliable radar observation points, and then weights and fuses them with the inertial prediction state. This allows the fusion weights to dynamically and adaptively adjust according to the observation quality, overcoming the problem that fixed covariance settings cannot comprehensively reflect the observation geometry and the degree of dynamic interference. The radar observation quality is evaluated based on the measurement uncertainty and trajectory continuity judgment results. When the observation quality deteriorates, the radar measurement weights are reduced and inertia is used to maintain the output; when the quality recovers, the radar weights are restored, ensuring the continuity of the trajectory output. Therefore, this application effectively solves the problem of trajectory discontinuity caused by the instability of visual and laser observations in underground space environments where GNSS is restricted or denied, improving the robustness and reliability of mobile vehicle trajectory determination. Attached Figure Description

[0025] Figure 1 This is a flowchart of a method for determining the motion state of a mobile carrier in underground space according to an embodiment of this application; Figure 2 This is a schematic diagram of Doppler residual screening results in different underground space scenarios according to one embodiment of this application; Figure 3 This is a schematic diagram illustrating the self-velocity estimation and uncertainty results according to one embodiment of this application; Figure 4 This is a schematic diagram comparing the fused trajectory estimation result with the reference trajectory in a basement scene according to one embodiment of this application; Figure 5This is a schematic diagram comparing the results of trajectory reliability judgment and degradation maintenance under abnormal observation conditions according to one embodiment of this application; Figure 6 This is a schematic diagram of the hardware structure of a mobile carrier motion state determination system for underground space according to an embodiment of this application. Detailed Implementation

[0026] The specific embodiments of this application will be further described below with reference to the accompanying drawings.

[0027] It is readily understood that, based on the technical solution of this application, various structural and implementation methods can be interchanged by those skilled in the art without altering the essential spirit of this application. Therefore, the following detailed embodiments and accompanying drawings are merely illustrative examples of the technical solution of this application and should not be considered as the entirety of this application or as limitations or restrictions on the technical solution of the application.

[0028] This embodiment provides a method for determining the motion state of a mobile vehicle in underground spaces. It is applied to a processor / computing unit onboard the mobile vehicle or a remote server. The mobile vehicle is equipped with a millimeter-wave radar and an inertial measurement unit (IMU). During installation, the relative poses between the millimeter-wave radar coordinate system, the IMU payload system, and the mobile vehicle structural coordinate system are kept fixed, and the initial values ​​of the radar's installation rotation relative to the IMU, the lever arm, and the time deviation are recorded. This fixed installation relationship is used to subsequently map the radar Doppler velocity uniformly into the inertial state estimation framework, avoiding systematic velocity errors caused by sensor installation offsets. Figure 1 As shown, the method includes the following steps: S100: Obtain radar point cloud observation data through the millimeter-wave radar, obtain inertial measurement data through the inertial measurement unit, and establish a time correspondence between the radar point cloud observation data and the inertial measurement data.

[0029] In this step, radar point cloud observation data of the current environment is acquired using millimeter-wave radar. This observation data includes at least the distance, angle, and Doppler velocity information of each radar observation point. Simultaneously, angular velocity and acceleration data of the moving vehicle are acquired using an inertial measurement unit (IMU), which continuously outputs data at a frequency higher than the radar frame rate. Based on the timestamp information of the millimeter-wave radar and the IMU, a temporal correspondence is established between the radar point cloud observation data and the IMU data: interpolation and integration are performed on the IMU sampled data near the radar frame time, ensuring that each radar point cloud frame corresponds to a consistent inertial prediction state at a given time. When there is a small time deviation between the radar and the IMU, this time deviation is used as a compensation quantity in the inertial state propagation, ensuring that the velocity and attitude belong to the same physical moment in subsequent Doppler residual calculations.

[0030] S200: Construct the inertial prediction state and its covariance information of the mobile carrier based on the inertial measurement data. The inertial prediction state includes the prior attitude quaternion, prior velocity, and prior angular velocity at the current moment.

[0031] In this step, the attitude change of the moving vehicle at the current moment is predicted based on the angular velocity data output by the inertial measurement unit (IMU). The angular velocity data needs to be subtracted from the gyroscope zero bias before participating in attitude propagation. The velocity change of the moving vehicle at the current moment is predicted based on the acceleration data output by the IMU. The acceleration data is first subtracted from the accelerometer zero bias and rotated to the navigation coordinate system, and then participates in velocity propagation together with the gravity term to obtain the predicted velocity in the navigation coordinate system. Combining the velocity, attitude, and inertial state of the moving vehicle at the previous moment, the prior velocity, prior angular velocity, and their covariance information at the current moment are formed. This prior information includes not only velocity and angular velocity but also their covariance, which is subsequently used to construct the normalized residual in the radar Doppler residual, rather than just for fixed threshold judgment.

[0032] S300: Preprocess the radar point cloud observation data to extract the spatial direction, measured Doppler velocity, and echo intensity of each radar observation point.

[0033] In this step, basic preprocessing is performed on the millimeter-wave radar point cloud to remove radar observation points that are significantly outside the measurement range or have abnormal signals. The preprocessing considers the measurable range, field of view, echo intensity, and the physical upper limit of Doppler velocity, first eliminating points that clearly do not meet the sensor's operating conditions. Range, azimuth, elevation, and Doppler velocity information are extracted for each radar observation point. For the retained radar observation points, their three-dimensional coordinates in the radar coordinate system, unit line-of-sight direction, and echo intensity are calculated simultaneously, ensuring they can participate in both the velocity projection equation and the spatial density and weighted reliability calculations. Based on the spatial distribution and Doppler velocity distribution of the radar point cloud, a preliminary assessment is made to determine whether there are significant noise points, outliers, or dynamic interference points in the current frame's radar point cloud. The spatial dispersion and Doppler residual dispersion are used to jointly determine whether the current frame is in a strongly degraded state. If the number of points is small, the spatial direction is unidirectional, or the Doppler distribution is multi-peaked, it indicates that the frame may contain strong dynamic targets or multipath interference, requiring increased robustness screening in subsequent steps. Candidate radar observation points with self-velocity estimation value are retained as input for subsequent motion consistency modeling. These candidate radar observation points are not directly used as valid points, but rather incorporated into the calculation of motion consistency residuals guided by inertial priors.

[0034] S400: Based on the prior velocity, calculate the theoretical Doppler velocity of each radar observation point, compare the measured Doppler velocity of the same radar observation point with the theoretical Doppler velocity, and construct the Doppler motion consistency residual based on the comparison results.

[0035] In this embodiment, the inertial prediction state constructed in step S200 is used to constrain the selection of millimeter-wave radar observation points in the current frame. Specifically, the prior velocity and prior angular velocity in the inertial prediction state are introduced into the theoretical Doppler velocity calculation as a physical benchmark for determining whether each radar observation point conforms to the overall motion state of the carrier.

[0036] In this step, the theoretical Doppler velocity of the radar observation point under the current motion state of the carrier is calculated based on the prior velocity of the moving carrier and the spatial orientation of the radar observation point. The theoretical Doppler velocity is determined not only by the translational velocity but also by the rotational velocity term caused by the linkage between the radar and the inertial measurement unit, and a Doppler zero-bias term is introduced to accommodate installation and calibration errors in the actual deployment of low-cost millimeter-wave radar. The Doppler velocity actually measured at the radar observation point is compared with the theoretical Doppler velocity to construct a point-level Doppler motion consistency residual. To distinguish between real dynamic anomalies and acceptable deviations caused by prior uncertainties, the velocity prior covariance and radar Doppler noise are both included in the residual scale to calculate the normalized residual. The normalized residual is used as the basis for judging whether the radar observation point conforms to the overall motion state of the carrier. If the radar observation point comes from a static wall, tunnel boundary, or fixed facility, its Doppler velocity should be consistent with the radial velocity induced by the carrier motion; if it comes from pedestrians, vehicles, rotating equipment, smoke reflection, or multipath false alarms, the normalized residual will increase significantly. When the normalized residual of a radar observation point is small, it is determined that it comes from a static environment; when the normalized residual is large, it is determined that it is a dynamic target, a noise point, or an abnormal observation point. This determination is not a one-time hard rejection, but rather serves as initial evidence for subsequent multi-level screening and weight allocation.

[0037] S500: The radar observation points are screened according to the motion consistency residual, and abnormal observation points with motion consistency residual exceeding the set threshold are removed. The confidence weights are assigned to the retained radar observation points according to the motion consistency residual and the echo intensity.

[0038] In this step, the consistency of radar observation points is evaluated based on the normalized residuals of each observation point. The evaluation index integrates normalized residuals, echo intensity, and local spatial density. Abnormal observation points with motion consistency residuals exceeding a set threshold are removed. The threshold is adaptively updated using a robust statistical scale and is used in conjunction with a chi-square threshold to adapt to changes in noise levels, dynamic target proportions, and multipath intensity in different underground space scenarios. Radar observation points with small residuals and stable spatial distribution are retained. To ensure sufficient directional constraints on the self-velocity equation, the spatial distribution of the retained points is checked a second time, prioritizing the retention of radar observation points distributed in different azimuths. Confidence weights are assigned to the retained radar observation points based on the residual magnitude, echo intensity, and spatial distribution stability. The weights are obtained by multiplicatively fusing motion consistency weights, echo intensity weights, and spatial density weights. Strong dynamic residuals, weak echoes, and isolated points are automatically weighted less in subsequent velocity estimations.

[0039] S600: Verify the credibility of the retained radar observation points to obtain a set of credible radar observation points.

[0040] In this step, isolated noise points are eliminated based on the spatial relationships of radar observation points, using local spatial density and neighborhood connectivity. Velocity anomalies are eliminated based on the Doppler velocity consistency of the radar observation points. For spatially close radar observation points, their residuals after subtracting theoretical Doppler should have similar statistical distributions. If a radar observation point deviates significantly from its neighborhood residuals, it is identified as a velocity anomaly. Dynamic interference caused by moving targets is identified and suppressed based on continuous radar observation changes. Short-term tracking is performed on radar observation points or local clusters in consecutive frames. If a cluster exhibits an independent motion trend relative to inertial prediction, it is marked as dynamic interference and its weight is reduced. After the above multi-level screening, a set of reliable radar observation points for self-velocity estimation is obtained.

[0041] S700: Based on the measured Doppler velocity and observation direction in the set of credible radar observation points, obtain the estimated value of the self-velocity of the mobile carrier and its corresponding measurement uncertainty.

[0042] In this step, a self-velocity estimation equation for the mobile vehicle is established based on the measured Doppler velocity and observation direction from a set of reliable radar observation points. Under the assumption of a static environment, the measured Doppler velocity is the projection of the mobile vehicle's self-velocity onto the radar line-of-sight direction. The self-velocity is jointly estimated with the Doppler zero-bias estimation. A weighted least squares self-velocity estimation model is constructed based on the reliability weights of each radar observation point. The model simultaneously introduces a priori velocity regularization terms and a robust kernel function to ensure that the self-velocity estimation is not dragged apart by a few outliers when there are few radar observation points in the current frame or when the observation direction degrades. Based on the self-velocity estimation residuals and the distribution of observation weights, the measurement uncertainty of the current radar self-velocity observation is generated. The uncertainty is jointly determined by the residual variance, observation geometry, and the number of effective points, and explicitly reflects the point cloud direction degradation and weight concentration issues.

[0043] S800: The estimated velocity of the mobile carrier is weighted and fused with the inertial prediction state to update the inertial prediction state of the mobile carrier, and the motion trajectory of the mobile carrier is formed based on the updated inertial prediction state.

[0044] In this step, the radar self-velocity estimate is used as the input to the velocity measurement fusion estimation framework. The radar velocity measurement is projected from the radar coordinate system onto the filter prediction measurement space, taking into account attitude, velocity, angular velocity, lever arm, and installation extrinsic parameters. Based on the measurement uncertainty of the radar self-velocity observation, appropriate weights are assigned to the radar velocity measurement. The measurement uncertainty is combined with the observation geometry number, the number of reliable points, and the residual consistency to dynamically adjust the measurement covariance. Velocity constraints are strengthened when radar observation quality is good, and the impact on the back-end state is automatically reduced when observation quality degrades. The radar velocity measurement is used to update the inertial prediction state, suppressing velocity drift and position drift caused by inertial integration. In the filter update, the radar self-velocity directly corrects the velocity error and indirectly constrains the attitude and inertial measurement unit bias through state covariance coupling. Based on the fused and updated velocity and attitude, the position change of the moving vehicle at the current moment is calculated. The position change is recursively accumulated with historical pose results to obtain the pose of the moving vehicle at the current moment. The pose estimation results at each moment are accumulated in chronological order to form the continuous motion trajectory of the moving vehicle.

[0045] S900: Evaluate the current radar observation quality based on the measurement uncertainty and the continuity judgment result of the motion trajectory; when the observation quality decreases, reduce the weight of the mobile vehicle self-velocity estimate during fusion; when the observation quality recovers, restore the weight of the mobile vehicle self-velocity estimate during fusion, and then return to S400.

[0046] In this step, the current radar self-velocity observation quality is evaluated based on the radar self-velocity estimation residual, the number of credible radar observation points, and the velocity measurement uncertainty. The quality evaluation considers both the front-end self-velocity estimation quality and the consistency of back-end filtering information. When the radar self-velocity observation quality meets the requirements, the inertial state is continuously corrected using radar self-velocity to maintain trajectory continuity. When the radar observation quality deteriorates or the number of credible radar observation points is insufficient, the impact of radar velocity measurements on the fusion results is reduced, and short-time inertial propagation is used to maintain the trajectory output. In degraded mode, the system does not immediately interrupt the trajectory but first amplifies the measurement covariance and reduces the radar weights; if the anomaly persists or the information exceeds the safety threshold, radar updates are temporarily blocked, and continuous output is maintained by short-time inertial propagation. When the radar observation quality recovers, the role of radar self-velocity measurements in the fusion estimation is re-enhanced. To avoid abrupt trajectory changes when switching from degraded mode to normal mode, a smoothing recovery factor is used to gradually reduce the radar measurement covariance. Finally, the continuous motion trajectory determination result of the mobile vehicle in the underground space is output. The output results include pose trajectory, velocity sequence, trajectory quality flag, degradation status flag, and radar observation quality index.

[0047] The above-described scheme provided in this embodiment constructs an inertial prediction state including prior velocity, prior angular velocity, and their covariance in step S200, and explicitly incorporates it into the theoretical Doppler velocity calculation in step S400. This transforms the motion consistency judgment of each radar observation point from relying on a fixed empirical threshold to a physical consistency judgment based on the overall motion state of the carrier. It can adapt to changes in noise levels, dynamic target proportions, and multipath intensity in different underground space scenarios, avoiding the incorrect rejection of effective static observation points when the point cloud is sparse and the incorrect retention of dynamic target points when dynamic interference is severe. Based on the normalized motion consistency residual, the radar observation points are determined point by point, and the credibility weights are assigned by comprehensively considering echo intensity and spatial distribution density. This ensures that the observation point selection does not rely on the probabilistic extraction process of single-frame random sampling such as RANSAC, effectively guaranteeing the temporal continuity and robustness of the velocity estimation results. Step S800 weighted fusion of radar self-velocity and inertial prediction state, and feeding the updated inertial prediction state back into the residual calculation in step S400, achieves closed-loop mutual support between radar observation screening and back-end state estimation. The front end can utilize historical state information to improve the dynamic-static differentiation capability of the current frame, suppressing abnormal observations from entering the estimation process at the source. The velocity measurement uncertainty generated in step S700 comprehensively reflects the impact of residual variance, observation geometry, and effective point count on the reliability of self-velocity estimation, and is used in step S800 to dynamically adjust the fusion weights, realizing the dynamic adaptive adjustment of back-end fusion weights according to the front-end observation quality. Step S900 evaluates the radar observation quality based on the measurement uncertainty and trajectory continuity judgment results. When observations are abnormal, short-time inertial propagation is used to maintain the trajectory output, and radar constraints are smoothly enhanced when observations recover, ensuring the continuity and stability of motion trajectory output in the confined environment of underground GNSS.

[0048] In specific implementation, S100, which involves acquiring radar point cloud observation data through the millimeter-wave radar, acquiring inertial measurement data through the inertial measurement unit, and establishing a time correspondence between the radar point cloud observation data and the inertial measurement data, includes: S101: Establish the external parameter transformation relationship between the millimeter-wave radar and the inertial measurement unit: ; in, Represents the coordinate system of the inertial measurement unit. b To radar coordinate system r The extrinsic transformation matrix, Let be the rotation matrix from the carrier coordinate system to the radar coordinate system. The rod arm is the one that represents the phase center of the millimeter-wave radar antenna relative to the center of the inertial measurement unit. This represents the time deviation between the radar timestamp and the inertial measurement unit timestamp. For radar timestamps, For example, in a specific embodiment of a downhole inspection robot, the installation yaw angle of the radar coordinate system relative to the inertial measurement unit carrier system is approximately 2°, and the pitch and roll angles are both less than 1°. The lever arm value is... Initial value of time deviation In actual deployment, the aforementioned external parameters and time delay parameters can be obtained through calibration. Those skilled in the art can determine them according to the specific sensor model and installation method. The values ​​here are merely illustrative examples.

[0049] S102: Acquire radar point cloud observation data in the current environment using millimeter-wave radar.

[0050] The radar point cloud observation data acquired by the millimeter-wave radar is represented as follows: ; ; in, This indicates the number of frames in the radar point cloud. Indicates the radar observation point number. For the first Frame number Observation vectors of radar observation points; For distance; It is the azimuth angle; The pitch angle; To measure the Doppler velocity; Echo intensity; For the first The timestamp of the radar point cloud frame; The line-of-sight direction per unit of radar observation point; These are the three-dimensional point coordinates in the radar coordinate system. For example, in one specific implementation, a radar observation point has a distance of 6.0 m, an azimuth angle of 30°, an elevation angle of 0°, a measured Doppler velocity of -0.95 m / s, and an echo intensity of 18 dB. Therefore, the unit line-of-sight direction of this radar observation point is approximately... The three-dimensional coordinates are approximately The above values ​​are used to illustrate the specific transformation method from the original radar observation to the unit line-of-sight direction and three-dimensional coordinates. The specific values ​​may vary depending on the radar model and the environment, and should not be construed as limiting the scope of protection of this application.

[0051] S103: Obtain angular velocity and acceleration data of the moving vehicle through the inertial measurement unit.

[0052] The inertial measurement data acquired by the inertial measurement unit is represented as follows: ; in, For the first Sampling data from each inertial measurement unit; and These are the measurements from the accelerometer and gyroscope, respectively. and These are the actual specific force and angular velocity, respectively. , The zero bias of the accelerometer and gyroscope are respectively; , These are the corresponding measurement noises; for example, in one specific embodiment, the sampling frequency of the inertial measurement unit is 100 Hz, and the accelerometer measurement at a certain sampling time is... The zero-biased acceleration estimate is ; Gyroscope measurement is The gyroscope has zero bias. The angular velocity after deducting the zero bias can then be used as the input for subsequent attitude propagation. In actual implementation, the zero bias estimate is continuously updated as the filtering process continues. The above values ​​are only used to illustrate the method of deducting the measurement data from the zero bias and should not be construed as limiting the scope of protection of this application.

[0053] S104: Based on the timestamp information of the millimeter-wave radar and the inertial measurement unit, establish the time correspondence between the radar point cloud observation data and the inertial measurement data.

[0054] Interpolation and integration are performed on the inertial measurement unit (IMU) sampling data near the radar frame time to ensure that each radar point cloud frame corresponds to a consistent inertial prediction state at a given time. For cases where there is a small time deviation between the radar and the IMU, this time deviation is used as a compensation quantity during propagation to ensure that velocity and attitude belong to the same physical moment when calculating the Doppler residual.

[0055] The time correspondence between the radar point cloud observation data and the inertial measurement data is expressed as follows: ; in, To align to the first Inertial prediction state of frame radar timestamps; This indicates that the propagation of the inertial measurement unit is performed sequentially in time. Indicates the first The timestamp of the radar point cloud frame; For the discrete propagation function of the inertial measurement unit; This represents the inertial prediction state updated from the previous sampling time. The sampling interval of the inertial measurement unit. and These are the first inertial measurement units. The sampling time and the first sampling time Sampling times. For example, in one specific embodiment, the millimeter-wave radar frame rate is 10 Hz and the inertial measurement unit (IMU) sampling frequency is 100 Hz. Then, approximately 10 IMU samples are taken between two adjacent radar frames. When the time deviation is 0.004 s, the radar frame time can be shifted by 4 ms to select the corresponding IMU integration interval, thereby ensuring that the Doppler observation and inertial prior in the same frame correspond in time. In actual implementation, the time deviation can be obtained through calibration or online estimation. The above values ​​are merely examples and should not be construed as limiting the scope of protection of this application.

[0056] Furthermore, the construction of the inertial prediction state and its covariance information of the mobile carrier based on the inertial measurement data in S200 includes: S201: Based on the angular velocity data output by the inertial measurement unit, predict the prior attitude quaternion of the mobile carrier at the current moment.

[0057] A quaternion or Lie group exponential mapping is used for attitude propagation, ensuring that attitude updates remain on the rotation group and avoiding Euler angle singularities and linearization accumulation errors. The specific calculation formula is as follows: ; in, This represents the prior attitude quaternion predicted by the inertial measurement unit at the current moment; The attitude quaternion represents the previous time step; Represents quaternion multiplication; This represents the exponential mapping from a small-angle vector to a quaternion; These are gyroscope measurements; The gyroscope was at zero bias at the previous moment; The sampling interval is denoted as .

[0058] In one specific implementation, the sampling interval The angular velocity after deducting zero bias is 0.01s. The yaw angle increment within this sampling interval is approximately 0.0005 rad. Updating the quaternion through exponential mapping avoids the singularity problem caused by directly accumulating Euler angles. In actual implementation, the sampling interval depends on the sampling frequency of the inertial measurement unit. The above values ​​are merely examples and should not be construed as limiting the scope of protection of this application.

[0059] S202: Based on the acceleration data output by the inertial measurement unit and combined with the prior attitude quaternion, predict the prior velocity of the moving vehicle at the current moment: Accelerometer bias is first subtracted from the accelerometer zero bias and rotated to the navigation coordinate system. Then, it is combined with the gravity term to participate in velocity propagation, yielding the predicted velocity in the navigation coordinate system. The specific calculation formula is as follows:

[0060]

[0061] in, The predicted acceleration in the navigation coordinate system; This is the rotation matrix from the carrier coordinate system to the navigation coordinate system generated by the attitude quaternion; For accelerometer measurements; The accelerometer bias is the zero bias estimated at the previous moment; This is the gravity vector in the navigation coordinate system; Let be the prior velocity at the current moment in the navigation coordinate system. The velocity in the navigation coordinate system at the previous moment.

[0062] For example, the forward ratio of the current load system after deducting zero bias is approximately Sampling interval If the forward velocity at the previous moment was Then the increment of the forward velocity within this sampling interval is approximately The short-time velocity prior at each radar frame is formed by the cumulative sampling from multiple IMUs. Sampling interval If the forward velocity at the previous moment was Then the increment of the forward velocity for this sampling is approximately The short-time velocity prior is formed by the cumulative sampling of multiple inertial measurement units at the radar frame time. In actual implementation, the magnitude of the specific force and the velocity increment depend on the actual motion state of the moving vehicle. The above values ​​are only examples and should not be construed as limiting the scope of protection of this application.

[0063] S203: The prior angular velocity is calculated based on the gyroscope measurement value and the gyroscope zero bias at the previous moment. .

[0064] S204: Based on the covariance information from the previous time step, generate the predicted covariance information for the current time step: ; in, For the predicted covariance information; The covariance of the previous time step; This is the error state transition matrix; This is the noise driving matrix; This represents the process noise covariance of the inertial measurement unit.

[0065] In one specific implementation, the diagonal of the velocity covariance at the previous moment is [0.04, 0.04, 0.09] (m / s)², which increases to [0.045, 0.045, 0.10] (m / s)² after 0.1 s of inertial propagation. The larger this covariance, the higher the tolerance for subsequent normalized Doppler residuals. In actual implementation, the specific value of the covariance depends on the accuracy and motion state of the inertial measurement unit. The above values ​​are merely examples and should not be construed as limiting the scope of protection of this application.

[0066] S205: The inertial prediction state is obtained by sorting it out. and its covariance information .

[0067] In this embodiment, the prior velocity and prior angular velocity are used as motion references for subsequent millimeter-wave radar Doppler observation consistency judgment. During radar front-end processing, the prior state participates in the theoretical Doppler calculation of each radar point in a feedback manner, transforming radar point selection from empirical screening to physical consistency judgment of "whether the observation conforms to the overall motion of the carrier".

[0068] ; In the formula, The prior velocity used for point-level Doppler prediction in the radar coordinate system; The rotation matrix from the navigation system to the carrier system; This is an antisymmetric matrix constructed from angular velocities; For the lever arm of the radar relative to the IMU, this item is used to compensate for the rotation-induced velocity caused by sensor mounting eccentricity. For example, the navigation system predicts the velocity as follows: The carrier system and the radar system are approximately aligned, and the strut arm is... angular velocity is The order of magnitude of the rotationally induced velocity is approximately Although small in low-speed robot scenarios, it can be used to improve the consistency of Doppler residual modeling.

[0069] This scheme transforms the selection of radar observation points from empirical elimination based on fixed thresholds to physical consistency judgment based on the overall motion state of the carrier, rather than simply using fixed thresholds.

[0070] In this embodiment, the inertial prediction state is subsequently used to constrain the selection of millimeter-wave radar observation points. Specifically, in step S400, the prior velocity and prior angular velocity in the inertial prediction state are introduced into the theoretical Doppler velocity calculation, so that the motion consistency judgment of each radar observation point is based on the overall motion state of the carrier, rather than relying on a fixed empirical threshold.

[0071] Furthermore, in S300, the radar point cloud observation data is preprocessed to extract the spatial orientation, measured Doppler velocity, and echo intensity of each radar observation point, including: S301: Eliminate radar observation points that are outside the measurement range or have abnormal signals. The measurement range includes the range, the field of view range, and the physical upper limit of the Doppler velocity.

[0072] Preprocessing considers the measurable range, field of view, echo intensity, and the physical upper limit of Doppler, first eliminating points that clearly do not meet the sensor's operating conditions, thus reducing the burden on subsequent robust estimation. Specifically: ; in, The set of radar points after basic preprocessing; , These are the lower and upper limits of the distance; For horizontal field of view constraints; This represents the physical upper limit of Doppler velocity. This is the minimum echo intensity threshold.

[0073] For example, the effective distance range can be set to... The horizontal field of view is limited to The Doppler velocity limit is set to The echo intensity threshold is set to Points outside the above range can be removed as outliers in the basic preprocessing stage.

[0074] S302: Calculate the unit line-of-sight direction in the radar coordinate system based on the distance, azimuth, and elevation angle of each retained radar observation point to obtain the spatial direction.

[0075] The three-dimensional coordinates, unit line-of-sight direction, and signal quality factor of the retained radar observation points are calculated simultaneously, enabling them to participate in both the velocity projection equation and the calculation of spatial density and weighted confidence. The specific formulas are as follows: ; In the formula, For the first Frame number Characteristic description of each radar point; For three-dimensional position; Direction of sight; Doppler velocity; For example, the three-dimensional position of a point is... The unit's line of sight direction is Doppler velocity is If the echo intensity is 18 dB, its characteristic description includes three types of information: geometry, velocity, and signal quality, which can be used in subsequent weighted velocimetry.

[0076] S303: Extract the measured Doppler velocity and echo intensity of the retained radar observation points. Based on the spatial distribution and Doppler velocity distribution of the radar point cloud in the current frame, determine whether observation degradation exists. If observation degradation exists, increase the rigor of preprocessing for subsequent radar observation points.

[0077] The spatial dispersion and Doppler residual dispersion are used to jointly determine whether the current frame is in a strongly degraded state. If the number of points is small, the spatial direction is unidirectional, or the Doppler distribution is multi-peaked, it indicates that the frame may contain strong dynamic targets or multipath interference, and the robust screening strength needs to be increased in subsequent steps. The specific calculation method is as follows:

[0078] In the formula, As an indicator of spatial distribution degradation; The point cloud coordinate covariance matrix for the current frame; and These represent the minimum and maximum eigenvalues ​​of the matrix, respectively. To prevent small quantities with a denominator of zero; For Doppler velocity robust discrete scales. For example, the eigenvalues ​​of the covariance matrix of the current frame point cloud are... ,but This indicates that the point cloud directional distribution is relatively narrow; if the median absolute deviation of the Doppler velocity... This indicates that the speed distribution is relatively concentrated; conversely, if it reaches... The above may indicate dynamic interference.

[0079] After preprocessing according to the scheme in this embodiment, candidate radar observation points with self-velocity estimation value are retained as input for subsequent motion consistency modeling. The preprocessed radar observation points are not directly used as static points, but rather incorporated into the motion consistency residual calculation guided by inertial priors. This avoids mistaking strong echo points in dynamic targets for reliable static points.

[0080]

[0081] In the formula, This is the set of candidate radar observation points that have passed the preprocessing in this embodiment; This represents the minimum requirement for spatial distribution. This is the effective observation direction area, used to exclude points obstructed by installation, reflections from the carrier itself, or anomalies in the edge field of view. Let... The effective observation direction area is horizontal. And exclude the area directly behind the carrier that is obstructed; if the current frame If the intensity at a certain point is 18 dB, then that point can enter the candidate set. This indicates significant geometric degradation, which can improve the strength of subsequent robust screening.

[0082] Furthermore, in S400, based on the prior velocity, the theoretical Doppler velocity of each radar observation point is calculated. The measured Doppler velocity of the same radar observation point is compared with the theoretical Doppler velocity. Based on the comparison result, a Doppler motion consistency residual is constructed, including: Based on the prior velocity of the mobile carrier and the spatial orientation of the radar observation point, the theoretical Doppler velocity of the radar observation point under the current motion state of the carrier is calculated. The theoretical Doppler velocity is determined not only by the translational velocity but also by the rotational velocity term caused by the linkage between the radar and the inertial measurement unit, and a Doppler zero-bias term is introduced to accommodate installation and calibration errors in the actual deployment of low-cost millimeter-wave radar. Specifically, for the... k The first frame i For each radar observation point, its theoretical Doppler velocity is calculated using the following formula: ; in, For the first The theoretical Doppler velocity of each radar observation point; This is the Doppler zero bias term; for example, the line of sight from a static wall point is... The prior forward velocity of the radar coordinate system is Rotating lever arm compensation is Doppler zero bias The theoretical Doppler velocity is approximately If the measured value is close to this value, then this point is more consistent with the static environment assumption.

[0083] The Doppler velocities measured at actual radar observation points are compared with the theoretical Doppler velocities to construct point-level Doppler motion consistency residuals. To distinguish between real dynamic anomalies and acceptable deviations caused by prior uncertainties, the velocity prior covariance and radar Doppler noise are both incorporated into the residual scale. The Doppler motion consistency residuals are constructed according to the following formula. :

[0084] in, The covariance of the prior velocity in the radar coordinate system; For radar Doppler noise variance measurement; for example, measuring Doppler velocity. Theoretical Doppler velocity Then the residual If the standard deviation of the residual at that point is Then the Doppler motion consistency residual Generally speaking, this can be considered as good consistency.

[0085] Preferably, this step further includes: determining whether the radar observation point conforms to the overall motion state of the mobile vehicle based on the Doppler motion consistency residual; when the Doppler motion consistency residual is less than or equal to a set threshold, determining that the radar observation point meets the static environment motion consistency constraint and using it as a candidate static environment scattering point; when the Doppler motion consistency residual is greater than the set threshold, using the radar observation point as a candidate dynamic target, noise point, or abnormal observation point.

[0086] If the radar observation point originates from a static wall, tunnel boundary, or fixed facility, its Doppler velocity should be consistent with the radial velocity induced by the carrier's motion; if the radar observation point originates from pedestrians, vehicles, rotating equipment, smoke reflection, or multipath false alarms, the normalized residual will typically increase significantly.

[0087] In the formula, This indicates that the point satisfies the static environment motion consistency assumption; This indicates that the point is more likely to be a moving target, noise, or an anomaly observation; The normalized residual threshold can be set based on the chi-square distribution or an empirical confidence level. For example, with a confidence level of 0.95, the single-degree-of-freedom chi-square threshold is approximately... If a certain radar observation point Then it satisfies If another radar observation point Such reflections are more likely to belong to dynamic targets or anomalous reflections. This judgment is not a one-time hard rejection, but rather serves as initial evidence for subsequent multi-level screening and weight allocation. Maintaining a certain buffer at the critical point avoids excessive deletion of effective static scattering points when Doppler noise increases due to the complex environment of underground spaces. The specific calculation is as follows: ; In the formula, For point The relative reliability of static scattering points; This is a modulating factor related to the prior probability of dynamic or anomalous observations. This confidence level is used for subsequent weight construction, and is not limited to simple binary discrimination. For example, taking... ,when hour, Approximately 0.90 or higher; when When static reliability is significantly reduced, it is subsequently weakened in weighted least squares.

[0088] Figure 2This paper illustrates the distribution and selection results of Doppler residuals from radar observation points in different underground space scenarios. The horizontal axis represents the data sequence number, the vertical axis represents the Doppler residual, the threshold line indicates the residual selection range, and valid points and outliers represent radar observation points that satisfy and do not satisfy the motion consistency constraint, respectively. Some radar observation points exhibit large Doppler residuals due to noise, dynamic targets, or local reflection anomalies. Directly participating in self-velocity estimation can easily cause fluctuations in velocity calculations. This embodiment utilizes inertial prior calculations of theoretical Doppler velocities and combines residual thresholds, local statistical scales, and spatial distribution characteristics to select radar observation points, providing a stable and reliable set of radar observation points for subsequent weighted self-velocity estimation.

[0089] Furthermore, the step S500, which involves filtering radar observation points based on the motion consistency residuals, removing abnormal observation points whose motion consistency residuals exceed a set threshold, and assigning confidence weights to the retained radar observation points based on the motion consistency residuals and the echo intensity, includes: S501: Based on the motion consistency residuals of each radar observation point, a consistency evaluation is performed on the radar observation points. The evaluation index comprehensively considers normalized residuals, local spatial support, and signal strength to avoid retaining isolated strong echo noise points when selecting solely based on Doppler residuals. Specifically: ; in, For point-level consistency scoring, the smaller the value, the more reliable the result; and These are the weights for the echo intensity term and the neighborhood density term, respectively. This is the intensity normalization constant; For the point The set of spatial neighbors centered on it. For example, take... , , a certain point If the intensity is 18 dB and the number of neighboring points is 6, the score will be lower than that of points with the same residual but weak echo and isolated distribution, and therefore they will be reserved with higher priority.

[0090] S502: Removes abnormal radar observation points whose motion consistency residuals exceed a set threshold. The threshold is updated adaptively using a robust statistical scale and is used in conjunction with a chi-square threshold to adapt to changes in the underground space environment, fluctuations in radar reflection intensity, and different tunnel geometry conditions. Specifically: ; ; In the formula, This represents the median of the residuals in the current frame. This is the absolute median difference; It is the elastic scaling factor; is the scaling factor from MAD to standard deviation under a Gaussian distribution; For a degree of freedom of 1 and a confidence level of The chi-square threshold; This is the set of points after initial screening. For example, the median of the residuals in the current frame. , ,Pick At that time, the robust residual window is approximately If a certain point Even if the echo is strong, it will be judged as an abnormal candidate.

[0091] S503: Radar observation points with small residuals and stable spatial distribution are retained to form a set of retained radar observation points. To ensure that the self-velocity equation has sufficient directional constraints, the spatial distribution of the retained set of radar observation points is checked a second time, prioritizing the retention of static radar observation points distributed in different orientations to reduce ill-conditioned velocity estimation problems caused by single wall surfaces or single-direction point clouds.

[0092] ; In the formula, The set of radar observation points to be retained; The observation matrix consists of the line-of-sight directions of the candidate points; Represents the matrix condition number; The maximum number of allowed conditions; Let be the rank of the observation matrix. For example, the observation matrix composed of candidate points satisfies [condition missing]. and If we assume If the number of conditions exceeds 300, then the frame has three-dimensional self-velocity constraints; if the number of conditions exceeds 300, it indicates that the observation direction is too concentrated and the radar velocity measurement weight should be reduced.

[0093] The confidence weights are obtained by multiplicatively fusing motion consistency weights, echo intensity weights, and spatial density weights, and then normalized. This design automatically reduces the weight of observation points with large normalized residuals, low echo intensity, and spatially isolated observation points in subsequent velocity estimation, rather than relying entirely on a single rejection threshold. The confidence weights are calculated as follows: ; ; in, For the first The credibility weight of each radar observation point; This represents the normalization operation; For robust kernel weight functions; The set of radar observation points to be retained; The robust kernel truncation threshold; , These are the intensity and spatial density weighting indices, respectively. For spatial neighborhood set; This refers to the number of neighboring points. For example, the normalized residual of a radar observation point. Cutoff threshold The Tukey weight is approximately 0.98; another radar observation point The time weight is 0. If a point has a high intensity and the number of neighboring points is close to 0, then the time weight is 0. After normalization, a higher final credibility weight will be obtained.

[0094] More preferably, S600 verifies the credibility of the retained radar observation points to obtain a set of credible radar observation points, including: S601: Spatial location relationship eliminates isolated noise points.

[0095] Isolated noise points are eliminated based on the spatial relationships of radar observation points. By eliminating isolated points through local spatial density and neighborhood connectivity, stable reflective structures such as tunnel walls, pipelines, and equipment boundaries can be preserved without relying on image texture.

[0096] Specifically, for each radar observation point, with it as the center and a preset radius... For a spatial neighborhood, count the number of radar observation points within that neighborhood: ; In the formula, spatial radius The set of neighboring points within; The minimum number of points required to form a stable local structure. If the number of neighboring points is less than this value, it is identified as a spatial outlier. If the number of points in the neighborhood is sufficient and the distribution is stable, it is retained for subsequent velocity consistency checks.

[0097] For example, in one specific implementation, the spatial neighborhood radius is taken. Minimum number of neighborhood points If a radar observation point has only one neighbor within a 0.5 m radius, it can be considered a spatially isolated point and removed. If it has five neighbors with a stable distribution, it will be retained for subsequent velocity consistency checks. In practice, the neighborhood radius and minimum number of points can be adjusted according to the radar point cloud density. The above values ​​are merely examples and should not be construed as limiting the scope of protection of this application.

[0098] S602: Doppler velocity consistency eliminates velocity anomalies.

[0099] Based on the Doppler velocity consistency of radar observation points, velocity anomalies are eliminated. For radar observation points that are spatially close and belong to the same static structure, their residuals after deducting the theoretical Doppler should have similar statistical distributions. If a radar observation point deviates significantly from its neighboring residuals, it is identified as a velocity anomaly or multipath point. Specifically, for each radar observation point, the degree of deviation from the median of the residuals of other observation points in its neighborhood is calculated: ; ; In the formula, This is a local Doppler consistency index; For the first i Doppler residuals at each radar observation point; For point The absolute median of the residuals within the neighborhood; Threshold for identifying local velocity anomalies; This is the set of points filtered for speed consistency. If Exceeding the set threshold If the local velocity anomaly is not detected, the point is determined to be a local velocity anomaly and is removed; otherwise, it is retained for dynamic evaluation in consecutive frames. For example, in one specific implementation, the residual of a radar observation point is 0.30 m / s, the median of its neighborhood residuals is 0.05 m / s, and the absolute median difference of the neighborhood residuals is 0.05 m / s. Then, the local velocity consistency index is approximately 3.37; if the local velocity anomaly detection threshold is... If the threshold is set to 3, the point can be identified as a local velocity anomaly and eliminated. In actual implementation, the discrimination threshold can be dynamically adjusted according to the environment. The above values ​​are only examples and should not be construed as limiting the scope of protection of this application.

[0100] S603: Continuous frame dynamic target recognition and suppression.

[0101] Based on continuous radar observation changes, dynamic interference caused by moving targets is identified and suppressed. Short-term tracking of radar observation points or local clusters in consecutive frames is performed, and inertial prediction of the state is used to compensate for carrier motion between the current frame and the previous frame. Specifically, for the first... c For each radar observation cluster, calculate its residual velocity relative to the carrier motion compensation: ; In the formula, For the first The residual velocity of each radar scattering cluster relative to the carrier motion after compensation; and These are the centroids of the cluster in the current frame and the previous frame, respectively; For inter-frame motion compensation based on inertial prediction; For dynamic cluster velocity threshold; This is a dynamic cluster set. For example, the centroid difference of a certain scattering cluster after inertial motion compensation between two adjacent frames is... If the radar frame interval is 0.1 s, then the residual velocity norm is approximately If dynamic cluster threshold This cluster can be marked as dynamic interference. In actual implementation, the dynamic threshold can be dynamically adjusted according to the scenario. The above values ​​are only examples and should not be construed as limiting the scope of protection of this application.

[0102] S604: Obtain the set of reliable radar observation points.

[0103] After the multi-level screening process described in S601 to S603, the radar observation points are updated to obtain a set of reliable radar observation points for self-velocity estimation. This final set simultaneously satisfies basic measurement constraints, motion consistency constraints, spatial density constraints, local Doppler consistency constraints, and continuous frame dynamic exclusion constraints, serving as input for subsequent self-velocity estimation. For example, in one specific implementation, if the original number of radar points in a frame is 120, after basic range screening, 90 points remain; after motion consistency and MAD screening, 55 points remain; and after local Doppler and dynamic cluster elimination, 42 points remain. These 42 points in the final set of reliable radar observation points are then used for self-velocity estimation. This step-by-step screening process effectively reduces the impact of false alarms from dynamic pedestrians or smoke on the trajectory. In actual implementation, the number of points retained at each stage depends on the specific environment and sensor characteristics. The above values ​​are merely examples and should not be construed as limiting the scope of protection of this application.

[0104] Furthermore, in S700, based on the measured Doppler velocity and observation direction in the set of credible radar observation points, the estimated value of the mobile carrier's self-velocity and its corresponding measurement uncertainty are obtained, including: S701: Establish the self-velocity estimation equation: Based on the measured Doppler velocity and observation direction from a set of reliable radar observation points, an estimation equation for the self-velocity of the moving vehicle is established. Under the assumption of a static environment, the measured Doppler velocity is the projection of the moving vehicle's self-velocity onto the radar's line-of-sight direction. To accommodate the Doppler null bias of low-cost radar, the self-velocity and Doppler null bias are jointly estimated. For the ... k The first frame i For a given reliable radar observation point, the self-velocity estimation equation is expressed as: ; in, Let be the self-velocity of the moving vehicle to be estimated in the radar coordinate system; For Doppler zero bias; For measurement error; A set of reliable radar observation points; S702: Based on the self-velocity estimation equation, and using the measured Doppler velocity and observation direction in the set of credible radar observation points, the self-velocity estimate is solved by weighted least squares. A weighted least squares self-velocity estimation model is constructed based on the confidence weights of each radar observation point. The model simultaneously incorporates a priori velocity regularization term and a robust kernel function to ensure that the self-velocity estimation is not swayed by a few outliers when there are few radar observation points in the current frame or when the observation direction degrades. The objective function of the weighted least squares model is expressed as: ; ; ; In the formula, For robust cost function; The observation matrix; for The first in OK; These are a priori regularization terms; This is the prior covariance or regularity intensity matrix; Let be the parameter vector to be estimated; A vector composed of Doppler observations; The measurement error is for the k-th frame.

[0105] The objective function is solved by iterative weighted least squares. The weights are updated according to the residual size in each iteration until the change in self-velocity is less than a set threshold or the maximum number of iterations is reached, thus obtaining the estimated value of the self-velocity of the mobile vehicle.

[0106] S703: Measurement uncertainty generation: Based on the self-velocity estimation residuals and the observation weight distribution, the measurement uncertainty of the current radar self-velocity observation is generated. The uncertainty is jointly determined by the residual variance, observation geometry, and effective number of points, and explicitly reflects point cloud orientation degradation and weight concentration issues. This uncertainty is then used for back-end filtering updates, realizing the transfer of front-end quality to back-end weights.

[0107] Measurement uncertainty is represented by the velocity measurement covariance, which is determined by the following formula: ; ; in, For velocity measurement covariance, The variance of the velocity-weighted residuals; It is a 3×3 identity matrix; This is the weight matrix; Valid points; This is the estimated value after convergence.

[0108] For example, in one specific implementation, four static radar observation points are selected, with their line-of-sight directions approximating forward, left-front, right-front, and lateral, respectively. The Doppler observations are [-1.10, -0.80, -0.70, -0.15] m / s, which constitutes a joint constraint on the three-dimensional velocity and Doppler zero bias of the radar coordinate system. In practice, dozens of reliable points are typically used for overdetermined solutions. The iteration stopping threshold can be set to a velocity change of less than 0.01 m / s or a maximum of 5 iterations. If the number of valid points is 42, and the weighted sum of squared residuals is 0.36 (m / s)², then the residual variance is approximately 0.36 / (42-4) = 0.0095 (m / s)². If the observation geometric condition number is 35, the covariance is relatively small; if the condition number increases to over 200, even if the residual is small, the velocity measurement uncertainty should be amplified. The above values ​​are merely examples and should be determined based on the specific sensors and scenarios used in actual implementation. They should not be construed as limiting the scope of protection of this application.

[0109] Figure 3 The figure shows a comparison of the self-velocity estimation, velocity error, and velocity uncertainty of millimeter-wave radar. The velocity estimation results in the three directions characterize the motion state of the moving vehicle in the radar coordinate system; the velocity error reflects the deviation between the estimated result and the reference result; and the uncertainty characterizes the reliability of the radar velocity measurement in the current frame. After employing motion consistency constraints and multi-dimensional weighting, the self-velocity estimation can maintain good continuity even with local anomalies and can simultaneously provide velocity measurement quality information. Figure 3 The figure shows a comparison of the velocity estimation, velocity error, and velocity uncertainty of millimeter-wave radar. Comparison Method 1 and Comparison Method 2 correspond to the implementation methods of RAVE (Radar Auto-Velocity Estimator) and Reve (Radar Ego Velocity Estimator), respectively. The velocity estimation results in the three directions are used to characterize the motion state of the moving vehicle in the radar coordinate system. The velocity error reflects the deviation between the estimated result and the reference result, and the uncertainty characterizes the reliability of the radar velocity measurement in the current frame.

[0110] Specifically, in this embodiment, step S700 establishes a self-velocity estimation equation based on the measured Doppler velocity and observation direction from the set of credible radar observation points, and uses weighted least squares to obtain the estimated value of the mobile vehicle's self-velocity. Simultaneously, the measurement uncertainty is determined jointly based on the weighted residual variance, the number of effective points, and the number of observation geometric conditions, and the velocity measurement covariance is used to determine the measurement uncertainty. This uncertainty reflects not only the accuracy of the velocity estimation but also explicitly the issues of point cloud orientation degradation and weight concentration. When the observation geometry condition number is large, even if the residual is small, the velocity measurement uncertainty is correspondingly amplified to avoid contaminating the fusion results with low-quality measurements. For example... Figure 3 As shown, after applying motion consistency constraints and multidimensional weighting, the self-velocity estimation can maintain good continuity even with local anomalies in the observations, and can simultaneously provide velocity measurement quality information. The velocity error is controlled within a small range, and the uncertainty can dynamically change with the observation quality. This indicates that this application not only outputs the radar self-velocity results, but also transforms the residuals, weights, and observation geometry into measurement uncertainty, providing a basis for subsequent radar / inertial measurement unit fusion updates and adaptive weight adjustment.

[0111] Furthermore, in S800, the step of weightedly fusing the estimated velocity of the mobile vehicle with the inertial prediction state to update the inertial prediction state of the mobile vehicle includes: S801: Calculate the speed measurement residual and the predicted speed value according to the following formula: The radar self-velocity estimate is used as the velocity measurement input for the fusion estimation framework. The radar velocity measurement is projected from the radar coordinate system onto the filter prediction measurement space, simultaneously considering attitude, velocity, angular velocity, lever arm, and installation extrinsic parameters. This allows the measurement residuals to directly constrain inertial velocity and zero-bias error. The specific calculation formula is as follows: ; ; in, For velocity measurement residuals; This is the estimated value of the self-velocity of the moving carrier; The radar coordinate system velocity prediction value is derived from the inertial prediction state. For example, the prior attitude quaternion; for example, the radar front-end output velocity. The radar coordinate system velocity obtained from inertial prediction is The velocity measurement residual is The residual is used to filter update speed and related errors.

[0112] S802: Adaptively adjust the measurement uncertainty based on the observation geometric condition number and the front-end observation quality factor: Based on the measurement uncertainty of radar velocity observations, appropriate weights are assigned to radar velocity measurements. The measurement uncertainty is dynamically adjusted by combining it with the observation geometric condition number, the number of reliable points, and residual consistency. Velocity constraints are strengthened when radar observation quality is good, and the impact on the back-end state is automatically reduced when observation quality degrades. The measurement uncertainty is adaptively adjusted based on the observation geometric condition number and the front-end observation quality factor. ; ; In the formula, For the adaptively adjusted radar velocity measurement covariance; For measurement uncertainty; This is the geometric degradation adjustment coefficient; For observation geometric condition number; To prevent small amounts from being divided by zero; For front-end observation quality factor; To prevent small amounts from being divided by zero; The minimum number of valid points is expected. For reference, the residual variance. For example... , The point-in-time factor is 1; if , The front-end quality factor is approximately If the geometric condition number is large, then It will be amplified further.

[0113] S803: When the measurement uncertainty after adaptive adjustment increases, the weight of the mobile carrier self-velocity estimate in the fusion update decreases accordingly.

[0114] S804: The inertial prediction state is updated using radar velocity measurements to suppress velocity and position drift caused by inertial integration. In the filtered update, the radar self-velocity directly corrects the velocity error, and the attitude and inertial measurement unit bias are indirectly constrained through state covariance coupling. In this step, the Kalman gain is calculated according to the following formula. : ; in, For the predicted covariance information, The Jacobian matrix for velocity measurement with respect to error states. For adaptively adjusted radar velocity measurement covariance; S805: Update the status according to the following formula: ; ; in, This is the updated error status. For inertial prediction state, This is the updated inertial prediction state. This indicates that the error state is injected into the nominal state; the state is expressed as: ; ; In the formula, This refers to the nominal state of the system. This is an error state; , , These are small errors in position, velocity, and attitude, respectively. , Zero bias error; This represents IMU process noise; the meanings of other symbols are the same as before. For example, the nominal state can include position. ,speed Attitude quaternions, acceleration zero bias, and gyroscope zero bias; the error state only describes small perturbations of these quantities, which facilitates filtering linearization and real-time updates.

[0115] S806: Update the covariance according to the following formula: ; in, This is the updated covariance information; S807: Calculate the position increment at the current moment based on the velocity in the updated inertial prediction state; add the position increment to the position at the previous moment to obtain the position at the current moment; accumulate the position and attitude at each moment in chronological order to form the motion trajectory of the mobile carrier.

[0116] The updated velocity is used to recursively calculate the position, and the updated attitude is used to keep the carrier's motion direction consistent with the inertial measurement direction, thereby continuously accumulating the radar velocity-constrained state into a trajectory.

[0117] ; In the formula, This represents the position increment under the current navigation system. To integrate the updated navigation system speed; Acceleration for the navigation system; The rotation matrix from the carrier system to the navigation system; For accelerometer measurement; To achieve zero bias in the accelerometer; This represents the gravity vector; the other symbols have the same meaning as before. For example, the velocity after fusion and updating is... Time interval When the acceleration correction term is small, the position increment is approximately If it exists The forward acceleration then increases by approximately the second-order term. .

[0118] like Figure 4As shown, the fused trajectory can be continuously output along the basement passage and maintains good consistency with the reference trajectory. Individual inertial integral trajectories diverge over time due to the lack of external correction, and individual radar velocity trajectories fluctuate when the observation direction degrades. In contrast, the fused trajectory using the method of this application maintains consistency with the reference trajectory in long straight passages and turning areas, with significantly reduced position drift. This indicates that this application is applicable to determining the continuous motion trajectory of mobile vehicles in underground spaces.

[0119] The position, attitude, velocity, and mass factor at each moment are retained during the recursive process, which facilitates subsequent judgment on whether there are abrupt changes in the trajectory and whether degradation marking is required.

[0120] ; In the formula, This is the current output position; This represents the homogeneous transformation of the pose of the moving carrier at the current moment; This is the rotation matrix obtained from the current updated pose. For example, the position at the previous time step was... The position increment at this moment is The current output position is And together with the current pose, they form a homogeneous pose matrix.

[0121] The trajectory is output in the order of timestamps, and retains radar observation quality, fused information and degraded status flags, so that subsequent mission systems can distinguish between normal trajectories, short-term inertial maintenance trajectories and corrected trajectories after observation recovery.

[0122]

[0123] In the formula, As of the date A continuous sequence of motion trajectories at any given moment; Indicates the first A time-tracking unit includes at least a timestamp. ,Location ,attitude ,speed Comprehensive trajectory quality factor and radar update status flags ; This indicates that the radar is participating in the update. This indicates that the radar is temporarily blocked. For example, the... At any given moment, the trajectory sequence stores 100 sets of timestamps, positions, attitudes, velocities, quality factors, and update flags; if at certain moments... If the output is a short-term inertial maintenance output, it indicates that the corresponding trajectory segment is a short-term inertial maintenance output, which is convenient for post-processing or task systems to distinguish the reliability.

[0124] In this embodiment, the updated inertial prediction state is fused and recursively updated to determine the consistency of radar observations at the next moment. Specifically, in step S400, the updated velocity and covariance information is fed back to the next frame as prior input for the calculation of theoretical Doppler velocity in the next frame. This enables the front-end radar observation screening to continuously utilize historical state information to improve the dynamic-static differentiation capability of the current frame, forming a closed-loop processing link of "prior guidance for observation and observation correction of state".

[0125] Furthermore, the evaluation of the current radar observation quality based on the measurement uncertainty and the continuity judgment result of the motion trajectory in S900; reducing the weight of the mobile vehicle self-velocity estimate during fusion when the observation quality decreases, and restoring the weight of the mobile vehicle self-velocity estimate during fusion when the observation quality recovers, includes: S901: Based on the position changes, velocity changes, and attitude changes at adjacent moments, determine whether there are any abnormal sudden changes or discontinuities in the motion trajectory.

[0126] The continuity judgment simultaneously constrains position increment, velocity change, and attitude change, and feeds the judgment result back to the subsequent quality assessment to prevent trajectory jumps caused by abnormal radar updates.

[0127] ; ; In the formula, As an indicator of trajectory continuity; and Weights for velocity and attitude changes; This represents the mapping from quaternions to small-angle vectors; The continuity threshold; and These are the maximum permissible speed and maximum acceleration of the mobile carrier, respectively.

[0128] The continuity assessment simultaneously constrains position increment, velocity change, and attitude change, and feeds the assessment result back into the subsequent quality evaluation to prevent trajectory jumps caused by abnormal radar updates. Specifically, when the position increment at adjacent time points exceeds the maximum allowable velocity and acceleration constraints, or the attitude change exceeds a set threshold, the current trajectory is determined to have an abnormal abrupt change or discontinuity. This assessment result serves as one of the inputs for the radar observation quality evaluation in step S902. For example, with a sampling interval of 0.1s and a maximum allowable velocity... Maximum acceleration The upper limit of the adjacent position increment is approximately 0.205 m. If an update causes a position jump of 0.60 m, a trajectory continuity anomaly detection should be triggered even if the radar residual surface is small.

[0129] S902: Evaluate the current radar observation quality based on the measurement uncertainty, the number of reliable radar observation points, and the continuity of the trajectory. The quality evaluation considers both the front-end self-velocity estimation quality and the consistency of the back-end filtering information, comprehensively judging the reliability of the radar velocity measurement. In this step, the radar observation quality is evaluated in the following way: ; ;

[0130] in, For radar observation quality factor, This is the normalized squared innovation statistic; This represents the number of valid points in the set of reliable radar observation points. For innovative covariance; The reference value for the observation geometric condition number; for example, the radar velocity residual is... The innovation covariance is approximately equal to ,but If the number of valid points is 42 and the geometric condition number is low, then A relatively large residual indicates that the frame can be used as a reliable update. If dynamic targets cause the residual to increase to a certain value... The NIS will increase significantly. In actual implementation, the above values ​​depend on the specific sensor accuracy and motion state, and should not be construed as limiting the scope of protection of this application.

[0131] S903: When the radar observation quality deteriorates, the measurement uncertainty is increased in the following manner: ; This step addresses the measurement uncertainty in the degradation mode. When radar observation quality deteriorates, the impact of radar velocity measurements on the fusion result is reduced, and short-time inertial propagation is used to maintain the trajectory output. In degradation mode, the system does not immediately interrupt the trajectory but first amplifies the measurement covariance and reduces the radar weights. For example, when… At this time, the degraded covariance is approximately 5 times the normal covariance, and the impact of radar update is significantly reduced; if and Then you can directly order It uses inertial short-time propagation to maintain continuous trajectory output.

[0132] S904: When the radar observation quality is below a safety threshold, the mobile vehicle's self-velocity estimate is blocked from participating in the weighted fusion. ;in, This is a radar measurement activation indicator. This indicates that the radar measurements are blocked from participating in the weighted fusion. For a degree of freedom of 3 and a confidence level of βThe card threshold, The minimum number of valid points is used. In this step, when the radar observation quality is lower than the safety threshold, the radar update is temporarily blocked, and continuous output is maintained by relying on short-term inertial propagation and the velocity constraint of the previous moment.

[0133] S905: When the radar measurement is blocked, the state output is maintained only by inertial short-time propagation, and the covariance is increased to reflect the increase in uncertainty. ;in, This is additional process noise in inertial maintenance mode. The time interval is denoted as ; in this step, when radar measurements are blocked, the state output is maintained only by inertial short-time propagation, and the covariance is increased to reflect the increase in uncertainty.

[0134] S906: When the radar observation quality is restored, the weight of the mobile vehicle's self-velocity estimate in the weighted fusion is gradually restored in the following manner: ; ;in, As a recovery factor, To preset the recovery time constant, This refers to the measurement uncertainty during the recovery phase. For example, the recovery time constant. If the radar frame interval is 0.1 s, the recovery factor increases by 0.05 per frame; after 20 consecutive stable radar observations, It can gradually reach 1, and the measurement covariance smoothly transitions from a degenerate value to a normal value, avoiding a sudden pullback in the trajectory.

[0135] As mentioned above, parallel to S902, when the radar self-velocity observation quality meets the requirements, the radar self-velocity is used to continuously correct the inertial state and maintain trajectory continuity. In normal mode, the radar self-velocity participates in the forward filtering update, and the updated velocity and covariance are fed back to the radar front end of the next frame to improve the motion consistency residual calculation of the next frame. The judgment condition for normal fusion mode is: When the above conditions are met, the radar self-velocity measurement is deemed reliable. Following the normal filtering update process in step S800, the system performs a weighted fusion of the radar self-velocity estimate and the inertial prediction state, updates the inertial prediction state and its covariance information, and feeds the updated state back to step S400 of the next frame to improve the motion consistency residual calculation for the next frame. (If a confidence level is taken...) At that time, the chi-square threshold for 3 degrees of freedom is approximately 7.81; if and , Then let And enable radar updates.

[0136] The final output determines the continuous motion trajectory of the mobile vehicle in underground spaces, low-light environments, or other complex environments. The output includes pose trajectory, velocity sequence, trajectory quality indicators, degradation status indicators, and radar observation quality indices. These results can be used by mobile robots, underground inspection equipment, rescue platforms, or construction surveying vehicles for path recording, task execution, safety monitoring, and post-processing analysis. The output is represented as follows: ; In the formula, This is the final set of output results; For continuous motion trajectories; for example, the final output may include pose, velocity, and [other parameters] every 0.1 s. , and When the inspection robot travels approximately 30 m in 30 seconds, the output trajectory can contain approximately 300 time points, marking the time periods of observed degradation, inertial maintenance, and normal fusion.

[0137] Thus, this embodiment completes a full processing cycle. The inertial prediction state constrains the selection of radar observation points in the current frame, and the fused updated state recursively updates the radar observation consistency judgment in the next frame, forming a closed-loop processing link of "inertial prior guiding radar observation selection → radar observation correcting inertial state → updated state feedback to the next frame prior," realizing bidirectional flow and mutual support of front-end and back-end information.

[0138] like Figure 5 As shown, by employing the reliability assessment and degradation maintenance mechanism of this application, the system can reduce the radar measurement weight or temporarily block abnormal updates when radar observations are abnormal, maintaining continuous trajectory output. After observation recovery, the radar constraints are smoothly enhanced to prevent sudden trajectory pullback. This mechanism effectively avoids abrupt trajectory changes caused by abnormal observations, ensuring the continuity and reliability of the output trajectory under complex observation conditions in underground spaces. Without reliability assessment, abnormal radar measurements cause significant deviations in the trajectory at local locations, and the lack of a smooth transition mechanism after recovery results in trajectory jumps.

[0139] This application also provides a system for determining the motion state of a mobile carrier in underground space, such as... Figure 6As shown, the electronic device includes at least one processor 61 and at least one memory 62. The at least one memory 62 stores program information. After reading the program information, the at least one processor 61 executes the method for determining the motion state of a mobile carrier in underground space as described in any of the above embodiments. The device may further include an input device 63 and an output device 64. The processor 61, memory 62, input device 63, and output device 64 can be communicatively connected. The memory 62, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer-executable programs, and modules. The processor 61 executes various functional applications and data processing by running the non-volatile software programs, instructions, and modules stored in the memory 62, thereby implementing the method for determining the motion state of a mobile carrier in underground space provided by any of the above embodiments. The memory 62 may include a program storage area and a data storage area. The program storage area may store the operating system and at least one application program required for a function; the data storage area may store data created based on the use of the method for determining the motion state of a mobile carrier in underground space. Furthermore, memory 62 may include high-speed random access memory and non-volatile memory, such as at least one disk storage device, flash memory device, or other non-volatile solid-state storage device. In some embodiments, memory 62 may optionally include memory remotely located relative to processor 61, which can be connected via a network to apparatus performing the method for determining the motion state of a mobile vehicle in underground space. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof. Input device 63 may receive user clicks and generate signal inputs related to user settings and function controls of the method for determining the motion state of a mobile vehicle in underground space. Output device 64 may include a display device such as a display screen. When one or more modules are stored in memory 62 and executed by one or more processors 61, the method for determining the motion state of a mobile vehicle in underground space as described in any of the above method embodiments is performed.

[0140] As needed, the above technical solutions can be combined to achieve the best technical effect.

[0141] The above are merely the principles and preferred embodiments of this application. It should be noted that, for those skilled in the art, several other modifications can be made based on the principles of this application, and these modifications should also be considered within the scope of protection of this application.

Claims

1. A method for determining the motion state of a mobile carrier in underground space, characterized in that, The mobile carrier is equipped with a millimeter-wave radar and an inertial measurement unit, and the method includes: The radar point cloud observation data is acquired through the millimeter-wave radar, and the inertial measurement data is acquired through the inertial measurement unit. A time correspondence between the radar point cloud observation data and the inertial measurement data is established. The inertial prediction state and its covariance information of the mobile carrier are constructed based on the inertial measurement data. The inertial prediction state includes the prior attitude quaternion, prior velocity, and prior angular velocity at the current moment. The radar point cloud observation data is preprocessed to extract the spatial orientation, measured Doppler velocity, and echo intensity of each radar observation point; Based on the prior velocity, the theoretical Doppler velocity for each radar observation point is calculated. The measured Doppler velocity at the same radar observation point is compared with the theoretical Doppler velocity. Based on the comparison results, a Doppler motion consistency residual is constructed, including: The theoretical Doppler velocity of the radar observation point is calculated using the following formula: ； in, For the first The theoretical Doppler velocity of each radar observation point The line-of-sight direction per unit of radar observation point. Let be the rotation matrix from the carrier coordinate system to the radar coordinate system. Let be the rotation matrix from the carrier coordinate system to the navigation coordinate system generated by the attitude quaternion. Let be the prior velocity at the current moment in the navigation coordinate system. For the prior angular velocity, The rod arm is the one that represents the phase center of the millimeter-wave radar antenna relative to the center of the inertial measurement unit. This is the Doppler zero-bias term; The Doppler motion consistency residuals are constructed using the following formula. : in, To measure the Doppler velocity, The covariance of the prior velocity in the radar coordinate system; To measure the noise variance of radar Doppler; The radar observation point is determined to conform to the overall motion state of the mobile vehicle based on the Doppler motion consistency residual. When the Doppler motion consistency residual is less than or equal to a set threshold, the radar observation point is determined to meet the static environment motion consistency constraint and is regarded as a candidate static environment scattering point. When the Doppler motion consistency residual is greater than the set threshold, the radar observation point is regarded as a candidate dynamic target, noise point, or abnormal observation point. The radar observation points are screened based on the motion consistency residuals, and abnormal observation points with motion consistency residuals exceeding a set threshold are removed. Confidence weights are then assigned to the retained radar observation points based on the motion consistency residuals and the echo intensity. The reliability of the retained radar observation points is verified to obtain a set of reliable radar observation points; Based on the measured Doppler velocity and observation direction in the set of credible radar observation points, the estimated value of the self-velocity of the mobile carrier and its corresponding measurement uncertainty are obtained. The estimated self-velocity of the mobile vehicle is weighted and fused with the inertial prediction state to update the inertial prediction state of the mobile vehicle, and the motion trajectory of the mobile vehicle is formed based on the updated inertial prediction state. The current radar observation quality is evaluated based on the measurement uncertainty and the continuity judgment result of the motion trajectory; when the observation quality decreases, the weight of the mobile vehicle self-velocity estimate during fusion is reduced; when the observation quality recovers, the weight of the mobile vehicle self-velocity estimate during fusion is restored, and then the process returns to the step of constructing the Doppler motion consistency residual.

2. The method for determining the motion state of a mobile carrier oriented towards underground space according to claim 1, characterized in that, The step of acquiring radar point cloud observation data through the millimeter-wave radar, acquiring inertial measurement data through the inertial measurement unit, and establishing a time correspondence between the radar point cloud observation data and the inertial measurement data includes: Establish the extrinsic parameter transformation relationship between the millimeter-wave radar and the inertial measurement unit: ； in, Represents the coordinate system of the inertial measurement unit. b To radar coordinate system r The extrinsic transformation matrix, This represents the time deviation between the radar timestamp and the inertial measurement unit timestamp. For radar timestamps, For inertial measurement unit timestamps; The radar point cloud observation data acquired by the millimeter-wave radar is represented as follows: ； ； in, This indicates the number of frames in the radar point cloud. Indicates the radar observation point number. For the first Frame number Observation vectors of radar observation points; For distance; It is the azimuth angle; The pitch angle; Echo intensity; For the first The timestamp of the radar point cloud frame; These are the three-dimensional point coordinates in the radar coordinate system. The inertial measurement data acquired by the inertial measurement unit is represented as follows: ； in, For the first Sampling data from each inertial measurement unit; and These are the measurements from the accelerometer and gyroscope, respectively. and These are the actual specific force and angular velocity, respectively. , The zero bias of the accelerometer and gyroscope are respectively; , These are the corresponding measured noises; The time correspondence between the radar point cloud observation data and the inertial measurement data is expressed as follows: ； in, To align to the first Inertial prediction state of frame radar timestamps; This indicates that the propagation of the inertial measurement unit is performed sequentially in time. Indicates the first The timestamp of the radar point cloud frame; For the discrete propagation function of the inertial measurement unit; This represents the inertial prediction state updated from the previous sampling time. The sampling interval of the inertial measurement unit. and These are the first inertial measurement units. The sampling time and the first sampling time Each sampling time.

3. The method for determining the motion state of a mobile carrier oriented towards underground space according to claim 2, characterized in that, The step of constructing the inertial prediction state and its covariance information of the mobile carrier based on the inertial measurement data includes: Based on the angular velocity data output by the inertial measurement unit, predict the prior attitude quaternion of the mobile carrier at the current moment: ； in, This represents the prior attitude quaternion predicted by the inertial measurement unit at the current moment; The attitude quaternion represents the previous time step; Represents quaternion multiplication; This represents the exponential mapping from a small-angle vector to a quaternion; These are gyroscope measurements. The gyroscope was at zero bias at the previous moment; The sampling interval; Based on the acceleration data output by the inertial measurement unit, and combined with the prior attitude quaternion, the prior velocity of the mobile carrier at the current moment is predicted: in, Predicted acceleration in the navigation coordinate system; For accelerometer measurements; The accelerometer bias is the zero bias estimated at the previous moment; This is the gravity vector in the navigation coordinate system; The velocity at the previous moment in the navigation coordinate system; The prior angular velocity is calculated based on the gyroscope measurement and the gyroscope zero bias at the previous moment: ; Based on the covariance information from the previous time step, the predicted covariance information for the current time step is generated: ； in, For the predicted covariance information; The covariance of the previous time step; This is the error state transition matrix; This is the noise driving matrix; For the process noise covariance of the inertial measurement unit; The inertial prediction state is obtained by processing. and its covariance information .

4. The method for determining the motion state of a mobile carrier in underground space according to claim 3, characterized in that, The preprocessing of the radar point cloud observation data, extracting the spatial orientation, measured Doppler velocity, and echo intensity of each radar observation point, includes: Radar observation points that are outside the measurement range or have abnormal signals are excluded. The measurement range includes the range, the field of view range, and the physical upper limit of the Doppler velocity. The spatial direction is obtained by calculating the unit line-of-sight direction in the radar coordinate system based on the distance, azimuth, and elevation angle of each retained radar observation point; Extract the measured Doppler velocity and echo intensity of the retained radar observation points; Based on the spatial distribution and Doppler velocity distribution of the radar point cloud in the current frame, determine whether there is observation degradation in the radar point cloud in the current frame; When observation degradation exists, the rigor of preprocessing for subsequent radar observation points should be increased.

5. The method for determining the motion state of a mobile carrier oriented towards underground space according to claim 4, characterized in that, The process involves filtering radar observation points based on the motion consistency residuals, removing abnormal observation points whose motion consistency residuals exceed a set threshold, and assigning confidence weights to the retained radar observation points based on the motion consistency residuals and the echo intensity. The confidence weights are calculated as follows: ； in, For the first The credibility weight of each radar observation point; This represents the normalization operation; For robust kernel weight functions; The set of radar observation points to be retained; The robust kernel truncation threshold; , These are the intensity and spatial density weighting indices, respectively. For spatial neighborhood set; The reference neighborhood number.

6. The method for determining the motion state of a mobile carrier oriented towards underground space according to claim 5, characterized in that, The method of obtaining the estimated value of the mobile vehicle's self-velocity and its corresponding measurement uncertainty based on the measured Doppler velocity and observation direction in the set of credible radar observation points includes: Establish the self-velocity estimation equation: ； in, Let be the self-velocity of the moving vehicle to be estimated in the radar coordinate system; For Doppler zero bias; For measurement error; A set of reliable radar observation points; Based on the self-velocity estimation equation, and using the measured Doppler velocity and observation direction in the set of credible radar observation points, the self-velocity estimate is solved by weighted least squares. The objective function of the weighted least squares is expressed as: ； ； ； In the formula, For robust cost function; The observation matrix; for The first in OK; These are a priori regularization terms; This is the prior covariance or regularity intensity matrix; Let be the parameter vector to be estimated; The measurement uncertainty is represented by the velocity measurement covariance, which is determined by the following formula: ； ； in, For velocity measurement covariance, The variance of the velocity-weighted residuals; It is a 3×3 identity matrix; This is the weight matrix; Valid points; This is the estimated value after convergence.

7. The method for determining the motion state of a mobile carrier oriented towards underground space according to claim 6, characterized in that, The step of weightedly fusing the estimated self-velocity of the mobile vehicle with the inertial prediction state to update the inertial prediction state of the mobile vehicle includes: Calculate the velocity measurement residual and the predicted velocity value using the following formulas: ； ； in, For velocity measurement residuals; This is the estimated value of the self-velocity of the moving carrier; The radar coordinate system velocity prediction value is derived from the inertial prediction state. For the prior attitude quaternion; The measurement uncertainty is adaptively adjusted based on the observation geometric condition number and the front-end observation quality factor: In the formula, For the adaptively adjusted radar velocity measurement covariance; For measurement uncertainty; This is the geometric degradation adjustment coefficient; For observation geometric condition number; To prevent small amounts from being divided by zero; For front-end observation quality factor; The minimum number of valid points is expected. For reference residual variance; When the measurement uncertainty after adaptive adjustment increases, the weight of the mobile carrier self-velocity estimate in the fusion update decreases accordingly. Calculate the Kalman gain using the following formula. : ； in, For the predicted covariance information, The Jacobian matrix for velocity measurement with respect to error states. For adaptively adjusted radar velocity measurement covariance; Update the status according to the following formula: ； ； in, This is the updated error status. For inertial prediction state, This is the updated inertial prediction state. This indicates that the error state will be injected into the nominal state; Update the covariance using the following formula: ； in, This is the updated covariance information; Based on the velocity in the updated inertial prediction state, calculate the position increment at the current moment; add the position increment to the position at the previous moment to obtain the position at the current moment; accumulate the positions and attitudes at each moment in chronological order to form the motion trajectory of the mobile vehicle.

8. The method for determining the motion state of a mobile carrier oriented towards underground space according to claim 7, characterized in that, The evaluation of the current radar observation quality is based on the measurement uncertainty and the continuity judgment result of the motion trajectory; when the observation quality decreases, the weight of the mobile vehicle self-velocity estimate during fusion is reduced, and when the observation quality recovers, the weight of the mobile vehicle self-velocity estimate during fusion is restored, including: Based on the changes in position, velocity, and attitude at adjacent moments, determine whether the motion trajectory has any abnormal abrupt changes or discontinuities; The quality of radar observations is evaluated in the following ways: ； ； in, For radar observation quality factor, This is the normalized squared innovation statistic; This represents the number of valid points in the set of reliable radar observation points. For innovative covariance; This serves as a reference value for the observed geometric condition number; When the radar observation quality deteriorates, the measurement uncertainty is increased in the following way: ; For measurement uncertainty in degradation mode; When the radar observation quality is below a safety threshold, the mobile vehicle's self-velocity estimate is blocked from participating in the weighted fusion: ;in, This is a radar measurement activation indicator. This indicates that the radar measurements are blocked from participating in the weighted fusion. For a degree of freedom of 3 and a confidence level of β The card threshold, The minimum number of valid points; When the radar measurement is blocked, the state output is maintained only by inertial short-time propagation, and the covariance is increased to reflect the increase in uncertainty: ;in, This is additional process noise in inertial maintenance mode. For time intervals; When the radar observation quality is restored, the weight of the mobile vehicle's self-velocity estimate in the weighted fusion is gradually restored in the following manner: ; ;in, As a recovery factor, To preset the recovery time constant, This refers to the measurement uncertainty during the recovery phase.

9. A system for determining the motion state of a mobile carrier in underground space, comprising a memory, a processor, and a computer program stored in the memory, characterized in that, The processor executes the computer program to implement the steps of the method for determining the motion state of a mobile carrier in underground space as described in any one of claims 1-8.

Images