High-precision navigation positioning method and system in dense smoke environment based on millimeter wave radar

Abstract

The invention discloses a millimeter-wave radar-based high-precision navigation positioning method and system in a dense smoke environment. The method comprises the following steps: S1, respectively acquiring data of a millimeter-wave radar, a laser radar, an inertial measurement unit and a global navigation satellite system; s2, a data processing step; s3, respectively constructing a self-speed residual factor, a first surface feature factor and a map matching factor; constructing a second surface feature factor; constructing a pre-integration factor; respectively constructing a pseudo-range measurement residual factor and a clock residual factor; and S4, integrating each factor into a global optimization framework to carry out optimization solution. According to the high-precision navigation and positioning method and system in the dense smoke environment based on the millimeter-wave radar, under the condition of GNSS signal rejection, the system is automatically degraded into a data positioning and navigation mode based on fusion of the 4D millimeter-wave radar and the laser inertial odometer, and reliable navigation and positioning can still be realized in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the field of satellite radar positioning technology, and in particular relates to a high-precision navigation and positioning method and system in a dense smoke environment based on millimeter wave radar. Background Art

[0002] Existing satellite positioning systems generally utilize a combination of GNSS satellites and inertial measurement units (IMUs). Leveraging the unique characteristics of GNSS, these systems provide direct global position information, effectively correcting for the accumulated errors in the IMU over time, thereby improving overall positioning accuracy. However, in real-world applications, GNSS signals are susceptible to obstruction, shielding, or interference, making it difficult for receivers to consistently receive stable GNSS information. This can lead to positioning interruptions or significant drift. For example, in complex environments such as dense smoke, GNSS signals often face denial, meaning that received signals are interfered with or obstructed, preventing the device from acquiring valid position information. Summary of the Invention

[0003] In order to solve the problems in the prior art, the present invention aims to provide a high-precision navigation and positioning method and system in a dense smoke environment based on millimeter-wave radar, which adopts a fusion solution of 4D millimeter-wave radar, laser inertial odometry, inertial measurement unit and GNSS. In the case of GNSS signal denial, the system will automatically degenerate into a data positioning and navigation mode based on the fusion of 4D millimeter-wave radar and laser inertial odometry, ensuring reliable navigation and positioning in complex environments.

[0004] In order to achieve the above object, the present invention provides a high-precision navigation and positioning method in a dense smoke environment based on millimeter wave radar, comprising the steps of:

[0005] S1: Acquire data from millimeter wave radar, lidar, inertial measurement unit, and global navigation satellite system respectively;

[0006] S2: performing target screening and noise reduction on the millimeter wave radar data to obtain first data; performing distortion compensation on the laser radar data to obtain second data; performing integration processing on the inertial measurement unit data to obtain third data; and analyzing the global navigation satellite system data to obtain fourth data by satellite status and elevation angle screening.

[0007] S3: constructing the self-velocity residual factor, the first surface characteristic factor, and the map matching factor of the first data respectively; constructing the second surface characteristic factor of the second data; constructing the pre-integration factor of the third data; and constructing the pseudorange measurement residual factor and the clock residual factor of the fourth data respectively;

[0008] S4: Integrate the self-velocity residual factor, the first surface characteristic factor, the map matching factor, the second surface characteristic factor, the pre-integration factor, the pseudorange measurement residual factor, and the clock residual factor into a global optimization framework for optimization and solution.

[0009] As an embodiment, in the event of signal denial from the global navigation satellite system, automatically degenerating into a data positioning and navigation mode based on the cooperation of the millimeter wave radar and the lidar;

[0010] In the data positioning and navigation mode based on the cooperation between the millimeter wave radar and the laser radar:

[0011] In the step S4, the self-velocity residual factor, the first surface characteristic factor, the map matching factor and the second surface characteristic factor are integrated into a global optimization framework for optimization and solution.

[0012] As an embodiment, the step S1 further includes the following steps:

[0013] The reference frame of the inertial measurement unit is used as the body coordinate system of the system, which is recorded as b system;

[0014] The reference system fixed to the earth, with its origin coinciding with the b system at the initial moment and its z direction pointing to the center of mass of the earth, is taken as the local world system, denoted as the w system;

[0015] The ECEF reference system defined in the global navigation satellite system, whose origin is located at the center of mass of the earth and is stationary relative to the earth, is denoted as the e-system;

[0016] The ECI reference system with its origin at the Earth's mass center and at rest relative to the stars is denoted as the E system;

[0017] The coordinate system whose origin coincides with the W system and whose three axes point to the east, north and sky respectively and are in the opposite direction of the center of mass of the earth is called the N system.

[0018] As an implementation method, the self-velocity residual factor is:

