Autonomous mobile platform seamless positioning method and system for cross-GNSS available and denial environment

Abstract

The invention belongs to the technical field of navigation positioning, and particularly relates to an autonomous mobile platform seamless positioning method and system for cross-GNSS available and denial environments. According to the method, global coordinate system conversion is realized through excitation motion, global coordinate system conversion is realized online, global coordinate system positioning can be switched without unmanned aerial vehicle landing, and tedious unmanned aerial vehicle take-off and landing is avoided; through a dynamic weight adaptive fusion algorithm based on real-time quality monitoring and a Sigmoid smooth function, a measurement noise covariance matrix corresponding to a GNSS is intelligently adjusted in real time, positioning interruption or jump caused by environment change and sensor switching is eliminated, continuity and smoothness of a platform motion track are ensured, real seamless positioning is realized, and the positioning precision is improved. The platform can complete cross-regional complex tasks completely and autonomously without manual intervention, so that the operation efficiency and safety are improved, and high automation of the whole process is realized.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of navigation and positioning, and particularly relates to a seamless positioning method and system for an autonomous mobile platform in a GNSS available and denial environment. BACKGROUND

[0002] With the development of robotics and automation technology, autonomous mobile platforms such as unmanned aerial vehicles and ground robots are increasingly widely used in various industries, such as logistics and warehousing, tunnel / pipeline inspection, construction, post-disaster search and rescue, city mapping, and security patrol. These application scenarios often involve a common challenge: the mobile platform needs to transition between outdoor open areas and indoor, tunnel, and urban canyon areas where the Global Navigation Satellite System (GNSS) signal is denied or unstable.

[0003] To ensure the smooth completion of tasks, autonomous mobile platforms must have accurate and continuous positioning capabilities in any environment. Currently, the positioning technologies in the industry to address this challenge mainly include: GNSS-based positioning: In outdoor open areas, platforms mainly rely on GNSS for positioning. To achieve high accuracy, Real-Time Kinematic (RTK) or Precise Point Positioning (PPP) is usually used, which can achieve centimeter-level positioning. This is the basis for platforms to perform tasks such as flying and driving in a wide area.

[0004] SLAM-based positioning: When the platform enters indoor, underground, or obstructed areas, GNSS signals are lost. At this time, the mainstream alternative is Simultaneous Localization and Mapping (SLAM) technology. According to the different core sensors, it can be divided into: Visual SLAM: captures the visual texture features of the environment through a camera, estimates the platform's pose in real time, and builds a map; LiDAR SLAM: perceives the three-dimensional geometric structure of the environment through a LiDAR, and realizes positioning by matching point cloud data. LiDAR is less sensitive to light changes and has an advantage over visual SLAM in dark or highly variable light environments.

[0005] Multi-sensor fusion positioning: This is the current mainstream research direction, usually referring to the tight coupling of Inertial Measurement Unit (IMU) data with visual or LiDAR data, such as VINS and LIO, to improve the accuracy and robustness of SLAM systems in fast motion or feature-sparse environments.

[0006] Although the above technologies provide a way to solve the positioning problem, when dealing with the transition area from GNSS availability to denial, the existing solutions generally have serious defects: (1) Positioning mode switching is hard and unsafe: Most systems use a "threshold triggered" hard switching logic. For example, the SLAM system is started and initialized only after detecting the loss of GNSS signal. This separate switching mode causes a large jump or uncertainty in the platform's position estimation, which, when fed back to the flight / motion controller, can cause the platform to shake violently, stop suddenly, lose control of its attitude, or even collide at the entrance, posing a huge safety hazard; (2) Contradiction between the application domains of different positioning technologies and switching difficulties: The fundamental contradiction faced by existing technologies is that GNSS-RTK and SLAM, two mainstream technologies, have completely opposite advantages and disadvantages and application domains, as follows: A. SLAM's disadvantage in outdoor and RTK's absolute advantage: SLAM is essentially a relative positioning technology that calculates its own motion by perceiving continuous changes in the environment. This inevitably leads to a drift error that accumulates over time and distance. In long-distance and large-scale outdoor movement, this accumulated error will quickly increase to an unacceptable level. In contrast, GNSS-RTK is an absolute positioning technology that directly provides a position in the global geographic coordinate system and does not have cumulative drift. As long as the signal is good, its accuracy will always be maintained at the centimeter level.

[0007] B. SLAM's advantage in indoor and RTK's failure: However, when the platform enters an indoor or sheltered area, GNSS signals are completely interrupted, and RTK positioning fails completely. At this time, the advantage of SLAM is highlighted. It does not rely on any external signals and can achieve high-precision relative positioning using on-board sensors such as lidar to perceive the local environmental geometry such as walls, columns, and ceilings. In this environment, SLAM is currently the only mature technology that can provide continuous high-precision positioning.

[0008] It is this rigid requirement of "RTK must be used outdoors and SLAM must be used indoors" that makes switching between the two a huge technical challenge. How to bridge the gap between two positioning systems with completely different principles and error characteristics is a core problem that existing technologies have failed to solve properly.

