Unmanned aerial vehicle integrated navigation method in semi-enclosed space and related equipment

Abstract

The application relates to the technical field of unmanned aerial vehicle navigation, in particular to a semi-closed space unmanned aerial vehicle combined navigation method and related equipment, which comprises the following steps: acquiring initial navigation data, including unmanned aerial vehicle acceleration and angular velocity information collected by an inertial measurement unit, heading information acquired by a heading sensor, and three-dimensional position information measured by a position sensor; selecting state variables based on the above data, constructing a state equation describing the change of the state variables with time; meanwhile, establishing an observation equation by using the three-dimensional position information output by the position sensor; estimating and updating the state variables by fusing the state equation and the observation equation through an extended Kalman filtering algorithm; and finally outputting the updated state variables as high-precision position and / or speed information required by unmanned aerial vehicle navigation. The method effectively improves the accuracy and robustness of unmanned aerial vehicle navigation through multi-sensor data fusion and state estimation.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) navigation technology, specifically to a UAV integrated navigation method and related equipment in a semi-enclosed space. Background Technology

[0002] In recent years, with the rapid development of robotics and artificial intelligence technologies, the application of drones has gradually expanded from military and consumer fields to industrial production. Using drones for tasks such as high-voltage line inspection, terrain mapping, and material volume calculation has greatly improved operational efficiency and quality. As drone application scenarios expand from open-air environments to semi-enclosed spaces, the requirements for drone navigation and control methods have also increased. Navigation technology is the foundation of drone flight, and the accuracy and stability of positioning technology determine the application scenarios of drones.

[0003] With the expansion of robotics and drone applications, outdoor positioning systems are no longer sufficient. In indoor environments, electromagnetic waves are attenuated due to blockages, rendering GNSS systems ineffective, necessitating the use of alternative positioning methods. Common indoor positioning technologies include Bluetooth positioning, visual SLAM (Simultaneous Localization and Mapping), and UWB (Ultra Wide Band) positioning. In complex, semi-enclosed spaces, a single navigation system struggles to meet the requirements for accuracy and robustness. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a method and related equipment for UAV integrated navigation in a semi-enclosed space, which addresses the shortcomings of the prior art and solves the technical problems of GNSS failure, unstable positioning of a single sensor, and inertial navigation drift in a semi-enclosed space.

[0005] The objective of this invention is achieved through the following technical solutions: In a first aspect, the present invention provides a method for integrated navigation of unmanned aerial vehicles (UAVs) in a semi-enclosed space, comprising: Acquire initial navigation data, which includes UAV acceleration and angular velocity information acquired by the inertial measurement unit, heading information acquired by the heading sensor, and three-dimensional position information of the UAV in the navigation coordinate system acquired by the position sensor. Based on the initial navigation data, select the state variables required for navigation and establish state equations describing the changes of the state variables over time. Based on the three-dimensional position information output by the position sensor, an observation equation corresponding to the state equation is established; The extended Kalman filter algorithm is used to fuse the state equation and the observation equation to estimate and update the state variables; Output the updated state values ​​as the position and / or velocity information required for drone navigation.

[0006] As a further improvement of the present invention, the position sensor is at least one of a visual SLAM system or an ultra-wideband UWB positioning system; the position sensor is at least one of a visual SLAM system or an ultra-wideband UWB positioning system; when the position sensor is a visual SLAM system, the established observation equation is a linear equation, and the observations are the position coordinates and heading angle output by the visual SLAM system.

[0007] As a further improvement of the present invention, the state quantity required for navigation is selected, and the state quantity is:

[0008] In the formula, For state variables, This indicates the attitude information of the drone. This represents the three-dimensional position coordinates of the UAV in the ENU navigation coordinate system. This represents the velocity of the UAV in the ENU coordinate system. and These represent the bias values ​​of the gyroscope and accelerometer in the inertial navigation system, respectively.

[0009] As a further improvement of the present invention, when the position sensor is a UWB positioning system, the way to establish the observation equation includes loose coupling mode and tight coupling mode. In the loosely coupled mode, the observation equation is a linear equation, and the corresponding observation is the position coordinates calculated by the UWB positioning system. In the tightly coupled mode, the observation equation is a nonlinear equation, and the corresponding observation is the raw distance information between the UAV tag and multiple fixed base stations in the UWB positioning system.

