MEMS-based integrated navigation method and device

Abstract

The present application discloses a MEMS-based combined navigation method and device, relating to the field of inertial navigation technology. The MEMS-based combined navigation method includes: acquiring initial navigation data, target position, and obstacle data; performing time synchronization and standardization on the initial navigation data to obtain reference navigation data; preprocessing the reference navigation data according to a data preprocessing model to obtain target navigation data; performing data fusion and state estimation on the target navigation data to obtain a target state estimate; performing path planning based on the target position, the obstacle data, and the target state estimate to obtain a target path; and navigating based on the target state estimate and the target path. Through precise collaboration of multiple steps, the present application effectively solves the problem of decreased navigation accuracy of drones in complex environments due to sensor error accumulation or signal loss, thereby ensuring high navigation accuracy and stability.

Description

Technical Field

[0001] The present application relates to the field of inertial navigation technology, and in particular to a MEMS-based integrated navigation method and device. Background Art

[0002] With the rapid development of drone technology, the demand for drone applications in logistics and transportation, environmental monitoring, disaster relief, geographic mapping, and other fields is increasing. In complex environments, drone navigation and positioning accuracy requirements are becoming increasingly stringent. For example, when flying between high-rise buildings in cities, in forested areas, or underground or enclosed environments (such as tunnels and mines), conventional navigation systems face severe challenges. In particular, when the Global Positioning System (GPS) signal is lost or interfered with, the drone's positioning and navigation become unreliable, leading to mission failure. Therefore, improving the navigation accuracy of drones in various environments, especially ensuring accurate positioning when GPS signals are unavailable, is a pressing need in the current development of drone technology.

[0003] Current drone navigation technology mainly relies on inertial navigation systems (INS), global navigation satellite systems (GNSS) positioning systems, and other sensors. The inertial measurement unit (IMU) measures acceleration and angular velocity through accelerometers and gyroscopes to estimate the position, velocity, and attitude of the drone. The GNSS system provides global positioning information and is suitable for open areas, but the positioning accuracy will be affected when the signal is blocked or lost. LiDAR and millimeter-wave radar can be used for environmental perception and obstacle avoidance, especially in indoor or low-visibility environments. Wheel speed sensors can help ground drones (such as autonomous vehicles) obtain speed information for positioning in GPS-free environments.

[0004] Although existing practices can meet the basic navigation needs of drones under certain conditions, they still face some challenges in practical applications. First, the accumulation of sensor errors is a long-standing problem. In particular, the errors of IMU sensors will gradually accumulate during long-term flight, resulting in a decrease in the accuracy of position and attitude estimation. Secondly, GNSS signal loss or interference frequently occurs in environments such as urban high-rise buildings and tunnels, resulting in the inability to update positioning information in real time, thereby affecting the continuity and accuracy of navigation. In addition, dynamic changes in the environment (such as the appearance of obstacles, changes in wind speed, etc.) may also lead to inaccurate and unstable sensor data, making traditional multi-sensor fusion methods face greater challenges. Therefore, the decline in navigation accuracy of drones in complex environments due to sensor error accumulation or signal loss has become an urgent problem to be solved.

[0005] The above content is only used to assist in understanding the technical solution of this application and does not constitute an admission that the above content is prior art. Summary of the Invention

[0006] The purpose of this application is to provide a MEMS-based combined navigation method and device, aiming to solve the technical problem of reduced navigation accuracy of drones in complex environments due to sensor error accumulation or signal loss.

[0007] To achieve the above objectives, the present application proposes a MEMS-based integrated navigation method, which includes:

[0008] Obtain initial navigation data, target position, and obstacle data;

[0009] performing time synchronization and standardization on the initial navigation data to obtain reference navigation data;

[0010] Preprocessing the reference navigation data according to a data preprocessing model to obtain target navigation data;

[0011] performing data fusion and state estimation on the target navigation data to obtain a target state estimation;

[0012] Performing path planning based on the target position, the obstacle data, and the target state estimation to obtain a target path;

[0013] Navigation is performed based on the target state estimate and the target path.

[0014] In addition, to achieve the above objectives, the present application also proposes a MEMS-based integrated navigation device, the device comprising:

[0015] Data acquisition module, used to obtain initial navigation data, target position and obstacle data;

[0016] a standardization module, configured to perform time synchronization and standardization on the initial navigation data to obtain reference navigation data;

[0017] A preprocessing module, configured to preprocess the reference navigation data according to a data preprocessing model to obtain target navigation data;

[0018] A state estimation module is used to perform data fusion and state estimation on the target navigation data to obtain a target state estimation;

[0019] A path planning module, configured to perform path planning based on the target position, the obstacle data, and the target state estimation to obtain a target path;

[0020] A navigation module is used to navigate according to the target state estimation and the target path.

[0021] In addition, to achieve the above-mentioned purpose, the present application also proposes a MEMS-based combined navigation device, which includes: a memory, a processor, and a computer program stored on the memory and runnable on the processor, and the computer program is configured to implement the steps of the MEMS-based combined navigation method as described above.

[0022] In addition, to achieve the above-mentioned purpose, the present application also proposes a storage medium, which is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by the processor, the steps of the MEMS-based combined navigation method as described above are implemented.

[0023] In addition, to achieve the above-mentioned purpose, the present application also provides a computer program product, which includes a computer program. When the computer program is executed by a processor, it implements the steps of the MEMS-based combined navigation method as described above.

[0024] One or more technical solutions proposed in this application have at least the following technical effects:

[0025] First, the drone uses sensors to collect initial navigation data, including position, velocity, acceleration, and point cloud information, and acquires target location and obstacle data. This provides comprehensive environmental awareness for subsequent navigation, ensuring data accuracy and integrity. The system then synchronizes and standardizes this initial data to eliminate timing and dimensional differences between sensors, ensuring that data from different sources can be compared and processed on the same scale, reducing the impact of errors on subsequent steps. The reference navigation data is then preprocessed using a trained data preprocessing model to remove noise and extract valid information, thereby improving data quality and providing more accurate input for data fusion and navigation decision-making. The drone then performs data fusion on the target navigation data, integrating information from multiple sensors to estimate the state of the drone, thereby improving the robustness and accuracy of the system. The drone then calculates a safe and efficient path from its current position to the target, avoiding obstacles and incorporating real-time state estimation to ensure stability and safety during flight. Finally, based on the planned path, the drone's flight control system tracks the target path in real time, responding promptly to environmental changes and avoiding navigation errors caused by dynamic environmental changes or signal loss. Through precise collaboration of multiple steps, this application effectively solves the problem of reduced navigation accuracy of drones in complex environments due to accumulation of sensor errors or signal loss, ensuring high precision and stability of navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0027] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0028] Figure 1 A flow chart of the first embodiment of the MEMS-based combined navigation method provided in this application;

[0029] Figure 2 A flow chart of the second embodiment of the MEMS-based combined navigation method provided in this application;

[0030] Figure 3 A schematic diagram of the system architecture of the MEMS-based integrated navigation method provided in Example 2 of the present application;

[0031] Figure 4 This is a schematic diagram of the module structure of the MEMS-based integrated navigation device according to an embodiment of the present application;

[0032] Figure 5 Schematic diagram of the device structure of the hardware operating environment involved in the MEMS-based combined navigation method in the embodiment of the present application.

[0033] The purpose, features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0034] It should be understood that the specific embodiments described herein are merely used to explain the technical solutions of the present application and are not intended to limit the present application.

[0035] In order to better understand the technical solution of the present application, a detailed description will be given below in conjunction with the accompanying drawings and specific implementation methods.

[0036] With the rapid development of drone technology, its applications in logistics, monitoring, rescue, and mapping are becoming increasingly widespread, and the requirements for navigation and positioning accuracy are also increasing, especially in complex or signal-obstructed environments. Currently, drones primarily rely on inertial navigation systems, global navigation satellite systems, and other sensors such as lidar and millimeter-wave radar for positioning and obstacle avoidance. However, these technologies have limitations when faced with accumulated sensor errors, GNSS signal loss or interference, and dynamic environmental changes, affecting the reliability and accuracy of drones in various environments.

[0037] The main solution of this embodiment is that the drone uses multiple sensors to collect environmental data, including location, speed, and obstacle information, and performs time synchronization and standardization. After preprocessing using a preprocessing model, the multi-sensor data is fused to accurately estimate the drone's state. Subsequently, combining target location and obstacle information, the drone calculates a safe and efficient flight path and tracks the path in real time, ensuring flight stability and safety, effectively addressing navigation challenges even in dynamically changing environments.

[0038] It should be noted that the execution subject of the embodiments of this application can be a computing service device with data processing, network communication, and program execution functions, such as a tablet computer, personal computer, mobile phone, etc., or an electronic device or drone capable of performing the above functions. It can be a flying drone or a land drone. The following uses a flying drone as an example to describe this embodiment and the following embodiments.

[0039] Based on this, the embodiment of the present application provides a combined navigation method based on MEMS, referring to Figure 1 , Figure 1 This is a flow chart of the first embodiment of the MEMS-based combined navigation method of the present application.

[0040] In this embodiment, the MEMS-based integrated navigation method includes steps S10 to S60:

[0041] Step S10, obtaining initial navigation data, target location and obstacle data;

[0042] It should be noted that initial navigation data refers to the system's preliminary estimate of the drone's position, speed, and attitude (i.e., heading, pitch angle, and roll angle) when the drone is started or takes off. Specifically, these data usually include: (1) Current position: The drone's initial longitude, latitude, altitude, or position in other coordinate systems. Usually, before starting the flight, it is obtained through GNSS or preliminarily estimated through other sensors (such as lidar, IMU, etc.). (2) Initial speed: The drone's initial speed (such as ground speed and vertical speed, etc.), usually obtained through IMU or wheel speed sensor. (3) Initial attitude: The drone's initial attitude (such as heading, pitch, and roll angle), obtained through the accelerometer and gyroscope in the IMU, or estimated through other sensors.