[0019]

[0020] Among them, v r Indicates the speed in the radar coordinate system; Indicates the speed of the radar itself;

[0021] The first surface characteristic factor:

[0022]

[0023] Where n represents the first plane unit normal vector; represents the spatial coordinates of the millimeter-wave radar target point; d represents the distance of the plane;

[0024] The map matching factor:

[0025]

[0026]

[0027] Where G represents the weight matrix, N represents the total number of coordinate points in the local world system, represents the coordinates of the kth point in the local world system; represents the coordinates of the jth effective millimeter-wave radar target point in the local world system; C k C represents the distribution variance of the N nearest points around the coordinates of the kth point in the local world system; j Represents the distribution variance of the N points closest to the jth effective millimeter-wave radar target point in the local world system.

[0028] As an embodiment, in the step S3, the surface feature of the second data is extracted using the LOAM method to obtain the second surface feature factor:

[0029]

[0030] in, represents the second plane unit normal vector; represents the spatial coordinates of the laser radar target point; d represents the distance of the plane.

[0031] As an implementation method, the pseudorange measurement residual factor is:

[0032]

[0033] Among them, ω E is the Earth's rotational angular velocity, t f is the satellite signal transmission time, R z (-ω E t f ) is the rotation of the Earth during the satellite signal transmission process, is the actual position of the satellite in the ECEF reference frame when the receiver receives the satellite signal. represents the position of the receiver, c represents the speed of light, represents the selection vector, δt is the receiver clock error, is the satellite clock error, and represent the delay errors caused by the troposphere and the current layer respectively;

[0034] The clock residual factor:

[0035]

[0036] Among them, δt k and δt k-1 are the receiver clock errors at time k and time k-1 respectively, is the rate of change of the receiver clock error, and Δt is the time difference between the kth moment and the k-1th moment.

[0037] As an implementation method, in step S4, a fixed-size sliding window state estimation method is adopted to integrate the residual factors into a global optimization framework; at the same time, the maximum a posteriori probability estimation method is used to estimate the optimal value of the system state quantity.

[0038] As an implementation method, in step S4, the optimal value of the system state quantity χ is estimated by formula (8):

[0039]

[0040] Among them, ||r p -H p X|| is the marginal term of the optimization problem, which represents the influence of the state at the previous time point on the state at the current time period. p Related to the residual, H p is the Hessian matrix; Represents observation data The i-th item of , f represents the total number of observations, and r represents the residual term.

[0041] A high-precision navigation and positioning system in a dense smoke environment based on a millimeter wave radar for implementing the high-precision navigation and positioning method in a dense smoke environment based on a millimeter wave radar of the present invention comprises:

[0042] A data acquisition module is used to acquire data from millimeter wave radar, lidar, inertial measurement unit and global navigation satellite system respectively;

[0043] a data preprocessing module configured to perform target screening and noise reduction on millimeter-wave radar data to obtain first data; perform distortion compensation on lidar data to obtain second data; perform integration processing on the inertial measurement unit data to obtain third data; and perform satellite status and elevation angle screening on global navigation satellite system data to obtain fourth data;

[0044] a residual factor construction module for respectively constructing the self-velocity residual factor, the first surface characteristic factor, and the map matching factor of the first data; constructing the second surface characteristic factor of the second data; constructing the pre-integration factor of the third data; and respectively constructing the pseudorange measurement residual factor and the clock residual factor of the fourth data; and

[0045] An optimization solution module is used to integrate the self-velocity residual factor, the first surface characteristic factor, the map matching factor, the second surface characteristic factor, the pre-integration factor, the pseudorange measurement residual factor and the clock residual factor into a global optimization framework for optimization solution.

[0046] As an implementation method, it further includes:

[0047] a mode selection module configured to automatically degenerate to a data positioning and navigation mode based on the cooperation of the millimeter-wave radar and the lidar in the event of signal denial from the global navigation satellite system; and

[0048] The coordinate system setting module is used to use the reference system of the inertial measurement unit as the body coordinate system of the system, which is recorded as the b system; a reference system fixed to the earth, with its origin coinciding with the b system at the initial moment and its z direction pointing to the center of mass of the earth as the local world system, which is recorded as the w system; the ECEF reference system defined in the global navigation satellite system, whose origin is located at the center of mass of the earth and is stationary relative to the earth, is recorded as the e system; the ECI reference system, whose origin is located at the center of mass of the earth and is stationary relative to the stars, is recorded as the E system; and a coordinate system whose origin coincides with the w system, whose three axes point to the east, north and sky respectively, and are in the opposite direction of the center of mass of the earth, is called the n system.

[0049] The present invention adopts the above technical solution, so it has the following beneficial effects:

[0050] Multi-sensor fusion technology, based on millimeter-wave radar, laser inertial odometry, inertial measurement units, and global navigation satellite systems, meets the needs of high-precision positioning and navigation in complex environments, particularly in extreme conditions such as GNSS signal denial and low visibility. By integrating data from multiple sensors, the system achieves high-precision positioning and navigation even in dense smoke. Its robust anti-interference performance and high reliability make it widely applicable in firefighting, emergency rescue, and other fields, helping to ensure the smooth execution of critical missions. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0052] Figure 1 This is a flowchart of a high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to an embodiment of the present application;

[0053] Figure 2 This is a schematic diagram of the principle of a high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar in an embodiment of the present application. DETAILED DESCRIPTION

[0054] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0055] In the description of the present invention, it should be noted that the terms "upper," "lower," "left," "right," "center," "vertical," "horizontal," "inner," and "outer," etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings and are intended solely to facilitate and simplify the description of the present invention. They are not intended to indicate or imply that the devices or components referred to must have, be constructed, or operate in a specific orientation, and therefore should not be construed as limiting the present invention. Furthermore, the terms "first," "second," and "third," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0056] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "installed," "installed," "connected," and "connected" should be understood in a broad sense. For example, "connected" can mean a fixed connection, a detachable connection, or an integral connection; a mechanical connection or an electrical connection; a direct connection, an indirect connection through an intermediate medium, or internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on the specific circumstances.

[0057] See also Figure 1 and Figure 2 , a high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar in an embodiment of the present invention comprises the following steps:

[0058] The reference frame of the inertial measurement unit is used as the body coordinate system of the system, which is recorded as b system;

[0059] The reference system fixed to the earth, with its origin coinciding with the b system at the initial moment and its z direction pointing to the center of mass of the earth, is taken as the local world system, denoted as the w system;

[0060] The ECEF reference system defined in the global navigation satellite system, whose origin is located at the center of mass of the earth and is stationary relative to the earth, is denoted as the e-system;

[0061] The ECI reference system with its origin at the Earth's mass center and at rest relative to the stars is denoted as the E system;

[0062] The coordinate system whose origin coincides with the W system and whose three axes point to the east, north and sky respectively and are in the opposite direction of the center of mass of the earth is called the N system.

[0063] S1: Acquire data from millimeter wave radar, lidar, inertial measurement unit, and global navigation satellite system respectively;

[0064] S2: Performing target screening and noise reduction on the millimeter-wave radar data to obtain the first data. This target point screening and noise reduction process removes irrelevant or erroneous echoes to improve data quality and increase the signal-to-noise ratio, enabling more accurate target identification and tracking.

[0065] Distortion compensation is performed on the lidar data to obtain the second data. Distortion compensation can reduce the impact of smoke environment factors on data quality and correct the geometric distortion that may be introduced by the optical system.

[0066] Integrating the data of the inertial measurement unit to obtain third data, and using a pre-integration factor method to efficiently process the IMU data;

[0067] The fourth data is obtained by analyzing the satellite status and elevation angle of the global navigation satellite system data. The best satellite combination is selected by analyzing the satellite status and elevation angle to improve the positioning accuracy and reliability of the results. The flight time of the electromagnetic wave from the satellite to the receiver is calculated by the offset of the pseudo-random noise (PRN) signal of the receiver, thereby inferring the distance;

[0068] S3: constructing the self-velocity residual factor, the first surface characteristic factor, and the map matching factor of the first data respectively; constructing the second surface characteristic factor of the second data; constructing the pre-integration factor of the third data; and constructing the pseudorange measurement residual factor and the clock residual factor of the fourth data respectively;

[0069] (1) The self-velocity residual factor, surface feature factor, and map matching factor play a role in integrating different data sources and different features in the interaction process to improve the overall performance of the system. The self-velocity residual factor, the first surface feature factor, and the map matching factor are constructed as follows:

[0070] The self-velocity residual factor helps correct the motion state of the system by comparing the actual measured speed with the predicted speed to reduce the deviation caused by measurement error. In the SLAM setting, the target point in the environment is stationary while the radar itself moves, then the target point i has a consistent speed in the radar coordinate system. For any point v in an environment that is stationary relative to the world coordinate system w =0, using millimeter wave radar external parameters Its velocity v in the radar coordinate system can be calculated r , the self-velocity residual factor:

[0071]

[0072] Among them, v r Indicates the speed in the radar coordinate system; Indicates the speed of the radar itself;

