A mobile platform localization method based on millimeter-wave radar and inertial sensor fusion in a smoke environment

Abstract

This invention discloses a mobile platform localization method based on the fusion of millimeter-wave radar and inertial sensors in smoke-filled environments, belonging to the field of radar and inertial sensor fusion localization technology. This method acquires millimeter-wave radar point clouds and inertial sensor data, achieving time synchronization and coordinate unification. Based on the inertial sensor data, a predicted state is generated. Reliable radar points are selected by combining predicted velocity and radar point observations. Point-level Doppler radial velocity residuals are constructed for these reliable radar points, and the pose and velocity are updated using robustly weighted joint correction by radar and inertial sensors. This method reduces the impact of smoke on the degradation of visual and geometric observations, improving the continuity and robustness of mobile platform localization.

Description

Technical Field

[0001] This invention relates to the field of radar and inertial sensor fusion positioning technology, and in particular to a mobile platform positioning method that uses millimeter-wave radar point-level Doppler radial velocity and inertial sensor data for fusion positioning in a smoke environment. Background Technology

[0002] When mobile platforms perform navigation tasks in smoke-filled environments, they need to continuously estimate their position, attitude, and velocity. In the field of mobile platform localization, visual localization and laser localization typically rely on clear textures, edges, or stable geometry; inertial sensors output inertial sensor data such as angular velocity and acceleration, which can be used for high-frequency motion prediction, but will produce integral drift when used alone for state propagation; millimeter-wave radar can output point cloud positions and Doppler radial velocities under low visibility conditions, where the Doppler radial velocity represents the relative velocity component of a radar point along the radar's line of sight. Radar and inertial sensor fusion localization methods utilize radar geometry, Doppler velocity, and state propagation based on inertial sensor data for state estimation. Taking a fire inspection robot navigating in a smoke-filled floor as an example, cameras struggle to acquire stable textures, and laser radar echoes may be sparse or unstable due to smoke and obstructions, while millimeter-wave radar can still output point cloud positions and Doppler radial velocities, and inertial sensors can provide continuous short-term motion predictions. The radar and inertial sensor odometry framework demonstrates that point-level Doppler constraints can directly act on velocity and attitude-related states, and that preserving point-level line-of-sight constraints retains more observation information than compressing the entire Doppler frame into a single velocity prior.

[0003] In existing technologies, visual observations in smoke environments lose stable textures due to low visibility, and some geometric observations become unstable due to sparse echoes, low confidence points, or inconsistent motion points. If the mobile platform relies solely on inertial sensors, velocity and position errors accumulate over time. If radar Doppler information is compressed into a full-frame velocity prior, the line-of-sight constraints between different radar points are weakened, making it difficult to adequately suppress positioning drift in smoke environments. Therefore, radar-inertial fusion positioning in smoke scenarios still requires a positioning scheme centered on reliable radar point selection, point-level Doppler radial velocity constraints, and joint correction of predicted states based on inertial sensor data. Summary of the Invention

[0004] This invention addresses the problems in existing technologies by providing a mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in smoke environments. This method acquires millimeter-wave radar point cloud data and inertial sensor data, achieving time synchronization, coordinate unification, state prediction based on inertial sensor data, reliable radar point selection combined with predicted velocity, construction of point-level Doppler radial velocity residuals, and joint calibration of radar and inertial sensors. By suppressing inertial sensor integral drift through point-level Doppler constraints, it achieves the technical effect of improving the continuity and robustness of positioning in smoke environments.

[0005] The specific solution of the present invention includes the following steps:

[0006] First, the system acquires millimeter-wave radar point cloud data and inertial sensor data in a smoke-filled environment. The radar point cloud data provides spatial position, radial Doppler velocity, and echo confidence, while the inertial sensor data provides angular velocity and acceleration. Second, the system performs synchronization based on timestamps and achieves coordinate unification using rotational and lever extrinsic parameters between the radar and inertial sensors. Third, the system uses the inertial sensor data for state prediction, obtaining predicted position, predicted attitude, and predicted velocity. Then, the system filters reliable radar points based on echo confidence, ranging distance, and the consistency of radial velocity obtained from the predicted velocity to reduce the impact of low-confidence echoes and inconsistent motion points on fusion correction. Subsequently, the system establishes point-level Doppler radial velocity residuals for each reliable radar point, ensuring that the line-of-sight direction of each radar point remains an independent constraint. Finally, the system performs joint radar and inertial sensor correction on the predicted state obtained from the inertial sensor data based on robustly weighted error state updates and outputs the positioning result.