[0010] As a further improvement of the present invention, the process of state estimation using the extended Kalman filter algorithm includes: linearizing and discretizing the nonlinear state equation, calculating the Kalman filter gain, and correcting the state prediction value using the information output by the observation equation.

[0011] As a further improvement of the present invention, the use of the extended Kalman filter algorithm to fuse the state equation and the observation equation includes linearizing and discretizing the nonlinear state equation.

[0012] As a further improvement of the present invention, when any position sensor fails or malfunctions due to environmental interference, the system switches to a combined navigation mode consisting of another position sensor and an inertial navigation system to output information.

[0013] Secondly, the present invention provides a UAV integrated navigation system for a semi-enclosed space, comprising: The data acquisition module is used to acquire initial navigation data, which includes UAV acceleration and angular velocity information acquired by the inertial measurement unit, heading information acquired by the heading sensor, and three-dimensional position information of the UAV in the navigation coordinate system acquired by the position sensor. The state output module is used to select the state variables required for navigation based on the initial navigation data, and to establish state equations describing the changes of the state variables over time. The observation output module is used to establish an observation equation corresponding to the state equation based on the three-dimensional position information output by the position sensor. The state update module is used to estimate and update the state variables by fusing the state equation and the observation equation using the extended Kalman filter algorithm. The navigation command output module is used to output updated state quantities as position and / or speed information required for UAV navigation.

[0014] Thirdly, the present invention provides a computer device, comprising: a processor and a computer-readable storage medium; A processor, adapted to execute computer programs; A computer-readable storage medium storing a computer program, which, when executed by the processor, implements the UAV integrated navigation method in a semi-enclosed space as described above.

[0015] Fourthly, the present invention provides a computer-readable storage medium storing a computer program adapted to be loaded by a processor and executed as described above for the UAV integrated navigation method in a semi-enclosed space.

[0016] The beneficial effects of this invention are as follows: This invention provides a UAV integrated navigation method in a semi-enclosed space. It uses an inertial measurement unit (IMU) to acquire UAV acceleration and angular velocity information, a heading sensor to acquire heading information, and a position sensor to acquire three-dimensional position information as initial navigation data. It directly fuses multi-source heterogeneous sensor data to establish state equations and observation equations, solving the position drift problem caused by insufficient sensor data fusion in traditional navigation systems. Based on the three-dimensional position information output by the position sensor, an observation equation is established, forming a closed-loop feedback structure with the state equation. An extended Kalman filter algorithm is used to recursively estimate and update state variables, which is more suitable for nonlinear systems than traditional Kalman filtering methods, improving the dynamic accuracy of state estimation. The sensor data form complementary constraints in the state and observation equations, synergistically extending the iterative update mechanism of the Kalman filter. This ensures that the output of position and velocity information includes both the short-term high-precision characteristics of inertial navigation and the long-term stability of the position sensor. The final output navigation information achieves a better balance between dynamic response speed and long-term positioning accuracy, achieving a synergistic improvement in accuracy and robustness compared to single-sensor navigation schemes. This method effectively solves the problem of accumulated positioning errors caused by sensor noise, drift and environmental interference in UAV navigation by deeply coupling multi-sensor data fusion and extended Kalman filtering, enabling the navigation system to maintain centimeter-level positioning accuracy and millisecond-level response speed even in complex maneuvering scenarios. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 This is a schematic diagram of the structure of the UAV integrated navigation system in a semi-enclosed space in an embodiment of the present invention; Figure 2 This is a schematic diagram of the UAV integrated navigation method in a semi-enclosed space according to an embodiment of the present invention; Figure 3 This is an internal structural diagram of a computer device in an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives and technical solutions of this invention clearer and easier to understand, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. The specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0020] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings and specific embodiments. The described embodiments are only some embodiments of the present invention, and not all embodiments.