[0009] (3) Data fragmentation caused by non-uniform coordinate systems: GNSS positioning uses a unified global geographic coordinate system, while SLAM systems usually establish a local coordinate system with the starting point as the origin when they are started. This results in data such as survey points and defect locations found during inspection being located in two independent, non-aligned coordinate systems. This makes it impossible to seamlessly splice the data, making it extremely difficult for subsequent data analysis and task coordination.

[0010] (4) Automated process interruption, relying on manual intervention: Due to the high risk of the switching process and the uncertainty of the SLAM system, many so-called autonomous tasks still require experienced operators to switch to manual mode for intervention at key nodes such as entrances and exits and tunnel entrances. This seriously disrupts the automated process of the task and reduces the efficiency and reliability of the operation.

[0011] To adapt to different positioning needs in indoor and outdoor environments, multiple positioning methods need to be switched. For example, patent CN109827574A requires hovering and waiting for the initial SLAM mapping to succeed before switching between indoor and outdoor environments, resulting in abrupt changes in position estimation and a fragmented overall movement trajectory. Another example is patent CN204989490U, which requires setting a common area where both systems function, hovering, and then switching, again resulting in abrupt changes in position estimation. Yet another example is patent CN119665975A, which, while achieving a transition from a unified coordinate system to a local coordinate system, has a rigid switching logic that fails to smoothly switch based on the advantages and disadvantages of different positioning methods, thus failing to achieve seamless switching. Summary of the Invention

[0012] In view of this, the present invention provides a seamless positioning method for autonomous mobile platforms across GNSS available and denied environments. This method introduces "multi-source heterogeneous sensor fusion and dynamic weight adaptive fusion" so that the system always regards all core positioning sensors such as GNSS, LiDAR and IMU as different information sources within a unified system from the start to the end of the mission, rather than independent modules that operate in time-division, thereby achieving a smooth transition of sensors.

[0013] To achieve the above objectives, the present invention adopts the following technical solution: A seamless positioning method for autonomous mobile platforms across GNSS-available and denied environments includes the following steps: S0. The UAV starts up in an outdoor area with good GNSS signal. The system is dominated by high-precision GNSS-RTK information, and the LiDAR-Inertial SLAM subsystem is also started in parallel. S1. Within a fixed location or a small area, the UAV performs a preset excitation motion and synchronously collects data, obtaining a series of paired pose data. , },in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors The pose output at all times; S2, regarding the pose data obtained in step S1 { , The least squares-based optimization algorithm is used to solve the problem, and a unique transformation matrix is ​​calculated that can transform all SLAM poses to the global coordinate system and align them with the RTK poses. ; S3. The transformation matrix obtained in step S2 The verification is performed, and once the verification is successful, the transformation matrix is ​​used as the basis for the next step. This enables precise alignment between the SLAM local coordinate system "L" and the GNSS global geographic coordinate system "G". S4. After coordinate system alignment, the UAV uses a dynamic weight adaptive fusion algorithm based on real-time quality monitoring to seamlessly switch between GNSS available and denied environments.

[0014] Preferably, the UAV described in step S1 is equipped with a GNSS-RTK receiver.

[0015] Preferably, the excitation motion and data acquisition in step S1 are synchronized, specifically as follows: Within a stationary or small area, the drone performs a pre-set excitation motion, while the system simultaneously records the GNSS-RTK pose at a high frequency at every moment. "and the pose output by LiDAR-Inertial SLAM" This yields a series of paired pose data. , }

[0016] More preferably, the excitation motion includes forward, backward, translation, rotation, and any combination of one or more of these movements.

[0017] Preferably, the least squares optimization algorithm described in step S2 has an objective function that minimizes the error between poses at all corresponding time points, as shown in the following formula: (1) in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors The pose output at all times The transformation matrix to be solved.

[0018] Preferably, in step S3, the obtained transformation matrix is ​​verified according to a preset threshold. If the error is less than the preset threshold, the verification is passed. The preset threshold is 0.5.

[0019] Preferably, the precise alignment of the SLAM local coordinate system "L" to the GNSS global geographic coordinate system "G" in step S3, to obtain the pose in the global geographic coordinate system, specifically refers to the pose in any local coordinate system output by the LiDAR-Inertial SLAM subsystem. By left-multiplying the transformation matrix in real time Directly transform into pose in the global geographic coordinate system: = cdot (2), in, The estimated pose of the LiDAR-Inertial SLAM subsystem at time t in the global coordinate system. The transformation matrix is... This represents the pose of the LiDAR-Inertial SLAM subsystem in the local coordinate system at time t.

[0020] Preferably, the dynamic weight adaptive fusion algorithm for real-time quality monitoring described in step S4 is as follows: Key quality indicators, including differential resolution, number of satellites, accuracy factor, and standard deviation reported by the receiver, are obtained from the GNSS-RTK receiver. The measurement noise covariance matrix is ​​then calculated based on these key quality indicators. And through multidimensional mapping Make dynamic adjustments when When the value changes, the covariance matrix is ​​smoothed by the Sigmoid function, so that the transition is gradual within 0.5-1 seconds, avoiding abrupt changes.