[0073] The first surface feature factor ensures that the system understands the environment accurately. By comparing the measured surface features with the estimated surface features, the robot's position and posture are adjusted to improve positioning accuracy. When there are enough laser radar points around the millimeter wave radar, the system uses the LOAM (Lidar Odometry and Mapping) method to generate a global laser point cloud map. When extracting a plane from a point cloud, the target point information provided by the millimeter-wave radar can assist in determining the plane features. When the target point detected by the millimeter-wave radar matches the plane features in the laser point cloud map, the surface feature residual factor is constructed. Select the target point with millimeter wave radar Several nearby laser points are used to determine whether they form a plane. If so, the plane parameters are recorded as [n, d], where n is the plane unit normal vector, d represents the distance of the plane, and p represents the spatial coordinate. For any point belonging to the plane, the equation n·p+d=0 holds. Thus, the first surface characteristic factor is constructed:

[0074]

[0075] Where n represents the first plane unit normal vector; represents the spatial coordinates of the millimeter-wave radar target point; d represents the distance of the plane;

[0076] The map matching factor helps the system determine whether the current location information is consistent with the information on the map, thereby correcting the position estimate and ensuring the accuracy of navigation. Under conditions of thick smoke and fog, the lidar will be severely scattered, resulting in a shorter effective measurement distance of the lidar. The millimeter-wave radar is less affected by the scattering effect, and the effective measurement distance attenuation is relatively unobvious. Therefore, in a thick smoke and foggy environment, it is very likely that the millimeter-wave radar has measured a valid target point, but because it is far away from the device, no valid laser point may be found around it. In order to solve the above problems, the system maintains a millimeter-wave radar point cloud map For effective millimeter-wave radar target points Calculate its coordinates in the world system On the map Find the N points closest to the effective millimeter-wave radar target point, and construct the map matching factor based on the relationship between these adjacent points and the target point:

[0077]

[0078] The weight matrix G is determined by the coordinate covariance of the relevant points:

[0079]

[0080] Where G represents the weight matrix, N represents the total number of coordinate points in the local world system, represents the coordinates of the kth point in the local world system; represents the coordinates of the jth effective millimeter-wave radar target point in the local world system; C k C represents the distribution variance of the N nearest points around the coordinates of the kth point in the local world system; j Represents the distribution variance of the N points closest to the jth effective millimeter-wave radar target point in the local world system.

[0081] (2) The construction method of the second surface characteristic factor is as follows:

[0082] In the step S3, the surface feature of the second data is extracted using the LOAM method to construct the second surface feature factor:

[0083]

[0084] in, represents the second plane unit normal vector; represents the spatial coordinates of the laser radar target point; d represents the distance of the plane.

[0085] The LiDAR data will first undergo distortion compensation to reduce the impact of smoke-induced environmental factors on data quality and correct for geometric distortion that may be introduced by the optical system. Based on distortion compensation, the system uses the LOAM method to extract the LiDAR's surface features to construct residual factors. To maintain system stability and reduce the amount of computation, the system only includes frames whose pose changes or time intervals exceed a certain threshold into the sliding window. These frames are called key frames, and the remaining frames are aggregated into a temporary map. For a point in the temporary map, the system converts its coordinates to the corresponding reference system based on the pose estimate obtained by IMU integration, the pose estimate of the frame, and external parameters. In addition, other points close to the point are searched in the temporary map to determine whether these points form a plane. If so, the system records the parameters of the plane and uses the relationship between the points on the plane to construct a second surface feature factor.

[0086] (3) The pre-integration factor is constructed as follows:

[0087] The measurement frequency of the Inertial Measurement Unit (IMU) is much higher than the frame rate of other devices. This system uses a pre-integration factor method to efficiently process IMU data. The system uses the integral quantity defined in the time interval between two moments to describe the change in the system posture. Therefore, the inertial residual factor is defined as:

[0088]

[0089] (4) The pseudorange measurement residual factor and clock residual factor are constructed as follows:

[0090] GNSS data will first analyze the satellite status and elevation angle to select the best satellite combination to improve positioning accuracy and reliability of the results. The flight time of the electromagnetic wave from the satellite to the receiver is calculated by the offset of the receiver's pseudo-random noise (PRN) signal, thereby inferring the distance. However, there is an error between the pseudo-range and the true distance. The relationship can be approximately described by the following formula:

[0091]

