Integrated navigation method and device based on MEMS

Abstract

The invention discloses an MEMS-based integrated navigation method and device, and relates to the technical field of inertial navigation, and the MEMS-based integrated navigation method comprises the following steps: obtaining initial navigation data, a 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 target state estimation; performing path planning according to the target position, the obstacle data and the target state estimation to obtain a target path; and performing navigation according to the target state estimation and the target path. Through precise cooperation of multiple steps, the problem that the navigation precision of the unmanned aerial vehicle is reduced due to sensor error accumulation or signal loss in a complex environment is effectively solved, and high precision and stability of navigation are ensured.

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 surveying and mapping, etc. is increasing. In complex environments, the requirements for the navigation and positioning accuracy of drones are getting higher and higher. For example, when flying between high-rise buildings in cities, in forested areas, underground or closed environments (such as tunnels and mines), conventional navigation systems face severe challenges. Especially when the Global Positioning System (GPS) signal is lost or interfered with, the positioning and navigation of drones will become unreliable, resulting in the failure of flight missions. Therefore, how to improve the navigation accuracy of drones in various environments, especially to ensure accurate positioning when GPS signals are not available, is an urgent 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, speed 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, especially for IMU sensors, where errors 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, thus 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 decrease in navigation accuracy of drones in complex environments due to sensor error accumulation or signal loss has become a problem that needs to be solved urgently.

[0005] The above contents are only used to assist in understanding the technical solution of the present application and do not constitute an admission that the above contents are 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, the method comprising:

[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 according to 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-mentioned purpose, the present application also proposes a MEMS-based combined navigation device, the device comprising:

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

[0016] A standardization module, used for performing time synchronization and standardization on the initial navigation data to obtain reference navigation data;

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

[0018] A state estimation module, used for performing data fusion and state estimation on the target navigation data to obtain a target state estimation;

[0019] A path planning module, used to perform path planning according to 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 objectives, the present application also proposes a MEMS-based combined navigation device, which includes: a memory, a processor, and a computer program stored in the memory and executable 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 a 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, and when the computer program is executed by a processor, the steps of the MEMS-based combined navigation method as described above are implemented.

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

[0025] First, the UAV collects initial navigation data through sensors, including information such as position, speed, acceleration, point cloud, etc., and obtains target position and obstacle data. Subsequent navigation provides comprehensive environmental perception and ensures the accuracy and integrity of the data. Then, the system synchronizes and standardizes these initial data, eliminates the data timing and dimension differences between sensors, ensures that data from different sources can be compared and processed on the same scale, and reduces the impact of errors on subsequent steps. Then, the reference navigation data is preprocessed through the trained data preprocessing model to remove noise and extract effective information, thereby improving data quality and providing more accurate input for data fusion and navigation decisions. After that, the UAV performs data fusion on the target navigation data, integrates the information of multiple sensors for state estimation, obtains the state information of the UAV, and improves the robustness and accuracy of the system. Then, the UAV calculates a safe and efficient path from the current position to the target, avoids obstacles and considers real-time state estimation to ensure stability and safety during flight. Finally, based on the planned path, the UAV tracks the target path in real time through the flight control system and responds to environmental changes in a timely manner, thereby avoiding navigation errors caused by dynamic changes in the environment 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 accuracy 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 drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0028] Figure 1 A schematic diagram of a flow chart provided for Embodiment 1 of the combined navigation method based on MEMS of the present application;

[0029] Figure 2 A schematic diagram of a flow chart provided for Embodiment 2 of the combined navigation method based on MEMS of the present application;

[0030] Figure 3 A schematic diagram of the system architecture of the MEMS-based integrated navigation method provided in the second embodiment 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 described 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 only used to explain the technical solutions of the present application and are not used 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 more and more extensive, and the requirements for navigation and positioning accuracy are also increasing, especially in complex or signal-blocked environments. At present, drones mainly 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 facing sensor error accumulation, GNSS signal loss or interference, and dynamic environmental changes, which affects the reliability and accuracy of drones in various environments.

[0037] The main solution of the embodiment of the present application is: the drone uses multiple sensors to collect environmental data, including position, speed, obstacle information, etc., and performs time synchronization and standardization processing. After preprocessing through the preprocessing model, the multi-sensor data is fused to accurately estimate the drone state. Subsequently, combined with the target position and obstacle information, the drone calculates a safe and efficient flight path and tracks the path in real time to ensure the stability and safety of the flight, and can effectively cope with navigation challenges even in a dynamically changing environment.

[0038] It should be noted that the execution subject of the embodiment of the present application can be a computing service device with data processing, network communication and program running functions, such as a tablet computer, a personal computer, a mobile phone, etc., or an electronic device capable of realizing the above functions, a drone, etc. It can be a flying drone or a land drone. The following takes a flying drone as an example to illustrate 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 combined navigation method includes steps S10 to S60:

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