[0021] Example 1 Existing semi-enclosed spaces (such as tunnels, underground garages, factories, and mines) present complex environments with issues like signal obstruction, multipath interference, and drastic changes in lighting, leading to significant shortcomings in traditional UAV navigation methods. While a single inertial navigation system can autonomously output navigation information unaffected by external environmental interference, it suffers from cumulative errors, resulting in a sharp decline in navigation accuracy after prolonged operation, failing to meet the precise navigation requirements of UAVs in semi-enclosed spaces. A single visual SLAM system is prone to feature point matching failures in semi-enclosed environments with insufficient lighting and sparse feature points, leading to positioning failures. A single ultra-wideband (UWB) positioning system is susceptible to signal attenuation and latency errors in scenarios with multiple obstructions and interferences, making it difficult to guarantee positioning accuracy. Furthermore, existing integrated navigation methods often fail to consider the special environment of semi-enclosed spaces and lack switching mechanisms for sensor failures, resulting in insufficient reliability and redundancy of the navigation system. This makes it unsuitable for the complex and ever-changing navigation scenarios within semi-enclosed spaces, hindering the achievement of stable and accurate navigation for UAVs.

[0022] The following detailed description of the UAV integrated navigation method in a semi-enclosed space according to this application, with reference to specific embodiments, is as follows: Figure 1 , Figure 2 As shown, this embodiment takes a semi-enclosed space as an underground factory as an example, and the drone is a multi-rotor drone. However, it is not limited to this scenario and drone type. The technical solution of this application is applicable to other semi-enclosed spaces (such as tunnels and mines) and other types of drones (such as fixed-wing drones).

[0023] S1: Acquire initial navigation data, which includes the UAV acceleration and angular velocity information acquired by the inertial measurement unit, the heading information acquired by the heading sensor, and the three-dimensional position information of the UAV in the navigation coordinate system acquired by the position sensor. The UAV integrated navigation system in the semi-enclosed space involved in this application mainly consists of the following hardware modules: inertial measurement unit, heading sensor, position sensor, and airborne processing unit.

[0024] Inertial measurement units (IMUs) are used to measure the three-axis acceleration and three-axis angular velocity information of a UAV in its body coordinate system. Their output frequency is typically between 100 and 500 Hz. IMUs usually employ microelectromechanical systems (MEMS) inertial sensors, where the gyroscope has a zero-bias stability better than 10 degrees per hour, and the accelerometer has a zero-bias stability better than 100 micrograms.

[0025] The heading sensor measures the Earth's magnetic field vector using a triaxial magnetometer to calculate the UAV's current heading angle. The heading sensor employs an anisotropic magnetoresistive sensor, and its heading angle calculation accuracy is better than 0.5 degrees within a 10-meter range.

[0026] Position sensors are used to acquire the UAV's 3D position information in the navigation coordinate system. These sensors can be one or a combination of visual SLAM or ultra-wideband (UWB) positioning systems. When using a visual SLAM system, environmental features are extracted and simultaneous localization and mapping (UWB) are performed by processing continuous image sequences, outputting the UAV's 3D coordinates in the ENU navigation coordinate system. When using a UWB positioning system, the 3D position coordinates of the UAV in the navigation coordinate system are calculated by measuring the signal propagation time between the tag carried by the UAV and multiple fixed base stations. Visual SLAM systems can use binocular cameras as sensing devices, with an image acquisition frequency of 30 to 60 frames per second. ORB features are used for feature extraction and matching to meet real-time requirements. UWB positioning systems can use the DW1000 chip, with a ranging accuracy better than 10 centimeters and a positioning update frequency of at least 20 Hz.

[0027] The onboard processing unit executes the extended Kalman filter algorithm and outputs navigation information. Modules interact with each other via standard communication interfaces. The inertial measurement unit and heading sensor are typically connected to the onboard processing unit via RS422 or CAN bus, while the visual SLAM system or UWB positioning system transmits positioning data via Ethernet or a high-speed serial interface.

[0028] Specifically, the inertial measurement unit (IMU) acquires acceleration and angular velocity information in the UAV's body coordinate system in real time. The acceleration information reflects the UAV's motion along the three body axes, while the angular velocity information reflects the UAV's rotation rate around these axes. The heading sensor measures the Earth's magnetic field vector using a triaxial magnetometer to calculate the UAV's current heading angle. Position sensors, depending on their type, acquire the UAV's three-dimensional position information in the navigation coordinate system.