[0021] More preferably, the measurement noise covariance matrix is ​​calculated based on the acquired key quality indicators. Specifically: Based on the obtained location uncertainty, the basic value of the covariance matrix is ​​obtained, and then multiplied by the precision factor DOP as a magnification factor to obtain the covariance matrix used, as shown in the following formula: (3) Here, acc refers to the uncertainty of position. For diagonal matrix functions, is the precision factor, and s is the scaling factor.

[0022] The multidimensional mapping pair The specific steps for dynamic adjustment are as follows: If the difference solution state is a fixed solution and the horizontal precision factor HDOP is less than 1.5, the height precision h acc <0.1, output For high trust , at this time corresponds to a minimum value, ; If the differential solution state is a floating solution, the output is of medium confidence, and the medium confidence ; Otherwise, the output is of low confidence, and at this time corresponds to a maximum value, and the low confidence .

[0023] HDOP, that is, the horizontal dilution of precision, is a dimensionless number used to describe the impact of the geometric layout of GPS satellites in the sky on the horizontal positioning accuracy. Horizontal positioning includes longitude and latitude.

[0024] HDOP is a multiplier or amplification factor. It does not represent the magnitude of the error itself, such as how many meters, but indicates how many times the basic measurement error will be amplified.

[0025] HDOP < 1: Ideal state, very precise.