[0042] It should be noted that the initial navigation data refers to the system's initial estimate of the position, speed, and attitude (i.e., heading, pitch angle, and roll angle, etc.) of the drone when it is started or takes off. Specifically, these data usually include: (1) Current position: The initial longitude, latitude, altitude, or other coordinate system position of the drone. 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 initial speed of the drone (such as ground speed and vertical speed, etc.), usually obtained through IMU or wheel speed sensor, etc. (3) Initial attitude: The initial attitude of the drone (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, which is 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 a drone performs a real-time monitoring mission, the target location may be adjusted as 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, visual sensors, etc.). These data may include: (1) The spatial position of the obstacle: The relative position of the obstacle is obtained through sensors such as lidar and millimeter-wave radar, usually expressed as the coordinates of the obstacle (such as X, Y, Z coordinates) and displayed in the form of a map or point cloud. (2) The size and shape of the obstacle: Some sensors (such as lidar) not only provide the location data of the obstacle, but also can obtain information such as the shape and size of the obstacle through scanning, which is crucial for obstacle avoidance algorithms. (3) The dynamic characteristics of the obstacle: 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, firstly, 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 as a starting point to provide a reference for navigation and control of subsequent flights. Secondly, according to mission requirements, the drone receives or reads the target position through the flight planning system, which is usually a specific geographic coordinate or a designated target area. The flight planning system converts the target position 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. These data are processed to 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] IMU is a sensor that measures the motion and attitude changes of drones by integrating accelerometers and gyroscopes. The accelerometer measures acceleration and can sense the acceleration and deceleration of the drone in space; the gyroscope measures angular velocity and provides a sense of how the drone is rotating. The main function of IMU is to provide real-time motion status data for drones, especially to help estimate position, speed and attitude when GNSS signals are lost. Acceleration data refers to the acceleration information measured by the accelerometer in the IMU, which is usually 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, which is usually used to calculate the attitude changes of the drone, such as heading, pitch and roll angles.

[0048] A GNSS receiver is a device that calculates geographic location by receiving signals from satellites. It uses satellite positioning systems (such as GPS) to provide global positioning data to accurately determine the longitude, latitude, and altitude of the drone above the earth's surface. GNSS receivers are often used to provide reference position 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 GNSS receivers, which help determine the real-time position and speed of the drone.

[0049] LiDAR is a sensor that uses laser pulses to scan the surrounding environment and measure the reflection time to build a three-dimensional model of the environment. LiDAR can generate point cloud data with high precision. These data describe the spatial position and shape of objects around the drone and are often used for environmental modeling, obstacle detection and obstacle avoidance. Point cloud data refers to the set of three-dimensional spatial coordinate points formed by the reflected laser signal obtained by LiDAR, which describes 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 often used to detect moving targets, monitor the surrounding environment, and assist in obstacle avoidance. Distance-speed data refers to the millimeter wave radar calculating the distance and relative speed of the target object by measuring the reflected wave of the target object. This data is used to estimate the spatial relationship between the drone and other objects and help with dynamic obstacle avoidance.

[0051] Wheel speed sensors are installed on the ground vehicle of the drone (such as an autonomous vehicle) to estimate the ground speed of the vehicle by measuring the speed of tire rotation. They can provide accurate movement speed information, especially when the drone is in ground mode (such as taxiing, takeoff or landing), which can supplement the data of other sensors. Wheel speed data refers to the wheel speed data obtained by the wheel speed sensor, which is used to estimate the ground speed of the drone, especially when the GPS signal is lost, to provide a speed reference.

[0052] First, the drone obtains acceleration data and angular velocity data through the IMU, uses the accelerometer to measure the acceleration change and calculates the speed through integration, while the gyroscope provides angular velocity information to help estimate the attitude change of the drone. Secondly, the satellite positioning data is obtained through the GNSS receiver. GNSS provides accurate geographic location (latitude, longitude, altitude) to determine the current position and speed of the drone, especially providing high-precision positioning support in open environments. Then, the laser radar is used to obtain point cloud data. The laser radar scans the environment through laser pulses and measures the reflection time to generate the three-dimensional spatial coordinates of surrounding objects, helping to identify obstacles and build environmental models. Next, the drone obtains distance-speed data through the millimeter wave radar. The millimeter wave radar measures the relative distance and speed of the target object by transmitting and receiving reflected waves, which is suitable for the detection and obstacle avoidance of dynamic targets. Finally, the wheel speed sensor estimates the ground speed of the drone by measuring the rotation speed of the wheels, especially in the ground mode to supplement the data of other sensors. By combining the data from IMU, GNSS, lidar, millimeter-wave radar and wheel speed sensor, the initial navigation data of the drone is obtained, including position, speed, attitude and other information, 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 the data obtained from different sensors (such as IMU, GNSS, LiDAR, millimeter-wave radar, wheel speed sensor) 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 merge. Standardization refers to converting data from different sensors into a unified scale or unit so that the data can be processed and compared under 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 point in time or time period. Reference navigation data usually includes the following: (1) Position: comprehensive position estimation after 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 of all, the drone will time synchronize the data obtained by each sensor according to 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 directly used to ensure that each data point reflects the status at the same time point. This process can eliminate the data deviation caused by time differences, 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 in the same range (such as 0 to 1). The purpose of this standardization is to avoid excessive impact of some 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 fully reflects the real-time position, speed, attitude and other information of the drone, 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 drone.

[0056] As an example, the steps of time synchronizing and standardizing the initial navigation data to obtain reference navigation data include: taking 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] Data sampling points refer to the measurement data acquired by the sensor at each time point. These sampling points include acceleration, angular velocity, position information, laser radar point cloud data, etc. They are separate data units recorded continuously by the sensor at a certain frequency. Each sampling point usually contains the measurement value of the sensor at a certain moment. The timestamp is the time mark of each data sampling point or data frame, indicating the specific moment of data collection. The timestamp is usually recorded based on the system time or UTC time to identify the position of the sensor data on the time axis. Through the timestamp, the data of different sensors can be synchronized to the same time reference, which is convenient for subsequent data fusion. The reference time source refers to a selected main 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 sampling frequency of the IMU is higher, it can usually provide a more accurate time reference, so its timestamp will be used as the time standard for synchronizing other sensor data.