[0029] In some embodiments of this application, the raw data output by the inertial measurement unit (IMU) is first preprocessed, including operations such as zero-bias compensation, scale factor correction, and temperature drift compensation, to improve data quality. The heading angle information output by the heading sensor needs to be corrected for magnetic declination to obtain the true heading angle. The position information output by the position sensor needs to be time-synchronized to ensure that it is aligned with the data from the IMU in time.

[0030] Furthermore, the data acquisition phase also includes real-time monitoring of the data quality of each sensor. When the output data of a sensor experiences abnormal fluctuations or continuous loss, the system records the fault status of that sensor, providing a basis for subsequent mode switching. The data quality monitoring method can employ a detection algorithm based on residual analysis; when the statistical characteristics of the observed information deviate from the normal range, the corresponding sensor data is determined to be unreliable.

[0031] S2: Based on the initial navigation data, select the state variables required for navigation and establish state equations describing the changes of the state variables over time.

[0032] Specifically, based on the error propagation characteristics of inertial navigation systems, fifteen state variables are selected to describe the current motion state of the UAV and sensor errors.

[0033] The state variable X is defined as a vector containing the following elements: roll angle, pitch angle, yaw angle, eastward position, northward position, azimuth position, eastward velocity, northward velocity, azimuth velocity, gyroscope three-axis zero bias, and accelerometer three-axis zero bias. The state variables are:

[0034] In the formula, For state variables, This indicates the attitude information of the drone. This represents the three-dimensional position coordinates of the UAV in the ENU navigation coordinate system. This represents the velocity of the UAV in the ENU coordinate system. and These represent the bias values ​​of the gyroscope and accelerometer in the inertial navigation system, respectively.

[0035] The roll angle, pitch angle, and yaw angle together describe the attitude information of the UAV in the navigation coordinate system. Eastward position, northward position, and skyward position represent the three-dimensional coordinates of the UAV in the ENU navigation coordinate system. Eastward velocity, northward velocity, and skyward velocity represent the three-dimensional velocity components of the UAV in the ENU coordinate system. The three-axis zero bias of the gyroscope represents the zero bias error of the three-axis gyroscope in the inertial measurement unit, and the three-axis zero bias of the accelerometer represents the zero bias error of the three-axis accelerometer.

[0036] In some embodiments of this application, the state equations are in the form of continuous-time nonlinear differential equations, describing the propagation of state variables over time. The state equations describe the kinematic relationship of attitude angles, the integral relationship of position, and the coupling relationship between velocity and attitude. The state equations also include process noise terms to describe the effects of model uncertainties and sensor noise.

[0037] Specifically, the state equation is:

[0038] in, , which represents the transformation matrix from the body coordinate system to the navigation coordinate system.

[0039] Furthermore, the state equations involve a rotational transformation from the body coordinate system to the navigation coordinate system, which is achieved through a direction cosine matrix, uniquely determined by the current attitude angle. Since the extended Kalman filter algorithm requires computation in the discrete time domain, the continuous state equations need to be discretized in practical implementation. Discretization methods can employ the standard Euler method or the fourth-order Runge-Kutta method. The Euler method is computationally simple but has lower accuracy, suitable for scenarios with short sampling periods or low accuracy requirements. The Runge-Kutta method offers higher accuracy but involves greater computation, suitable for scenarios with high navigation accuracy requirements.

[0040] S3: Based on the three-dimensional position information output by the position sensor, establish the observation equation corresponding to the state equation.

[0041] Specifically, the observation equations establish the mathematical relationship between state variables and sensor observations, serving as the key input for the extended Kalman filter algorithm to update the state. The method for establishing the observation equations varies depending on the type of position sensor.

[0042] In some embodiments of this application, when the position sensor is a visual SLAM system, the established observation equation is a linear equation. Specifically, the UAV's three-dimensional position coordinates output by the visual SLAM system are used as observations, directly corresponding to the position component in the state variables. The observation equation is:

[0043] in, For the position coordinate output of the visual SLAM system, Let yaw angle be the aircraft's angle. In this visual SLAM / inertial navigation loose combination model, the state equation is nonlinear, while the measurement equation is linear. Therefore, an extended Kalman filter is used to maintain the system state variables, outputting position and velocity information for aircraft navigation.