[0007] In point-level Doppler constraints, the predicted radial velocity of the i-th radar point is expressed as:

[0008] (1)

[0009] The corresponding residual is expressed as:

[0010] (2)

[0011] In formulas (1) and (2), Indicates the number is The measured radial Doppler velocity of the radar point, Indicates the number is The predicted radial velocity of the radar point, Indicates the number is The line-of-sight unit vector of the radar point in the inertial sensor coordinate system. This represents the predicted velocity in the inertial sensor coordinate system. This indicates that the gyroscope measures angular velocity. This indicates that the gyroscope has zero bias. The superscript indicates the lever arm that reaches the center of the inertial sensor. The symbol × represents the vector transpose. The above formula is used to directly convert the line-of-sight direction and Doppler velocity of each radar point into velocity residuals that can participate in state correction.

[0012] The set of reliable radar points is represented as:

[0013] (3)

[0014] In formula (3), Represents a set of reliable radar points; Indicates the radar point number; Indicates the number is The confidence level of the radar point echo; Indicates the confidence threshold; Indicates the number is The position vector of the radar point in the radar coordinate system or unified coordinate system; This indicates the ranging distance of the radar point; Indicates the near-field distance threshold; Indicates the number is Measured radial Doppler velocity at radar points; Indicates the number is The predicted radial velocity of the radar point; This represents the radial velocity consistency threshold. In one implementation, it is the normalized confidence threshold. The near-field distance threshold is set between 0.3 and 0.8. Radial velocity consistency threshold: 0.2m to 1.0m. The range is 0.2 m / s to 1.0 m / s; when the confidence score of the radar output is a non-normalized score, it is mapped to the corresponding threshold range according to the radar manufacturer's definition. This formula is used to limit the radar points that enter the joint calibration of radar and inertial sensor.

[0015] Robust weights are represented as:

[0016] (4)

[0017] In formula (4), Indicates the number is The residual weights of the radar points; Indicates the number is The point-level Doppler radial velocity residual of the radar point; This represents the robust scaling parameter. In one implementation, the robust scaling parameter... The speed range is from 0.2 m / s to 1.5 m / s; in another embodiment, The weighting is determined based on the median absolute deviation of the residuals of reliable points in the current radar frame, and upper and lower limits are set in conjunction with the radar noise level. This formula assigns smaller weights to radar points with larger absolute residual values, thereby reducing the impact of inconsistent moving radar points and anomalous echoes on the fusion positioning results.

[0018] Explanation of technical effects: This invention utilizes the relatively stable motion observation capability of millimeter-wave radar Doppler radial velocity in a smoke environment to provide external velocity constraints for inertial sensor data integration prediction; it preserves the constraint differences of different radar point line-of-sight directions through point-level residuals, avoiding information loss caused by whole-frame velocity priors; and it reduces the influence of low-confidence echoes and motion inconsistencies by reliable point screening and robust weighting, thereby improving the positioning continuity and robustness of the mobile platform in a smoke environment. Attached Figure Description

[0019] Figure 1 A flowchart illustrating a mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment, provided as an embodiment of the present invention;

[0020] Figure 2 A schematic diagram of the structure and module interaction of a mobile platform positioning system based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment, provided for an embodiment of the present invention;

[0021] Figure 3 This is a schematic diagram of the positioning data stream fused from millimeter-wave radar and inertial sensors, provided in an embodiment of the present invention. Detailed Implementation

[0022] Reference Figure 1 This embodiment provides a mobile platform localization method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment. The mobile platform is equipped with millimeter-wave radar and inertial sensors to perform navigation tasks in a smoke environment. The core of this method is the joint correction of millimeter-wave radar point-level Doppler radial velocity constraints, reliable radar point selection, and inertial sensor predicted state.

[0023] In step S101, the mobile platform acquires millimeter-wave radar point cloud data and inertial sensor data in the smoke environment. This step involves radar point cloud, angular velocity, and acceleration observations; the problem addressed is obtaining sensor data usable for motion estimation even when visual or laser observations degrade. Specifically, it involves reading the spatial position of radar points, radial Doppler velocity, echo confidence, and timestamp, and simultaneously recording the angular velocity, acceleration, and timestamp output by the inertial sensor. The goal is to provide radar motion observations and inertial sensor predictive observations for subsequent fusion of millimeter-wave radar and inertial sensors.