[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 some sensors, and these data are not strictly synchronized, linear interpolation can be used to fill in 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, the "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 laser radar within a certain period of time. Each point cloud frame contains a set of three-dimensional point data scanned and reflected by a laser radar sensor at a certain 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 for data fusion. Time difference refers to the difference between two timestamps, usually indicating the time interval between two data points. In this step, the time difference refers to 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 refers to the timestamp recorded at each sampling point of the IMU sensor, which usually has a high sampling frequency and is used to provide an accurate time reference. In this step, the IMU timestamp is used as the reference point for time synchronization. The data of other sensors such as GNSS and laser radar will be aligned with the IMU timestamp to ensure the time consistency of the data. The second data refers to the data obtained by replacing the timestamp in the point cloud data by finding the IMU timestamp closest to the timestamp of the point cloud frame in the point cloud data during the time synchronization process. 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 be used as a reference time point for other sensor data. This is done to ensure that all sensor data can be aligned under the same time reference to avoid fusion errors caused by time differences. Secondly, according to the selected reference time source, the satellite positioning data, distance-speed data, and wheel speed data are linearly interpolated. The specific operation is to use the interpolation algorithm to calculate the estimated value of these sensor data under the IMU timestamp based on the gap between the timestamp of each sensor data and the IMU timestamp, and obtain the first data synchronized with the IMU time. This step ensures the consistency of data from different sensors in time, thereby avoiding error propagation caused by time asynchrony. Then, the IMU timestamp with the smallest time difference between the timestamp of each point cloud frame in the lidar point cloud data is found from the reference time source (i.e., the IMU timestamp). In this way, the closest IMU timestamp can be found to ensure that the lidar data can be accurately synchronized with the IMU data. After that, 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 includes 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 and ensure that multi-sensor data contributes equally to navigation accuracy when fused. Finally, after these steps, the reference navigation data obtained contains the time and standardized data of all sensors after synchronization, 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 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 trend of these data in a certain period of time. For example, for the mean of acceleration data, the sum of the values ​​of all acceleration data sampling points is divided by the total number of data points to obtain a value reflecting the average level of acceleration. The standard deviation is a statistic that describes the degree of data dispersion, indicating 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, which can help convert the data into a unified dimension and avoid the impact caused by dimension differences.

[0063] The third data refers to the data obtained by standardizing the acceleration data, angular velocity data, first data and second data with zero mean and unit variance. The standardization process is to subtract the mean of the group of data from each data point and divide it by the standard deviation of the group of data, so as to convert the data into a standard normal distribution with a mean of 0 and a standard deviation of 1. In this way, data of different units and dimensions can be compared and processed on the same scale. The third data is the standardized data set, which can ensure that all sensor data are used with equal weight. Two-dimensional conversion is the process of converting the standardized data from the original one-dimensional form to a two-dimensional form. Generally, two-dimensional conversion refers to converting the data of each sensor (such as acceleration, angular velocity, etc.) into a value in a two-dimensional coordinate system according to certain rules or mapping relationships, such as by mapping the acceleration and angular velocity data to the two-dimensional coordinate axes respectively, or merging multiple data channels into two dimensions, so as to be processed in the subsequent navigation system. This conversion makes the data more suitable for calculations such as navigation and path planning, which is convenient for 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 respectively. The mean is obtained by summing all the sampled values ​​in each data set and dividing it by the number of data points. Next, the standard deviation is calculated by squaring the difference between each data point and the mean, summing it, and then squaring it and dividing it by the number of data points, so that the fluctuation range of the data can be quantified. The purpose of calculating the mean and standard deviation is to ensure that different sensor data are compared at the same scale for subsequent standardization. Then, the acceleration data, angular velocity data, first data, and second data are standardized using the calculated mean and standard deviation. Specifically, the robot subtracts the mean of the group of data from each data point and then divides it by the standard deviation to ensure that the mean of each group of data is 0 and the standard deviation is 1. This step eliminates the dimension and unit differences between different data sources so that these data will not be biased due to inconsistent scales in subsequent fusion. Finally, the drone will perform a two-dimensional conversion 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 conversion facilitates subsequent preprocessing. In this embodiment, zero mean unit variance standardization 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 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 according to 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 window refers to dividing continuous time series data into multiple segments according to a certain time range (window size). In the embodiment, the time window will be used to divide acceleration data and angular velocity data. This division is usually carried out 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 subset containing data within a certain time. In this way, the data can be locally processed or analyzed, which is convenient for subsequent matrix operations.

[0069] Acceleration matrix data is the acceleration data divided by time windows and converted into a matrix form. In each time window, the acceleration values ​​at each moment in the time period are stored. Each time window corresponds to a matrix row (or column), and the elements in each row (column) are the values ​​of acceleration in different dimensions (usually the X, Y, and Z axis directions). Through matrix processing, acceleration data can be more convenient for subsequent mathematical calculations, feature extraction, and fusion. Angular velocity matrix data is the angular velocity data divided by time windows and converted into a matrix form, which is similar to acceleration matrix data. Each time window corresponds to a matrix row, which stores the angular velocity data at different time points in the time window (usually the rotation speed around the X, Y, and Z axes). This matrix method converts time series data into a two-dimensional structure, which helps to optimize processing.

[0070] Position refers to the coordinate position of the drone in three-dimensional space at a certain moment, usually provided by GNSS. Position data includes X, Y, and Z coordinates, indicating the specific position of the drone relative to the earth's reference system. Position data is very critical information in the navigation system, helping the drone understand its current spatial position. Speed ​​refers to the speed of the drone relative to the ground or reference coordinate system at a certain moment, usually provided by GNSS or wheel speed sensors. Speed ​​data includes speed components in the three directions of X, Y, and Z, which are used to describe the movement rate and direction of the drone. The combination of speed and position data can be used to calculate the trajectory of the drone.

[0071] Position-velocity matrix data is the data that is integrated with position and velocity data and divided into matrix form according to time windows. In each time window, the matrix will simultaneously contain the position (usually X, Y, Z coordinates) and velocity (X, Y, Z components) of the drone at that moment. In this way, it is convenient for subsequent navigation algorithm processing and state estimation. Especially when multi-sensor data is fused, the combination of position and velocity data can more accurately describe the motion state of the drone. The two-dimensional depth map combines the point cloud data of the lidar and millimeter-wave radar with the distance-velocity data and converts them into a two-dimensional image or matrix. The lidar and millimeter-wave radar provide three-dimensional point cloud data, which represents the depth information of the drone's surrounding environment. By projecting these three-dimensional data onto a two-dimensional plane, a depth map can be obtained, in which each pixel represents the depth value of a spatial position (the distance from the sensor).

[0072] First, the drone divides the data into multiple sections according to a preset time interval (for example, every second or every fixed number of sampling points). Each section contains the acceleration or angular velocity value within the time window, and organizes these values ​​into a two-dimensional matrix. The purpose of this is to convert the original time series data into a matrix structure so that the data can better adapt to the subsequent data preprocessing model processing, thereby facilitating the model to perform feature extraction and pattern recognition. Next, the positioning information of each time window (the X, Y, Z coordinates of the position and the X, Y, Z components of the velocity) is extracted and arranged in chronological order to form a matrix containing multiple time window data. This process can help the drone unify the position and velocity information into a matrix form, which is convenient for subsequent fusion and processing, especially when the model performs path prediction and state estimation, it can more efficiently use these dynamically changing data. Then, for point cloud data and distance-speed data, the drone maps the three-dimensional point cloud data originally obtained from the lidar and millimeter wave radar to a two-dimensional plane through a projection algorithm, and obtains the depth or distance information of each pixel representing a certain position in space. Through this conversion, the originally complex three-dimensional data is simplified into an easy-to-process two-dimensional form. Finally, the drone will integrate all processed data - acceleration matrix, angular velocity matrix, position-velocity matrix, 2D depth map and wheel speed data - to form reference navigation data. This step integrates the data from various sensors into a unified format, which is convenient for subsequent data preprocessing models to preprocess.

[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 can automatically identify the noise signals contained in the data after learning a large amount of training data, and process these signals through different convolutional layers, pooling layers and other mechanisms to reduce the impact of noise on subsequent processing. The training process uses annotated reference navigation data sets, and optimizes 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 the data preprocessing model to remove noise and enhance important information. This data will serve as the input of the UAV's subsequent navigation algorithm. Target navigation data usually includes the real-time status information of the UAV, such as precise position, speed, attitude, etc., or environmental perception information related to navigation. After denoising through the preprocessing model, the target navigation data will be more accurate and have a higher signal-to-noise ratio, allowing the UAV to perform tasks such as path planning, positioning, and obstacle avoidance based on more reliable data.

[0076] It can be understood that, first of all, the data preprocessing model is trained by a large amount of labeled historical navigation data, with the purpose of learning how to identify and remove noise in the data. During the training process, the historical reference navigation data set is first input into the CNN model, where each data set contains noisy acceleration, angular velocity, position, velocity, point cloud and other information, and these data have been annotated to distinguish between noise and valid data. The CNN model automatically extracts features from the data through multiple convolutional layers, learns the pattern of noise layer by layer, and optimizes the network weights through the back propagation algorithm, so that the model can effectively identify noise features and filter out these noises. After the training is completed, the model can automatically identify the noise part when given new reference navigation data, remove the noise and retain valid information. The processed data is called target navigation data, which 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 the convolutional neural network. 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 the 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 the output feature map Y:

[0079] Y=W*X+b

[0080] Among them, Y is the feature map output by the convolution layer (convolution feature map), W is the convolution kernel or filter, the size is usually k×k (for example, 3×3), X is the input data (for example, reference navigation data), the size is n×n, b is the bias term, and * represents the convolution operation.

[0081] The pooling layer is used to downsample the convolutional feature map in order to reduce the data dimension and the amount of computation and retain 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 in each pooling window, while average pooling takes the average of all values ​​in the window. This operation helps reduce the model's sensitivity to position and enhances the robustness of the model, 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. 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 , with a bias of 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, with a size of m×m (output after the pooling layer), and 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 done by convolution operations to make data smoother in time or space and reduce the impact of mutations or noise. For example, applying a convolution kernel to the input data for sliding weighted averaging can reduce the impact of noise or mutation points, thereby improving the stability and accuracy of the data. Feature extraction refers to processing the input data through a convolution layer to automatically learn and extract useful features. In this step, the convolution layer scans the original navigation data (such as acceleration, angular velocity, etc.) through local perception and automatically learns the patterns or laws of these data. These features can describe the key information in the input data and help with subsequent navigation decisions.

[0088] The convolution feature map is the result obtained after the convolution layer is processed. It presents the characteristic response of each local area in the input data. Each convolution kernel generates a feature map to show the characteristics of the area it scans. The convolution feature map can help the model identify the pattern of the input data, such as acceleration changes, speed trends and other important information. Downsampling is an operation performed by the pooling layer. The purpose is to reduce the amount of calculation by reducing the resolution of the data. The downsampling operation is usually implemented by taking the maximum value or average value in the pooling window. Through this process, the spatial resolution of the data is reduced and the dimension of the feature is also reduced, but the key information of the data can still be maintained.

[0089] The downsampled feature map is the result of the pooling layer downsampling the convolutional feature map. 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 calculation and can effectively improve the robustness of the model 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 finally generates the target data by weighted summing the output of each layer. In this step, the model obtains navigation data with global significance by integrating the previous local features, such as clear, denoised acceleration, speed, and position information. These globally combined feature data will become the target navigation data and will be passed to the subsequent navigation decision system.

[0090] First, the drone uses the convolution layer in the data preprocessing model to smooth and extract features from the reference navigation data. Specifically, the convolution layer uses multiple convolution kernels to scan the reference navigation data (such as acceleration, angular velocity, speed, etc.) and extract local features from the data. Each convolution kernel performs weighted convolution on the data to capture pattern changes in the data, such as acceleration fluctuations, speed changes, etc. These features help remove noise and enhance effective information. Through this processing, the data becomes smoother and some local abnormal fluctuations are removed, thereby improving the quality of the data. Next, the drone uses the pooling layer in the data preprocessing model to downsample the convolution feature map. The pooling layer uses maximum pooling or average pooling to select the maximum value or average value in each pooling window to reduce the spatial resolution of the data. The purpose of this is to reduce the amount of calculation while retaining the most important and significant parts of the feature map, removing unimportant or redundant information, and 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 ensures that the key information in the data is preserved by converting local features into global features, 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 obtained 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 sense the same environment at the same time, but their respective measurement accuracy and response speed are different, and there may be certain noise and errors. The purpose of data fusion is to take advantage of the advantages of different sensors, integrate data from multiple sources, and use mathematical models to reduce the impact of single sensor errors, so as to obtain 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 to estimate the actual state of the drone (such as position, speed, attitude, etc.) by processing multi-sensor data. Due to the influence of sensor errors, external interference and other factors, the raw data directly read 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 by 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 UAV obtained through data fusion and state estimation algorithms (such as adaptive Kalman filtering). This state information usually includes position (such as GPS coordinates), speed (such as velocity vector), attitude (such as roll angle, pitch angle, yaw angle), etc., which are crucial for the navigation, path planning and control of the UAV. The key to target state estimation is to obtain real-time and accurate state information by continuously updating and correcting sensor data, providing a basis for subsequent navigation decisions. The final target state estimation is the result of combining all sensor inputs and dynamically optimizing them through algorithms such as adaptive Kalman filtering, which can effectively reduce noise and improve the accuracy of state estimation.

[0095] It can be understood that, first, the target navigation data processed by the data preprocessing model of the drone is integrated through the data fusion method. Specifically, this step uses algorithms such as Kalman filtering (or extended Kalman filtering / adaptive Kalman filtering) to weight the observation data from multiple sensors, taking into account the data accuracy and credibility of each sensor, thereby reducing the impact of single sensor errors. Then, based on the fused data, the state estimation algorithm is used to estimate the current state of the drone, including key parameters such as its position, speed, and attitude. During the state estimation process, the algorithm matches the sensor data with the dynamic model of the drone, corrects possible measurement errors, and realizes accurate calculation of the actual state of the drone. Finally, after data fusion and state estimation, the target state estimation, that is, the optimal state information such as the drone position, speed, and attitude, is obtained, providing an accurate basis for subsequent navigation decisions and path planning.

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

[0097] It should be noted that path planning refers to calculating the optimal route from the current state to the target position based on the target position of the drone, obstacle data in the surrounding environment, and the current target state estimate. 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, flight time, and other requirements. The goal of path planning is to ensure that the drone can safely and efficiently reach the target point from the starting point in a dynamic or complex environment. The target path refers to the optimal flight route calculated by the path planning algorithm. It is the path that the drone needs to follow from the current position to the target position. The target path not only needs to avoid obstacles, but also must meet the performance constraints (such as speed, flight altitude, etc.) and safety requirements of the drone. The accuracy of the target path directly affects the flight stability and mission completion efficiency of the drone, especially in complex or dynamic environments. The target path will be continuously adjusted and optimized as the environment changes. Therefore, the target path contains a series of calculated waypoints (position coordinates, speed, attitude, etc.), and after path planning and optimization, it is usually the shortest, safest and mission-compliant flight path.

[0098] It can be understood that, first, the drone determines the input of path planning by receiving the target position, obstacle data and target state estimation. The target position is the final destination of the drone flight, and the target state estimation includes the current position information, speed, attitude and other states of the drone, which helps the drone understand its current position and flight capabilities. Obstacle data comes from sensors such as lidar and millimeter wave radar, which are used to describe dynamic or static obstacles in the flight environment. Based on this information, the drone uses a path planning algorithm (such as A* algorithm, etc.) to calculate the optimal path. First, the path planning algorithm determines a general flight direction based on the target position and current state estimation. Then, the algorithm takes into account the distribution and position of obstacles in real time to avoid collisions between the drone and obstacles, and further optimizes the path based on the flight constraints of the drone (such as maximum speed, turning radius, etc.). Finally, through the optimization process, an optimal flight path from the current state to the target position is calculated. This path ensures that the drone can complete the task safely and quickly, and avoids obstacles and other potential dangerous areas during flight. This calculated path is the target path, and the drone will navigate and control the flight according to this path.

[0099] As an example, the target state estimation 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 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 a 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, which 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] The node set is a collection of all nodes in the path planning process. In this embodiment, the path is composed of a series of nodes. Each sampling point obtained by sampling will generate a new node through the algorithm, and finally form a path from the initial node to the target node. The node set is the core of the path search. As the sampling process proceeds, the node set will continue to increase until the target position is reached. Random sampling refers to randomly selecting a number of points in 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 position. The core of this embodiment is to expand the search tree through continuous random sampling until the target node is searched.

[0102] The sampling point is a point obtained by a random sampling method, which is located in the reachable area. The sampling point is the basis for generating new nodes in the path planning process. After each sampling point is generated, this embodiment will try 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, which determines the speed and accuracy of the path extension. If the step size is too large, the path will not be fine enough, and 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] The new node is generated by the direction from the nearest node to the sampling point and the preset step size. This new node is located in the reachable area and is the basis for the Rapidly-exploring Random Tree (RRT) algorithm to expand the search tree. The generated new node will be added to the node set. As the algorithm is executed, the size of the search tree will gradually increase until the target location is found. The initial path is the path from the initial node to the target location. It is a path generated by gradually expanding the node set, connecting random sampling points and the nearest nodes. Although the initial path is a path from the starting point to the end point, it may not be the smoothest or shortest path, because the RRT algorithm explores the space by gradually expanding, and the path is often stiff. Spline smoothing refers to optimizing the initial path to make the path smoother and more natural, avoiding unnecessary sharp turns or excessive twists and turns. Spline curves are a mathematical tool that is often used to fit smooth curves. It connects the nodes of the path through multiple smooth curve segments. In path planning, the role of spline smoothing is to remove unnecessary turns in the path, make the path more suitable for the flight requirements of the drone, and improve flight efficiency and safety.