[0044] The observation matrix in the observation equation maps the state space to the observation space. For position coordinate observations, the elements in the observation matrix corresponding to the position state are identity matrices, while the elements corresponding to other states are zero. Furthermore, the heading angle information output by the heading sensor can also be included in the observation equation as an observation. In this case, the observation matrix is ​​expanded accordingly to include observations of the yaw angle state.

[0045] In another embodiment, when the position sensor is a UWB positioning system, the observation equations can be established in two ways: loosely coupled mode and tightly coupled mode.

[0046] In loosely coupled mode, the observation equation is a linear equation. In this mode, the UWB positioning system first completes the internal positioning calculation, outputting the UAV's three-dimensional position coordinates in the navigation coordinate system. These position coordinates, as an observation, directly establish a linear mapping relationship with the position component in the state variables. The observation vector in the observation equation contains the three-dimensional position coordinates and heading angle information output by the UWB system. The elements of the observation matrix are determined according to the correspondence between the state variables and the observations. Observation noise is typically modeled as a white noise process. Specifically, the observation equation is:

[0047] In the formula, where For the position coordinate output of the UWB system, The yaw angle of the aircraft is used to maintain the system state variables using an extended Kalman filter, and the output is the position and velocity information for aircraft navigation.

[0048] In tightly coupled mode, the observation equations are nonlinear. Instead of using the position coordinates calculated internally by the UWB system, the raw distance information between the UAV tag and multiple fixed base stations in the UWB positioning system is directly used as the observation. Each base station distance observation can be represented as a nonlinear function of the Euclidean distance between the UAV's current location and the base station locations. The nonlinear observation function calculates the spatial distance from the UAV's current location to each base station location for each distance observation. The nonlinear observation equation is:

[0049] The observation vector in tightly coupled mode includes the UAV's yaw angle and distance observations to each base station. The advantage of tightly coupled mode is that it can make full use of all raw distance observation information for state estimation, avoiding information loss in the position calculation process in loosely coupled mode. However, computationally, it requires the use of the extended Kalman filter algorithm to linearize the nonlinear observation equations.

[0050] S4: Using the extended Kalman filter algorithm, the state equation and the observation equation are fused to estimate and update the state variables.

[0051] Specifically, the extended Kalman filter algorithm includes two main stages: time update and measurement update, and achieves the optimal estimation of state variables through iterative calculation.

[0052] The time update phase first involves linearizing and discretizing the state equations. Linearization is achieved by calculating the Jacobian matrix of the state equations with respect to the state variables, approximating the nonlinear equations as linear equations at the operating point of each filtering cycle. Discretization transforms the continuous-time state equations into discrete-time state equations, using a state transition matrix to describe the propagation of the state variables within a sampling period. Subsequently, the state transition matrix and the process noise covariance matrix are used to calculate the predicted state value and the prediction error covariance matrix.

[0053] In the measurement update phase, the state prediction is corrected using the innovation output from the observation equation. First, the Kalman filter gain is calculated; this gain matrix represents the correction weight of the observation information on the state estimate, and its calculation involves the observation matrix, the observation noise covariance matrix, and the prediction error covariance matrix. Then, the state update is calculated using the Kalman filter gain and the innovation vector, defined as the difference between the actual observation and the predicted observation calculated from the state prediction according to the observation equation. Finally, the state update is superimposed on the state prediction to obtain the current state estimate, and the updated error covariance matrix is ​​calculated using the Kalman filter gain and the prediction error covariance matrix.

[0054] In some embodiments of this application, the specific implementation steps of the extended Kalman filter algorithm are as follows. First, initialize the initial values ​​of the state variables and the error covariance matrix, typically by determining the initial attitude angles and zero-bias estimates based on the static calibration process of the inertial navigation system. Then, enter the main filtering loop, performing the following calculations sequentially within each filtering cycle: calculate the predicted state value at the current time using the state estimate and state transition matrix from the previous time step; calculate the prediction error covariance matrix using the error covariance matrix and process noise covariance matrix from the previous time step; calculate the innovation vector and observation matrix using the observation data and observation equation from the current time step; calculate the Kalman filter gain matrix; update the state estimate using the Kalman filter gain and innovation vector; update the error covariance matrix using the Kalman filter gain and prediction error covariance matrix. After completing the above calculations, return to the beginning of the main filtering loop to prepare for processing the observation data at the next time step.