[0043] The target location is the location that the drone is scheduled to reach, usually set by the flight mission or the user. The target location may be a specific geographic coordinate (such as longitude and latitude or ground coordinates), or a point in three-dimensional space (including altitude, position, etc.). The target location is usually closely related to the mission requirements, such as: (1) Mission objectives: For example, the drone needs to reach a certain location to perform tasks such as cargo delivery, environmental monitoring, and photography. (2) Path planning: The target location is also used as the final target point for path planning. The drone needs to plan an optimal path based on the current state, avoid obstacles, and make dynamic adjustments during flight. (3) In some cases, the target location can be dynamic. For example, when the drone performs a real-time monitoring mission, the target location may be adjusted as the real-time data changes.

[0044] Obstacle data refers to the location information and other relevant feature data of physical obstacles that a drone may encounter during flight. Obstacle data is usually obtained by environmental perception sensors (such as lidar, millimeter-wave radar, and visual sensors). These data may include: (1) Spatial position of obstacles: Obtain the relative position of obstacles through sensors such as lidar and millimeter-wave radar, usually expressed as the coordinates of the obstacle (such as X, Y, and Z coordinates) and displayed in the form of a map or point cloud. (2) Size and shape of obstacles: Some sensors (such as lidar) not only provide the location data of obstacles, but can also obtain information such as the shape and size of obstacles through scanning, which is crucial for obstacle avoidance algorithms. (3) Dynamic characteristics of obstacles: If there are moving obstacles in the environment (such as other flying objects, pedestrians, vehicles, etc.), the sensor must also obtain the speed and direction of these obstacles in real time to perform dynamic obstacle avoidance.

[0045] It is understandable that, first, the drone collects data such as acceleration, angular velocity, geographic location (latitude, longitude, altitude) through sensors such as inertial measurement units and GNSS receivers. These data are transmitted to the drone's navigation system through the initialization process and serve as a starting point to provide a reference for navigation and control of subsequent flights. Secondly, based on mission requirements, the drone receives or reads the target location through the flight planning system, which is usually a specific geographic coordinate or a designated target area. The flight planning system converts the target location into a coordinate form that the drone can understand and uses it for subsequent path planning and navigation decisions. Finally, the drone scans the surrounding environment through lidar, millimeter-wave radar or visual sensors to obtain the spatial position, shape and other characteristics of obstacles, such as distance and speed, in real time. After processing, these data form an obstacle map for the flight control system to update the flight path in real time to avoid collisions with obstacles.

[0046] As an example, the method is applied to a drone, which is equipped with an IMU, a GNSS receiver, a lidar, a millimeter-wave radar, and a wheel speed sensor. The step of obtaining initial navigation data includes: obtaining acceleration data and angular velocity data through the IMU; obtaining satellite positioning data through the GNSS receiver; obtaining point cloud data through the lidar; obtaining distance-speed data through the millimeter-wave radar; obtaining wheel speed data through the wheel speed sensor; and obtaining initial navigation data based on the acceleration data, the angular velocity data, the satellite positioning data, the point cloud data, the distance-speed data, and the wheel speed data.

[0047] An IMU is a sensor that measures a drone's motion and attitude changes by integrating an accelerometer and gyroscope. The accelerometer measures acceleration and can sense the drone's acceleration and deceleration in space; the gyroscope measures angular velocity and provides a sense of how the drone is rotating. The main function of the IMU is to provide real-time motion status data for the drone, especially to help estimate position, velocity, and attitude when the GNSS signal is lost. Acceleration data refers to the acceleration information measured by the accelerometer in the IMU and is typically used to calculate the speed and position changes of the drone. Angular velocity data refers to the angular velocity information measured by the gyroscope in the IMU and is typically used to calculate the drone's attitude changes, such as heading, pitch, and roll angles.

[0048] A GNSS receiver is a device that calculates a drone's geographic location by receiving signals from satellites. It uses satellite positioning systems (such as GPS) to provide global positioning data, accurately determining a drone's longitude, latitude, and altitude above the Earth's surface. GNSS receivers are typically used to provide baseline location data, especially in open environments, where GNSS signals can effectively help drones determine their precise location. Satellite positioning data refers to the longitude, latitude, and altitude data provided by a GNSS receiver, helping to determine a drone's real-time position and speed.

[0049] LiDAR is a sensor that scans the surrounding environment using laser pulses and measures the reflection time to construct a three-dimensional model of the environment. LiDAR generates highly accurate point cloud data, which describes the spatial position and shape of objects around the drone. This data is commonly used for environmental modeling, obstacle detection, and avoidance. Point cloud data refers to the set of three-dimensional coordinate points formed by the reflected laser signals captured by the LiDAR, describing the specific position and shape of objects or obstacles in the environment.

[0050] Millimeter-wave radar is a sensor that detects objects by emitting millimeter-wave signals and receiving their reflected signals. It can penetrate adverse weather conditions such as fog, rain, and snow to detect the distance and relative speed of target objects. Millimeter-wave radar is commonly used to detect moving targets, monitor the surrounding environment, and assist in obstacle avoidance. Distance-velocity data is calculated by millimeter-wave radar by measuring the reflected waves from the target object. This data is used to estimate the spatial relationship between the drone and other objects, assisting in dynamic obstacle avoidance.

[0051] Wheel speed sensors are installed on a drone's ground vehicle (such as an autonomous vehicle) and estimate the vehicle's ground speed by measuring tire rotation. They provide accurate motion speed information, particularly when the drone is in ground mode (such as during taxiing, takeoff, or landing), complementing data from other sensors. Wheel speed data, obtained by the wheel speed sensor, is used to estimate the drone's ground speed, providing a speed reference, especially when GPS signals are lost.

[0052] First, the drone uses an IMU to acquire acceleration and angular velocity data. The accelerometer measures acceleration changes and calculates velocity through integration, while the gyroscope provides angular velocity information to help estimate the drone's attitude changes. Second, a GNSS receiver acquires satellite positioning data. GNSS provides precise geographic location (latitude, longitude, and altitude) to determine the drone's current position and velocity, providing high-precision positioning support, particularly in open environments. LiDAR then acquires point cloud data. LiDAR scans the environment using laser pulses and measures reflection time to generate the three-dimensional spatial coordinates of surrounding objects, helping to identify obstacles and build an environmental model. Next, the drone uses millimeter-wave radar to acquire distance-velocity data. Millimeter-wave radar measures the relative distance and velocity of target objects by transmitting and receiving reflected waves, making it suitable for dynamic target detection and obstacle avoidance. Finally, wheel speed sensors estimate the drone's ground speed by measuring wheel rotational speed, especially in ground mode, complementing the data from other sensors. By combining these data from IMU, GNSS, lidar, millimeter-wave radar and wheel speed sensors, the drone's initial navigation data, including position, speed, attitude and other information, is obtained, providing accurate basic data for subsequent flight control and path planning.

[0053] Step S20, performing time synchronization and standardization on the initial navigation data to obtain reference navigation data;

[0054] It should be noted that time synchronization refers to aligning data obtained from different sensors (such as IMU, GNSS, LiDAR, millimeter-wave radar, and wheel speed sensors) to the same time base. Since these sensors have different operating frequencies and sampling times, the data they obtain may have time differences, resulting in inconsistent data or inability to directly fuse them. Standardization refers to converting data from different sensors into a unified scale or unit so that the data can be processed and compared within the same framework. Since the dimensions and measurement ranges of different sensors may vary greatly, for example, the units of acceleration data and angular velocity data output by IMU are usually m / s. 2 and rad / s, while the positioning data output by GNSS is in units of latitude, longitude and altitude. Reference navigation data refers to a set of data that has been time-synchronized and standardized. It represents the comprehensive navigation status of the drone at a specific time point or time period. Reference navigation data usually includes the following: (1) Position: The comprehensive position estimate after the fusion of GNSS, IMU and lidar data. (2) Speed: Speed ​​information calculated through data such as IMU and wheel speed sensors, usually including ground speed and vertical speed. (3) Attitude: Attitude information such as heading, pitch and roll angle obtained by combining IMU and sensor data.

[0055] It is understandable that first, the drone will time-synchronize the data obtained by each sensor based on the timestamp of each sensor. Since the sampling frequencies of IMU, GNSS, lidar, millimeter-wave radar and wheel speed sensors are different, the time points of data collection may be inconsistent, so the data of all sensors need to be adjusted to the same time point. Usually, interpolation methods are used to supplement the data of sensors with less sampling, or the data with the most recent timestamp is used directly to ensure that each data point reflects the status at the same time point. This process can eliminate the data deviation caused by time difference, making subsequent data fusion more accurate. Next, standardization is performed. Since the data output by different sensors have different units and dimensions, the standardization step is to convert them into a unified scale. For example, the acceleration of the IMU (m / s 2 ) into units compatible with the location data (latitude, longitude, altitude), or normalization is applied to all data so that their values ​​fall within the same range (such as 0 to 1). The purpose of this standardization is to avoid excessive influence of certain sensor data during the fusion process due to inconsistent scales, thereby improving the stability and accuracy of data fusion. Finally, a set of sensor data including time alignment and standardization is obtained, namely reference navigation data, which comprehensively reflects the real-time position, speed, attitude and other information of the UAV, and serves as the basis for subsequent navigation, path planning and control decisions. After time synchronization and standardization, the accuracy and consistency of the data are improved, providing a reliable guarantee for the navigation accuracy of the UAV.

[0056] As an example, the steps of time synchronizing and standardizing the initial navigation data to obtain reference navigation data include: using the timestamp of each data sampling point in the IMU as a reference time source; linearly interpolating the satellite positioning data, the distance-speed data, and the wheel speed data according to the reference time source to obtain first data; searching from the reference time source for the IMU timestamp with the smallest time difference with the point cloud frame in the point cloud data; replacing the timestamp of the point cloud frame in the point cloud data with the IMU timestamp to obtain second data; and standardizing the acceleration data, the angular velocity data, the first data, and the second data to obtain reference navigation data.