[0024] In step S102, the system performs time synchronization and coordinate unification. This step involves radar frames and inertial sensor measurement sequences; it addresses the inconsistency in residual construction caused by different time references and coordinate systems of different sensors. Specifically, it determines the inertial sensor measurement corresponding to the current radar frame based on the timestamp, and transforms the radar point line-of-sight unit vector to the inertial sensor coordinate system according to the pre-calibrated external parameters of the radar-to-inertial sensor coordinate system rotation, while retaining the external parameters of the lever arm from the radar to the center of the inertial sensor. The effect is that the radar Doppler observation and the predicted state obtained based on inertial sensor data can be fused in a unified coordinate system.

[0025] In one implementation, the external parameter calibration of the radar to the inertial sensor coordinate system includes: after the mobile platform is installed, firstly, the initial value of the lever arm is measured according to the sensor installation position, and the initial value of the rotational external parameter is obtained according to the installation attitude; then, multi-frame millimeter-wave radar point cloud data containing linear motion and turning motion and inertial sensor data are collected, and calibration residuals are constructed using the radar point line-of-sight direction, Doppler radial velocity and inertial sensor predicted velocity after time synchronization; finally, the rotational external parameter and lever arm external parameter are optimized with the goal of minimizing the calibration residual, and the optimized external parameters are written into the mobile platform's memory for use in step S102 and subsequent Doppler radial velocity residual calculation.

[0026] In step S103, the system performs state prediction based on inertial sensor data. This step involves the state of the moving platform at the previous moment and the current inertial sensor measurements; the problem it addresses is the need for continuous high-frequency motion prediction between radar frames; specifically, it updates the attitude using angular velocity, updates the velocity and position using acceleration, and propagates error states including position, attitude, velocity, gyroscope bias, accelerometer bias, and gravity vector; the result is the acquisition of predicted position, predicted attitude, and predicted velocity for reliable radar point selection, Doppler residual linearization, and joint correction.

[0027] State prediction based on inertial sensor data can be expressed as:

[0028] (5)

[0029] (6)

[0030] (7)

[0031] In formulas (5) to (7), , and They represent the first Predicted attitude, predicted velocity, and predicted position at any given moment; , and These represent the corrected attitude, velocity, and position at the previous moment, respectively. This represents an exponential mapping that maps angular velocity increments to rotational increments; This indicates that the gyroscope measures angular velocity; This indicates that the accelerometer measures acceleration; This indicates that the gyroscope has zero bias. This indicates that the accelerometer has zero bias. Represents the gravity vector; This indicates the time interval between measurements from adjacent inertial sensors.

[0032] In step S104, the system filters reliable radar points to form a reliable radar point set. This step targets each radar point in the current radar frame; it addresses the problem of low-confidence echoes, near-field instability, and inconsistent motion points in a smoke environment. Specifically, radar points with echo confidence below a confidence threshold and radar points with ranging distances less than a near-field threshold are removed. The predicted radial velocity is calculated using the predicted velocity obtained in step S103 and the radar point's line-of-sight direction. Consistency filtering is then performed based on the difference between the predicted radial velocity and the radial Doppler velocity. The goal is to reduce the impact of abnormal radar points on subsequent state corrections.

[0033] In step S105, the system establishes point-level Doppler radial velocity residuals for each radar point in the reliable radar point set. This step deals with the radial Doppler velocity and line-of-sight unit vector of the reliable radar points; the problem it addresses is that the line-of-sight direction constraints of different radar points are lost in the overall frame velocity prior. Specifically, it is based on the numbered... Measured radial Doppler velocity at radar points Line-of-sight unit vector Prediction speed Gyroscope for measuring angular velocity gyroscope zero bias and lever arm Calculate the point-level Doppler radial velocity residual The desired effect is that each radar point participates in the fusion process as an independent radial velocity constraint.

[0034] The formula for calculating point-level Doppler residuals is:

[0035] (8)

[0036] In formula (8), Indicates the number is The point-level Doppler radial velocity residual of the radar point Indicates the number is The measured radial Doppler velocity of the radar point, Indicates the number is The line-of-sight unit vector of the radar point in the inertial sensor coordinate system. This represents the predicted velocity in the inertial sensor coordinate system. This indicates that the gyroscope measures angular velocity. This indicates that the gyroscope has zero bias. The superscript indicates the lever arm that reaches the center of the inertial sensor. The symbol × represents the cross product of vectors. In this formula... This represents the velocity term at the radar installation location, which is formed by the combined translational velocity and the rotational velocity of the boom arm.

[0037] In step S106, the system performs joint calibration of the radar and inertial sensor. This step addresses the inertial sensor's predicted state and multiple point-level Doppler radial velocity residuals. The problem it solves is that the inertial sensor data integration propagation drifts over time, and inconsistent observations may still exist at radar points. Specifically, robust weights are determined based on the magnitude of each residual, applied to the corresponding residuals, and an error state update equation is constructed based on the weighted residuals to correct the predicted position, attitude, and velocity. The effect is to suppress inertial sensor integration drift using radar Doppler velocity constraints and reduce the impact of abnormal observations on the positioning results.

