Cross-environment multi-modal fusion positioning method for emergency rescue

Abstract

The application relates to the technical field of positioning and navigation, and discloses a cross-environment multi-modal fusion positioning method for emergency rescue, which comprises the following steps: monitoring preset switching trigger conditions; in response to all the preset switching trigger conditions being met, switching from an outdoor positioning mode to an indoor positioning mode, otherwise, keeping the outdoor positioning mode; when the indoor positioning mode is selected to be executed, the following steps are included: obtaining first intermediate data; obtaining a first optimization result by performing data optimization on the first intermediate data; performing abnormality judgment on the first optimization result; if there is abnormality, acquiring and outputting indoor position coordinates through a secondary correction strategy; otherwise, directly outputting the first optimization result as the indoor position coordinates. The application can obtain absolute position in an outdoor open area, and when entering an indoor area or a satellite signal invalid area, can automatically and smoothly switch to a multi-sensor fusion indoor positioning mode, thereby ensuring the continuity and global consistency of positioning.

Description

Technical Field

[0001] This invention relates to the field of positioning and navigation technology, and in particular to a cross-environment multimodal fusion positioning method for emergency rescue. Background Technology

[0002] In emergency rescue operations, especially in complex environments such as high-rise buildings, underground spaces, large complexes, and tunnels, as well as in extreme situations where disasters cause "three disruptions" (power outages, internet outages, and road disruptions), real-time and accurate positioning of rescue personnel is a core challenge for ensuring life safety and improving rescue efficiency. Traditional positioning methods often prove ineffective in these complex and diverse scenarios where infrastructure is damaged.

[0003] Currently, the main positioning technologies and their limitations in emergency rescue scenarios include:

[0004] 1. Outdoor positioning technology: Primarily based on Global Navigation Satellite Systems (GNSS, such as BeiDou), it can provide good positioning services in open outdoor environments. However, once entering indoor spaces, tunnels, or urban canyons with tall buildings, satellite signals will be severely attenuated or completely lost, leading to positioning failure.

[0005] 2. Indoor positioning technology:

[0006] Technologies that rely on pre-existing infrastructure, such as Wi-Fi positioning, Bluetooth (BLE / iBeacon), Ultra-Wideband (UWB), and RFID positioning, typically require the pre-deployment and maintenance of Wi-Fi access points, Bluetooth beacons, or UWB anchors. During disasters, these facilities are highly vulnerable to damage or power outages, rendering the positioning system inoperable.

[0007] Technologies that do not rely on fixed facilities:

[0008] Inertial navigation systems (INS) and pedestrian dead reckoning (PDR) do not rely on external signals, but they suffer from the problem of errors accumulating over time. In long-term rescue missions, positioning deviations may be too large, and complex motion postures affect their accuracy.

[0009] Technologies such as geomagnetic matching positioning and visual SLAM (simultaneous localization and mapping) rely on the stability of environmental features and pre-stored map data (geomagnetic maps, 3D models). Disasters such as building collapses and deformations, as well as high temperatures and dense smoke from fires, can cause drastic changes in environmental features, malfunction of optical sensors, and render map data unusable, making these technologies difficult to apply.

[0010] 3. Indoor and Outdoor Integrated Positioning Technology: Existing technologies still face challenges in achieving smooth and reliable seamless handover between indoor and outdoor environments. For example, positioning based on 5G base stations also faces the risk of base station damage during disasters. Handover mechanisms often rely on single or unstable signal indicators, which can easily lead to delayed handovers or misjudgments.

[0011] The shortcomings of existing technologies in emergency rescue applications include: outdoor positioning methods fail indoors; indoor positioning methods either rely on vulnerable infrastructure, are greatly affected by drastic environmental changes, or suffer from serious accumulation of errors; and positioning information between indoor and outdoor scenarios is difficult to connect and switch effectively and robustly.

[0012] Therefore, there is an urgent need for a cross-environment emergency rescue positioning technology that can adapt to open outdoor spaces, complex indoor spaces, and transitional areas in between, requires little or no reliance on pre-built infrastructure, and maintains robust and accurate positioning in extreme environments (such as high temperatures, dense smoke, and structural collapse). Summary of the Invention

[0013] To address the problems existing in the prior art, the technical solution provided by the present invention includes:

[0014] A cross-environment multimodal fusion localization method for emergency rescue includes the following steps:

[0015] S1. Monitor at least one preset switching trigger condition for indicating a switch from an outdoor environment to an indoor environment;