[0057] A data sampling point refers to the measurement data acquired by the sensor at each point in time. These sampling points include acceleration, angular velocity, position information, LiDAR point cloud data, etc. They are individual data units continuously recorded by the sensor at a certain frequency. Each sampling point typically contains the sensor's measurement value at a certain moment. A timestamp is a timestamp for each data sampling point or data frame, indicating the specific moment the data was collected. Timestamps are typically recorded based on system time or UTC time and are used to identify the position of sensor data on the timeline. Through timestamps, data from different sensors can be synchronized to the same time base, facilitating subsequent data fusion. A reference time source refers to a selected primary time reference point or sensor. The timestamp of each data sampling point in the IMU is usually selected as the reference time source. Because the IMU has a higher sampling frequency and can generally provide a more accurate time reference, its timestamp is used as the time standard for synchronizing data from other sensors.

[0058] Linear interpolation is a mathematical method used to estimate the value between known data points. Specifically, if there is a time interval between the data sampling points of certain sensors, and these data are not strictly synchronized, linear interpolation can be used to fill the missing time point data. The interpolation process is based on the linear relationship between two adjacent known data points to infer the value of the missing data. In this way, "intermediate" data under the reference time source can be obtained. The first data refers to the data obtained by linearly interpolating the satellite positioning data, distance-speed data and wheel speed data. These data are interpolated based on the reference time source of the IMU, so that these data points are synchronized with the time of the IMU, ensuring that they are in the same time reference system and can be fused with other sensor data.

[0059] A point cloud frame is a collection of spatial point cloud data acquired by a LiDAR sensor over a specific period of time. Each point cloud frame contains a set of 3D point data scanned and reflected by the LiDAR sensor at a specific moment, representing the spatial layout of the surrounding environment. In a multi-sensor system, these point cloud frames need to be time-aligned with other sensor data to facilitate data fusion. The time difference is the difference between two timestamps, typically representing the time interval between two data points. In this step, the time difference is the difference between the IMU timestamp and the timestamp of a point cloud frame in the point cloud data. By calculating the time difference, the closest time point can be found for data synchronization. The IMU timestamp is the timestamp recorded by the IMU sensor at each sampling point. It typically has a high sampling frequency and provides a precise time reference. In this step, the IMU timestamp serves as the reference point for time synchronization. Data from other sensors, such as GNSS and LiDAR, is aligned with the IMU timestamp to ensure data temporal consistency. The second data is the data obtained during the time synchronization process by finding the IMU timestamp closest to the timestamp of the point cloud frame in the point cloud data and replacing it with the timestamp in the point cloud data. Specifically, the timestamp in the point cloud data will be replaced by the IMU timestamp, so that the point cloud data and the IMU data are aligned in time, thereby ensuring the accuracy of data fusion.

[0060] First, the timestamp of each data sampling point in the IMU is selected as the reference time source. Because the IMU has a high sampling frequency and strong time accuracy, the timestamp it provides can serve as a reference time point for other sensor data. This ensures that all sensor data are aligned to the same time base, avoiding fusion errors caused by time differences. Second, linear interpolation is performed on the satellite positioning data, range-velocity data, and wheel speed data based on the selected reference time source. Specifically, an interpolation algorithm is used to calculate the estimated value of each sensor data at the IMU timestamp based on the difference between the timestamp of each sensor data and the IMU timestamp, thereby obtaining the first data synchronized with the IMU time. This step ensures the temporal consistency of data from different sensors, thereby avoiding error propagation caused by time asynchrony. Then, the IMU timestamp with the smallest time difference from the timestamp of each point cloud frame in the lidar point cloud data is searched from the reference time source (i.e., the IMU timestamp). This method can find the closest IMU timestamp, ensuring that the lidar data is accurately synchronized with the IMU data. Afterwards, the timestamp of the point cloud frame in the lidar point cloud data is replaced with the closest IMU timestamp to obtain the second data. This operation ensures that the time information of the lidar is completely aligned with the time information of the IMU, avoiding navigation errors caused by time inconsistency. Finally, the acceleration data, angular velocity data, first data and second data in the IMU are standardized. The standardization operation involves converting these data into a unified range and unit. The purpose of this is to eliminate the dimensional differences between different sensor data so that they can be processed on the same scale, ensuring that multi-sensor data contributes equally to navigation accuracy when fused. Finally, after these steps, the reference navigation data obtained contains the synchronized time and standardized data of all sensors, providing an accurate and consistent basis for subsequent navigation and path planning.

[0061] As an example, the step of standardizing the acceleration data, the angular velocity data, the first data and the second data to obtain reference navigation data includes: calculating the mean and standard deviation of the acceleration data, the angular velocity data, the first data and the second data; calculating third data based on the mean, the standard deviation, the acceleration data, the angular velocity data, the first data and the second data; and performing a two-dimensional conversion on the third data to obtain reference navigation data.

[0062] The mean refers to the average value in a set of data. Specifically, for a set of acceleration data, angular velocity data, first data (interpolated sensor data), and second data (synchronized lidar data), the mean is the sum of all data points divided by the number of data points, reflecting the central tendency of these data within a certain time period. For example, the mean of acceleration data is the sum of the values ​​of all acceleration data sampling points divided by the total number of data points, resulting in a value reflecting the average level of acceleration. The standard deviation is a statistic that describes the degree of dispersion of data and indicates the degree of difference between the data and the mean. Specifically, it reflects the degree of deviation of each data point in the data set from the mean. The larger the standard deviation, the greater the volatility of the data. In this step, the standard deviation of the acceleration data, angular velocity data, first data, and second data is calculated to obtain the dispersion of these data. This helps convert the data into a unified dimension and avoid the impact caused by dimensional differences.

[0063] The third data refers to data obtained by normalizing the acceleration data, angular velocity data, first data, and second data to zero mean and unit variance. The normalization process involves subtracting the mean of each data point from the group's data and dividing it by the standard deviation, thereby transforming the data into a standard normal distribution with a mean of 0 and a standard deviation of 1. This allows data of different units and dimensions to be compared and processed on the same scale. The third data is the standardized data set, ensuring that all sensor data is used with equal weight. Two-dimensional transformation is the process of converting the standardized data from its original one-dimensional form to a two-dimensional form. Typically, two-dimensional transformation involves converting each sensor's data (such as acceleration and angular velocity) into values ​​in a two-dimensional coordinate system according to certain rules or mapping relationships. For example, this involves mapping acceleration and angular velocity data onto two-dimensional coordinate axes, or merging multiple data channels into two dimensions for subsequent processing in the navigation system. This transformation makes the data more suitable for calculations such as navigation and path planning, facilitating further analysis and decision-making.

[0064] First, the drone calculates the mean and standard deviation of the acceleration data, angular velocity data, first data, and second data. The mean is obtained by summing all sample values ​​in each data set and dividing by the number of data points. Next, the standard deviation is calculated by squaring the difference between each data point and the mean, summing the square root, and dividing by the number of data points. This quantifies the data's fluctuation range. The purpose of calculating the mean and standard deviation is to facilitate subsequent standardization, ensuring that data from different sensors are compared at the same scale. The calculated mean and standard deviation are then used to standardize the acceleration data, angular velocity data, first data, and second data. Specifically, the robot subtracts the mean of each data point from the data set and then divides by the standard deviation, ensuring that the mean of each data set is 0 and the standard deviation is 1. This step eliminates differences in dimensions and units between different data sources, preventing bias in the subsequent fusion of these data due to inconsistent scales. Finally, the drone will perform a two-dimensional transformation on the standardized data (i.e., the third data), usually mapping multiple data dimensions (such as acceleration, angular velocity, position, etc.) into a two-dimensional coordinate system. This transformation facilitates subsequent preprocessing. In this embodiment, zero mean unit variance normalization is used, that is:

[0065]

[0066] Where X is the original data (such as acceleration, angular velocity, first data, second data), μ X is the mean of the data X, σ X is the standard deviation of the data X, and X′ is the standardized data.

[0067] As an example, the step of performing a two-dimensional conversion on the third data to obtain reference navigation data includes: dividing the acceleration data and angular velocity data in the third data into acceleration matrix data and angular velocity matrix data according to a time window; constructing position-velocity matrix data based on the position and velocity in the satellite positioning data in the third data; converting the point cloud data and distance-velocity data in the third data into a two-dimensional depth map; and obtaining reference navigation data based on the acceleration matrix data, the angular velocity matrix data, the position-velocity matrix data, the two-dimensional depth map, and the wheel speed data in the third data.

[0068] Time windowing refers to dividing continuous time series data into multiple segments according to a certain time range (window size). In this embodiment, time windows are used to divide acceleration data and angular velocity data. This division is usually performed according to a certain time interval (for example, per second or per fixed number of frames), dividing the entire data stream into multiple small subsets, each containing data within a certain time period. In this way, the data can be locally processed or analyzed, facilitating subsequent matrix operations.

[0069] Acceleration matrix data is obtained by dividing acceleration data into time windows and converting it into a matrix. Within each time window, the acceleration values ​​at each moment in that time period are stored. Each time window corresponds to a matrix row (or column), and the elements in each row (column) represent the acceleration values ​​in different dimensions (usually the X, Y, and Z axes). Through matrix processing, acceleration data can be more conveniently used for subsequent mathematical calculations, feature extraction, and fusion. Angular velocity matrix data is obtained by dividing angular velocity data into time windows and converting it into a matrix, similar to acceleration matrix data. Each time window corresponds to a matrix row, which stores angular velocity data (usually rotational velocity around the X, Y, and Z axes) at different time points within that time window. This matrixing method converts time series data into a two-dimensional structure, facilitating optimization processing.

[0070] Position refers to the drone's coordinate position in three-dimensional space at a given moment, typically provided by GNSS. Position data, including X, Y, and Z coordinates, indicates the drone's specific position relative to the Earth's reference frame. Position data is crucial information in navigation systems, helping drones understand their current spatial location. Speed ​​refers to the drone's speed relative to the ground or a reference frame at a given moment, typically provided by GNSS or wheel speed sensors. Speed ​​data includes velocity components in the X, Y, and Z directions, describing the drone's rate and direction of motion. Combining speed and position data, the drone's trajectory can be inferred.