[0092] in is the selection vector, Δt s represents the satellite clock error, T s,r Represents the delay of electromagnetic wave propagation in the atmospheric troposphere, I s,r The delay of the ionosphere on the propagation of electromagnetic waves, ∈ s,rRepresents measurement noise. In addition, satellite signals may be delayed due to reflections from ground obstacles, which is the multipath effect. In order to reduce this effect, this system uses laser point cloud and millimeter wave radar to build a map to eliminate signals affected by obstacles. In this system, the coordinates of the GNSS receiver are obtained in combination with external parameters. The system assumes that the w coordinate system coincides with the ENU reference system, i.e., the n system, and the transformation between them can be represented by a rotation matrix. The parameters of the coordinate conversion from the ENU, i.e., the n system, to the ECEF reference system, i.e., the e system, are calculated using latitude, longitude, and altitude. Due to the rotation of the earth, the ECEF reference system when the satellite transmits the signal does not coincide with the receiver coordinate system, and the relationship between them can be represented by a rotation matrix. Therefore, the pseudorange measurement residual factor can be expressed as:

[0093]

[0094] Among them, ω E is the Earth's rotational angular velocity, t f is the satellite signal transmission time, R z (-ω E t f ) is the rotation of the Earth during the satellite signal transmission process, is the actual position of the satellite in the ECEF reference frame when the receiver receives the satellite signal. represents the position of the receiver, c represents the speed of light, represents the selection vector, δt is the receiver clock error, is the satellite clock error, and represent the delay errors caused by the troposphere and the current layer respectively;

[0095] In addition, the system also takes into account the influence of GNSS receiver clock drift. The system considers the evolution of clock offset from the previous moment to this moment as:

[0096]

[0097] Based on the above, the clock residual factor can be constructed as:

[0098]

[0099] Among them, δt k and δt k-1 are the receiver clock errors at time k and time k-1 respectively, is the rate of change of the receiver clock error, and Δt is the time difference between the kth moment and the k-1th moment.

[0100] At this point, the residual factor generated by each sensor reflects the uncertainty and error of its device measurement. By constructing residual factors related to time and space, the relationship between sensors can be effectively captured.

[0101] S4: Integrate the self-velocity residual factor, the first surface characteristic factor, the map matching factor, the second surface characteristic factor, the pre-integration factor, the pseudorange measurement residual factor, and the clock residual factor into a global optimization framework for optimization and solution.

[0102] The system utilizes graph optimization methods to integrate all residual factors into a global optimization framework, aiming to minimize the sum of all residual factors, thereby improving positioning and navigation accuracy. The optimized results provide the system's optimal state estimate, including information such as position, attitude, and velocity. These estimates fuse data from different sensors, overcoming the limitations of individual sensors. As a result, the system can maintain high-precision positioning and navigation even in complex environments, such as dense smoke or strong light.

[0103] In this embodiment, in the event of signal denial from the global navigation satellite system, the system automatically degenerates into a data positioning and navigation mode based on the cooperation of the millimeter wave radar and the laser radar;

[0104] In the data positioning and navigation mode based on the cooperation between the millimeter wave radar and the laser radar:

[0105] In the step S4: the self-velocity residual factor, the first surface characteristic factor, the map matching factor and the second surface characteristic factor are integrated into a global optimization framework for optimization and solution.

[0106] In the step S4, a state estimation method of a fixed-size sliding window is adopted to integrate the residual factors into a global optimization framework; at the same time, the maximum a posteriori probability estimation method is used to estimate the optimal value of the system state quantity.

[0107] In order to keep the amount of calculation within a controllable range, the system adopts a fixed-size sliding window state estimation method, which only fuses the data of multiple sensors for the system state at the latest series of moments to optimize the system's positioning and navigation state. At the same time, the maximum a posteriori probability estimation (MAP) method is used to estimate the state of the system in the known observation data. Estimate the optimal value of the system state quantity χ based on:

[0108]

[0109] Among them, ||r p -H pχ|| is the marginal term of the optimization problem, which represents the influence of the state at the previous time point on the state of the current time period, r p Related to the residual, H p is the Hessian matrix; Represents observation data The i-th item of , f represents the total number of observations, and r represents the residual term.

[0110] It combines priors with observations from multiple sensors to find the state that maximizes the posterior probability. Therefore, the problem is decomposed into a superposition of residual factors that depend on the state variables and the various observables. Based on this, its solution conforms to the quadratic programming paradigm. This solution employs the LevenBerg-Marquardt method from the Ceres Solver to find its optimal estimate, thereby achieving high-precision positioning and navigation.

[0111] A high-precision navigation and positioning system in a dense smoke environment based on a millimeter wave radar for implementing the high-precision navigation and positioning method in a dense smoke environment based on a millimeter wave radar of the present invention comprises:

[0112] A data acquisition module is used to acquire data from millimeter wave radar, lidar, inertial measurement unit and global navigation satellite system respectively;

[0113] a data preprocessing module configured to perform target screening and noise reduction on millimeter-wave radar data to obtain first data; perform distortion compensation on lidar data to obtain second data; perform integration processing on the inertial measurement unit data to obtain third data; and perform satellite status and elevation angle screening on global navigation satellite system data to obtain fourth data;