[0016] S2. In response to the fulfillment of all the preset switching trigger conditions, switch from outdoor positioning mode to indoor positioning mode; otherwise, maintain outdoor positioning mode.

[0017] S31. When the outdoor positioning mode is selected, outdoor positioning is performed based on at least satellite navigation data and inertial measurement unit data, using a fusion algorithm based on extended Kalman filtering, to obtain outdoor position coordinates;

[0018] S32. When selecting to execute indoor positioning mode, including:

[0019] S321. Acquire at least one first data from at least one preset sensor, and obtain first intermediate data after preprocessing;

[0020] S322. Perform data optimization on the first intermediate data to obtain a first optimization result;

[0021] S323. Perform anomaly judgment on the first optimization result: if there is an anomaly, obtain and output the indoor location coordinates through the secondary correction strategy; otherwise, directly output the first optimization result as the indoor location coordinates.

[0022] The data optimization includes:

[0023] S3221. The data correction result is obtained by iteratively correcting the first intermediate data through extended Kalman filtering;

[0024] S3222. The data correction results are optimized by using the alternating direction multiplier method to obtain the first optimization result.

[0025] Preferably, the preset switching trigger condition includes a combination of the following conditions:

[0026] The satellite navigation signal quality index is below the first preset threshold.

[0027] Furthermore, the LiDAR point cloud density is higher than the second preset threshold;

[0028] Furthermore, the number of UWB neighbor nodes is no less than the third preset threshold.

[0029] Preferably, the preprocessing includes:

[0030] S3211. Time synchronization: The time synchronization of each data object in the first data is achieved by triggering each sensor to sample and / or performing spline interpolation on asynchronous data through a preset pulse signal;

[0031] S3212. Data calibration: Perform IMU zero bias compensation on the first data; and when it is determined that the device is in a stationary state based on the IMU data in the first data, perform dynamic calibration on the air pressure data in the first data;

[0032] S3213. Feature extraction: Perform dynamic threshold segmentation and feature mapping on the thermal imaging data in the first data;

[0033] S3214. Floor Correction: Detect the consistency between the floor indication of the air pressure data and the IMU integral data in the first data: if they are consistent, update the floor information accordingly; if they are inconsistent, use UWB ranging data to constrain and correct the floor information.

[0034] Preferably, the method of using alternating direction multipliers to collaboratively optimize the data correction results includes:

[0035] Based on the extended Kalman filter local position estimation of each device and the ultra-wideband ranging data between devices, the first optimization result is obtained by iteratively optimizing the following objective function:

[0036] Where, p i Let i be the position to be optimized. For extended Kalman filter local location estimation of device i; W i Let be the dynamic weight matrix of device i at the current moment; Let i be the set of neighboring devices; ρ is the ultrawideband ranging value between device i and device j; ρ is the ranging constraint weight.

[0037] Preferably, the method for adjusting the dynamic weight matrix includes:

[0038] The weights are dynamically adjusted using the following formula to adjust the influence of each device's positioning result on the collaborative optimization process:

[0039]

[0040] Among them, W i Let be the dynamic weight matrix of device i at the current time, and α be the forgetting factor. These are the localization variances in the x, y, and z directions, respectively.

[0041] Preferably, the secondary correction strategy includes:

[0042] The anomaly handling has at least one triggering condition and a corresponding exit condition, and the secondary correction strategy responds to a combination of one or more of the triggering conditions; the secondary correction strategy includes one or more of the following combinations: thermal feature-driven scheme; air pressure combination positioning scheme; cross-floor misjudgment suppression scheme.

[0043] Preferably, the thermal characteristic-dominant scheme includes:

[0044] Determine if all of the following conditions are met: ① The LiDAR matching residual is greater than 1 meter for 5 consecutive tests; ② There are at least 3 stable high-temperature features, wherein the high-temperature features include: temperature greater than a first temperature threshold and contour area greater than 0.1m². 2 ; ③ Check whether the number of neighbor nodes in the UWB ranging constraint is less than 1;

[0045] If satisfied, the centroid coordinates of the region with a temperature exceeding the first temperature in the thermal image are extracted, mapped to the LiDAR coordinate system using a calibration matrix, and used as EKF observations to replace LiDAR point cloud matching.

[0046] Preferably, the air pressure combination positioning scheme includes:

[0047] Determine if the following conditions are met: thermal imaging-dominated localization has a residual of >1m for 10 consecutive iterations, and the number of UWB neighbor nodes is 0;

[0048] If satisfied, then perform IMU dead reckoning according to the following formula:

[0049] Where p is the position vector; v is the velocity vector; Δt is the time interval from time k-1 to time k; R(q) k-1Let q be the attitude quaternion at time k-1. k-1 The resulting rotation matrix is ​​used to transform vectors from the body coordinate system to the world coordinate system; a m The raw values ​​of triaxial acceleration measured by the IMU; b a This is the zero-bias estimate of the IMU accelerometer.

[0050] Preferably, the cross-floor misjudgment suppression scheme includes:

[0051] Perform a single-device air pressure reference reset according to the following formula:

[0052] in, and These are the barometer reference heights before and after the update; k p h is the Kalman gain coefficient; baro The current altitude value measured by the barometric pressure sensor; h IMU This represents the change in altitude obtained through IMU integration.

[0053] Preferably, the feature mapping method includes:

[0054] Stable high-temperature structures above a preset high-temperature threshold are extracted as thermal imaging landmarks. Their centroids in the thermal imaging coordinate system are calculated, and they are mapped to the LiDAR coordinate system through a pre-calibrated rigid transformation matrix to achieve cross-modal association with the LiDAR point cloud.

[0055] Beneficial effects

[0056] 1. This invention not only includes a robust positioning strategy for complex indoor environments, but also integrates an outdoor satellite navigation positioning mode and designs an intelligent switching mechanism based on multimodal information. This enables the positioning device to obtain absolute position using satellite navigation in open outdoor areas, and to automatically and smoothly switch to a multi-sensor fusion indoor positioning mode when entering indoor areas or areas where satellite signals are lost, ensuring the continuity and global consistency of positioning.

[0057] 2. This invention employs a data optimization strategy combining Extended Kalman Filtering (EKF) and Alternating Direction Multiplier Method (ADMM), introducing ranging data between UWB devices and a dynamic weight adjustment mechanism to achieve dynamic collaborative positioning between mobile devices. This eliminates the reliance on pre-set UWB anchor points or other fixed infrastructure, significantly improving the system's availability and deployment flexibility in disaster scenarios.

[0058] 3. This invention achieves cross-modal correlation and mapping between thermal imaging features and LiDAR point clouds by introducing thermal imaging data. By designing a thermal feature-driven localization scheme in the anomaly handling mechanism, the system can still use stable high-temperature structures as landmarks for localization even in harsh environments such as smoke and dust that cause LiDAR or visual sensor performance degradation or even failure, significantly improving the success rate and robustness of localization.

[0059] 4. Through deep fusion of multimodal data (including IMU, LiDAR, UWB, thermal imaging, barometer, etc.), combined with EKF for local iterative correction and ADMM for global collaborative optimization, the accumulation of errors from a single sensor and environmental interference are effectively suppressed. Through floor correction strategies and cross-floor misjudgment suppression schemes in anomaly handling, the floor misjudgment rate is significantly reduced, ensuring the consistency and accuracy of positioning results in multi-floor environments, with theoretical positioning errors reaching the meter level. Attached Figure Description

[0060] Figure 1 This is a schematic diagram of a cross-environment multimodal fusion positioning method for emergency rescue provided in a preferred embodiment of the present invention. Detailed Implementation

[0061] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described below with reference to the accompanying drawings. In the description of this invention, it should be understood that the terms "upper," "lower," "front," "rear," "left," "right," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the invention.

[0062] This invention provides a cross-environment multimodal fusion positioning method for emergency rescue. This method can intelligently monitor environmental switching conditions and automatically and smoothly switch and fuse positioning modes between outdoor and indoor modes. Outdoor positioning mainly relies on satellite navigation and inertial measurement unit data, while indoor positioning innovatively integrates information from multiple sensors. It improves positioning accuracy and robustness through an optimization strategy that combines Extended Kalman Filter (EKF) and Alternating Direction Method of Multipliers (ADMM), and is supplemented by a carefully designed two-level correction strategy to cope with extreme situations such as abnormal sensor data or significant deviations in positioning results. Thus, it aims to achieve uninterrupted and highly reliable positioning and tracking in complex and ever-changing emergency rescue scenarios.

[0063] The technical solution of the present invention will be described in detail below with reference to specific embodiments. It should be understood that these embodiments are only used to more clearly illustrate the technical content of the present invention, and are not intended to limit the scope of protection of the present invention.

[0064] Example 1

[0065] like Figure 1 As shown, this embodiment discloses a cross-environment multimodal fusion positioning method for emergency rescue, including the following steps:

[0066] S1. Monitor at least one preset switching trigger condition to indicate a transition from an outdoor to an indoor environment. The purpose of this step is to enable the system to automatically perceive its current macroscopic environmental type (outdoor or indoor / sheltered environment), providing a basis for subsequently selecting an appropriate positioning strategy. The preset switching trigger condition is crucial for intelligent decision-making; it can be a status indication from a single sensor or a logical combination of information from multiple sensors.

[0067] In one specific implementation, the preset switching trigger condition can be a composite condition, designed to improve the accuracy and reliability of environmental judgment, including:

[0068] 1. Satellite navigation signal quality index below a first preset threshold: The quality of satellite navigation signals (such as those provided by GPS, GLONASS, Galileo, or BeiDou systems) can be comprehensively evaluated using parameters such as carrier-to-noise ratio (CNR), number of visible satellites, and position accuracy factor (DOP). When the device is indoors or severely obstructed, satellite signal quality degrades significantly. Therefore, an empirical first preset threshold can be set (e.g., CNR below 25 dBHz, or fewer than 4 visible satellites). When the monitored signal quality index falls below this threshold, it indicates a decrease in the reliability of satellite positioning.

[0069] 2. Furthermore, the LiDAR (Light Detection and Ranging) point cloud density exceeds a second preset threshold: LiDAR senses the three-dimensional structure of its surroundings by emitting laser beams and receiving reflected signals. In open outdoor environments, the effective point cloud density scanned by LiDAR is relatively low (especially in the absence of nearby reflective surfaces); while in indoor environments, the point cloud density is typically significantly higher due to the presence of walls, furniture, and other objects. Therefore, a second preset threshold can be set. When the effective point cloud density sensed by LiDAR (e.g., the number of points per frame or the number of points in a specific area) exceeds this threshold, it indicates that the device may be in a relatively structured indoor environment.

[0070] 3. The number of UWB (Ultra-Wideband) neighbor nodes is not less than the third preset threshold: UWB technology is commonly used for high-precision indoor ranging and positioning. In indoor environments where UWB anchor nodes are deployed or where there are other friendly units wearing UWB devices, the device can detect these UWB nodes. A third preset threshold can be set (e.g., detecting at least 1 or 2 UWB neighbor nodes). When the number of nearby friendly nodes detected by the UWB communication module is not less than this threshold, it further confirms that the device has entered an indoor environment with UWB positioning capabilities.

[0071] When the above conditions are met simultaneously, the system can more confidently determine that the device has switched from an outdoor environment to an indoor environment.

[0072] S2. In response to the fulfillment of all the preset switching trigger conditions, switch from outdoor positioning mode to indoor positioning mode; otherwise, maintain outdoor positioning mode. This intelligent switching mechanism based on multi-source information ensures the matching of positioning mode with the actual environment, which is the foundation for achieving seamless positioning across environments. Furthermore, when switching from outdoor positioning mode to indoor positioning mode, the following actions can be performed: ① Turn off BeiDou radio frequency to reduce power consumption. ② Use the last valid BeiDou position as the origin of the SLAM map.

[0073] S31. When the outdoor positioning mode is selected, outdoor positioning is performed using an extended Kalman filter (EKF)-based fusion algorithm, based at least on satellite navigation data and inertial measurement unit (IMU) data, to obtain outdoor position coordinates. The IMU typically includes accelerometers and gyroscopes, providing three-axis acceleration and angular velocity information for the device. Integration can be used to estimate attitude changes over short periods. EKF, as an effective nonlinear state estimation algorithm, is well-suited for fusing GNSS observations (such as latitude, longitude, and velocity) and IMU measurements with different update rates and noise characteristics. This provides smoother, more continuous outdoor positioning results than a single GNSS measurement, and allows for dead reckoning even during brief GNSS signal interruptions.

[0074] S32. When selecting to execute indoor positioning mode, including:

[0075] S321. Acquire at least one first data from at least one preset sensor, and obtain first intermediate data after preprocessing.

[0076] The preset sensors may include, but are not limited to, IMUs, barometers, thermal imaging cameras, LiDAR, and UWB modules, which can perceive the environment and target status from different dimensions. Preprocessing is a crucial step in ensuring the quality of subsequent data fusion and positioning accuracy, and its specific operations may include the following aspects:

[0077] S3211. Time Synchronization: Due to differences in data acquisition frequencies and triggering mechanisms among different sensors, the acquired raw data exhibits temporal asynchrony. To ensure the accuracy of data fusion, time synchronization of the data from each sensor is necessary. One effective method is to synchronously trigger the sampling operations of each sensor using a preset pulse signal (e.g., generated by a unified clock source). Another method, for asynchronously acquired data, is to employ mathematical methods such as spline interpolation to interpolate the data based on the timestamps of each data point onto a unified time base, thereby achieving time synchronization of the data objects within the first data set. The accuracy of time synchronization directly affects the precision of subsequent state estimation and fusion.

[0078] S3212. Data Calibration: Raw sensor data often contains systematic errors or deviations caused by environmental influences. Therefore, data calibration is required. Specifically, for IMU data, due to its inherent bias drifting over time, IMU bias compensation is necessary. This can be achieved by estimating and subtracting the bias value during system initialization or a specific calibration procedure. Furthermore, for barometric pressure data, which is susceptible to environmental factors such as airflow, this invention proposes a dynamic calibration mechanism: when the device (rescue personnel or equipment carrying sensors) is determined to be stationary based on the IMU data in the first data set (e.g., velocity and angular velocity are below preset thresholds for a period of time), it can be assumed that the pressure change at this moment is mainly caused by changes in actual altitude or fluctuations in reference pressure. If the actual altitude is determined to be unchanged, this stationary state can be used to dynamically calibrate the barometric pressure data, for example, updating the reference pressure value to improve the accuracy of barometric altitude measurement.

[0079] S3213. Feature Extraction: For specific sensor data, extract salient features that aid in localization. For example, for thermal imaging data in the first data set, in emergency rescue scenarios (such as fire scenes), high-temperature areas are often important environmental features or indicators. This invention uses a dynamic threshold segmentation method to process thermal imaging data. This method can automatically adjust the segmentation threshold according to the global or local characteristics of the image, thereby effectively segmenting high-temperature areas from complex backgrounds. After segmentation, feature mapping is further performed on these high-temperature areas. A specific feature mapping method is as follows: extract stable high-temperature structures above a preset high-temperature threshold (e.g., temperatures above 150°C) as thermal imaging coordinates, and calculate their centroid coordinates in the thermal imaging coordinate system. Then, through a pre-calibrated rigid transformation matrix (which describes the rotation and translation relationship between the thermal imaging camera coordinate system and the LiDAR coordinate system), the centroid coordinates are mapped to the LiDAR coordinate system. The advantage of doing so is that it realizes cross-modal association between thermal imaging features and LiDAR point cloud data, providing a foundation for subsequent fusion localization and anomaly handling (such as using thermal imaging features as a substitute when LiDAR fails).