[0071] Position-velocity matrix data is the integration of position and velocity data, divided into matrix form according to time windows. Within each time window, the matrix contains both the drone's position (typically X, Y, Z coordinates) and velocity (X, Y, Z components) at that moment. This facilitates subsequent navigation algorithm processing and state estimation. In particular, when fusing multi-sensor data, combining position and velocity data can more accurately describe the drone's motion state. A two-dimensional depth map is a combination of lidar and millimeter-wave radar point cloud data with distance-velocity data, converting them into a two-dimensional image or matrix. Lidar and millimeter-wave radar provide three-dimensional point cloud data, representing depth information about the drone's surroundings. By projecting this three-dimensional data onto a two-dimensional plane, a depth map is generated, where each pixel represents the depth value of a spatial location (distance from the sensor).

[0072] First, the drone divides the data into multiple segments at preset time intervals (e.g., every second or every fixed number of sampling points). Each segment contains the acceleration or angular velocity values ​​within that time window, and these values ​​are organized into a two-dimensional matrix. This process converts the original time series data into a matrix structure, making it more suitable for subsequent data preprocessing models, thereby facilitating feature extraction and pattern recognition. Next, the positioning information (X, Y, and Z coordinates of position and X, Y, and Z components of velocity) for each time window is extracted and arranged in chronological order to form a matrix containing data from multiple time windows. This process helps the drone unify the position and velocity information into a matrix format, facilitating subsequent fusion and processing. This allows for more efficient utilization of this dynamically changing data, particularly when performing path prediction and state estimation. Then, for both point cloud and range-velocity data, the drone uses a projection algorithm to map the 3D point cloud data originally acquired from lidar and millimeter-wave radar onto a 2D plane, obtaining depth or distance information for each pixel representing a specific location in space. This conversion simplifies the originally complex 3D data into a more manageable 2D format. Finally, the drone integrates all processed data—the acceleration matrix, angular velocity matrix, position-velocity matrix, 2D depth map, and wheel speed data—to form reference navigation data. This step fuses the data from various sensors into a unified format, making it easier for subsequent data preprocessing models to perform preprocessing.

[0073] Step S30, preprocessing the reference navigation data according to a data preprocessing model to obtain target navigation data;

[0074] It should be noted that the data preprocessing model refers to a trained Convolutional Neural Network (CNN) model, whose task is to detect and remove noise from the input reference navigation data. Specifically, this CNN model has been trained on a large amount of training data and can automatically identify the noise signals contained in the data. It processes these signals through different convolutional layers, pooling layers and other mechanisms to reduce the impact of noise on subsequent processing. The training process uses an annotated reference navigation data set and optimizes the network parameters so that the model can effectively identify which are useless noise data, thereby achieving automatic noise removal. When this trained CNN model is applied to the reference navigation data, it analyzes the data and removes noise through the learned features, ultimately producing purer input data.

[0075] Target navigation data refers to navigation data that has been processed by a data preprocessing model to remove noise and enhance important information. This data serves as input to the drone's subsequent navigation algorithms. Target navigation data typically includes real-time status information about the drone, such as its precise position, speed, and attitude, as well as environmental perception information relevant to navigation. After being denoised by the preprocessing model, target navigation data is more accurate and has a higher signal-to-noise ratio, enabling the drone to perform tasks such as path planning, positioning, and obstacle avoidance based on more reliable data.

[0076] As you can understand, the data preprocessing model is first trained using a large amount of labeled historical navigation data to learn how to identify and remove noise from the data. During training, the historical reference navigation dataset is first fed into the CNN model. Each dataset contains noisy acceleration, angular velocity, position, velocity, point cloud, and other information, all labeled to distinguish between noise and valid data. The CNN model automatically extracts features from the data through multiple convolutional layers, learning the noise patterns layer by layer. Backpropagation is then used to optimize network weights, enabling the model to effectively identify and filter out noise signatures. After training, the model is able to automatically identify and remove noise from new reference navigation data, while retaining valid information. This processed data, known as target navigation data, contains more accurate and cleaner drone navigation information for subsequent path planning and navigation control.

[0077] As an example, the data preprocessing model includes a convolution layer, a pooling layer and a fully connected layer; the step of preprocessing the reference navigation data according to the data preprocessing model to obtain target navigation data includes: smoothing and extracting features of the reference navigation data through a convolution layer to obtain a convolution feature map; downsampling the convolution feature map through a pooling layer to obtain a downsampled feature map; and globally combining the downsampled feature map through a fully connected layer to obtain target navigation data.

[0078] The convolution layer is a basic component in convolutional neural networks. Its function is to perform local perception and feature extraction on the input data. In this step, the convolution layer scans the input reference navigation data (such as acceleration, angular velocity, position, etc.) through a convolution operation, and gradually extracts local features in the data through multiple convolution kernels (also called filters). Each convolution kernel performs a sliding window operation on the input data, calculates the weighted sum of the local area, and thus extracts the features of the corresponding area (such as the pattern of acceleration change, the trend of speed change, etc.). These features can help the model identify key information in the data, especially in extracting valuable patterns in complex data. The main operation of the convolution layer is the convolution operation. The convolution kernel W processes the input data X and generates an output feature map Y:

[0079] Y=W*X+b

[0080] Where Y is the feature map output by the convolutional layer (convolution feature map), W is the convolution kernel or filter, which is usually of size k×k (e.g. 3×3), X is the input data (e.g. reference navigation data), which is of size n×n, b is the bias term, and * indicates the convolution operation.

[0081] The pooling layer is used to downsample the convolution feature map in order to reduce the data dimension and computational complexity while retaining important feature information. The pooling layer usually uses maximum pooling or average pooling to downsample the features of the local area. For example, maximum pooling selects the maximum value within each pooling window, while average pooling takes the average of all values ​​within the window. This operation helps reduce the model's sensitivity to position and enhances the model's robustness, especially in the face of noise or local data changes. The pooling layer can effectively retain the most important feature information. The operation of the pooling layer is usually maximum pooling or average pooling. Assuming that maximum pooling is used and the pooling window size is p×p (for example, 2×2), the output Z after pooling is:

[0082]

[0083] Among them, Z i,j is the output after the pooling operation, Y m,n It is an element in the convolution feature map, and max means taking the maximum value in the pooling window.

[0084] The fully connected layer is the last layer in CNN, and its function is to perform high-dimensional mapping and global combination of the feature maps output by the pooling layer. Each neuron in each fully connected layer is connected to all neurons in the previous layer to integrate all local features extracted by the previous layer and perform global information processing. Through the fully connected layer, the model can integrate local features into global features to help make decisions or classifications. In this step, the fully connected layer converts the pooled feature map into target navigation data, that is, the complete navigation information after denoising. The fully connected layer is a standard layer in the neural network. The input is the feature map Z obtained after pooling, and the output is the target navigation data O. Assume that the weight of the fully connected layer is W f , bias is b f , then the output O can be expressed as:

[0085] O=W f ·Z+b f

[0086] O is the output of the fully connected layer (target navigation data), Z is the downsampled feature map after pooling, the size is m×m (output after the pooling layer), W f is the weight matrix of the fully connected layer, size is k×m, b f is a bias term, and · represents matrix multiplication.

[0087] Smoothing is achieved through convolution operations, which smooths data in time or space, reducing the impact of mutations or noise. For example, applying a convolution kernel to the input data and performing a sliding weighted average can reduce the impact of noise or mutation points, thereby improving data stability and accuracy. Feature extraction refers to processing input data through a convolutional layer to automatically learn and extract useful features. In this step, the convolutional layer scans the raw navigation data (such as acceleration and angular velocity) through local perception and automatically learns the patterns or regularities of this data. These features can describe the key information in the input data and assist in subsequent navigation decisions.

[0088] The convolutional feature map is the result of convolutional layer processing. It presents the characteristic responses of each local area in the input data. Each convolution kernel generates a feature map that displays the characteristics of the area it scans. The convolutional feature map can help the model identify patterns in the input data, such as acceleration changes, velocity trends, and other important information. Downsampling is an operation performed by the pooling layer. Its purpose is to reduce the amount of computation by reducing the resolution of the data. The downsampling operation is usually implemented by taking the maximum or average value within the pooling window. Through this process, the spatial resolution of the data is reduced and the feature dimension is reduced, while still maintaining the key information of the data.

[0089] The downsampled feature map is the result of downsampling the convolutional feature map by the pooling layer. Compared with the convolutional feature map, the downsampled feature map has a lower resolution, but it retains the most significant features in the data. The downsampled feature map reduces the amount of computation and can effectively improve the model's robustness in the face of noise or position changes. Global combination is the operation of the fully connected layer, which integrates the local information in the pooled feature map into a global feature representation. The fully connected layer generates the target data by weighted summing the output of each layer. In this step, the model integrates the previous local features to obtain globally meaningful navigation data, such as clear, denoised acceleration, velocity, and position information. This globally combined feature data becomes the target navigation data and is passed to the subsequent navigation decision system.

[0090] First, the drone uses the convolutional layer in the data preprocessing model to smooth and extract features from the reference navigation data. Specifically, the convolutional layer uses multiple convolution kernels to scan the reference navigation data (such as acceleration, angular velocity, and speed) and extract local features from the data. Each convolution kernel performs a weighted convolution on the data, capturing pattern variations in the data, such as acceleration fluctuations and velocity changes. These features help remove noise and enhance effective information. This processing smoothes the data and removes some localized abnormal fluctuations, thereby improving data quality. Next, the drone uses the pooling layer in the data preprocessing model to downsample the convolutional feature map. The pooling layer uses methods such as max pooling or average pooling to select the maximum or average value in each pooling window, reducing the spatial resolution of the data. This approach aims to reduce computational complexity while retaining the most important and significant components of the feature map and removing unimportant or redundant information, thereby improving the efficiency and stability of subsequent processing. Finally, the drone uses the fully connected layer in the data preprocessing model to globally combine the downsampled feature maps. The fully connected layer performs a weighted summation of all local information in the pooled downsampled feature map, integrates it into a global feature, and outputs the final target navigation data. This step, by converting local features into global features, ensures that key information in the data is preserved while providing accurate, denoised input for subsequent navigation decisions.

[0091] Step S40, performing data fusion and state estimation on the target navigation data to obtain a target state estimation;