[0026] 1 < HDOP < 2: Good, sufficient for most navigation. [[ID=3​​​​​​​​​​​​​​​​​​​​Further preferably, during the transition region approach phase: as the UAV moves toward the entrance of the GNSS denial zone, the fusion filter continuously and in parallel processes data from GNSS and LiDAR-Inertial SLAM. When the GNSS signal quality is excellent, the fusion filter assigns extremely high weights to the GNSS data, and the dynamic weight adaptive fusion algorithm uses the GNSS measurement noise covariance matrix... Set to a minimum value and use it to continuously calibrate and correct for the tiny drifts that LiDAR-Inertial SLAM may produce; The seamless entry into the GNSS denial zone phase: When the platform enters the transition zone, GNSS satellite signals begin to be blocked, and the differential state of RTK and positioning accuracy factor will deteriorate. The dynamic weighted adaptive fusion algorithm will monitor GNSS quality indicators in real time and, according to the mapping function, […]. The target value is set to the maximum, representing extreme unreliability, and a smoothing function is used to... The value smoothly increases from "high confidence" to "maximum"; the weight of GNSS thus smoothly decays to 0, while the weight of LiDAR-Inertial SLAM smoothly increases to 100%. The dominance of the system has been seamlessly and smoothly handed over to LiDAR-Inertial SLAM; During the GNSS denial zone operation phase: the platform is indoors or in a tunnel, relying entirely on the highly robust LiDAR-Inertial SLAM for positioning and navigation on an established map with global coordinates; The seamless exit from the GNSS denial zone: When the platform completes its internal operations and moves toward the exit, the GNSS signal begins to recover. When the filter detects that the GNSS signal has recovered and the quality meets the standards, it will smoothly increase the weight of GNSS again, seamlessly handing over the positioning dominance back to RTK. The return and mission completion phase: The platform fully switches back to GNSS-dominated positioning mode and autonomously returns or executes subsequent outdoor missions in a precise global coordinate system.

[0033] In another aspect of the invention, a seamless positioning system for an autonomous mobile platform across GNSS-available and denied environments is provided, the system comprising: Perception layer: Responsible for the drivers of all sensors, publishing raw data from GNSS, LiDAR, and IMU to the ROS network; Positioning and creating layers includes: GNSS-RTK processing module: parses NMEA messages from RTK receivers and outputs global coordinates, differential status, number of satellites, and PDOP information; The LiDAR-Inertial SLAM module is developed based on the fast-lio2 algorithm framework, tightly coupling and fusing LiDAR and IMU data to output high-frequency local odometry information and point cloud maps. Dynamic weight adaptive fusion module: This module subscribes to the outputs of the above two modules and performs the coordinate alignment, dynamic weight allocation and state fusion of the present invention, and finally outputs a unique, smooth, high-precision pose estimate in the global coordinate system; Planning and Control Layer: Subscribes to the pose output by the fusion positioning module, performs global path planning and local dynamic obstacle avoidance, and generates motion commands to send to the flight controller.

[0034] Compared with the prior art, the present invention has the following beneficial effects: (1) This invention provides a universal positioning method and system that enables autonomous mobile platforms to achieve safe, smooth, accurate and seamless switching between GNSS-available and GNSS-denied environments.

[0035] (2) The present invention realizes global coordinate system transformation by exciting motion, realizes global coordinate system transformation online, and can switch to global coordinate system positioning without the need for UAV to land, thus avoiding the cumbersome UAV take-off and landing.

[0036] (3) The method of the present invention adjusts the measurement noise covariance matrix of GNSS in real time and intelligently by using a dynamic weight adaptive fusion algorithm based on real-time quality monitoring and a Sigmoid smoothing function. This eliminates positioning interruptions or jumps caused by environmental changes and sensor switching, ensuring the continuity and smoothness of the platform's motion trajectory and achieving truly seamless positioning.

[0037] (4) The method of this invention adopts a parallel fusion framework based on the dynamic weight adaptive fusion algorithm of real-time quality monitoring, rather than serial switching, avoiding the traditional switching mode of "turning off one and turning on one". The fusion idea of ​​"parallel throughout the process and dynamic dominance" is the fundamental way to achieve seamless transition. This design makes the process of handing over control from GNSS dominance to SLAM dominance a smooth fade-in and fade-out process of "weights waxing and waning", which fundamentally eliminates the sudden change in state estimation and ensures the absolute smoothness of platform movement.

[0038] (5) This method enables the platform to complete complex cross-regional tasks completely autonomously without human intervention, thereby improving operational efficiency and safety and achieving a high degree of automation throughout the entire process.

[0039] (6) This method is not limited to any specific scenario and is applicable to all autonomous mobile platforms that need to move between indoor and outdoor, underground and other mixed environments. By eliminating the risk of switching, the security of the platform at key nodes such as entrances and exits is greatly improved.

[0040] (7) This method achieves centimeter-level global positioning throughout the entire process, so that data collected at any location, whether indoors or outdoors, has a unified spatiotemporal reference. Attached Figure Description

[0041] Figure 1 Block diagram of satellite denial and available positioning systems; Figure 2 Here is a flowchart of the dynamic weight fusion algorithm; Figure 3 This is the pseudocode for the dynamic weight fusion algorithm. Detailed Implementation

[0042] This invention provides a seamless positioning method for autonomous mobile platforms across GNSS-available and denied environments, such as... Figure 1 As shown, it includes the following steps: S0. The UAV starts up in an outdoor area with good GNSS signal. The system is dominated by high-precision GNSS-RTK information, and the LiDAR-Inertial SLAM subsystem is also started in parallel. S1. Within a fixed location or a small area, the UAV performs a preset excitation motion and synchronously collects data, obtaining a series of paired pose data. , },in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors The pose output at all times; Specifically, the UAV described in step S1 is equipped with a GNSS-RTK receiver.

[0043] Specifically, within a fixed location or a small area, the platform performs a pre-set "excitation motion," such as a combination of forward, backward, translation, and rotation movements. The purpose of this is to ensure the IMU receives sufficient excitation and that the LiDAR can observe rich geometric features from different angles, thereby ensuring the SLAM system is properly initialized. During this period, the system synchronously records the GNSS-RTK pose at a high frequency at every moment. Initial pose output by LiDAR-Inertial SLAM .

[0044] S2, regarding the pose data obtained in step S1 { , The least squares-based optimization algorithm is used to solve the problem, and a unique transformation matrix is ​​calculated that can transform all SLAM poses to the global coordinate system and align them with the RTK poses. ; Specifically, the system collects a series of paired pose data. , Afterwards, the problem is transformed into a classic rigid body transformation problem. The goal is to compute a unique transformation matrix that can transform all SLAM poses to the global coordinate system and align them with the RTK poses. This invention employs a least-squares-based optimization algorithm to solve for this matrix, with the objective function being to minimize the error between poses at all corresponding time points: (1) in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors The pose output at all times The transformation matrix to be solved.

[0045] S3. The transformation matrix obtained in step S2 The verification is performed, and once the verification is successful, the transformation matrix is ​​used as the basis for the next step. This enables precise alignment between the SLAM local coordinate system "L" and the GNSS global geographic coordinate system "G". Specifically, the transformation matrix is ​​solved. Afterwards, the system will perform a verification to ensure that the residual, i.e., the aligned trajectory error, is less than a preset threshold, which is 0.5. Once the verification is successful, the transformation matrix... Locked. From this point forward, the pose in any local coordinate system output by the LiDAR-Inertial SLAM subsystem will be locked for the entire mission. Each value is transformed into its pose in the global geographic coordinate system in real time by left multiplying it by the matrix, as shown in the formula below. Thus, a unified, globally consistent positioning benchmark is firmly established.

[0046] = cdot (2), in, The estimated pose of the LiDAR-Inertial SLAM subsystem at time t in the global coordinate system. The transformation matrix is... This represents the pose of the LiDAR-Inertial SLAM subsystem in the local coordinate system at time t.

[0047] Global coordinate system pre-alignment and state transfer before entry ensure the uniformity of the coordinate system before and after switching and the continuity of the platform's motion state, which is a prerequisite for avoiding position jumps.

[0048] S4. After coordinate system alignment, the UAV uses a dynamic weight adaptive fusion algorithm based on real-time quality monitoring to seamlessly switch between GNSS available and denied environments.

[0049] Specifically, step S4 describes the dynamic weight adaptive fusion algorithm based on real-time quality monitoring: Key quality indicators, including differential resolution, number of satellites, accuracy factor, and standard deviation reported by the receiver, are obtained from the GNSS-RTK receiver. The measurement noise covariance matrix is ​​then calculated based on these key quality indicators. And through multidimensional mapping Make dynamic adjustments when When the value changes, the covariance matrix is ​​smoothed by the Sigmoid function, so that the transition is gradual within 0.5-1 seconds, avoiding abrupt changes.

[0050] Multidimensional mapping function pairs Three covariance matrices are dynamically adjusted and generated as inverse weights for fusion. The Sigmoid function performs a smooth transformation when switching between the three covariance matrices, so that one covariance matrix can smoothly transition to another.

[0051] High trust corresponds to a minimum value, low trust corresponds to a maximum value. A maximum value indicates that the data being merged no longer trusts the corresponding data, while a minimum value indicates that the data being fully trusted corresponds to trust.

[0052] The measurement noise covariance matrix is ​​calculated based on the acquired key quality indicators. ,like Figure 2 As shown, specifically: Based on the obtained location uncertainty, the basic value of the covariance matrix is ​​obtained, and then multiplied by the precision factor DOP as a magnification factor to obtain the covariance matrix used, as shown in the following formula: (3); Here, acc refers to the uncertainty of position. For diagonal matrix functions, is the precision factor, and s is the scaling factor.

[0053] The multidimensional mapping pair Make dynamic adjustments, such as Figure 2 and Figure 3 As shown, specifically: If the difference solution state is a fixed solution and the horizontal precision factor HDOP is less than 1.5, the height precision h acc <0.1, output For a high level of trust, at this time Corresponding to the minimum value, high trust level ; If the difference solution state is a floating solution, the output is... Medium trust level, medium trust level ; Otherwise, the output At this time, the level of trust is low. Corresponding to the maximum value, low trust level ; Multidimensional mapping functions mainly divide different precision ranges by difference decomposition states, and fixed solutions are generated. A necessary condition is that a floating solution or a single-point solution is generated. A necessary condition; other solutions are However, when the difference decomposition state switches, the directly calculated covariance will fluctuate greatly, leading to drastic position changes. Therefore, a sigmoid function is used to smooth the switching between the three covariance matrices, such as... Switch to .

[0054] The core principle of the dynamic weighted adaptive fusion algorithm is as follows: In the fusion filter, the confidence level of each sensor measurement is determined by its corresponding "measurement noise covariance matrix R". The smaller the value of the R matrix, the more reliable the sensor measurement, and the higher its weight in the final state estimation, and vice versa. The key to this invention is the design of an algorithm to intelligently adjust the measurement noise covariance matrix corresponding to the GNSS in real time. .

[0055] Input and Quantization: The algorithm continuously acquires a series of key quality indicators from the GNSS receiver, mainly including: 1. Differential Solution RTK Fix Status: This is the most important indicator, such as fixed solution, floating-point solution, single-point solution, etc.

[0056] 2. Number of Satellites: The number of satellites involved in the solution.

[0057] 3. Precision Factor (PDOP): Position accuracy attenuation factor; the smaller the value, the better.

[0058] 4. Standard deviation reported by the receiver: The positioning uncertainty estimated by the receiver itself.

[0059] Final result: By using weight mapping and smoothing mapping functions, the process of handing over control from GNSS dominance to SLAM dominance is transformed into a smooth fade-in and fade-out process of "weights waxing and waning", which fundamentally eliminates abrupt changes in state estimation and ensures the absolute smoothness of platform movement.

[0060] Specifically, step S4 describes the seamless switching between GNSS available and denied environments, including the transition area approach phase, the seamless entry into the GNSS denied zone phase, the operation phase within the GNSS denied zone phase, the seamless exit from the GNSS denied zone phase, and the return and mission completion phase.

[0061] The transition region approach phase: As the UAV moves toward the entrance of the GNSS denial zone, the fusion filter continuously and in parallel processes data from GNSS and LiDAR-Inertial SLAM. When the GNSS signal quality is excellent, the filter assigns extremely high weights to the GNSS data. The dynamic weight adaptive fusion algorithm then uses the GNSS measurement noise covariance matrix... Set to a minimum value and use it to continuously calibrate and correct the tiny drifts that LiDAR-Inertial SLAM may produce. At this time, the SLAM system is not only building a map, but also preparing for the upcoming switch. The seamless entry into the GNSS denial zone phase: When the platform enters the transition zone, GNSS satellite signals begin to be blocked, and the RTK solution state, for example, degrades from a "fixed solution" to a "floating-point solution" or "single-point solution," and the positioning accuracy factor deteriorates. The key to this invention—the dynamic weight allocation algorithm—monitors these GNSS quality indicators in real time. When the indicators do not meet the preset health thresholds: differential decomposition into a fixed solution, HDOP < 1.5, h_acc < 0.1, the filter automatically and smoothly reduces the weight of GNSS data in the fusion solution, while smoothly increasing the weight of LiDAR-Inertial SLAM. This weight transfer process is continuous and gradual. When the GNSS signal is completely lost, its weight naturally drops to zero, and the system's dominance is seamlessly and smoothly handed over to LiDAR-Inertial SLAM. Since the SLAM system has already been started and aligned with the global coordinate system, the platform's position and attitude estimation is completely continuous throughout the entire process, without any abrupt changes.

[0062] During the GNSS denied zone operation phase: the platform, indoors or in tunnels, relies entirely on the highly robust LiDAR-Inertial SLAM for positioning and navigation on an established map with global coordinates. The introduction of the IMU effectively compensates for LiDAR drift in geometrically degraded scenarios such as corridors and halls, ensuring accuracy during long-term operations. All collected data points are directly assigned global geographic coordinates.

[0063] The seamless exit from the GNSS denial zone: When the platform completes its internal operations and moves towards the exit, the process is the reverse of the entry. When the filter detects that the GNSS signal has recovered and the quality meets the standards, it will smoothly increase the weight of GNSS and decrease the weight of SLAM again.

[0064] The return and mission completion phase: The platform fully switches back to GNSS-dominated positioning mode and autonomously returns or executes subsequent outdoor missions in a precise global coordinate system.

[0065] Example 1 To more clearly illustrate the technical solution of the present invention, the following detailed description will be based on a quadcopter drone equipped with the positioning system of the present invention performing a "large bridge inspection" task.

[0066] (I) System Hardware Composition The core hardware configuration of the quadcopter drone platform in this embodiment is as follows: Airborne main control computer: Equipped with NVIDIA Orin NX 8GB, serving as the platform for all algorithms, responsible for sensor data processing, fusion positioning calculation, and path planning.

[0067] GNSS-RTK receiver: It adopts the BeiTian 468 high-precision dual-frequency receiver module, which can receive L1 / L2 band signals and obtain differential data from network base stations through the airborne 4G communication module. It can achieve centimeter-level positioning in open outdoor areas, such as fixed solutions.

[0068] LiDAR: Employs a lightweight 3D LiDAR, mid360, with a 360° circumferential field of view, for sensing the three-dimensional geometry of the surrounding environment.

[0069] Flight controller: It adopts the open-source PX4 flight controller, which is responsible for the low-level attitude control and motor drive of the UAV, and receives motion commands from the upper-level main control computer.

[0070] (II) System Software Architecture The software system runs on the Ubuntu operating system and the ROS framework of the robot operating system on the main control computer, and mainly includes the following core modules: Perception layer: Responsible for the drivers of all sensors, publishing raw data from GNSS, LiDAR, and IMU to the ROS network; Positioning and creating layers includes: GNSS-RTK processing module: parses NMEA messages from the RTK receiver and outputs information such as global coordinates, differential status, number of satellites, and PDOP.

[0071] The LiDAR-Inertial SLAM module is developed based on the fast-lio2 algorithm framework. It tightly couples and fuses LiDAR and IMU data to output high-frequency local odometry information and point cloud maps.

[0072] Dynamic weight adaptive fusion module: This module subscribes to the outputs of the two modules mentioned above and performs the coordinate alignment, dynamic weight allocation and state fusion of the present invention, and finally outputs a unique, smooth, high-precision pose estimate in the global coordinate system.

[0073] Planning and Control Layer: Subscribes to the pose output by the fusion positioning module, performs global path planning and local dynamic obstacle avoidance, and generates motion commands to send to the flight controller.

[0074] (III) Task Flow Mission objective: The drone takes off from outside the bridge, autonomously enters the area beneath the bridge, inspects the inventory at points A and B according to a preset route, and then autonomously flies out of the bridge and returns to the takeoff point to land.

[0075] S0. The UAV starts up in an outdoor area with good GNSS signal. The software system is dominated by high-precision GNSS-RTK information, while the LiDAR-Inertial SLAM subsystem starts up in parallel. The drone was powered on in an open area outside the bridge. Figure 1 As shown, at this time, the GNSS-RTK processing module is working normally, obtaining the RTK fixed solution within a few seconds and providing high-precision global geographic coordinates to the fusion module. At the same time, the LiDAR-InertialSLAM module is also started in parallel.

[0076] S1. Within a fixed location or a small area, the UAV performs a preset excitation motion and synchronously collects data, obtaining a series of paired pose data. , },in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors The pose output at any given time: The operator issues the takeoff command. The drone ascends vertically to a height of 5 meters and hovers. The system automatically executes the "excitation motion," which controls the drone to fly in a 2m x 2m "U" shape. During this period, the dynamic weighted adaptive fusion module simultaneously records the global pose trajectory provided by GNSS-RTK and the local pose trajectory output by LiDAR-Inertial SLAM.

[0077] S2~S3: The pose data obtained in step S1 { , The solution and verification are performed using a least-squares-based optimization algorithm. After the "U"-shaped flight is completed, the fusion module immediately runs a least-squares-based optimization algorithm to calculate the transformation matrix from the SLAM local coordinate system to the WGS-84 global coordinate system. Calculations showed that the average residual after aligning the two trajectories was 2.5 cm, which is less than the preset threshold of 10 cm, indicating successful coordinate system alignment. The ground station interface displayed: "Global coordinate system initialization complete, task can be executed."

[0078] S4. After coordinate system alignment, the UAV uses a dynamic weight adaptive fusion algorithm based on real-time quality monitoring to seamlessly switch between GNSS available and denied environments.

[0079] Includes: Outdoor, autonomous approach to the bridge phase: The drone autonomously flies towards the bridge according to a preset flight path. During this phase, the RTK remains in a fixed solution state. The dynamic weighting algorithm within the fusion module will convert the GNSS measurement noise covariance matrix... Set to minimum value Meanwhile, the LiDAR-Inertial SLAM module is also continuously running, and its output pose is... The matrix is ​​transformed to the global coordinate system in real time and continuously corrected by highly weighted RTK data, with almost no error accumulation.

[0080] Transition area, seamlessly leading under the bridge: The drone flew at a constant speed of 1.5 m / s towards the bottom of the roller shutter door. The instant the drone passed through the doorway, the metal roof and walls completely blocked the GNSS signal. The GNSS-RTK processing module detected that the differential solution state instantly degraded from "fixed solution" to "no solution." The dynamic weighting algorithm responded immediately: it adjusted the weighting based on the mapping function. Setting the target value to the maximum value represents extreme unreliability. And through a smoothing function, within approximately 1 second, the smoothing process is... The value smoothly increases from "high confidence" to "maximum". The weight of GNSS thus smoothly decays to 0, while the weight of LiDAR-Inertial SLAM smoothly increases to 100%.

[0081] Result: The final fused pose output showed no abrupt changes, and the UAV's flight attitude remained as stable as a rock, smoothly flying under the bridge without any deceleration or human intervention.

[0082] Indoors, conducting inspections under the bridge: Once inside the bridge, the drone relied entirely on LiDARS-Inertial SLAM for positioning. It flew along a pre-defined path between points A and B, with its onboard camera capturing images and recording the situation at each designated point.

[0083] Transition zone, seamlessly exiting the bridge: After completing the inventory, the drone flew towards the exit. As it approached the door, the GNSS signal began to recover. The RTK state initially returned to a "floating-point solution," and then, 1-2 seconds after flying out of the door, it regained a "fixed solution." The dynamic weighting algorithm restarted, performing the reverse operation as before, smoothly restoring the GNSS weight to over 95%, seamlessly handing over positioning control back to the RTK.

[0084] Outdoors, autonomous return and landing: Once outdoors, the drone fully reverts to RTK-based positioning, plans its path based on global coordinates, accurately flies back to the takeoff point, and lands smoothly.

[0085] Example 2 This embodiment provides a seamless positioning system for autonomous mobile platforms across GNSS available and denied environments, characterized in that the system includes: Perception layer: Responsible for the drivers of all sensors, publishing raw data from GNSS, LiDAR, and IMU to the ROS network; Positioning and creating layers includes: GNSS-RTK processing module: parses NMEA messages from RTK receivers and outputs global coordinates, differential status, number of satellites, and PDOP information; The LiDAR-Inertial SLAM module is developed based on the fast-lio2 algorithm framework, tightly coupling and fusing LiDAR and IMU data to output high-frequency local odometry information and point cloud maps. Dynamic weight adaptive fusion module: This module subscribes to the outputs of the above two modules and performs the coordinate alignment, dynamic weight allocation and state fusion of the present invention, and finally outputs a unique, smooth, high-precision pose estimate in the global coordinate system; Planning and Control Layer: Subscribes to the pose output by the fusion positioning module, performs global path planning and local dynamic obstacle avoidance, and generates motion commands to send to the flight controller.

Claims

1. A seamless positioning method for autonomous mobile platforms across GNSS available and denied environments, characterized in that, The method comprises the following steps: S0, the UAV starts in an outdoor area with good GNSS signals, and the system takes high-precision GNSS-RTK information as the main guide, and meanwhile, a LiDAR-Inertial SLAM subsystem is started in parallel; S1. Within a fixed location or a small area, the UAV performs a preset excitation motion and synchronously collects data, obtaining a series of paired pose data. , },in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors The pose output at all times; S2, to the pose data obtained in step S1 , }is solved by using a least square based optimization algorithm, and a unique transformation matrix is calculated, which can transform all SLAM poses to the global coordinate system and align with the RTK pose ; S3. The transformation matrix obtained in step S2 The verification is performed, and once the verification is successful, the transformation matrix is ​​used as the basis for the next step. This enables precise alignment between the SLAM local coordinate system "L" and the GNSS global geographic coordinate system "G". S4, after the coordinate systems are aligned, the UAV performs seamless switching between GNSS available and GNSS denial environments based on a dynamic weight adaptive fusion algorithm of real-time quality monitoring.