[0080] S3214. Floor Correction: In complex environments such as multi-story buildings, accurate floor determination is crucial. This invention corrects the floor information by detecting the consistency between the barometric pressure data and the IMU integral data (mainly referring to the height change obtained by double integration of the IMU's vertical acceleration) in the first data set. If the floor information indicated by both is consistent, the current floor information can be updated reliably accordingly. However, if the two indications are inconsistent (e.g., the barometer misjudges the floor due to sudden changes in local airflow, or the IMU integral drifts due to long-term accumulated errors), UWB ranging data is introduced for constraint and correction. For example, if the UWB ranging shows a distance to a known fixed anchor point or neighboring device located on a certain floor that matches the floor height, this information can be used to correct inconsistent floor indications, thereby improving the robustness of floor determination.

[0081] After the above preprocessing steps S11 to S14, the obtained sensor data (i.e., the first intermediate data) are synchronized in time, the main errors are calibrated, the key features are extracted, and the floor information is initially corrected, laying a solid foundation for subsequent data fusion and optimization.

[0082] S322. Perform data optimization on the first intermediate data to obtain a first optimization result. The purpose of this step is to further optimize the first intermediate data to obtain a more accurate and robust first optimization result, namely, the preliminary positioning coordinates of the target. In some preferred embodiments, specific methods for data optimization are given, including:

[0083] S3221. The first intermediate data is iteratively corrected using an extended Kalman filter to obtain the corrected data result. EKF is a commonly used nonlinear state estimation algorithm that can effectively fuse measurement information from different sensors to estimate the system's state (such as position, velocity, attitude, etc.). In this invention, EKF can use the IMU's kinematic model as the prediction model and observations from other sensors (such as calibrated barometric altitude, LiDAR matching results, UWB ranging information, or mapped thermal imaging feature positions, etc.) as the update basis to iteratively estimate and correct the target's position and attitude. The output of EKF serves as the local position estimate for each device or sensor node.

[0084] S3222. The data correction results are collaboratively optimized using the Alternating Direction Multiplier Method (ADMM) to obtain a first optimized result. To further improve positioning accuracy, especially in scenarios with multiple mutually sensing devices (e.g., multiple rescue personnel or robots), this invention introduces the Alternating Direction Multiplier Method (ADMM) to collaboratively optimize the EKF data correction results of each device, thereby obtaining the final first optimized result. ADMM is a powerful distributed optimization algorithm that can effectively solve constrained optimization problems. In some preferred embodiments, the collaborative optimization process can be achieved by iteratively optimizing a specific objective function. This objective function can be expressed as:

[0085]

[0086] Where, p i Let i be the position to be optimized. For extended Kalman filter local location estimation of device i; W i Let be the dynamic weight matrix of device i at the current moment; Let i be the set of neighboring devices; ρ is the ultrawideband ranging value between device i and device j; ρ is the ranging constraint weight.

[0087] The first term of the objective function The aim is to optimize the position p i Approaching its EKF local estimate And through the weight matrix W i Adjust its influence. (Second item) This is a constraint term designed to optimize the inter-device distance ||p||. i -p j ||As close as possible to its UWB ranging value By iteratively solving this objective function using the ADMM algorithm, the positions of each device after collaborative optimization can be obtained, which is the first optimization result.

[0088] To make the collaborative optimization process more adaptive, the dynamic weight matrix W i Adjustments can be made dynamically based on the quality of the equipment's own positioning results. A specific adjustment method is as follows:

[0089]

[0090] Among them, W i Let be the dynamic weight matrix of device i at the current time; α is the forgetting factor, and 0 < α ≤ 1, used to balance the influence of historical weights and current variance information; These represent the positioning variances in the x, y, and z directions, respectively. A smaller positioning variance in a particular direction indicates a more reliable positioning result in that direction, thus its corresponding weight should be larger, and vice versa. Dynamically adjusting these weights in this way allows devices with higher positioning quality to contribute more to the overall result during collaborative optimization, thereby improving the accuracy and robustness of the overall optimization outcome.

[0091] S323. Perform anomaly judgment on the first optimization result: if there is an anomaly, obtain and output the indoor location coordinates through the secondary correction strategy; otherwise, directly output the first optimization result as the indoor location coordinates.

[0092] It should be understood that in complex environments such as emergency rescue, sensor data may exhibit severe anomalies due to sudden environmental changes, inherent malfunctions, or other reasons, leading to significant deviations or even invalidation of the initial optimization result. In such cases, directly outputting this result could mislead rescue operations. Therefore, this invention establishes an anomaly detection mechanism and a two-stage correction strategy.

[0093] The anomalies include common indicators in the field, such as residuals and covariance, which are outside the preset normal range.

[0094] If an anomaly is detected, a secondary correction strategy is activated to acquire and output the corrected indoor location coordinates. The anomaly handling mechanism has at least one triggering condition (e.g., multiple consecutive LiDAR matching failures, prolonged UWB signal loss, significant discrepancies between the positioning result and the kinematic model) and a corresponding exit condition (e.g., the anomaly source returns to normal, or the correction strategy itself fails). The secondary correction strategy aims to provide a usable positioning result by utilizing currently available and relatively reliable sensor information or backup solutions. In some preferred embodiments, it may include a combination of one or more of the following schemes, and the system will select and execute the corresponding correction scheme based on the specific combination of anomaly triggering conditions:

[0095] Thermal Feature Dominant Scheme: This scheme primarily addresses situations where major positioning sensors such as LiDAR fail, but thermal imaging features remain usable. Its triggering conditions can be: ① The LiDAR matching residual (e.g., the root mean square error of point cloud registration) exceeds a preset threshold (e.g., 1 meter) five consecutive times, indicating that the LiDAR positioning result is unreliable; ② Simultaneously, the number of stable and identifiable high-temperature features detected in the environment is not less than a preset number (e.g., 3), where the high-temperature feature is defined as, for example, a temperature greater than a first temperature threshold and a contour area in the thermal image greater than 0.1 square meters, ensuring the significance and stability of these features; ③ Furthermore, the number of neighboring nodes in the UWB ranging constraint (i.e., the number of friendly devices capable of effective UWB ranging) is less than a preset value (e.g., 1), indicating that the UWB cooperative positioning constraint is weak or unavailable.

[0096] If all the above conditions are met simultaneously, the LiDAR is deemed to be faulty and the thermal imaging features are usable. In this case, the strategy extracts the centroid coordinates of regions in the thermal imaging image where the temperature exceeds a first temperature threshold (e.g., 150°C), and maps them from the thermal imaging coordinate system to the LiDAR coordinate system using the rigid transformation matrix pre-calibrated in S13. Then, these mapped thermal feature coordinates are used as EKF observations to replace the previously faulty LiDAR point cloud matching results, thus continuing the localization process. This method leverages the salience and relative stability of heat sources in environments such as fire scenes, providing an effective alternative localization method for faulty LiDAR systems.

[0097] Barometric Alignment (BAA) Scheme: This scheme is primarily designed to address extreme situations where thermal feature-dominated positioning also fails or is inapplicable (e.g., no significant heat source in the environment, or interference with thermal imaging), and UWB cooperative positioning is also unavailable. The triggering conditions are: the residual of 10 consecutive positioning results from thermal imaging-dominated positioning (i.e., the aforementioned thermal feature-dominated scheme) still exceeds a preset threshold (e.g., 1 meter), indicating poor thermal feature positioning performance; and the number of UWB neighbor nodes remains at 0, indicating that UWB cooperative positioning is impossible. If these conditions are met, the system will switch to a combined positioning scheme based on IMU and barometer. One specific implementation involves performing IMU dead reckoning, the position update formula of which can be expressed as:

[0098] Where p is the position vector; v is the velocity vector; Δt is the time interval from time k-1 to time k; R(q) k-1 Let q be the attitude quaternion at time k-1. k-1 The resulting rotation matrix is ​​used to transform vectors from the body coordinate system to the world coordinate system; a m The raw values ​​of triaxial acceleration measured by the IMU; b a This is the zero-bias estimate from the IMU accelerometer. Simultaneously, the vertical position in the IMU dead reckoning result can be corrected by combining altitude information measured by the barometer, thus mitigating its vertical drift. Although this approach has limited accuracy and accumulates errors over time, it can still provide a basic, relatively usable positioning reference for a short period when other major sensors fail.

[0099] Cross-floor misjudgment suppression scheme: This scheme is mainly used to correct floor misjudgments caused by fluctuations in the barometer itself or vertical integration errors of the IMU. In multi-floor environments, even if the floor information has been initially corrected in S14, subsequent misjudgments may still occur due to continuous interference or error accumulation. This scheme further improves the accuracy of floor judgment through a single-device barometric pressure reference reset mechanism. Its reference reset formula can be expressed as:

[0100] in, and These are the barometer reference heights before and after the update; k p h is the Kalman gain coefficient; baro The current altitude value measured by the barometric pressure sensor; h IMU This represents the change in altitude obtained through IMU integration.

[0101] The core idea of ​​this formula is to compare the "current barometer reading relative to the old reference height" with the "change in altitude indicated by the IMU". If there is a discrepancy between the two, the old reference height is considered to be correct. This may be inaccurate and requires adjustment. The amount of adjustment is related to the deviation and the gain K. In this way, the IMU's ability to accurately measure relative height changes over a short period can be utilized to dynamically calibrate the barometer's reference height, thereby suppressing misjudgments across floors.

[0102] Through the above-mentioned graded, multi-strategy two-level correction mechanism, the present invention can intelligently switch to a suitable backup scheme when the main positioning scheme (EKF+ADMM fusion) malfunctions, based on the specific malfunction type and available sensor information, thereby maximizing the continuity of positioning output and availability in extreme and harsh environments.

[0103] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A cross-environment multimodal fusion positioning method for emergency rescue, characterized in that, Including the following steps: S1. Monitor at least one preset switching trigger condition for indicating a switch from an outdoor environment to an indoor environment; S2. In response to the fulfillment of all the preset switching trigger conditions, switch from outdoor positioning mode to indoor positioning mode; otherwise, maintain outdoor positioning mode. S31. When the outdoor positioning mode is selected, outdoor positioning is performed based on at least satellite navigation data and inertial measurement unit data, using a fusion algorithm based on extended Kalman filtering, to obtain outdoor position coordinates; S32. When selecting to execute indoor positioning mode, including: S321. Acquire at least one first data from at least one preset sensor, and obtain first intermediate data after preprocessing; S322. Perform data optimization on the first intermediate data to obtain a first optimization result; S323. Perform anomaly detection on the first optimization result: if anomaly exists, obtain and output the indoor location coordinates through a secondary correction strategy; otherwise, directly output the first optimization result as the indoor location coordinates. The data optimization includes: S3221. The data correction result is obtained by iteratively correcting the first intermediate data through extended Kalman filtering; S3222. The data correction results are jointly optimized using the alternating direction multiplier method to obtain the first optimization result; The method of using alternating direction multipliers to collaboratively optimize the data correction results includes: Based on the extended Kalman filter local position estimation of each device and the ultra-wideband ranging data between devices, the first optimization result is obtained by iteratively optimizing the following objective function: ； in, Let i be the position to be optimized. Let j be the position to be optimized. For equipment Extended Kalman filter local location estimation; Let be the dynamic weight matrix of device i at the current moment; For equipment The set of neighboring devices; ρ represents the ultrawideband ranging value between device i and device j; ρ is the ranging constraint weight. The secondary correction strategy includes: The anomaly handling has at least one triggering condition and a corresponding exit condition, and the secondary correction strategy responds to a combination of one or more of the triggering conditions; the secondary correction strategy includes one or more of the following combinations: thermal feature-driven scheme; air pressure combination positioning scheme; cross-floor misjudgment suppression scheme.

2. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 1, characterized in that, The preset switching trigger conditions include a combination of the following conditions: The satellite navigation signal quality index is below the first preset threshold. Furthermore, the LiDAR point cloud density is higher than the second preset threshold; Furthermore, the number of UWB neighbor nodes is no less than the third preset threshold.

3. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 1, characterized in that, The preprocessing includes: S3211. Time synchronization: The time synchronization of each data object in the first data is achieved by triggering each sensor to sample and / or performing spline interpolation on asynchronous data through a preset pulse signal; S3212. Data calibration: Perform IMU zero bias compensation on the first data; and when it is determined that the device is in a stationary state based on the IMU data in the first data, perform dynamic calibration on the air pressure data in the first data; S3213. Feature extraction: Perform dynamic threshold segmentation and feature mapping on the thermal imaging data in the first data; S3214. Floor Correction: Detect the consistency between the floor indication of the air pressure data and the IMU integral data in the first data: if they are consistent, update the floor information accordingly; if they are inconsistent, use UWB ranging data to constrain and correct the floor information.

4. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 1, characterized in that, The method for adjusting the dynamic weight matrix includes: The weights are dynamically adjusted using the following formula to adjust the influence of each device's positioning result on the collaborative optimization process: ； in, Let i be the dynamic weight matrix of device i at the current moment. Forgetting factor, These are the localization variances in the x, y, and z directions, respectively.

5. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 1, characterized in that, The dominant thermal feature scheme includes: Determine if all of the following conditions are met: ① The LiDAR matching residual is greater than 1 meter for 5 consecutive tests; ② There are at least 3 stable high-temperature features, wherein the high-temperature features include: temperature greater than a first temperature threshold and contour area greater than 0.1 m². 2 ; ③ Check whether the number of neighbor nodes in the UWB ranging constraint is less than 1; If satisfied, the centroid coordinates of the region with a temperature exceeding the first temperature in the thermal image are extracted, mapped to the LiDAR coordinate system through a calibration matrix, and used as EKF observations to replace LiDAR point cloud matching.

6. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 1, characterized in that, The air pressure combination positioning scheme includes: Determine if the following conditions are met: thermal imaging-dominated localization has a residual of >1m for 10 consecutive iterations, and the number of UWB neighbor nodes is 0; If satisfied, then perform IMU dead reckoning according to the following formula: Where p is the position vector; v is the velocity vector; and Δt is the time interval from time k-1 to time k. The attitude quaternion at time k-1 The resulting rotation matrix is ​​used to transform vectors from the body coordinate system to the world coordinate system; Raw values ​​of triaxial acceleration measured by the IMU; This is the zero-bias estimate of the IMU accelerometer.

7. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 1, characterized in that, The cross-floor misjudgment suppression scheme includes: Perform a single-device air pressure reference reset according to the following formula: ;in, These are the reference heights of the barometer before and after the update; The Kalman gain coefficient; The current altitude value measured by the barometric pressure sensor; This represents the change in altitude obtained through IMU integration.

8. The cross-environment multimodal fusion positioning method for emergency rescue as described in claim 3, characterized in that, The feature mapping method includes: Stable high-temperature structures above a preset high-temperature threshold are extracted as thermal imaging landmarks. Their centroids in the thermal imaging coordinate system are calculated, and they are mapped to the LiDAR coordinate system through a pre-calibrated rigid transformation matrix to achieve cross-modal association with the LiDAR point cloud.

Images