[0092] It should be noted that data fusion refers to the integration of data acquired from different sensors (such as IMU, GNSS, LiDAR, millimeter-wave radar, etc.) to provide a more accurate and comprehensive state estimate. In a drone, multiple sensors can simultaneously perceive the same environment, but their measurement accuracy and response speed vary, and they may contain certain noise and errors. The purpose of data fusion is to leverage the strengths of different sensors, integrate data from multiple sources, and use mathematical models to reduce the impact of single sensor errors, thereby obtaining more reliable and accurate information. For example, the acceleration and angular velocity data provided by the IMU can be fused with GNSS positioning data to obtain more stable track information.

[0093] State estimation is the process of processing multi-sensor data to estimate the actual state of the drone (such as position, speed, attitude, etc.). Due to sensor errors, external interference, and other factors, the raw data read directly from the sensor is usually not completely accurate. Therefore, state estimation uses mathematical methods (such as filtering algorithms) to process sensor data to estimate the true state of the drone. In this embodiment, state estimation is achieved through adaptive Kalman filtering, the purpose of which is to infer the optimal state of the drone based on the sensor observations and the dynamic model of the system.

[0094] Target state estimation refers to the final state information of the drone obtained through data fusion and state estimation algorithms (such as adaptive Kalman filtering). This state information typically includes position (such as GPS coordinates), velocity (such as velocity vector), attitude (such as roll angle, pitch angle, yaw angle), etc. This information is crucial for the drone's navigation, path planning, and control. The key to target state estimation is to continuously update and correct sensor data to obtain real-time and accurate state information, providing a basis for subsequent navigation decisions. The final target state estimate is the result of combining all sensor inputs and dynamically optimizing them through algorithms such as adaptive Kalman filtering. It can effectively reduce noise and improve the accuracy of state estimation.

[0095] It is understood that first, the drone's target navigation data, processed according to the data preprocessing model, is integrated through data fusion methods. Specifically, this step utilizes algorithms such as Kalman filtering (or extended Kalman filtering / adaptive Kalman filtering) to weight the observation data from multiple sensors, taking into account the accuracy and reliability of each sensor's data, thereby reducing the impact of single sensor errors. Then, based on the fused data, a state estimation algorithm is used to estimate the drone's current state, including key parameters such as its position, velocity, and attitude. During the state estimation process, the algorithm matches the sensor data with the drone's dynamic model, correcting for any measurement errors and accurately calculating the drone's actual state. Finally, after data fusion and state estimation, the target state estimate is obtained, namely, the optimal drone state information such as position, velocity, and attitude, providing an accurate basis for subsequent navigation decisions and path planning.

[0096] Step S50, performing path planning based on the target position, the obstacle data, and the target state estimation to obtain a target path;

[0097] It should be noted that path planning involves calculating the optimal route from the drone's current state to the target location based on the drone's target location, obstacle data in the surrounding environment, and the current estimated target state. This process requires not only finding a feasible path but also considering multiple factors, such as avoiding obstacles, avoiding unsafe areas during flight, and meeting speed and flight time requirements. The goal of path planning is to ensure that the drone can safely and efficiently reach its target location from its starting point in a dynamic or complex environment. The target path is the optimal flight route calculated by the path planning algorithm. It is the path that the drone must follow to get from its current location to the target location. The target path must not only avoid obstacles but also meet the drone's performance constraints (such as speed and altitude) and safety requirements. The accuracy of the target path directly affects the drone's flight stability and mission completion efficiency. This is especially true in complex or dynamic environments, where the target path is constantly adjusted and optimized as the environment changes. Therefore, the target path consists of a series of calculated waypoints (position coordinates, speed, attitude, etc.). After path planning and optimization, it is usually the shortest, safest flight path that meets the mission requirements.

[0098] As you can understand, the drone first determines the inputs for path planning by receiving target location, obstacle data, and target state estimates. The target location is the drone's final flight destination, while the target state estimate includes the drone's current position, speed, attitude, and other information. This information helps the drone understand its current location and flight capabilities. Obstacle data comes from sensors such as lidar and millimeter-wave radar, which characterize dynamic and static obstacles in the flight environment. Based on this information, the drone uses a path planning algorithm (such as the A* algorithm) to calculate the optimal path. First, the path planning algorithm determines a general flight direction based on the target location and current state estimate. Next, the algorithm takes into account the distribution and location of obstacles in real time to avoid collisions with them. It further optimizes the path based on the drone's flight constraints (such as maximum speed and turning radius). Finally, through the optimization process, an optimal flight path is calculated from the current state to the target location. This path ensures that the drone can complete its mission safely and quickly, avoiding obstacles and other potential hazards during flight. This calculated path is the target path, which the drone uses for navigation and flight control.

[0099] As an example, the target state estimation includes a current position; the path planning is performed based on the target position, the obstacle data and the target state estimation to obtain a target path, including: obtaining a reachable area based on the obstacle data; taking the current position as an initial node and adding the initial node to a node set; randomly sampling in the reachable area to obtain a sampling point; selecting the nearest node with the smallest distance to the sampling point from the node set; generating a new node in the reachable area based on the direction between the sampling point and the nearest node and a preset step size, and adding the new node to the node set; repeating the step of randomly sampling in the reachable area to obtain a sampling point until the node set includes the target position; obtaining an initial path based on the initial node, the target position and the node set; and performing spline curve smoothing on the initial path to obtain a target path.

[0100] The current position refers to the current location of the drone, which is usually provided by sensors (such as IMU, GNSS, etc.) and serves as the starting point for path planning. The reachable area refers to the area that the drone can reach in the current environment and does not contain obstacles. This area is defined by obstacle data and is usually calibrated using obstacle information obtained by sensors (such as lidar, millimeter-wave radar, etc.). The reachable area is a safe space where the drone can fly. The goal of path planning is to ensure that the flight path of the drone is always within the reachable area. The initial node is the starting point of path planning and is usually equal to the current position. In this embodiment, the node represents a position (state) on the flight path. Each node corresponds to a specific state (such as position, speed, etc.). The initial node is the first node added to the node set at the beginning of path planning, marking the starting state of the drone.

[0101] A node set is a collection of all nodes stored in the path planning process. In this embodiment, a path is composed of a series of nodes. Each sampling point generated by the algorithm generates a new node, ultimately forming a path from the initial node to the target node. The node set is the core of the path search. As the sampling process progresses, the node set continues to grow until the target location is reached. Random sampling refers to the random selection of several points within the reachable area. These points are called sampling points. Through random sampling, potential path nodes can be generated and the search area can be expanded to quickly explore the target location. The core of this embodiment is to expand the search tree through continuous random sampling until the target node is found.

[0102] Sampling points are points obtained through random sampling methods and are located within the reachable area. Sampling points are the basis for generating new nodes during the path planning process. After each sampling point is generated, this embodiment will attempt to find the optimal path based on these points. The nearest node refers to the node with the smallest distance from the current sampling point in the node set. In this embodiment, the process of generating a new node starts from the current sampling point, finds the closest existing node to it, and then expands the path based on the node. After finding the nearest node, the algorithm will generate a new node based on its relationship with the sampling point. The preset step size refers to the distance from the nearest node to the new node. The step size is a constant set when the path is generated. It determines the speed and accuracy of the path extension. If the step size is too large, the path will not be precise enough. If the step size is too small, the search process will become too slow. The preset step size is usually set according to the flight capability of the drone, the complexity of the environment, and the path accuracy requirements.

[0103] A new node is generated by calculating the direction from the nearest node to the sample point and a preset step size. This new node is located within the reachable area and forms the basis for expanding the search tree in the Rapidly Exploring Random Tree (RRT) algorithm. The generated new node is added to the node set, and as the algorithm executes, the search tree gradually expands until the target location is found. The initial path is the path from the initial node to the target location. It is generated by gradually expanding the node set, connecting randomly sampled points and the nearest node. Although the initial path is a path from the starting point to the end point, it may not be the smoothest or shortest path. This is because the RRT algorithm explores the space through gradual expansion, and the path is often abrupt. Spline smoothing optimizes the initial path to make it smoother and more natural, avoiding unnecessary sharp turns or excessive twists. Spline curves are a mathematical tool commonly used to fit smooth curves. They connect the nodes of a path through multiple smooth curve segments. In path planning, spline smoothing is used to remove unnecessary turns in the path, making it more suitable for drone flight requirements and improving flight efficiency and safety.

[0104] First, the drone scans its surroundings using sensors like lidar and millimeter-wave radar, collecting obstacle data. This data is used to create an environmental model representing the areas around it where obstacles exist. Then, using this obstacle data, the drone can determine the accessible area by setting a safety boundary. The accessible area is the area outside of obstacles where the drone can safely fly. The drone converts the obstacle data into a two- or three-dimensional map, indicating which areas are obstacles and which areas are accessible. The boundaries of the accessible area need to be updated in real time based on dynamic changes in the environment, especially when dynamic obstacles (such as other aircraft or moving objects) appear, to ensure that the drone can avoid collisions at all times.

[0105] Next, during the path planning process, the drone will use the current position as the initial node and the target position (which may be static or dynamic) as the end node to initialize the path planning. Using the RRT algorithm, the current position is first used as the starting node and added to the node set. Next, new sampling points are randomly sampled within the reachable area. These sampling points are potential path points to help the drone explore the path. Then, the node closest to the current sampling point is selected from the current node set, which ensures the consistency and rationality of the path generation. Based on the relative direction between the sampling point and the selected nearest node and the set preset step size, a new node is generated and added to the node set. In this way, as the path continues to expand, more and more nodes are added, and finally the node set contains the target position.

[0106] Finally, the path planning process repeats this process until the path from the starting position (current location) reaches the target location. The resulting path is then smoothed using a spline curve method to eliminate sharp turns and make the path smoother and more executable. This smoothing ensures that the path not only avoids obstacles but also maintains a stable and efficient flight trajectory during flight.

[0107] Step S60: Navigate according to the target state estimation and the target path.