2. The seamless positioning method of claim 1, wherein, The UAV in step S1 is provided with a GNSS-RTK receiver.

3. The seamless positioning method of claim 1, wherein, The excitation motion in step S1 is synchronously collected with data, specifically as follows: In situ or in a small range, the UAV performs a preset excitation motion, and meanwhile, the system synchronously records the GNSS-RTK position and posture at each moment at a high frequency and the position and posture output by LiDAR-Inertial SLAM , to obtain a series of paired position and posture data , ; preferably, the excitation motion comprises advancing, retreating, translating, rotating and any one or combination of the above actions.

4. The seamless positioning method of claim 1, wherein, The least square optimization algorithm in step S2 takes the error minimization between all corresponding time instants as the objective function, and the formula is as follows: (1) in, For GNSS-RTK sensors in The pose output at all times For LiDAR-Inertial SLAM sensors in The pose output at all times The transformation matrix to be solved.

5. The seamless positioning method of claim 1, wherein, In step S3, the obtained transformation matrix is verified according to a preset threshold value, if the error is less than the preset threshold value, the verification is passed, and the preset threshold value is 0.5; The accurate alignment of the SLAM local coordinate system "L" to the GNSS global geographic coordinate system "G" in step S3 obtains the global geographic coordinate system pose, specifically: the pose in any local coordinate system output by the LiDAR-Inertial SLAM subsystem By multiplying the transformation matrix on the left in real time , the pose in the global geographic coordinate system is directly transformed: = cdot (2), wherein, is the estimated pose of the LiDAR-Inertial SLAM subsystem in the global coordinate frame at time t, is the transformation matrix, is the pose of the LiDAR-Inertial SLAM subsystem in the local coordinate frame output at time t.