[0114] a residual factor construction module, configured to respectively construct an auto-velocity residual factor, a first surface characteristic factor, and a map matching factor of the first data; construct a second surface characteristic factor of the second data; construct a pre-integration factor of the third data; and respectively construct a pseudo-range measurement residual factor and a clock residual factor of the fourth data;

[0115] An optimization solution module is used to integrate the self-velocity residual factor, the first surface characteristic factor, the map matching factor, the second surface characteristic factor, the pre-integration factor, the pseudorange measurement residual factor and the clock residual factor into a global optimization framework for optimization solution.

[0116] In this embodiment, the system further includes:

[0117] a mode selection module configured to automatically degenerate to a data positioning and navigation mode based on the cooperation of the millimeter-wave radar and the lidar in the event of signal denial from the global navigation satellite system; and

[0118] The coordinate system setting module is used to use the reference system of the inertial measurement unit as the body coordinate system of the system, which is recorded as the b system; a reference system fixed to the earth, with its origin coinciding with the b system at the initial moment and its z direction pointing to the center of mass of the earth as the local world system, which is recorded as the w system; the ECEF reference system defined in the global navigation satellite system, whose origin is located at the center of mass of the earth and is stationary relative to the earth, is recorded as the e system; the ECI reference system, whose origin is located at the center of mass of the earth and is stationary relative to the stars, is recorded as the E system; and a coordinate system whose origin coincides with the w system, whose three axes point to the east, north and sky respectively, and are in the opposite direction of the center of mass of the earth, is called the n system.

[0119] Based on 4D millimeter-wave radar technology, this system provides a series of target position data within the radar coordinate system, as well as information on the frequency shift caused by the Doppler effect. This data provides two key insights into the radar's own motion state. First, the Doppler shift reflects the relative velocity between the radar and the target; assuming the target originates from a stationary object, this velocity effectively indicates the radar's own motion speed. Second, the spatial distribution of the target provides crucial clues to the radar's own position. The system exhibits excellent environmental adaptability and operates stably under a wide range of conditions. Its strong penetrating power makes it insensitive to changes in illumination, making it particularly suitable for use in dark or highly reflective scenes. Furthermore, the system integrates multiple sensors, coordinated through graph-optimized data fusion technology, to achieve stable and consistent positioning results, enabling precise positioning and navigation.

[0120] Specifically: The Laser Inertial Odometry (LiDAR Initial Odometry, LIO) can provide high-resolution three-dimensional point cloud data, accurately capture environmental details, and assist in building environmental models. The Inertial Measurement Unit (IMU) is used to detect and monitor motion status, provide fast-response acceleration and angular velocity information, and relatively accurate measurement of rotation, which can effectively supplement the accuracy of navigation data. The Global Navigation Satellite System (GNSS) is responsible for providing direct and global positioning information to ensure positioning accuracy in open areas, which is crucial for the collaborative operation of multiple devices over a large area.

[0121] In terms of data fusion, this solution adopts an optimization-based tight coupling solution, aiming to provide more stable and reliable performance than the loose coupling solution currently widely used in the market. The system is equipped with a 4D millimeter-wave radar that provides decimeter-level positioning accuracy, while the laser inertial odometer provides centimeter-level positioning accuracy. However, as long-term operations proceed, both devices will gradually accumulate errors. In this context, utilizing the characteristics of GNSS to provide direct global position information can effectively correct the accumulated errors generated by the 4D millimeter-wave radar and laser inertial odometer during long-term use, thereby improving the overall positioning accuracy. However, in complex environments such as thick smoke, GNSS signals often face denial, that is, the received signal is interfered with or blocked, resulting in the device being unable to obtain valid position information.

[0122] Therefore, this system incorporates a fusion solution that combines 4D millimeter-wave radar, laser inertial odometry, an inertial measurement unit (IMU), and GNSS to leverage the strengths of each device and enable them to work together to achieve high-precision positioning. It's important to note that 4D millimeter-level accuracy for 4D millimeter-level radar, laser inertial odometry at the centimeter level, and GNSS at the meter level. This discrepancy in accuracy makes data fusion challenging. A loose coupling approach would result in a loss of accuracy for both the 4D millimeter-wave radar and laser inertial odometry, and lead to unstable positioning results in dense smoke environments. Therefore, this system adopts a tight coupling strategy. Rather than directly using coordinates provided by GNSS for positioning, it instead constructs residual factors by acquiring pseudorange information. Furthermore, the system integrates the residual factors measured by the 4D millimeter-wave radar, laser inertial odometry, and inertial measurement unit (IMU) within a single framework for optimization. In the event of GNSS signal denial, the system automatically degenerates to a positioning and navigation mode based on the fusion of 4D millimeter-wave radar and laser inertial odometry data, ensuring reliable navigation and positioning even in complex environments.