[0038] The error state equation for joint correction by radar and inertial sensors can be expressed as:

[0039] (9)

[0040] In formula (9), The Jacobian matrix representing the point-level Doppler residual with respect to the error state; Indicates the robust weights The diagonal weight matrix is ​​formed; The prior covariance matrix representing the predicted state; This represents the error state increment to be solved; This represents the residual vector composed of multiple point-level Doppler radial velocity residuals; superscript This represents the matrix transpose. The result is... Then, the system injects it into the predicted state to obtain the updated pose and velocity.

[0041] In the motion inconsistency marking process, the system compares the point-level Doppler radial velocity residual after joint correction by the radar and inertial sensor with a motion consistency threshold; when the absolute value of the residual is greater than the motion consistency threshold, a motion inconsistency mark is generated for the corresponding radar point. The motion consistency threshold can be the radial velocity consistency threshold in formula (3). Alternatively, it can be based on the robust scaling parameter in formula (4). Set as to Radar points with motion inconsistency markers have their weight reduced in the next radar frame and in a preset number of subsequent radar frames. The preset number can be set to 1 to 5 frames based on the mobile platform speed and radar frame rate, thereby avoiding the continuous impact of short-term anomalies on fusion correction.

[0042] In step S107, the system outputs the updated pose and velocity of the mobile platform. This step deals with the state after joint correction by the radar and inertial sensors; the problem it solves is that the navigation control unit needs continuous positioning input in a smoke environment; specifically, the updated pose and velocity are provided to the navigation control unit of the mobile platform, and the above process continues when the next radar frame arrives; the effect achieved is to form a continuous positioning output for smoke environments.

[0043] Reference Figure 2 The system includes a radar data acquisition module, an inertial sensor data acquisition module, a synchronization calibration module, an inertial sensor prediction module, a reliable point construction module, a fusion correction module, and a positioning output module. The radar data acquisition module and the inertial sensor data acquisition module output radar point cloud data and inertial sensor data to the synchronization calibration module, respectively. The synchronization calibration module outputs time synchronization results, line-of-sight unit vectors in a unified coordinate system, rotational extrinsic parameters, and lever extrinsic parameters to the inertial sensor prediction module, reliable point construction module, and fusion correction module. The inertial sensor prediction module outputs predicted position, predicted attitude, and predicted velocity to the reliable point construction module and the fusion correction module. The reliable point construction module performs radial velocity consistency screening based on the predicted velocity and outputs a set of reliable radar points to the fusion correction module. The fusion correction module outputs the updated pose and velocity of the mobile platform to the positioning output module. (Refer to...) Figure 3 The data stream starts from smoke radar points and inertial sensor data, and goes through time synchronization, coordinate unification, inertial prediction, reliable radar point selection, Doppler residual construction and fusion correction, and finally outputs the pose and velocity of the mobile platform.

Claims

1. A method for locating a mobile platform in a smoke-filled environment based on the fusion of millimeter-wave radar and inertial sensors, characterized in that, This includes the following steps performed by the mobile platform's processor: S101. Collect millimeter-wave radar point cloud data and inertial sensor data in a smoke environment. The millimeter-wave radar point cloud data includes the spatial position of the radar point, radial Doppler velocity, echo confidence, and timestamp. The inertial sensor data includes angular velocity, acceleration, and timestamp. S102. Synchronize the millimeter-wave radar point cloud data and the inertial sensor data in time, and convert the radar point line of sight to the inertial sensor coordinate system according to the external parameters between the radar coordinate system and the inertial sensor coordinate system. S103. Predict the state of the mobile platform based on the inertial sensor data to generate predicted position, predicted attitude and predicted speed. S104. Based on the echo confidence, ranging distance, and the consistency of the radial velocity determined based on the predicted velocity, reliable radar points are selected from the millimeter-wave radar point cloud data to form a reliable radar point set. S105. Establish point-level Doppler radial velocity residuals for each radar point in the set of reliable radar points; S106. Based on the multiple point-level Doppler radial velocity residuals, the predicted state obtained based on inertial sensor data is jointly corrected by radar and inertial sensors to generate an updated pose and velocity of the mobile platform. S107. Output the updated pose and velocity of the mobile platform as the positioning result in the smoke environment.