[0108] It should be noted that navigation refers to the autonomous flight of a drone by controlling its flight system based on the current state estimate (such as position, speed, attitude, etc.) and the planned target path. Specifically, navigation includes the following key aspects: (1) State estimation feedback: Using the position information, speed, attitude and other data contained in the target state estimate, the drone's flight direction and attitude are adjusted in real time to ensure that it can accurately follow the target path. (2) Path tracking: The drone needs to track the path according to the predetermined target path to ensure that the position during flight remains on the correct trajectory of the path. This is usually achieved by discretizing the target path into a series of path points. The navigation system calculates the error between the current position and the next path point and adjusts the flight attitude based on the error. (3) Control system: The drone's flight control system (such as PID control, model predictive control, etc.) corrects the deviation of the drone in flight in real time by continuously adjusting control inputs such as thrust and rudder angle to ensure that it can fly according to the target path. (4) Obstacle avoidance and adjustment: If obstacles or environmental changes occur during flight, the navigation system also needs to avoid obstacles based on the target path and real-time environmental data, and may replan or adjust the target path to continue flying smoothly.

[0109] It can be understood that first, the drone evaluates its flight state in real time based on the target state estimate (including current position, speed, attitude, and other information) to determine the deviation from the target path. Then, using the waypoints planned in the target path, combined with the drone's current state, the control system tracks the path and adjusts the flight direction and speed to ensure that deviations are minimized during flight. Next, the flight control system adjusts flight control inputs such as thrust and rudder angle based on the state estimate and path error to keep the drone on the target path. Finally, if obstacles or environmental changes are encountered during flight, the navigation system will perform dynamic obstacle avoidance based on sensor feedback and update the flight path in real time to ensure that the drone completes its mission safely and stably and reaches the target location.

[0110] This embodiment provides a MEMS-based integrated navigation method. First, the drone uses sensors to collect initial navigation data, including position, velocity, acceleration, and point cloud information, and acquires target location and obstacle data. Subsequent navigation provides comprehensive environmental awareness and ensures data accuracy and integrity. The system then synchronizes and standardizes this initial data, eliminating data timing and dimensionality differences between sensors. This ensures that data from different sources can be compared and processed on the same scale, minimizing the impact of errors on subsequent steps. The reference navigation data is then preprocessed using a trained data preprocessing model to remove noise and extract valid information, thereby improving data quality and providing more accurate input for data fusion and navigation decision-making. The drone then performs data fusion on the target navigation data, integrating information from multiple sensors to perform state estimation. This generates drone state information, improving system robustness and accuracy. The drone then calculates a safe and efficient path from its current position to the target, avoiding obstacles and incorporating real-time state estimation to ensure stability and safety during flight. Finally, based on the planned path, the drone uses a flight control system to track the target path in real time, responding promptly to environmental changes and avoiding navigation errors caused by dynamic environmental changes or signal loss. Through precise collaboration of multiple steps, this application effectively solves the problem of reduced navigation accuracy of drones in complex environments due to accumulation of sensor errors or signal loss, ensuring high precision and stability of navigation.

[0111] Based on the first embodiment of the present application, in the second embodiment of the present application, the same or similar contents as those in the above embodiment 1 can be referred to the above introduction and will not be described in detail later. Figure 2 , Figure 2 This is a flow chart of the second embodiment of the MEMS-based integrated navigation method of the present application. Step S40 of the MEMS-based integrated navigation method includes steps S41 to S46:

[0112] Step S41, defining a process model and an observation model;

[0113] It should be noted that the process model describes how the system state (such as the position, velocity, attitude, etc. of the drone) changes over time and is usually expressed in the form of mathematical equations. It defines the dynamic equations of the system, that is, how the system state transitions from the current state to the next state. For drones, the process model is usually based on physical laws (such as the equations of motion) and control inputs (such as control commands, sensor measurements, etc.). In the adaptive Kalman filter, the process model is described by the following state equation:

[0114] x k =F k x k-1 +Bk u k +w k

[0115] Among them, x k is the current state, F k is the state transfer matrix (describing the transition from the previous state to the current state), B k is the control input matrix (indicating the impact of external control input on the state), u k is the control input, w k is the process noise, which is usually assumed to be zero-mean Gaussian white noise.

[0116] In drone applications, the process model can include the kinematic and dynamic equations of the drone. For example, based on the data from the IMU sensor, the changes in acceleration and angular velocity can be calculated to infer the position information of the drone at different times.

[0117] The observation model describes the relationship between the system state and the observed data. In other words, it defines how the observations obtained by sensors (such as GNSS, IMU, LiDAR, millimeter-wave radar, etc.) are mapped to the system state. For example, in the adaptive Kalman filter, the observation model is usually expressed as:

[0118] z k =H k x k +v k

[0119] Among them, z k is the observation value at the current moment (such as position, speed, etc.), H k is the observation matrix (describing the relationship between state and observation), x k is the system (drone) state at the current moment, v k is the observation noise, which is usually assumed to be zero-mean Gaussian white noise.

[0120] For drones, the observation model may include GNSS positioning data, IMU sensors (providing acceleration and angular velocity), and lidar or millimeter wave radar data (providing distance information), which are converted into state estimates through the observation model.

[0121] It's understandable that, first, when defining the process model, the drone's state (such as position, velocity, and attitude) is treated as a time-varying variable. The process model describes how the system state transitions from one moment to the next using a set of mathematical equations. For example, the drone's dynamic and kinematic equations, combined with the acceleration and angular velocity data provided by the IMU, are used to predict the drone's state at the next time step. The process model typically includes a state transition matrix (describing the transition patterns between states) and process noise (representing uncertainty). Next, the observation model is defined as the equations that describe the relationship between sensor measurements and the actual state. For example, positioning data acquired by GNSS and distance data from LiDAR can be used to estimate the drone's position or velocity. The observation model relates these measurements to the drone's state. The observation model typically includes an observation matrix and observation noise, which represent how the measurements reflect the system's actual state. The combination of these two models enables the adaptive Kalman filter to dynamically adjust and fuse multi-sensor information based on the predicted state and actual sensor observations, thereby improving navigation accuracy and robustness.

[0122] Step S42, obtaining an initial state estimate and an initial covariance matrix according to the target navigation data;

[0123] It should be noted that the initial state estimate x0 refers to the estimation of the system's initial state (such as position, velocity, attitude, etc.) based on existing sensor data or other known information at the beginning of the filtering process. It is the first prediction step of the Kalman filter and will provide a starting point for subsequent state updates of the filter. For example, in a drone application, assuming that sensors such as IMU, GNSS, and LiDAR are used to estimate the drone's initial state, the initial state estimate may include: position (such as latitude, longitude, and altitude information obtained by GNSS), velocity (derived from the acceleration and velocity of the IMU), and attitude (such as angular velocity and angle estimation from the IMU).

[0124] The initial covariance matrix P0 describes the uncertainty of the system state estimate. In Kalman filtering, the covariance matrix represents a measure of the uncertainty of the system state. Each element of the covariance matrix represents the correlation or degree of uncertainty between different states. The smaller the covariance matrix, the more accurate the system state estimate, while the smaller the covariance matrix, the less accurate the estimate. For example, if the initial position information is very uncertain (such as poor GNSS signal quality), the covariance value of the corresponding position will be large; if the initial velocity measurement is relatively accurate, the covariance value will be small.

[0125] As you can understand, first, the drone acquires target navigation data generated by a preprocessing model, including acceleration and angular velocity data from the IMU, satellite positioning data from the GNSS, and point cloud data from the LiDAR. Using this data, the drone uses kinematic or dynamic models to infer the drone's initial state, obtaining a preliminary state estimate. This typically includes key state variables such as position, velocity, and attitude. Next, the drone calculates the reliability of each state estimate based on sensor accuracy and signal-to-noise ratio, combined with prior information. It then assigns an uncertainty measure to each state: the initial covariance matrix.

[0126] Step S43, obtaining a reference state estimate based on the process model and the initial state estimate;

[0127] It should be noted that the reference state estimate refers to the current estimated value obtained by the drone through the prediction step after combining the process model and the initial state estimate. It is based on the state estimate at the previous moment (that is, the initial state estimate) and the process model to predict the state of the drone at the current moment. Reference state estimate (predicted state):

[0128]

[0129] in, is the predicted state estimate at time k, based on the state estimate x at time k-1 k-1 and control input u k .

[0130] It can be understood that first, the drone predicts the state of the system at the next moment based on the process model. This step is performed by combining the current initial state estimate and the control input (such as speed and acceleration) in the process model. Then, the process model will calculate the state estimate at the next moment based on the law of time evolution. This is the predicted reference state estimate. Finally, the reference state estimate provides the drone with a state prediction based on the current known data and motion laws, preparing for the next observation update step. It can effectively provide a reasonable preliminary estimate for subsequent data fusion, reduce state inaccuracies caused by sensor errors or noise, and thus improve overall navigation accuracy.

[0131] Step S44, estimating a predicted difference based on the observation model and the reference state;

[0132] It should be noted that the predicted difference is calculated by applying the observation model to the previous state estimation result (reference state estimate). It is the difference between the observed value and the expected observed value based on the current state estimate. For example, if the position and velocity of the drone are estimated using sensor data from IMU and GNSS, the reference state estimate will give a predicted position and velocity, which are then compared with the actual measurement values ​​of the sensors (such as the current position obtained by the GNSS receiver). The predicted difference is obtained by the gap between this actual measurement value and the predicted value. This difference indicates the size of the error between the current state estimate and the actual observation. The calculation formula is as follows:

[0133]

[0134] Among them, y k is the predicted difference, which represents the actual observation z k and predicted observations The gap between.

[0135] As you can understand, first, the drone predicts the expected observation value based on its current reference state estimate and observation model. Then, the drone compares the actual measured sensor data with the predicted value and calculates the difference between them, which is called the prediction difference. Finally, this prediction difference is used to adjust the current state estimate as part of the Kalman filter update process, helping to correct for deviations in the state estimate caused by sensor noise or errors, ensuring higher navigation accuracy for the drone.

[0136] Step S45, obtaining a Kalman gain according to the process model, the initial covariance matrix and the observation model;

[0137] It should be noted that the Kalman gain is a matrix that determines how to adjust the current state estimate based on the prediction error and observation error in the update step, and is used to balance the weight between the system's prediction and observation.