[0123] Based on multi-sensor fusion technology encompassing millimeter-wave radar, laser inertial odometry, an inertial measurement unit (IMU), and the Global Navigation Satellite System (GNSS), the system meets the demands for high-precision positioning and navigation in complex environments, particularly in extreme conditions such as GNSS signal denial and low visibility. By integrating data from multiple sensors, the system achieves high-precision positioning and navigation even in dense smoke. Its robust anti-interference performance and high reliability ensure broad application prospects in firefighting, emergency rescue, and other fields, helping to ensure the smooth execution of critical missions.

[0124] It should be noted that the steps of the high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar provided by the present invention can be implemented using the corresponding modules, devices, units, etc. in the water pump early fault diagnosis system. Those skilled in the art can refer to the technical solution of the system to implement the step flow of the method, that is, the embodiments in the system can be understood as preferred examples for implementing the method, which will not be elaborated here.

[0125] Those skilled in the art will appreciate that, in addition to implementing the system and its various devices provided by the present invention in program code, it is entirely possible to implement the same functions of the system and its various devices provided by the present invention in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc. by logically programming the method steps. Therefore, the system and its various devices provided by the present invention can be considered a hardware component, and the devices included therein for implementing the various functions can also be considered as structures within the hardware component; the devices for implementing the various functions can also be considered as both software modules implementing the method and structures within the hardware component.

[0126] It should be noted that the preferred embodiments of the present invention are given in the specification and drawings of the present invention. However, the present invention can be implemented in many different forms and is not limited to the embodiments described in this specification. These embodiments do not serve as additional limitations on the content of the present invention. The purpose of providing these embodiments is to make the understanding of the disclosure of the present invention more thorough and comprehensive. In addition, the above-mentioned technical features continue to be combined with each other to form various embodiments not listed above, which are all considered to be within the scope of the description of the present invention; further, it is obvious to those skilled in the art that improvements or changes can be made based on the above description, and all such improvements and changes should fall within the scope of protection of the claims attached to the present invention.

[0127] The present invention has been described in detail above with reference to the embodiments of the accompanying drawings. A person skilled in the art can make various modifications to the present invention based on the above description. Therefore, certain details in the embodiments should not be construed as limiting the present invention. The scope of protection of the present invention shall be determined by the scope defined in the appended claims.

Claims

1. A high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar, comprising the following steps: S1: Acquire data from millimeter wave radar, lidar, inertial measurement unit, and global navigation satellite system respectively; S2: performing target screening and noise reduction on the millimeter wave radar data to obtain first data; performing distortion compensation on the laser radar data to obtain second data; performing integration processing on the inertial measurement unit data to obtain third data; and analyzing the global navigation satellite system data to obtain fourth data by satellite status and elevation angle screening. S3: constructing the self-velocity residual factor, the first surface characteristic factor and the map matching factor of the first data respectively; constructing a second surface characteristic factor of the second data; Constructing a pre-integration factor of the third data; respectively constructing a pseudorange measurement residual factor and a clock residual factor of the fourth data; S4: Integrate the self-velocity residual factor, the first surface characteristic factor, the map matching factor, the second surface characteristic factor, the pre-integration factor, the pseudorange measurement residual factor, and the clock residual factor into a global optimization framework for optimization and solution.

2. The high-precision navigation and positioning method based on millimeter-wave radar in a dense smoke environment according to claim 1 is characterized in that: In the event of signal denial from the global navigation satellite system, automatically degenerating into a data positioning and navigation mode based on the cooperation of the millimeter-wave radar and the lidar; In the data positioning and navigation mode based on the cooperation between the millimeter wave radar and the laser radar: In the step S4, the self-velocity residual factor, the first surface characteristic factor, the map matching factor and the second surface characteristic factor are integrated into a global optimization framework for optimization and solution.

3. The high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to claim 2 is characterized in that: The S1 step also includes the following steps: The reference frame of the inertial measurement unit is used as the body coordinate system of the system, which is recorded as b system; The reference system fixed to the earth, with its origin coinciding with the b system at the initial moment and its z direction pointing to the center of mass of the earth, is taken as the local world system, denoted as the w system; The ECEF reference system defined in the global navigation satellite system, whose origin is located at the center of mass of the earth and is stationary relative to the earth, is denoted as the e-system; The ECI reference system with its origin at the Earth's mass center and at rest relative to the stars is denoted as the E system; The coordinate system whose origin coincides with the W system and whose three axes point to the east, north and sky respectively and are in the opposite direction of the center of mass of the earth is called the N system.