6. The seamless positioning method of claim 1, wherein, The dynamic weight adaptive fusion algorithm of real-time quality monitoring in step S4 is specifically as follows: Key quality indicators, including differential resolution, number of satellites, accuracy factor, and standard deviation reported by the receiver, are obtained from the GNSS-RTK receiver. The measurement noise covariance matrix is ​​then calculated based on these key quality indicators. And through multidimensional mapping Make dynamic adjustments when When the value changes, the covariance matrix is ​​smoothed by the Sigmoid function, so that the transition is gradual within 0.5-1 seconds, avoiding abrupt changes.

7. The seamless positioning method of claim 6, wherein, The measurement noise covariance matrix is calculated based on the acquired key quality indicators Specifically: Based on the obtained position uncertainty, a covariance matrix basic value is obtained, and then multiplied by a precision factor DOP as an amplification multiple to obtain a used covariance matrix, and the formula is as follows: （3） where acc refers to the uncertainty of the position, is a diagonal matrix function, is a precision factor, s is a scaling factor; The dynamic adjustment by the multi-dimensional mapping is specifically: adjusting the dynamic range of the image data If the difference solution is a fixed solution and the horizontal dilution of precision HDOP is less than 1.5, the height precision h acc < 0.1, the output is of high trustworthiness At this time corresponds to a minimum value, ; If the differential solution state is a floating solution, the output of is a medium confidence, the medium confidence ; Otherwise, output the is low, at which time corresponding maximum, low .