[0138] As you can understand, first, the drone uses a process model to predict its current state. The process model describes the evolution of the state over time based on the drone's kinematic or dynamic equations and infers a predicted state for the current moment. Second, the drone assesses the uncertainty of the predicted state based on the initial covariance matrix. The initial covariance matrix reflects the degree of uncertainty in the system state. A larger covariance indicates less understanding of the current state, which affects the calculation of the Kalman gain. The drone then calculates the prediction difference based on the difference between the observed and predicted values ​​obtained by the observation model, and estimates the error in the observation value using the covariance matrix. The Kalman gain is then calculated by weighing the relative magnitude of the observation error and the prediction error. The Kalman gain adjusts the state estimate based on the comparison of the two. A smaller Kalman gain relies more on prediction results, while a larger Kalman gain relies more on observation results. This helps the drone optimize its state estimate, further reducing errors and accurately estimating its position, velocity, or other state variables, thereby achieving more precise navigation control.

[0139] As an example, the step of obtaining the Kalman gain based on the process model, the initial covariance matrix and the observation model includes: obtaining a reference covariance matrix based on the process model and the initial covariance matrix; obtaining a predicted difference covariance matrix based on the observation model and the reference covariance matrix; and obtaining the Kalman gain based on the reference covariance matrix, the observation model and the predicted difference covariance matrix.

[0140] The reference covariance matrix is ​​a new covariance matrix derived from the process model and the initial covariance matrix in the Kalman filter. It is used to describe the uncertainty of the updated estimated state. The predicted difference covariance matrix is ​​calculated based on the observation model and the reference covariance matrix. It reflects the error and uncertainty generated when the observation data is combined with the reference covariance matrix, and reflects the system's confidence in the measurement error and state estimation error.

[0141] First, the drone combines the initial covariance matrix with the process noise matrix and performs matrix multiplication to obtain a new covariance matrix that describes the uncertainty of the system in state prediction, reflecting the error propagation of state estimation. Next, the drone multiplies the reference covariance matrix with the observation matrix, adds the observation noise matrix, and obtains the prediction difference covariance matrix through matrix addition. This matrix describes the errors that may occur in the system during the observation process. Finally, the drone multiplies the reference covariance matrix with the transpose of the observation matrix, and then multiplies it with the inverse matrix of the prediction difference covariance matrix, and finally obtains a matrix value called the Kalman gain, which determines the weight distribution of the model prediction value and the actual observation value, ensuring the appropriate adjustment of the observation data during the data fusion process. Through this step, the drone can estimate the target state more accurately, thereby improving navigation accuracy. Specifically, the reference covariance matrix:

[0142]

[0143] in, is the reference covariance matrix (also called the predicted covariance matrix) at time k, F k is the state transition matrix, P k-1 is the k-1 covariance matrix at time instant, Q k is the process noise covariance matrix, which represents the variance of the process noise and quantifies the uncertainty in the process model.

[0144] Prediction difference covariance matrix:

[0145]

[0146] Among them, S k is the forecast difference covariance matrix, often called the innovation covariance matrix, which represents the covariance of the forecast errors generated by the observation model; H k is the observation matrix, which represents the relationship between state and observation; R k is the observation noise covariance matrix, which represents the noise variance in the observation process and quantifies the uncertainty of the observation data.

[0147] Kalman gain:

[0148]

[0149] where K k is the Kalman gain, is the reference covariance matrix (forecast covariance matrix), which represents the uncertainty of the forecast state estimate, H k is the observation matrix, R k is the observation noise covariance matrix, T represents the transpose of the matrix, and -1 represents the inverse of the matrix.

[0150] Step S46: Update the reference state estimate according to the prediction difference and the Kalman gain to obtain a target state estimate.

[0151] It can be understood that the drone multiplies the predicted difference by the Kalman gain to obtain a correction, and then adds this correction to the reference state estimate to update the state values ​​such as position, velocity, and attitude. This update process ensures the correction of the target state estimate, making the target state closer to the true value and effectively reducing the impact of noise. In this way, the drone can accurately update the state based on new observation data and previous estimates, thereby improving the accuracy of positioning and navigation. Target state estimation:

[0152]

[0153] in, is the updated state estimate, which is obtained by weighting the prediction difference y k and the Kalman gain K k Forecast status Make corrections.

[0154] Update the covariance matrix:

[0155]

[0156] Among them, P k is the updated covariance matrix, which represents the uncertainty of the revised state estimate. This step ensures that after each update, the covariance matrix reflects the uncertainty of the current state estimate, and the estimate becomes more and more accurate as each observation is incorporated.

[0157] This embodiment first defines a process model and an observation model to establish a dynamic equation for how the system state changes over time. It also clarifies how to obtain state information based on sensor data. This provides a foundation for subsequent state estimation and ensures that the model accurately describes the UAV's motion and observation process. Next, using the target navigation data, a preliminary state estimate and state uncertainty are determined using the initial state estimate and initial covariance matrix. This initial setting provides a starting point for the Kalman filter, ensuring that state estimation begins with reliable initial conditions. Then, based on the process model, the initial state estimate is used to predict the state at the next moment, obtaining a reference state estimate. This prediction provides an estimate of the time evolution for subsequent steps and helps the system identify potential state errors. Subsequently, based on the observation model and the reference state estimate, the prediction difference—the difference between the sensor measurement and the predicted value—is calculated, providing feedback for state updates. Next, the Kalman gain is calculated by combining the process model, initial covariance matrix, and observation model to determine how to adjust the predicted state estimate. The Kalman gain helps the UAV balance the weight of the model prediction with the actual observation data during state estimation, ensuring optimal correction. Finally, the Kalman gain and prediction difference are used to update the reference state estimate, resulting in the final target state estimate, including position, velocity, attitude, and other information. This continuous updating and adjustment process provides more accurate navigation information, enhancing the positioning accuracy and robustness of the UAV in complex environments and effectively addressing the impact of sensor error accumulation and signal loss on navigation accuracy.

[0158] For example, in order to help understand the implementation process of the MEMS-based combined navigation method obtained by combining this embodiment with the above embodiment 1, please refer to Figure 3 , Figure 3 This is a schematic diagram of the system architecture of the MEMS-based integrated navigation method provided in Example 2 of the present application, specifically:

[0159] In this system, various navigation data are first acquired through multiple sensors, including fiber-optic / MEMS gyroscopes, GNSS receivers, millimeter-wave radars, lidars, and wheel speed sensors. Acceleration, velocity, position, angular velocity, and other data are collected. These sensor data are stored in memory (real-time sensor data). The data is then processed by an MCU chip, integrating the data from various sensors and combining it with a corresponding combined navigation algorithm to provide an accurate estimate of the drone's state. Furthermore, a high-precision satellite navigation card provides data input from the GNSS receiver, which is then passed to the MCU for processing along with other sensor data, ensuring high accuracy and real-time performance in path planning and navigation decisions.

[0160] The system first performs time synchronization and standardization to ensure that data from different sensors can be compared and processed using the same time base. Next, a data preprocessing model performs noise removal and feature extraction. Subsequently, the data undergoes data fusion and state estimation, fusing the signals from the various sensors and estimating the drone's state (such as position, velocity, and attitude). Finally, the system performs path planning based on target location and obstacle data, ensuring efficient and safe flight.

[0161] It should be noted that the above examples are only used to understand the present application and do not constitute a limitation on the MEMS-based combined navigation method of the present application. More simple transformations based on this technical concept are all within the scope of protection of the present application.

[0162] This application also provides a MEMS-based combined navigation device, please refer to Figure 4 , the MEMS-based integrated navigation device includes:

[0163] The data acquisition module 10 is used to obtain initial navigation data, target position and obstacle data;

[0164] a standardization module 20, configured to perform time synchronization and standardization on the initial navigation data to obtain reference navigation data;

[0165] A preprocessing module 30 is used to preprocess the reference navigation data according to a data preprocessing model to obtain target navigation data;

[0166] A state estimation module 40 is configured to perform data fusion and state estimation on the target navigation data to obtain a target state estimation;

[0167] A path planning module 50 is configured to perform path planning based on the target position, the obstacle data, and the target state estimation to obtain a target path;

[0168] The navigation module 60 is configured to perform navigation based on the target state estimation and the target path.

[0169] The MEMS-based integrated navigation device provided in this application, which utilizes the MEMS-based integrated navigation method described in the aforementioned embodiments, can address the technical issue of reduced navigation accuracy for drones in complex environments due to sensor error accumulation or signal loss. Compared to the prior art, the MEMS-based integrated navigation device provided in this application offers the same beneficial effects as the MEMS-based integrated navigation method described in the aforementioned embodiments. Other technical features of the MEMS-based integrated navigation device are the same as those disclosed in the aforementioned embodiments and are not further elaborated upon here.

[0170] The present application provides a MEMS-based integrated navigation device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the MEMS-based integrated navigation method in the above-mentioned embodiment 1.

[0171] Reference below Figure 5 , which shows a schematic structural diagram of a MEMS-based integrated navigation device suitable for implementing an embodiment of the present application. The MEMS-based integrated navigation device in the embodiment of the present application may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), in-vehicle terminals (e.g., in-vehicle navigation terminals), and fixed terminals such as digital TVs and desktop computers. Figure 5 The MEMS-based combined navigation device shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0172] like Figure 5As shown, the MEMS-based integrated navigation device may include a processing device 1001 (e.g., a central processing unit, a graphics processing unit, etc.), which can perform various appropriate actions and processes based on programs stored in a read-only memory (ROM) 1002 or programs loaded from a storage device 1003 into a random access memory (RAM) 1004. Various programs and data required for the operation of the MEMS-based integrated navigation device are also stored in the RAM 1004. The processing device 1001, ROM 1002, and RAM 1004 are connected to each other via a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to the I / O interface 1006: input devices 1007 including, for example, a touch screen, touchpad, keyboard, mouse, image sensor, microphone, accelerometer, gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), speaker, vibrator, etc.; storage devices 1003 including, for example, a magnetic tape, hard disk, etc.; and communication devices 1009. The communication device 1009 can allow the MEMS-based integrated navigation device to communicate with other devices wirelessly or by wire to exchange data. Although the figure shows a MEMS-based integrated navigation device with various systems, it should be understood that it is not required to implement or have all of the systems shown. More or fewer systems may be implemented or have instead.