[0055] Furthermore, the calculation of the Jacobian matrix in the extended Kalman filter algorithm is a crucial step. For the state equation, it is necessary to calculate the partial derivative matrix of the state transition matrix with respect to the state variables. For the observation equation, it is necessary to calculate the partial derivative matrix of the observation function with respect to the state variables. In tightly coupled UWB mode, the nonlinear characteristics of the observation function require recalculating the Jacobian matrix in each filtering cycle to ensure the accuracy of the linearization approximation.

[0056] S5: Output the updated state values ​​as position and / or velocity information required for UAV navigation.

[0057] Specifically, position and velocity components are extracted from the state estimation results output by the extended Kalman filter algorithm to serve as the UAV's navigation output information. The position information includes the UAV's three-dimensional coordinates in the ENU navigation coordinate system, and the velocity information includes the UAV's three-dimensional velocity vector in the ENU coordinate system. This navigation information can be used for applications such as UAV flight control, trajectory planning, and mission execution.

[0058] In some embodiments of this application, the navigation information output employs a multi-redundancy design to ensure the reliability of the navigation system. When any position sensor fails or malfunctions due to environmental interference, the system can automatically switch to a combined navigation mode consisting of another position sensor and an inertial navigation system to continue outputting information. For example, when the visual SLAM system fails to locate due to missing environmental features or changes in lighting, the system automatically switches to a combined mode of UWB positioning and inertial navigation; when the UWB positioning system malfunctions due to base station signal obstruction or multipath interference, the system automatically switches to a combined mode of visual SLAM and inertial navigation. This multi-redundancy design significantly improves the robustness and reliability of the combined navigation system in complex semi-enclosed spatial environments.

[0059] Furthermore, the mode switching judgment logic can be based on the following criteria: when the observation information of a certain position sensor exceeds a preset statistical threshold, the sensor data is determined to be unreliable, and its fusion weight is automatically reduced or the sensor data is completely excluded; when multiple position sensors are working normally, the system integrates the information of each sensor for fusion estimation; when only a single position sensor is available, the system operates in a combined mode of that sensor and the inertial navigation system; when all position sensors fail, the system degrades to a pure inertial navigation mode, at which point the effective working time of the inertial navigation is extended through online estimation and compensation of sensor zero bias.

[0060] Example 2 This embodiment provides a UAV integrated navigation system for a semi-enclosed space, including: The data acquisition module is used to acquire initial navigation data. This module integrates an inertial measurement unit, a heading sensor, and a position sensor. It can collect the UAV's acceleration, angular velocity, heading information, and three-dimensional position information in real time, and preprocess the acquired data (such as noise reduction and synchronization) to ensure the accuracy and consistency of the data, providing reliable raw data for subsequent modules. The selection of the inertial measurement unit, heading sensor, and position sensor is consistent with the above method embodiment, and can be selected according to the environmental characteristics of the semi-enclosed space. The module's hardware interface is adapted to the output format of each sensor, enabling rapid data acquisition and transmission.

[0061] The state output module is used to select the state variables required for navigation based on the initial navigation data and establish state equations describing the changes of these state variables over time. This module automatically selects state variables covering the UAV's attitude, position, velocity, and sensor biases using preset state variable selection rules. Based on Newton's laws of motion and attitude dynamics principles, it establishes state equations and performs preliminary optimization to ensure their accuracy and adaptability, providing support for the observation output module and the state update module.

[0062] The observation output module is used to establish observation equations corresponding to the state equations based on the 3D position information output by the position sensor. This module can automatically select the appropriate observation equation establishment method according to the type of position sensor (visual SLAM system or UWB positioning system). When the position sensor is a visual SLAM system, a linear observation equation is established; when the position sensor is a UWB positioning system, a loosely coupled or tightly coupled mode can be selected to establish the corresponding linear or nonlinear observation equations, ensuring the compatibility between the observation equations and the state equations and providing reliable observation constraints for the state update module.