8. The seamless positioning method of claim 1, wherein, The seamless switching between GNSS available and GNSS denial environments in step S4 comprises a transition area approach stage, a seamless entering GNSS denial area stage, a GNSS denial area operation stage, a seamless leaving GNSS denial area stage and a returning and task ending stage.

9. The seamless positioning method of claim 8, wherein, The transition region approaches stage: when the UAV moves towards the entrance of the GNSS denial zone, the fusion filter continuously and in parallel processes data from GNSS and LiDAR-Inertial SLAM, when the GNSS signal quality is good, the dynamic weight adaptive fusion algorithm will use the measurement noise covariance matrix of GNSS is set to a minimum value, and is used to constantly calibrate and correct the slight drift that LiDAR-Inertial SLAM may produce; The seamless entering GNSS-denied phase: when the platform enters the transition area, GNSS satellite signals start to be blocked, the differential solution state and positioning accuracy factor of RTK will become worse, the dynamic weight adaptive fusion algorithm monitors GNSS quality indicators in real time and according to the mapping function, sets the target value of to the maximum value, which represents extreme untrustworthiness, and through the smoothing function, the value of is smoothly increased from "high trustworthiness" to "maximum value"; the weight of GNSS is thus smoothly attenuated to 0, while the weight of LiDAR-Inertial SLAM is smoothly increased to 100%; the dominant right of the system has been smoothly and imperceptibly handed over to LiDAR-Inertial SLAM; In the GNSS denial area operation stage, the platform is completely dependent on the high-robustness LiDAR-Inertial SLAM to perform positioning and navigation in the map with global coordinates which has been established in the indoor or tunnel; The seamless leaving GNSS denial area stage: when the platform completes the internal operation and moves towards the exit, the GNSS signal starts to recover, and when the filter detects that the GNSS signal recovers and the quality meets the standard, the weight of the GNSS is smoothly increased again, and the positioning dominance is returned to the RTK without feeling; The returning and task ending stage: the platform completely switches back to the GNSS dominant positioning mode, and returns autonomously or performs subsequent outdoor tasks in the accurate global coordinate system.

10. An autonomous mobile platform seamless positioning system for across GNSS available and denial environments, characterized by, The system comprises: A perception layer: responsible for the drivers of all sensors, and publishes the raw data of GNSS, LiDAR and IMU to the ROS network; A positioning and mapping layer, comprising: A GNSS-RTK processing module: analyzing the NMEA message of the RTK receiver, and outputting global coordinates, differential solution state, satellite number and PDOP information; A LiDAR-Inertial SLAM module: developed based on the fast-lio2 algorithm framework, tightly coupling LiDAR and IMU data, and outputting high-frequency local odometry information and point cloud maps; A dynamic weight adaptive fusion module: subscribing to the outputs of the above two modules, and performing coordinate alignment, dynamic weight distribution and state fusion according to the application, and finally outputting a unique, smooth and high-precision pose estimation in the global coordinate system. Planning and control layer: subscribe the pose output from the fusion positioning module, perform global path planning and local dynamic obstacle avoidance, and generate motion instructions to send to the flight controller.

Images