[0173] In particular, according to the embodiments disclosed in the present application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, the embodiments disclosed in the present application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program comprising program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network via a communication device, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present application are executed.

[0174] The MEMS-based integrated navigation device provided in this application, which utilizes the MEMS-based integrated navigation method of the aforementioned embodiment, can resolve the technical problem of reduced navigation accuracy of drones in complex environments due to sensor error accumulation or signal loss. Compared to the prior art, the beneficial effects of the MEMS-based integrated navigation device provided in this application are the same as those of the MEMS-based integrated navigation method provided in the aforementioned embodiment. Other technical features of the MEMS-based integrated navigation device are the same as those disclosed in the aforementioned embodiment and are not further elaborated here.

[0175] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0176] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of this application. Therefore, the scope of protection of this application should be based on the scope of protection of the claims.

[0177] The present application provides a computer-readable storage medium having computer-readable program instructions (ie, computer program) stored thereon, wherein the computer-readable program instructions are used to execute the MEMS-based combined navigation method in the above-mentioned embodiment.

[0178] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, systems or devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, system or device. The program code contained on the computer-readable storage medium may be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0179] The computer-readable storage medium may be included in the MEMS-based integrated navigation device; or may exist independently without being assembled into the MEMS-based integrated navigation device.

[0180] The above-mentioned computer-readable storage medium carries one or more programs. When the above-mentioned one or more programs are executed by the MEMS-based combined navigation device, the MEMS-based combined navigation device: obtains initial navigation data, target position and obstacle data; performs time synchronization and standardization on the initial navigation data to obtain reference navigation data; preprocesses the reference navigation data according to a data preprocessing model to obtain target navigation data; performs data fusion and state estimation on the target navigation data to obtain a target state estimation; performs path planning according to the target position, the obstacle data and the target state estimation to obtain a target path; and navigates according to the target state estimation and the target path.

[0181] Computer program code for performing the operations of the present application may be written in one or more programming languages, or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on the remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., through the Internet using an Internet service provider).

[0182] The flow charts and block diagrams in the accompanying drawings illustrate the possible architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present application. In this regard, each box in the flow chart or block diagram can represent a module, program segment or a part of code, and the module, program segment or a part of code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order than that marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flow chart, and the combination of the boxes in the block diagram and / or flow chart can be implemented by a dedicated hardware-based system that performs the specified function or operation, or can be implemented by a combination of dedicated hardware and computer instructions.

[0183] The modules described in the embodiments of the present application may be implemented in software or hardware, wherein the name of a module does not necessarily limit the unit itself.

[0184] The readable storage medium provided in this application is a computer-readable storage medium, which stores computer-readable program instructions (i.e., a computer program) for executing the above-mentioned MEMS-based integrated navigation method. This computer-readable storage medium can solve the technical problem of reduced navigation accuracy of drones in complex environments due to sensor error accumulation or signal loss. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in this application are the same as those of the MEMS-based integrated navigation method provided in the above-mentioned embodiment, and will not be elaborated here.

[0185] The present application also provides a computer program product, comprising a computer program, which implements the steps of the above-mentioned MEMS-based combined navigation method when executed by a processor.

[0186] The computer program product provided in this application can address the technical problem of reduced navigation accuracy for drones in complex environments due to accumulated sensor errors or signal loss. Compared to the prior art, the beneficial effects of the computer program product provided in this application are similar to those of the MEMS-based integrated navigation method provided in the aforementioned embodiments, and are not further elaborated here.

[0187] The above description is only part of the embodiments of the present application and does not limit the patent scope of the present application. All equivalent structural transformations made by using the contents of the present application specification and drawings under the technical concept of the present application, or direct / indirect application in other related technical fields are included in the patent protection scope of the present application.

Claims

1. A MEMS-based integrated navigation method, characterized in that: The method comprises: Obtain initial navigation data, target position, and obstacle data; performing time synchronization and standardization on the initial navigation data to obtain reference navigation data; Preprocessing the reference navigation data according to a data preprocessing model to obtain target navigation data; performing data fusion and state estimation on the target navigation data to obtain a target state estimation; Performing path planning based on the target position, the obstacle data, and the target state estimation to obtain a target path; performing navigation based on the target state estimate and the target path; The method is applied to a drone equipped with an IMU, a GNSS receiver, a laser radar, a millimeter-wave radar, and a wheel speed sensor. The step of obtaining initial navigation data includes: Acquire acceleration data and angular velocity data through the IMU; Acquiring satellite positioning data through the GNSS receiver; Acquiring point cloud data through the laser radar; Acquiring distance-speed data through the millimeter-wave radar; acquiring wheel speed data through the wheel speed sensor; Obtaining initial navigation data according to the acceleration data, the angular velocity data, the satellite positioning data, the point cloud data, the distance-velocity data, and the wheel speed data; The step of performing time synchronization and standardization on the initial navigation data to obtain reference navigation data comprises: The timestamp of each data sampling point in the IMU is used as a reference time source; performing linear interpolation on the satellite positioning data, the distance-speed data, and the wheel speed data according to the reference time source to obtain first data; Searching for an IMU timestamp having the smallest time difference with a point cloud frame in the point cloud data from the reference time source; Replacing the timestamp of the point cloud frame in the point cloud data with the IMU timestamp to obtain second data; The acceleration data, the angular velocity data, the first data, and the second data are normalized to obtain reference navigation data.

2. The method according to claim 1, wherein The step of performing data fusion and state estimation on the target navigation data to obtain target state estimation comprises: Define process models and observation models; Obtaining an initial state estimate and an initial covariance matrix according to the target navigation data; obtaining a reference state estimate based on the process model and the initial state estimate; Estimating a predicted difference based on the observation model and the reference state; Obtaining a Kalman gain according to the process model, the initial covariance matrix, and the observation model; The reference state estimate is updated according to the predicted difference and the Kalman gain to obtain a target state estimate.

3. The method according to claim 2, wherein The step of obtaining the Kalman gain according to the process model, the initial covariance matrix and the observation model comprises: Obtaining a reference covariance matrix according to the process model and the initial covariance matrix; Obtaining a predicted difference covariance matrix based on the observation model and the reference covariance matrix; A Kalman gain is obtained according to the reference covariance matrix, the observation model and the predicted difference covariance matrix.

4. The method according to claim 1, wherein The target state estimate includes a current position; The step of performing path planning based on the target position, the obstacle data, and the target state estimation to obtain the target path includes: Obtaining a reachable area according to the obstacle data; Taking the current position as the initial node and adding the initial node to the node set; Random sampling is performed in the reachable area to obtain sampling points; Selecting the nearest node with the smallest distance to the sampling point from the node set; Generate a new node in the reachable area according to the direction between the sampling point and the nearest node and a preset step size, and add the new node to the node set; Repeat the steps of randomly sampling in the reachable area to obtain sampling points until the node set includes the target location; Obtaining an initial path according to the initial node, the target location, and the node set; The initial path is smoothed using a spline curve to obtain a target path.

5. The method according to claim 1, wherein The data preprocessing model includes a convolutional layer, a pooling layer, and a fully connected layer; The step of preprocessing the reference navigation data according to the data preprocessing model to obtain target navigation data includes: Performing smoothing and feature extraction on the reference navigation data through a convolution layer to obtain a convolution feature map; Downsampling the convolution feature map through a pooling layer to obtain a downsampled feature map; The downsampled feature maps are globally combined through a fully connected layer to obtain target navigation data.

6. The method according to claim 1, wherein The step of normalizing the acceleration data, the angular velocity data, the first data, and the second data to obtain reference navigation data includes: Calculating a mean and a standard deviation of the acceleration data, the angular velocity data, the first data, and the second data; Calculating third data according to the mean, the standard deviation, the acceleration data, the angular velocity data, the first data, and the second data; Perform two-dimensional conversion on the third data to obtain reference navigation data.

7. The method according to claim 6, wherein The step of performing two-dimensional conversion on the third data to obtain reference navigation data comprises: Dividing the acceleration data and angular velocity data in the third data into acceleration matrix data and angular velocity matrix data according to a time window; Constructing position-velocity matrix data according to the position and velocity in the satellite positioning data in the third data; Converting the point cloud data and the distance-speed data in the third data into a two-dimensional depth map; Reference navigation data is obtained according to the acceleration matrix data, the angular velocity matrix data, the position-velocity matrix data, the two-dimensional depth map, and the wheel speed data in the third data.

8. A MEMS-based integrated navigation device, characterized in that: The device comprises: Data acquisition module, used to obtain initial navigation data, target position and obstacle data; The data acquisition module is also used to obtain acceleration data and angular velocity data through the IMU; Obtain satellite positioning data through a GNSS receiver; Obtain point cloud data through lidar; Acquire distance-speed data through millimeter-wave radar; Obtain wheel speed data through wheel speed sensor; Obtaining initial navigation data according to the acceleration data, the angular velocity data, the satellite positioning data, the point cloud data, the distance-velocity data, and the wheel speed data; The data acquisition module is further configured to use the timestamp of each data sampling point in the IMU as a reference time source; performing linear interpolation on the satellite positioning data, the distance-speed data, and the wheel speed data according to the reference time source to obtain first data; Searching for an IMU timestamp having the smallest time difference with a point cloud frame in the point cloud data from the reference time source; Replacing the timestamp of the point cloud frame in the point cloud data with the IMU timestamp to obtain second data; normalizing the acceleration data, the angular velocity data, the first data, and the second data to obtain reference navigation data; a standardization module, configured to perform time synchronization and standardization on the initial navigation data to obtain reference navigation data; A preprocessing module, configured to preprocess the reference navigation data according to a data preprocessing model to obtain target navigation data; A state estimation module is used to perform data fusion and state estimation on the target navigation data to obtain a target state estimation; A path planning module, configured to perform path planning based on the target position, the obstacle data, and the target state estimation to obtain a target path; A navigation module is used to navigate according to the target state estimation and the target path.

Images