[0063] The state update module utilizes the extended Kalman filter algorithm to fuse the state equation and observation equation to estimate and update state variables. This module integrates the extended Kalman filter algorithm, automatically linearizing and discretizing the state equation, calculating the Kalman filter gain, and using the information from the observation equation to correct the state prediction values, achieving accurate estimation and updating of state variables. Simultaneously, it corrects sensor bias, improving navigation accuracy.

[0064] The navigation command output module outputs updated state variables as position and / or velocity information required for UAV navigation. This module organizes and converts the state variables output by the state update module, transmitting them in real-time to the UAV flight control system. This provides decision-making support for UAV flight control, and the module can also store navigation data as needed to support navigation system optimization.

[0065] In addition, the system also integrates a sensor status detection module to detect the output status of the position sensors in real time. When any position sensor fails or malfunctions, a module switching mechanism is triggered to control the data acquisition module, observation output module, and status update module to switch to a combined navigation mode consisting of another position sensor and an inertial navigation system, ensuring the continuity and reliability of the navigation system.

[0066] Specific limitations regarding the UAV integrated navigation system in semi-enclosed spaces can be found in the limitations of the UAV integrated navigation method in semi-enclosed spaces described above; the corresponding technical effects are equivalent and will not be repeated here. Each module in the aforementioned UAV integrated navigation system in semi-enclosed spaces can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device, or stored in the memory of a computer device as software, so that the processor can call and execute the corresponding operations of each module.

[0067] Example 3 Figure 3 An internal structural diagram of a computer device is shown in one embodiment. This computer device may specifically be a terminal or a server. Figure 3 As shown, the computer device includes a processor, memory, network interface, display, camera, and input device connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface is used for communication with external terminals via a network connection. When the computer program is executed by the processor, it implements a method for integrated navigation of unmanned aerial vehicles (UAVs) in a semi-enclosed space. The display screen can be an LCD screen or an e-ink display screen. The input device can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device's casing, or an external keyboard, touchpad, or mouse.

[0068] As will be understood by those skilled in the art, computer equipment Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present invention and does not constitute a limitation on the computer device to which the present invention is applied. A specific computing device may include more or fewer components than those shown in the figure, or combine certain components, or have the same component arrangement.

[0069] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above.

[0070] In one embodiment, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-described method.

[0071] In summary, the UAV integrated navigation method, system, computer equipment, and storage medium provided in this application for a semi-enclosed space employ a multi-sensor collaborative fusion technology, combining an inertial measurement unit (IMU), a heading sensor, and a visual SLAM / UWB positioning system. This overcomes the limitations of single sensors. The IMU provides autonomous navigation capabilities, unaffected by environmental interference, while the visual SLAM and UWB positioning systems provide accurate position information, complementing each other and effectively solving the problem of low navigation accuracy caused by signal obstruction and high interference in semi-enclosed spaces. By rationally selecting navigation state variables, incorporating the UAV's attitude, position, velocity, and the bias values ​​of the IMU's gyroscope and accelerometer, the system comprehensively reflects the UAV's navigation state and sensor errors. Simultaneously, by establishing suitable state equations and observation equations, and combining them with an extended Kalman filter algorithm, accurate estimation and updating of state variables are achieved, effectively suppressing noise interference and sensor errors. To address the environmental characteristics of semi-enclosed spaces, multiple methods for establishing observation equations are provided. Depending on the type of position sensor and navigation requirements, linear or nonlinear observation equations can be selected. A sensor switching mechanism is also designed so that when any position sensor fails or malfunctions, it can quickly switch to a combined navigation mode consisting of another sensor and an inertial navigation system. This improves the redundancy, anti-interference capability, and reliability of the navigation system, ensuring that the navigation system can adapt to the complex and ever-changing environment within a semi-enclosed space.

[0072] The various embodiments in this specification are described in a progressive manner. For directly identical or similar parts of the embodiments, refer to each other. Each embodiment focuses on its differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments. It should be noted that the technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as the combination of these technical features does not contradict each other, it should be considered within the scope of this specification.