4. The high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to claim 2 is characterized in that: The self-velocity residual factor: Among them, v r Indicates the speed in the radar coordinate system; Indicates the speed of the radar itself; The first surface characteristic factor: Where n represents the first plane unit normal vector; represents the spatial coordinates of the millimeter-wave radar target point; d represents the distance of the plane; The map matching factor: Where G represents the weight matrix, N represents the total number of coordinate points in the local world system, represents the coordinates of the kth point in the local world system; Represents the coordinates of the jth point of a valid millimeter-wave radar target point in the local world system; C k C represents the distribution variance of the N nearest points around the coordinates of the kth point in the local world system; j Represents the distribution variance of the N points closest to the jth effective millimeter-wave radar target point in the local world system.

5. The high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to claim 2 is characterized in that: In the step S3, the surface feature of the second data is extracted using the LOAM method to construct the second surface feature factor: in, represents the second plane unit normal vector; represents the spatial coordinates of the laser radar target point; d represents the distance of the plane.

6. The high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to claim 2 is characterized in that: The pseudorange measurement residual factor: Among them, ω E is the Earth's rotational angular velocity, t f is the satellite signal transmission time, R z (-ω E t f ) is the rotation of the Earth during the satellite signal transmission process, is the actual position of the satellite in the ECEF reference frame when the receiver receives the satellite signal. represents the position of the receiver, c represents the speed of light, represents the selection vector, δt is the receiver clock error, is the satellite clock error, and represent the delay errors caused by the troposphere and the current layer respectively; The clock residual factor: Among them, δt k and δt k-1 are the receiver clock errors at time k and time k-1 respectively, is the rate of change of the receiver clock error, and Δt is the time difference between the kth moment and the k-1th moment.

7. The high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to claim 2 is characterized in that: In the step S4, a state estimation method of a fixed-size sliding window is adopted to integrate the residual factors into a global optimization framework; at the same time, the maximum a posteriori probability estimation method is used to estimate the optimal value of the system state quantity.

8. The high-precision navigation and positioning method in a dense smoke environment based on millimeter-wave radar according to claim 7 is characterized in that: In step S4, the optimal value of the system state quantity χ is estimated by formula (8): Among them, ‖r p -H p χ‖ is the marginal term of the optimization problem, which represents the influence of the state at the previous time point on the state of the current time period, r p Related to the residual, H p is the Hessian matrix; Represents observation data The i-th item of , f represents the total number of observations, and r represents the residual term.

9. A high-precision navigation and positioning system in a dense smoke environment based on millimeter wave radar for implementing the high-precision navigation and positioning method in a dense smoke environment based on millimeter wave radar according to any one of claims 1 to 8, characterized in that: include: A data acquisition module is used to acquire data from millimeter wave radar, lidar, inertial measurement unit and global navigation satellite system respectively; A data preprocessing module is used to perform target screening and noise reduction on the millimeter wave radar data to obtain first data; and to perform distortion compensation on the laser radar data to obtain second data; Integrating the data of the inertial measurement unit to obtain third data; analyzing the data of the global navigation satellite system to obtain fourth data by screening the satellite status and elevation angle; a residual factor construction module, for respectively constructing a self-velocity residual factor, a first surface characteristic factor, and a map matching factor of the first data; constructing a second surface characteristic factor of the second data; Constructing a pre-integration factor of the third data; respectively constructing a pseudorange measurement residual factor and a clock residual factor of the fourth data; and An optimization solution module is used to integrate the self-velocity residual factor, the first surface characteristic factor, the map matching factor, the second surface characteristic factor, the pre-integration factor, the pseudorange measurement residual factor and the clock residual factor into a global optimization framework for optimization solution.

10. The high-precision navigation and positioning system based on millimeter-wave radar in a dense smoke environment according to claim 9, characterized in that: Also includes: a mode selection module, configured to automatically degenerate to a data positioning and navigation mode based on the cooperation of the millimeter-wave radar and the lidar in the event of signal denial from the global navigation satellite system; and The coordinate system setting module is used to use the reference system of the inertial measurement unit as the body coordinate system of the system, which is recorded as the b system; a reference system fixed to the earth, with its origin coinciding with the b system at the initial moment and its z direction pointing to the center of mass of the earth as the local world system, which is recorded as the w system; the ECEF reference system defined in the global navigation satellite system, whose origin is located at the center of mass of the earth and is stationary relative to the earth, is recorded as the e system; the ECI reference system, whose origin is located at the center of mass of the earth and is stationary relative to the stars, is recorded as the E system; and a coordinate system whose origin coincides with the w system, whose three axes point to the east, north and sky respectively, and are in the opposite direction of the center of mass of the earth, is called the n system.

Images