[0104] First, the drone scans the surrounding environment through sensors such as lidar and millimeter-wave radar to collect obstacle data. This data is used to create an environmental model that represents the areas around obstacles. Then, using this obstacle data, the drone can determine the accessible area by setting a safety boundary. The accessible area is the area outside the obstacles where the drone can fly safely. The drone will convert the obstacle data into a two-dimensional 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 according to the 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 any time.

[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 in 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. According to the relative direction between the sampling point and the selected nearest node and the 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 reaches the target location from the starting position (current position). Then, the path is smoothed using the spline curve method based on the generated path to eliminate sharp turns in the path, making the path smoother and more executable. Through smoothing, it is ensured that the path can not only avoid obstacles, but also achieve 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 estimation (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 estimation, the flight direction and attitude of the drone 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 flight control system of the drone (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 along 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 UAV evaluates its own flight state in real time based on the target state estimation (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 current state of the UAV, the control system tracks the path, adjusts the flight direction and speed, and ensures that the deviation is minimized during the flight. Next, the flight control system adjusts the flight control inputs, such as thrust, rudder angle, etc., based on the state estimation and path error, so that the UAV always maintains on the target path. Finally, if obstacles or environmental changes are encountered during the 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 UAV completes the task safely and stably and reaches the target position.

[0110] This embodiment provides a combined navigation method based on MEMS. First, the drone collects initial navigation data through sensors, including information such as position, speed, acceleration, point cloud, etc., and obtains target position and obstacle data. Subsequent navigation provides comprehensive environmental perception and ensures the accuracy and integrity of the data. Then, the system synchronizes and standardizes these initial data, eliminates the data timing differences and dimensional differences between sensors, ensures that data from different sources can be compared and processed on the same scale, and reduces the impact of errors on subsequent steps. Then, the reference navigation data is preprocessed through the trained data preprocessing model to remove noise and extract effective information, thereby improving data quality and providing more accurate input for data fusion and navigation decisions. After that, the drone performs data fusion on the target navigation data, integrates the information of multiple sensors for state estimation, obtains the state information of the drone, and improves the robustness and accuracy of the system. Then, the drone calculates a safe and efficient path from the current position to the target, avoids obstacles and considers real-time state estimation to ensure stability and safety during flight. Finally, based on the planned path, the drone tracks the target path in real time through the flight control system and responds to environmental changes in a timely manner, thereby avoiding navigation errors caused by dynamic changes in the environment 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 accuracy 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-mentioned embodiment 1 can refer to the above introduction, and will not be repeated 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, speed, attitude, etc. of the drone) changes over time, 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 equations of motion) and control inputs (such as control instructions, 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 state at the previous moment to the state at the current moment), 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, the changes in acceleration and angular velocity are calculated based on the data from the IMU sensor 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 state of the system. For example, in adaptive Kalman filtering, the observation model is usually expressed as:

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

[0119] Among them, z k is the observed value at the current moment (such as position, speed, etc.), H k is the observation matrix (describing the relationship between states and observations), 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 can be understood that, first, when defining the process model, the state of the drone (such as position, velocity, and attitude) is regarded as a variable that changes over time. The process model describes how the system state transfers from the current moment to the next moment through a set of mathematical equations. For example, the dynamic and kinematic equations of the drone are used, combined with the acceleration and angular velocity data provided by the IMU, to predict the state of the drone at the next time step. The process model usually includes a state transfer matrix (describing the transition law between states) and process noise (representing uncertainty). Next, the observation model is defined as an equation that describes the relationship between sensor measurements and actual states. For example, positioning data obtained by GNSS and distance data from LiDAR can be used to estimate the position or velocity of the drone, and the observation model associates these measurements with the drone state. The observation model usually includes an observation matrix and observation noise to represent how the measurement value reflects the true state of the system. 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 observation data, thereby improving navigation accuracy and robustness.

[0122] Step S42, obtaining an initial state estimation 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 initial state of the system (such as position, velocity, attitude, etc.) based on the existing sensor data or other known information at the beginning of the filtering process. It is the first prediction step of the Kalman filter, which will provide a starting point for the subsequent state update of the filter. For example, in the application of drones, assuming that sensors such as IMU, GNSS, and lidar are used to estimate the initial state of the drone, the initial state estimate may include: position (such as latitude, longitude, and altitude information obtained by GNSS), velocity (calculated by the acceleration and velocity of the IMU), and attitude (such as estimated by the angular velocity and angle of the IMU).

[0124] The initial covariance matrix P0 describes the uncertainty of the system state estimation. In Kalman filtering, the covariance matrix represents a measure of the uncertainty of the system state. Each of its elements represents the correlation or degree of uncertainty between different states. The smaller the covariance matrix, the more accurate the system state estimation is, and vice versa. For example, if the initial state position information is very uncertain (such as poor GNSS signal quality), the covariance value of the corresponding position will be larger; if the initial velocity measurement is more accurate, the covariance value will be smaller.

[0125] It is understandable that, first, the drone will obtain the target navigation data processed by the preprocessing model, including acceleration and angular velocity data from the IMU, satellite positioning data from the GNSS, point cloud data from the lidar, etc. Through these data, the drone uses the kinematic or dynamic model to infer the initial state of the drone and obtain a preliminary state estimate, which usually includes key state variables such as position, speed, and attitude. Secondly, the drone will calculate the reliability of each state estimate based on the accuracy and signal-to-noise ratio of the sensor, combined with prior information, and then assign an uncertainty measure to each state, which is the initial covariance matrix.

[0126] Step S43, obtaining a reference state estimate according to 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 (i.e. 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 UAV predicts the next state of the system based on the process model. This step is performed by combining the current initial state estimate and the control input (such as speed, acceleration) in the process model. Then, the process model will calculate the state estimate at the next moment according to the law of time evolution. This is the predicted reference state estimate. Finally, the reference state estimate provides the UAV 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 the state inaccuracy caused by sensor errors or noise, and thus improve the 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 prediction difference is calculated by applying the observation model to the previous state estimation result (reference state estimation), which is the difference between the observed value and the expected observed value based on the current state estimation. For example, if the position and velocity of the drone are estimated using sensor data from IMU and GNSS, the reference state estimation will give a predicted position and velocity, which are then compared with the actual measurement values ​​of the sensor (such as the current position obtained by the GNSS receiver). The prediction difference is obtained by the gap between this actual measurement value and the predicted value. This difference indicates the error size 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 observed z k Observation and prediction The gap between.

[0135] It can be understood that first, the drone predicts the observation value that should be obtained based on the current reference state estimate and the 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 the state estimate deviation caused by sensor noise or error, ensuring that the drone's navigation accuracy is higher.

[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 prediction and observation of the system.

[0138] It can be understood that, first, the UAV uses the process model to predict the current state. The process model describes the change of the state over time according to the kinematic or dynamic equations of the UAV, and infers the state prediction at the current moment. Secondly, the UAV evaluates 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. The larger the covariance, the less knowledge of the current state, which will affect the calculation of the Kalman gain. Then, the UAV calculates the prediction difference based on the difference between the observed value and the predicted value obtained by the observation model, and estimates the error of the observed value in combination with the covariance matrix. Next, the Kalman gain is calculated by weighing the relative size of the observation error and the prediction error. The Kalman gain value will adjust the state estimate based on the comparison between the two. A smaller Kalman gain will rely on more prediction results, and a larger Kalman gain will rely on more observation results, which can help the UAV optimize its state estimate, further reduce the error, and accurately estimate its position, speed or other state variables, thereby achieving more accurate navigation control.

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

[0140] The reference covariance matrix refers to a new covariance matrix inferred from the process model and the initial covariance matrix in the Kalman filter, which is used to describe the uncertainty of the estimated state after the update. The predicted difference covariance matrix is ​​calculated based on the observation model and the reference covariance matrix, which reflects the error and its uncertainty generated when the observation data is combined with the reference covariance matrix, and reflects the system's trust 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 obtains the predicted difference covariance matrix by multiplying the reference covariance matrix with the observation matrix and adding the observation noise matrix through matrix addition. This matrix describes the possible errors that the system may generate during the observation process. Finally, the drone multiplies the reference covariance matrix with the transpose of the observation matrix and then with the inverse matrix of the predicted difference covariance matrix to finally obtain a matrix value called the Kalman gain, which determines the weight distribution of the model prediction value and the actual observation value, ensuring the proper 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 prediction covariance matrix) at time k, F k is the state transition matrix, P k-1 is the k-1 covariance matrix, 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 (prediction covariance matrix), which represents the uncertainty of the predicted 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 that the target state estimate is corrected, making the target state closer to the true value and effectively reducing the impact of noise. In this way, the drone can perform accurate state updates based on new observations 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 the process model and the observation model, establishes the dynamic equation of how the system state changes over time, and clarifies how to obtain state information based on sensor data, which provides a basis for subsequent state estimation and ensures that the model can accurately describe the motion and observation process of the drone. Then, using the target navigation data, the initial state estimate and state uncertainty are determined through the initial state estimate and the initial covariance matrix. This initial setting provides a starting point for the Kalman filter, ensuring that the state estimation starts from a reliable initial condition. Then, based on the process model, the initial state estimate is used to predict the state at the next moment to obtain the 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 predicted difference, that is, the gap between the sensor measurement and the predicted value, is calculated to provide feedback for the state update. Next, the Kalman gain is calculated in combination with the process model, the initial covariance matrix and the observation model to determine how to adjust the predicted state estimate. The Kalman gain helps the drone balance the weight of the model prediction and the actual observation data during state estimation, ensuring the optimal correction effect. Finally, the reference state estimate is updated using the Kalman gain and prediction difference to obtain the final target state estimate, including position, velocity, attitude, etc. This process provides more accurate navigation information through continuous updating and adjustment, enhances the positioning accuracy and robustness of the UAV in complex environments, and effectively solves 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-mentioned embodiment 1, please refer to Figure 3 , Figure 3 A schematic diagram of the system architecture of the MEMS-based combined navigation method provided in the second embodiment of the present application, specifically:

[0159] In this system, different navigation data are first obtained through multiple sensors, including fiber optic / MEMS gyroscopes, GNSS receivers, millimeter wave radars, lidars, and wheel speed sensors, etc., to collect data such as acceleration, speed, position, and angular velocity. The data from these sensors is stored in the memory (real-time sensor data), and then processed by the MCU chip, integrating the data from different sensors and combining it with the corresponding combined navigation algorithm for calculation, ultimately providing an accurate state estimate of the drone. In addition, the high-precision satellite navigation card provides data input from the GNSS receiver and passes it to the MCU for processing together with other sensor data, ensuring high accuracy and real-time performance of path planning and navigation decisions.

[0160] In the system, time synchronization and standardization are first performed to ensure that data from different sensors can be compared and processed under the same time reference. Next, noise removal and feature extraction are performed through the data preprocessing model. Subsequently, the data passes through the data fusion and state estimation steps to fuse the signals from different sensors and estimate the state of the drone (such as position, speed, attitude, etc.). Finally, the system performs path planning based on the target position and obstacle data to ensure that the drone can fly efficiently and safely.

[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 protection scope of the present application.

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

[0163] A data acquisition module 10 is used to acquire 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, configured to preprocess the reference navigation data according to a data preprocessing model to obtain target navigation data;

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

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

[0168] The navigation module 60 is used to perform navigation according to the target state estimation and the target path.

[0169] The MEMS-based combined navigation device provided by the present application adopts the MEMS-based combined navigation method in the above-mentioned embodiment, which can solve the technical problem that the navigation accuracy of the UAV decreases due to sensor error accumulation or signal loss in a complex environment. Compared with the prior art, the beneficial effects of the MEMS-based combined navigation device provided by the present application are the same as the beneficial effects of the MEMS-based combined navigation method provided by the above-mentioned embodiment, and the other technical features of the MEMS-based combined navigation device are the same as the features disclosed in the above-mentioned embodiment method, which will not be repeated here.

[0170] The present application provides a MEMS-based combined navigation device, the MEMS-based combined navigation device 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 combined navigation method in the above-mentioned embodiment one.

[0171] Reference below Figure 5 , which shows a schematic diagram of the structure of a MEMS-based integrated navigation device suitable for implementing the 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), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 5 The MEMS-based combined navigation device shown is merely 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 processor, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM: Read Only Memory) 1002 or a program loaded from a storage device 1003 to a random access memory (RAM: Random Access Memory) 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, the ROM 1002, and the 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 may be connected to the I / O interface 1006: input devices 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; storage devices 1003 including, for example, a magnetic tape, a hard disk, etc.; and communication devices 1009. The communication device 1009 may allow the MEMS-based combined navigation device to communicate wirelessly or wired with other devices to exchange data. Although the MEMS-based combined navigation device with various systems is shown in the figure, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems may be implemented or have alternatively.

[0173] In particular, according to the embodiments disclosed in the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present application include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a 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 through 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 combined navigation device provided by the present application adopts the MEMS-based combined navigation method in the above embodiment, which can solve the technical problem that the navigation accuracy of the UAV decreases due to sensor error accumulation or signal loss in a complex environment. Compared with the prior art, the beneficial effects of the MEMS-based combined navigation device provided by the present application are the same as the beneficial effects of the MEMS-based combined navigation method provided by the above embodiment, and the other technical features of the MEMS-based combined navigation device are the same as the features disclosed in the method of the previous embodiment, which will not be repeated here.

[0175] It should be understood that the various parts disclosed in this application can be implemented by 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 is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

[0177] The present application provides a computer-readable storage medium having computer-readable program instructions (ie, computer programs) 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 the present 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 of the above. 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 of the above. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination 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 of the above.

[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 computer-readable storage medium carries one or more programs. When the one or more programs are executed by a 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 separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of 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., via the Internet using an Internet service provider).

[0182] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present application. In this regard, each box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the 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 sequence different from 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 with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0183] The modules involved in the embodiments described in this application may be implemented by software or hardware, wherein the name of the module does not constitute a limitation on the unit itself in some cases.

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

[0185] The present application also provides a computer program product, including 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 by the present application can solve the technical problem that the navigation accuracy of drones decreases due to sensor error accumulation or signal loss in complex environments. Compared with the prior art, the beneficial effects of the computer program product provided by the present application are the same as those of the MEMS-based combined navigation method provided by the above embodiment, and will not be elaborated here.

[0187] The above descriptions are only some embodiments of the present application, and are not intended to limit the patent scope of the present application. All equivalent structural changes made using the contents of the present application specification and drawings under the technical concept of the present application, or direct / indirect applications in other related technical fields are included in the patent protection scope of the present application.

Claims

1. A combined navigation method based on MEMS, 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 according to the target position, the obstacle data and the target state estimation to obtain a target path; Navigation is performed based on the target state estimate and the target path.

2. The method according to claim 1, characterized in that 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, characterized in that 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 according to 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, characterized in that The target state estimate includes a current position; The step of performing path planning according to the target position, the obstacle data and the target state estimation to obtain the target path comprises: Obtaining a reachable area according to the obstacle data; Taking the current position as an initial node and adding the initial node to a node set; Random sampling is performed in the reachable area to obtain sampling points; Selecting the nearest node with the shortest 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 length, and add the new node to the node set; Repeat the step 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 position 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, characterized in that 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 comprises: Performing smoothing and feature extraction on the reference navigation data through a convolution layer to obtain a convolution feature map; Downsampling the convolutional 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, characterized in that The method is applied to a drone, which is 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; Acquire 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; Initial navigation data is obtained 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.

7. The method according to claim 6, characterized in that The step of performing time synchronization and standardization on the initial navigation data to obtain reference navigation data comprises: Using 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; Finding 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 standardized to obtain reference navigation data.

8. The method according to claim 7, characterized in that The step of normalizing the acceleration data, the angular velocity data, the first data, and the second data to obtain reference navigation data comprises: Calculating the mean and standard deviation of the acceleration data, the angular velocity data, the first data, and the second data; Calculate the 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.

9. The method according to claim 8, characterized in that 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.

10. A MEMS-based integrated navigation device, characterized in that: The device comprises: A data acquisition module is used to obtain initial navigation data, target position and obstacle data; A standardization module, used for performing time synchronization and standardization on the initial navigation data to obtain reference navigation data; A preprocessing module, used for preprocessing the reference navigation data according to a data preprocessing model to obtain target navigation data; A state estimation module, used for performing data fusion and state estimation on the target navigation data to obtain a target state estimation; A path planning module, used to perform path planning according to 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