[0073] The above-described embodiments are merely preferred embodiments of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various improvements and substitutions without departing from the principles of the present invention, and these improvements and substitutions should also be considered within the scope of protection of the present invention. Therefore, the scope of protection of this invention should be determined by the scope of the claims.

Claims

1. A method for integrated navigation of unmanned aerial vehicles (UAVs) in a semi-enclosed space, characterized in that, include: Acquire initial navigation data, which includes UAV acceleration and angular velocity information acquired by the inertial measurement unit, heading information acquired by the heading sensor, and three-dimensional position information of the UAV in the navigation coordinate system acquired by the position sensor. Based on the initial navigation data, select the state variables required for navigation and establish state equations describing the changes of the state variables over time. Based on the three-dimensional position information output by the position sensor, an observation equation corresponding to the state equation is established; The extended Kalman filter algorithm is used to fuse the state equation and the observation equation to estimate and update the state variables; Output the updated state values ​​as the position and / or velocity information required for drone navigation.

2. The UAV integrated navigation method in a semi-enclosed space according to claim 1, characterized in that, The position sensor is at least one of a visual SLAM system or an ultra-wideband (UWB) positioning system; when the position sensor is a visual SLAM system, the established observation equation is a linear equation, and the observations are the position coordinates and heading angle output by the visual SLAM system.

3. The UAV integrated navigation method in a semi-enclosed space according to claim 1, characterized in that, The state variables required for navigation are selected as follows: In the formula, For state variables, This indicates the attitude information of the drone. This represents the three-dimensional position coordinates of the UAV in the ENU navigation coordinate system. This represents the velocity of the UAV in the ENU coordinate system. and These represent the bias values ​​of the gyroscope and accelerometer in the inertial navigation system, respectively.

4. The UAV integrated navigation method in a semi-enclosed space according to claim 2, characterized in that, When the position sensor is a UWB positioning system, the methods for establishing the observation equation include loosely coupled mode and tightly coupled mode; In the loosely coupled mode, the observation equation is a linear equation, and the corresponding observation is the position coordinates calculated by the UWB positioning system. In the tightly coupled mode, the observation equation is a nonlinear equation, and the corresponding observation is the raw distance information between the UAV tag and multiple fixed base stations in the UWB positioning system.

5. The UAV integrated navigation method in a semi-enclosed space according to claim 4, characterized in that, The process of state estimation using the extended Kalman filter algorithm includes: linearizing and discretizing the nonlinear state equation, calculating the Kalman filter gain, and correcting the state prediction value using the information output by the observation equation.

6. The UAV integrated navigation method in a semi-enclosed space according to claim 5, characterized in that, The process of using the extended Kalman filter algorithm to fuse the state equation and the observation equation includes linearizing and discretizing the nonlinear state equation.

7. The UAV integrated navigation method in a semi-enclosed space according to claim 1, characterized in that, When any position sensor fails or malfunctions due to environmental interference, the system switches to a combined navigation mode consisting of another position sensor and an inertial navigation system to output information.

8. A combined navigation system for unmanned aerial vehicles (UAVs) in a semi-enclosed space, characterized in that: include: The data acquisition module is used to acquire initial navigation data, which includes UAV acceleration and angular velocity information acquired by the inertial measurement unit, heading information acquired by the heading sensor, and three-dimensional position information of the UAV in the navigation coordinate system acquired by the position sensor. The state output module is used to select the state variables required for navigation based on the initial navigation data, and to establish state equations describing the changes of the state variables over time. The observation output module is used to establish an observation equation corresponding to the state equation based on the three-dimensional position information output by the position sensor. The state update module is used to estimate and update the state variables by fusing the state equation and the observation equation using the extended Kalman filter algorithm. The navigation command output module is used to output updated state quantities as position and / or speed information required for UAV navigation.

9. A computer device, characterized in that, include: Processor and computer-readable storage media; A processor, adapted to execute computer programs; A computer-readable storage medium storing a computer program, which, when executed by the processor, implements the UAV integrated navigation method in a semi-enclosed space as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program adapted to be loaded by a processor and executed as described in any one of claims 1 to 7, which is a method for integrated navigation of unmanned aerial vehicles in a semi-enclosed space.

Images