2. The mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment according to claim 1, characterized in that, Step S102 includes: determining the inertial sensor measurement sequence corresponding to the current radar frame based on the timestamp; performing coordinate transformation on the radar point line-of-sight unit vector according to the pre-calibrated external parameters of the radar to the inertial sensor coordinate system rotation; and participating in the subsequent Doppler radial velocity residual calculation according to the pre-calibrated external parameters of the lever arm at the center of the radar to the inertial sensor.

3. The mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment according to claim 1, characterized in that, Step S104 includes: removing radar points whose echo confidence is lower than the confidence threshold; removing radar points whose ranging distance is less than the near-field threshold; calculating the predicted radial velocity based on the predicted velocity generated in step S103 and the radar point's line-of-sight direction; and removing radar points whose difference between the predicted radial velocity and the radial Doppler velocity exceeds the consistency threshold.

4. The mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment according to claim 1, characterized in that, Step S105 involves establishing a point-level Doppler radial velocity residual for each radar point in the reliable radar point set, including: for the... Radial Doppler velocity was obtained from a reliable radar point. and the line-of-sight unit vector in the inertial sensor coordinate system Based on the predicted speed Gyroscope for measuring angular velocity gyroscope zero bias And the lever arm that reaches the center of the inertial sensor. Calculate the predicted radial velocity; and generate the first... Point-level Doppler radial velocity residuals of reliable radar points : (1) in, Indicates the first Point-level Doppler radial velocity residuals of a reliable radar point Indicates the first Radial Doppler velocity of a reliable radar point Indicates the first The line-of-sight unit vector of a reliable radar point in the inertial sensor coordinate system. This represents the predicted velocity in the inertial sensor coordinate system. This indicates that the gyroscope measures angular velocity. This indicates that the gyroscope has zero bias. The superscript indicates the lever arm that reaches the center of the inertial sensor. Represents vector transpose, symbol This represents the cross product of vectors.

5. The mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment according to claim 1, characterized in that, In step S106, the predicted state obtained based on inertial sensor data is jointly corrected by radar and inertial sensors according to multiple point-level Doppler radial velocity residuals, including: determining robust weights according to the magnitude of each point-level Doppler radial velocity residual; applying the robust weights to the corresponding point-level Doppler radial velocity residuals; constructing an error state update equation based on the weighted point-level Doppler radial velocity residuals; and using the error state update equation to correct the predicted position, predicted attitude, and predicted velocity.

6. The mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment according to claim 5, characterized in that, The state variables of the error state update equation include the position, attitude, velocity, gyroscope zero bias, accelerometer zero bias, and gravity vector of the moving platform; the extrinsic parameters between the radar coordinate system and the inertial sensor coordinate system are used as fixed calibrated quantities in the calculation of point-level Doppler radial velocity residuals.

7. The mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment according to claim 1, characterized in that, Also includes: When the absolute value of the point-level Doppler radial velocity residual after joint correction by radar and inertial sensor is greater than the motion consistency threshold, a motion inconsistency marker is generated; the weight of radar points with the motion inconsistency marker is reduced in the next radar frame and a preset number of subsequent radar frames; and joint correction by radar and inertial sensor is continued based on the reduced set of reliable radar points.

8. A system for implementing the mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment as described in claim 1, characterized in that, include: The radar data acquisition module is configured to acquire millimeter-wave radar point cloud data in a smoke environment; An inertial sensor data acquisition module is configured to acquire inertial sensor data. The synchronous calibration module is configured to perform time synchronization and coordinate unification, and outputs the radar point line-of-sight unit vector, rotation extrinsic parameters, and lever arm extrinsic parameters in a unified coordinate system. An inertial sensor prediction module is configured to generate a prediction state including predicted position, predicted attitude and predicted velocity based on inertial sensor data, and output the predicted velocity to a reliable point construction module. A reliable point construction module is configured to receive the predicted velocity and form a reliable radar point set based on echo confidence, ranging distance, and radial velocity consistency. The fusion correction module is configured to receive the reliable radar point set, the predicted state, the rotational extrinsic parameters, and the lever arm extrinsic parameters, establish a point-level Doppler radial velocity residual, and perform joint correction of the radar and inertial sensor. The positioning output module is configured to output the updated pose and velocity of the mobile platform.

9. A mobile platform, characterized in that, The system includes a millimeter-wave radar, an inertial sensor, a processor, and a memory. The memory stores a computer program, and when the processor executes the computer program, it implements the mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensor in a smoke environment as described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the mobile platform positioning method based on the fusion of millimeter-wave radar and inertial sensors in a smoke environment as described in any one of claims 1 to 7.

Images