Fusion navigation method and device, electronic equipment and readable storage medium

Abstract

The embodiment of the application provides a kind of fusion navigation method, device, electronic equipment and readable storage medium, by receiving each UWB base station at the same time sent UWB signal;According to the arrival time corresponding to each UWB signal, the preset coordinate data of each UWB base station and the preset propagation speed of UWB signal, the first UWB coordinate data and the first UWB speed data of unmanned aerial vehicle are calculated;The coordinate data and speed data of the unmanned aerial vehicle corresponding to GPS are replaced by the first UWB coordinate data and the first UWB speed data;The first UWB coordinate data, the first UWB speed data, auxiliary coordinate data and auxiliary speed data of unmanned aerial vehicle are fused and calculated using EKF.This way, the coordinate data and speed data corresponding to the original positioning accuracy lower GPS are replaced by the first UWB coordinate data and the first UWB speed data of higher positioning accuracy, and then EKF is used to fuse and calculate with auxiliary coordinate data and auxiliary speed data, to improve the fusion navigation accuracy of unmanned aerial vehicle.

Description

Technical Field

[0001] This application relates to the field of unmanned aerial vehicle (UAV) technology, and more particularly to a fusion navigation method, apparatus, electronic device, and readable storage medium. Background Technology

[0002] Unmanned aerial vehicles (UAVs) are unmanned aircraft controlled by radio remote control equipment and their own program control devices, or operated autonomously by computers, either completely or intermittently.

[0003] For a drone to achieve autonomous landing, it needs to know its current coordinates, speed, and yaw angle. Current technology uses an Extended Kalman Filter (EKF) within the Estimation and Control Library (ECL) to fuse GPS positioning, Inertial Measurement Unit (IMU), and magnetometer data to obtain the drone's coordinates, speed, and yaw angle information.

[0004] However, due to the large positioning error of GPS, the accuracy of fusion navigation for drones is currently low. Summary of the Invention

[0005] This application provides a fusion navigation method, apparatus, electronic device, and readable storage medium to solve the problem of low fusion navigation accuracy of UAVs in the prior art.

[0006] In a first aspect, embodiments of this application provide a fusion navigation method, including the following steps:

[0007] Receive UWB signals transmitted simultaneously by each of M Ultra Wide Band (UWB) base stations at the same time, where M is an integer greater than 3;

[0008] Based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, the first UWB coordinate data and the first UWB speed data of the UAV are calculated.

[0009] Replace the coordinate data and velocity data of the UAV corresponding to the GPS positioning technology in the ECL (Global Positioning System) library with the first UWB coordinate data and first UWB velocity data of the UAV.

[0010] When the Extended Kalman Filter (EKF) is in the fusion calculation state, according to the preset first Jacobian observation matrix, the EKF is used to perform fusion calculation on the first UWB coordinate data, the first UWB velocity data, the auxiliary coordinate data and the auxiliary velocity data of the UAV to obtain the target coordinate data and the target velocity data.

[0011] The drone's landing is controlled based on the target coordinate data and target speed data.

[0012] Optionally, calculating the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time corresponding to each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal includes:

[0013] The received M UWB signals are divided to obtain a first UWB signal and M-1 second UWB signals, wherein the first UWB signal is a reference UWB signal among the M UWB signals;

[0014] Calculate the difference between the arrival time of the first UWB signal and the arrival time of each second UWB signal to obtain M-1 time differences;

[0015] Based on the M-1 time differences and the propagation speed of the UWB signal, calculate the distance difference between the UWB base station corresponding to the first UWB signal and the UWB base station corresponding to each second UWB signal and the UAV, and obtain M-1 distance differences;

[0016] Based on the signal arrival time difference positioning algorithm, according to the M-1 distance differences and the preset coordinate data of each of the M UWB base stations, the first UWB coordinate data of the UAV is calculated, and the first UWB coordinate data of the UAV at at least two times is obtained.

[0017] The first UWB velocity data of the UAV is calculated based on the first UWB coordinate data of the UAV at at least two time points.

[0018] Optionally, the positioning algorithm based on signal time difference of arrival (TSA) calculates the first UWB coordinate data of the UAV according to the following formula, based on the M-1 distance differences and the preset coordinate data of each of the M UWB base stations:

[0019]

[0020] Wherein, x1 and y1 are the data in the UWB base station coordinates (x1, y1, z1) corresponding to the reference UWB signal, and x2, y2, z1 are the coordinates of the base station.n and y n Let (x2, y2, z2) be the coordinates of the UWB base stations corresponding to the M-1 second UWB signals. n y n , z n The data in ) , the r 21 r 31 ...r n1 These are the M-1 distance differences.

[0021] Optionally, the UAV is equipped with at least N UWB signal receiving antennas, where N is an integer greater than 1. The calculation of the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal includes:

[0022] For each of the UWB signal receiving antennas, based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, the second UWB coordinate data and the second UWB velocity data of the UAV are calculated to obtain N sets of target data. Each set of target data includes the second UWB coordinate data and the second UWB velocity data of the UAV determined based on the UWB signal received by one of the UWB signal receiving antennas.

[0023] The N sets of target data are weighted and calculated to obtain the first UWB coordinate data and the first UWB velocity data of the UAV.

[0024] Optionally, controlling the landing of the UAV based on the target coordinate data and target speed data includes:

[0025] Calculate the UWB yaw angle of the UAV based on the N sets of target data;

[0026] According to the preset second Jacobian observation matrix, the yaw angle of the UAV corresponding to the magnetometer measurement value in the ECL is replaced with the UWB yaw angle of the UAV, and the target yaw angle is obtained by fusion calculation through the EKF.

[0027] The landing of the UAV is controlled based on the target coordinate data, target speed data, and target yaw angle.

[0028] Optionally, when N equals 2, the formula for calculating the UWB yaw angle of the UAV based on the N sets of target data is as follows:

[0029]

[0030] Wherein, the ψ meas This indicates the UWB yaw angle of the drone. and The second UWB coordinate data of the two UAVs in the two sets of target data, the and stated Used to indicate respectively according to and The converted vector distance.

[0031] Optionally, before performing fusion calculations on the first UWB coordinate data, first UWB velocity data, auxiliary coordinate data, and auxiliary velocity data of the UAV using the EKF based on a preset first Jacobian observation matrix when the Extended Kalman Filter (EKF) is in the fusion calculation state, the method further includes:

[0032] Acquire GPS data, inertial measurement unit data, magnetometer data, and barometer data of the UAV;

[0033] Based on the UAV's GPS data, inertial measurement unit data, magnetometer data, and barometer data, the EKF initialization parameters are controlled and the system enters the fusion calculation state.

[0034] Secondly, embodiments of this application also provide a fusion navigation device, including:

[0035] The receiving module is used to receive the UWB signal transmitted by each of the M ultra-wideband UWB base stations at the same time, where M is an integer greater than 3.

[0036] The calculation module is used to calculate the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time corresponding to each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal.

[0037] The replacement module is used to replace the coordinate data and velocity data of the UAV corresponding to the GPS positioning technology in the Estimation and Control Library (ECL) with the first UWB coordinate data and first UWB velocity data of the UAV.

[0038] The fusion calculation module is used to perform fusion calculations on the first UWB coordinate data, first UWB velocity data, auxiliary coordinate data and auxiliary velocity data of the UAV according to the preset first Jacobian observation matrix when the extended Kalman filter (EKF) is in the fusion calculation state, so as to obtain the target coordinate data and target velocity data.

[0039] The first control module is used to control the landing of the UAV based on the target coordinate data and target speed data.

[0040] Thirdly, embodiments of this application also provide an electronic device, including:

[0041] A transceiver, a memory, a processor, and a program stored in the memory and executable on the processor; characterized in that the processor is configured to read the program in the memory to implement the steps of the positioning method as described in any one of the first aspects.

[0042] Fourthly, embodiments of this application also provide a readable storage medium for storing a program, which, when executed by a processor, implements the steps of the method described in the first aspect above.

[0043] In this embodiment, UWB signals transmitted simultaneously by each of M ultra-wideband (UWB) base stations are received. Based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, the first UWB coordinate data and the first UWB velocity data of the UAV are calculated. The coordinate data and velocity data of the UAV corresponding to the GPS positioning technology in the ECL (Elastic Compute Service) library are replaced with the first UWB coordinate data and the first UWB velocity data of the UAV. With the Extended Kalman Filter (EKF) in fusion calculation mode, the first UWB coordinate data, the first UWB velocity data, the auxiliary coordinate data, and the auxiliary velocity data of the UAV are fused using the EKF according to the preset first Jacobian observation matrix to obtain target coordinate data and target velocity data. The landing of the UAV is controlled based on the target coordinate data and the target velocity data. In this way, the coordinate and velocity data of the UAV corresponding to the original GPS positioning technology with lower positioning accuracy are replaced with the first UWB coordinate and velocity data with higher positioning accuracy. Then, EKF is used to fuse and calculate the first UWB coordinate data, the first UWB velocity data, the auxiliary coordinate data, and the auxiliary velocity data to obtain target coordinate and velocity data with higher positioning accuracy, thereby improving the fusion navigation accuracy of the UAV. Attached Figure Description

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

[0045] Figure 1 This is a flowchart illustrating the fusion navigation method provided in an embodiment of this application;

[0046] Figure 2 This is a schematic diagram of the structure of the fusion navigation device provided in the embodiments of this application;

[0047] Figure 3 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0048] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0049] The terms "first," "second," etc., used in the specification and claims of this application are used to distinguish similar objects and not to describe a specific order or sequence. It should be understood that such usage can be interchanged where appropriate so that embodiments of this application can be implemented in orders other than those illustrated or described herein, and the objects distinguished by "first," "second," etc., are generally of the same class and the number of objects is not limited; for example, a first object can be one or more. Furthermore, in the specification and claims, "and / or" indicates at least one of the connected objects, and the character " / " generally indicates that the preceding and following objects are in an "or" relationship.

[0050] like Figure 1 As shown in the schematic diagram, an embodiment of this application provides a fusion navigation method, which includes the following steps:

[0051] Step 101: Receive the UWB signal sent by each of the M ultra-wideband (UWB) base stations at the same time, where M is an integer greater than 3;

[0052] In this step, either the UAV receives UWB signals transmitted by each of the M UWB base stations at the same time, or the UAV's flight controller receives UWB signals transmitted by each of the M UWB base stations at the same time.

[0053] It should be understood that the UWB signal transmitted by each UWB base station at the same time can be used to characterize the close temporal synchronization of M UWB base stations.

[0054] For example, when M equals 4, when the UWB signal is transmitted for the first time, UWB base station 1, UWB base station 2, UWB base station 3 and UWB base station 4 each transmit a UWB signal at 12:10; when the UWB signal is transmitted for the second time, UWB base station 1, UWB base station 2, UWB base station 3 and UWB base station 4 each transmit a UWB signal at 12:11; when the UWB signal is transmitted for the third time, UWB base station 1, UWB base station 2, UWB base station 3 and UWB base station 4 each transmit a UWB signal at 12:12.

[0055] Step 102: Calculate the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal.

[0056] In this step, the arrival time refers to the time when the UWB signal is received by the UAV.

[0057] It should be understood that the preset coordinate data of a UWB base station is determined when the UWB base station is built. That is, the coordinate data of the UWB base station is determined according to the location where the UWB base station is built. M UWB base stations correspond to M preset coordinate data. The preset propagation speed of UWB signals is determined based on the physical characteristics of UWB positioning technology.

[0058] It should be noted that when each UWB base station transmits a UWB signal for the first time, since each UWB signal is transmitted synchronously, the transmission time of each UWB signal is the same, thus allowing the calculation of the total propagation time of each UWB signal. Based on the total propagation time of each UWB signal and the preset propagation speed of the UWB signal, the propagation distance of each UWB signal, i.e., the distance between each UWB base station and the UAV, can be calculated. Based on the distance between each UWB base station and the UAV, and the preset coordinate data of each UWB base station, the first UWB coordinate data of the UAV at the time of the first UWB signal transmission can be calculated.

[0059] When each UWB base station transmits a UWB signal for the second time, referring to the above implementation process, the first UWB coordinate data of the UAV at the time of the second transmission of the UWB signal can be obtained.

[0060] Furthermore, based on the first UWB coordinate data of the UAV corresponding to the first and second UWB signal transmissions, and the interval between the two UWB signal transmissions, the first UWB speed data of the UAV is calculated.

[0061] Step 103: Replace the coordinate data and velocity data of the UAV corresponding to the GPS positioning technology in the Estimation and Control Library (ECL) with the first UWB coordinate data and first UWB velocity data of the UAV.

[0062] It should be understood that, in the prior art, the ECL (Elastic Compute Link) for drone navigation typically includes coordinate and speed data of the drone obtained using GPS.

[0063] Step 104: When the Extended Kalman Filter (EKF) is in the fusion calculation state, according to the preset first Jacobian observation matrix, the EKF is used to perform fusion calculation on the first UWB coordinate data, the first UWB velocity data, the auxiliary coordinate data and the auxiliary velocity data of the UAV to obtain the target coordinate data and the target velocity data.

[0064] It should be understood that the aforementioned first Jacobian matrix refers to the observation matrix H of the UWB positioning technology relative to the state vector x. UWB That is, the corresponding Jacobian observation matrix is ​​relatively simple, being a 6×24 sparse matrix. The expression for the first Jacobian matrix is ​​as follows:

[0065]

[0066] It should be noted that the auxiliary coordinate data and auxiliary velocity data can be the UAV coordinate data and velocity data obtained by using IMU positioning technology.

[0067] It should be noted that the EKF mentioned above is a high-efficiency recursive filter.

[0068] Step 105: Control the landing of the UAV based on the target coordinate data and target speed data.

[0069] In this step, the drone controller controls the drone's landing based on the target coordinate data and target speed data.

[0070] It should be understood that only by continuously using the target coordinate data and target speed data obtained at the moment can the flight direction and trajectory of the UAV be corrected in real time during the flight of the UAV.

[0071] exist Figure 1In the embodiment shown in this application, UWB signals transmitted simultaneously by each of the M ultra-wideband (UWB) base stations are received. Based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, the first UWB coordinate data and the first UWB velocity data of the UAV are calculated. The first UWB coordinate data, the first UWB velocity data, the auxiliary coordinate data, and the auxiliary velocity data of the UAV are fused using EKF to obtain the target coordinate data and target velocity data. In this way, compared to the prior art, which uses GPS positioning technology to obtain UAV coordinate and velocity data and then fuses them with auxiliary coordinate and velocity data, this application replaces the navigation data obtained using GPS positioning technology with UWB positioning technology. Since the positioning accuracy of UWB positioning technology is higher than that of GPS positioning technology, more accurate UAV coordinate and velocity information can be obtained, thereby improving the fused navigation accuracy of the UAV.

[0072] Optionally, in some embodiments, step 102 includes:

[0073] The received M UWB signals are divided to obtain a first UWB signal and M-1 second UWB signals, wherein the first UWB signal is a reference UWB signal among the M UWB signals;

[0074] In this step, the first UWB signal and the second UWB signal are transmitted by M UWB base stations at the same time.

[0075] It should be understood that the UWB base station that transmits the aforementioned reference UWB signal can be a reference UWB base station.

[0076] Calculate the difference between the arrival time of the first UWB signal and the arrival time of each second UWB signal to obtain M-1 time differences;

[0077] It should be understood that since the first UWB signal and each of the second UWB signals are sent at the same time, the difference in propagation time between the first UWB signal and each of the second UWB signals can be calculated based on the arrival time of the first UWB signal and the arrival time of each of the second UWB signals.

[0078] For example, the arrival time of the first UWB signal is 10:10. When M equals 4, there are 3 second UWB signals. The arrival times of the 3 second UWB signals are 10:11, 10:12 and 10:13 respectively. Thus, the difference in propagation time between the first UWB signal and the 3 second UWB signals is 1 minute, 2 minutes and 3 minutes respectively.

[0079] Based on the M-1 time differences and the propagation speed of the UWB signal, calculate the distance difference between the UWB base station corresponding to the first UWB signal and the UWB base station corresponding to each second UWB signal and the UAV, and obtain M-1 distance differences;

[0080] It should be understood that the propagation speed of the UWB signal is a physical characteristic of UWB positioning technology, and will not be further explained here.

[0081] Based on the signal arrival time difference positioning algorithm, according to the M-1 distance differences and the preset coordinate data of each of the M UWB base stations, the first UWB coordinate data of the UAV is calculated, and the first UWB coordinate data of the UAV at at least two times is obtained.

[0082] The first UWB velocity data of the UAV is calculated based on the first UWB coordinate data of the UAV at at least two time points.

[0083] Optionally, the positioning algorithm based on signal time difference of arrival (TSA) calculates the first UWB coordinate data of the UAV according to the following formula, based on the M-1 distance differences and the preset coordinate data of each of the M UWB base stations:

[0084]

[0085] Wherein, x1 and y1 are the data in the UWB base station coordinates (x1, y1, z1) corresponding to the reference UWB signal, and x2, y2, z1 are the coordinates of the base station. n and y n Let (x2, y2, z2) be the coordinates of the UWB base stations corresponding to the M-1 second UWB signals. n y n , z n The data in ) , the r 21 r 31 ...r n1 These are the M-1 distance differences.

[0086] Optionally, the UAV is equipped with at least N UWB signal receiving antennas, where N is an integer greater than 1. The calculation of the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal includes:

[0087] For each of the UWB signal receiving antennas, based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, the second UWB coordinate data and the second UWB velocity data of the UAV are calculated to obtain N sets of target data. Each set of target data includes the second UWB coordinate data and the second UWB velocity data of the UAV determined based on the UWB signal received by one of the UWB signal receiving antennas.

[0088] The N sets of target data are weighted and calculated to obtain the first UWB coordinate data and the first UWB velocity data of the UAV.

[0089] Optionally, the at least N UWB signal receiving antennas on the UAV should be spaced as far apart as possible to improve measurement accuracy.

[0090] In this embodiment, by setting at least N UWB signal receiving antennas on the UAV, since each UWB signal receiving antenna corresponds to the second UWB coordinate data and the second UWB velocity data of the UAV, the first UWB coordinate data and the first UWB velocity data can be obtained by weighted calculation based on the N sets of target data. Since N sets of target data are used, when a large measurement error occurs in a single target data, the measurement error can be reduced by weighted calculation, thereby improving the measurement accuracy of the first UWB coordinate data and the first UWB velocity data of the UAV.

[0091] Optionally, controlling the landing of the UAV based on the target coordinate data and target speed data includes:

[0092] Calculate the UWB yaw angle of the UAV based on the N sets of target data;

[0093] According to the preset second Jacobian observation matrix, the yaw angle of the UAV corresponding to the magnetometer measurement value in the ECL is replaced with the UWB yaw angle of the UAV, and the target yaw angle is obtained by fusion calculation through the EKF.

[0094] The landing of the UAV is controlled based on the target coordinate data, target speed data, and target yaw angle.

[0095] In this embodiment of the application, the UWB yaw angle of the UAV is calculated based on the target coordinate data in the N sets of target data; that is, the UWB yaw angle of the UAV is calculated based on the N second UWB coordinate data.

[0096] It should be understood that the expression for the preset second Jacobian observation matrix is:

[0097] Among them, the The observation matrix for the heading angle (magnetic declination) relative to the attitude quaternion q is a 1×4 matrix.

[0098] Optionally, when N equals 2, the formula for calculating the UWB yaw angle of the UAV based on the N sets of target data is as follows:

[0099]

[0100] Wherein, the ψ meas This indicates the UWB yaw angle of the drone. and The second UWB coordinate data of the two UAVs in the two sets of target data, the and stated Used to represent the vector distances calculated from the measured coordinates.

[0101] Optionally, before performing fusion calculations on the first UWB coordinate data, first UWB velocity data, auxiliary coordinate data, and auxiliary velocity data of the UAV using the EKF based on a preset first Jacobian observation matrix when the Extended Kalman Filter (EKF) is in the fusion calculation state, the method further includes:

[0102] Acquire GPS data, inertial measurement unit data, magnetometer data, and barometer data of the UAV;

[0103] Based on the UAV's GPS data, inertial measurement unit data, magnetometer data, and barometer data, the EKF initialization parameters are controlled and the system enters the fusion calculation state.

[0104] like Figure 2 As shown in the embodiment of this application, a fusion navigation device 200 includes the following:

[0105] The receiving module 201 is used to receive the UWB signal transmitted by each of the M ultra-wideband UWB base stations at the same time, where M is an integer greater than 3.

[0106] The calculation module 202 is used to calculate the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time corresponding to each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal.

[0107] Replacement module 203 is used to replace the coordinate data and velocity data of the UAV corresponding to the GPS positioning technology in the Estimation and Control Library (ECL) with the first UWB coordinate data and first UWB velocity data of the UAV.

[0108] The fusion calculation module 204 is used to perform fusion calculation on the first UWB coordinate data, first UWB velocity data, auxiliary coordinate data and auxiliary velocity data of the UAV according to the preset first Jacobian observation matrix when the extended Kalman filter (EKF) is in the fusion calculation state, so as to obtain target coordinate data and target velocity data.

[0109] The first control module 205 is used to control the landing of the UAV based on the target coordinate data and target speed data.

[0110] Optionally, the computing module 202 includes:

[0111] The segmentation submodule is used to segment the received M UWB signals to obtain a first UWB signal and M-1 second UWB signals, wherein the first UWB signal is a reference UWB signal among the M UWB signals;

[0112] The first calculation submodule is used to calculate the difference between the arrival time of the first UWB signal and the arrival time of each second UWB signal to obtain M-1 time differences.

[0113] The second calculation submodule is used to calculate the distance difference between the UWB base station corresponding to the first UWB signal and the UWB base station corresponding to each second UWB signal and the UAV based on the M-1 time differences and the propagation speed of the UWB signal, and obtain M-1 distance differences.

[0114] The third calculation submodule is used to calculate the first UWB coordinate data of the UAV based on the signal arrival time difference positioning algorithm, according to the M-1 distance differences and the preset coordinate data of each of the M UWB base stations, and obtain the first UWB coordinate data of the UAV at at least two times.

[0115] The fourth calculation submodule is used to calculate the first UWB velocity data of the UAV based on the first UWB coordinate data of the UAV at the at least two time points.

[0116] Optionally, the UAV is equipped with at least N UWB signal receiving antennas, where N is an integer greater than 1, and the calculation module 202 includes:

[0117] The fifth calculation submodule is used to calculate the second UWB coordinate data and the second UWB velocity data of the UAV for each UWB signal receiving antenna, based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, to obtain N sets of target data. Each set of target data includes the second UWB coordinate data and the second UWB velocity data of the UAV determined based on the UWB signal received by one of the UWB signal receiving antennas.

[0118] The weighted calculation submodule is used to perform weighted calculations on the N sets of target data to obtain the first UWB coordinate data and the first UWB velocity data of the UAV.

[0119] Optionally, the first control module 205 includes:

[0120] The sixth calculation submodule is used to calculate the UWB yaw angle of the UAV based on the N sets of target data;

[0121] The replacement submodule replaces the yaw angle of the UAV corresponding to the magnetometer measurement value in the ECL with the UWB yaw angle of the UAV according to the preset second Jacobian observation matrix, and performs fusion calculation through the EKF to obtain the target yaw angle;

[0122] The control submodule is used to control the landing of the UAV based on the target coordinate data, target speed data, and target yaw angle.

[0123] Optionally, the fusion navigation device 200 further includes:

[0124] The acquisition module is used to acquire GPS data, inertial measurement unit data, magnetometer data, and barometer data of the UAV.

[0125] The second control module is used to control the EKF initialization parameters and enter the fusion calculation state based on the UAV's GPS data, inertial measurement unit data, magnetometer data, and barometer data.

[0126] The fusion navigation device 200 can achieve the functions described in the embodiments of this application. Figure 1 The various processes in the method embodiments, and the ways to achieve the same beneficial effects, will not be repeated here to avoid repetition.

[0127] This application also provides an electronic device. Please refer to [link to relevant documentation]. Figure 3 The electronic device may include a processor 301, a memory 302, and a program 3021 stored in the memory 302 and capable of running on the processor 301.

[0128] When program 3021 is executed by processor 301, it can achieve the following: Figure 1 Any steps in the corresponding method embodiments and the achievement of the same beneficial effects will not be repeated here.

[0129] Those skilled in the art will understand that all or part of the steps of the methods described in the above embodiments can be implemented by hardware related to program instructions, and the program can be stored in a readable medium.

[0130] This application embodiment also provides a readable storage medium storing a computer program, which, when executed by a processor, can perform the above-described functions. Figure 1 Any step in the corresponding method embodiment can achieve the same technical effect, and will not be repeated here to avoid repetition.

[0131] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element. Furthermore, it should be noted that the scope of the methods and apparatuses in the embodiments of this application is not limited to performing functions in the order discussed, but may also include performing functions substantially simultaneously or in the reverse order, depending on the functions involved. For example, the described methods may be performed in a different order than described, and various steps may be added, omitted, or combined. Additionally, features described with reference to certain examples may be combined in other examples.

[0132] The embodiments of this application have been described above with reference to the accompanying drawings. However, this application is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of this application without departing from the spirit and scope of the claims, and all of these forms are within the protection scope of this application.

Claims

1. A fusion navigation method, characterized in that, Applied to unmanned aerial vehicles (UAVs), wherein the UAV is equipped with at least N UWB signal receiving antennas, where N is an integer greater than 1, the fusion navigation method includes: Receive UWB signals transmitted simultaneously by each of M ultra-wideband (UWB) base stations at the same time, where M is an integer greater than 3; For each of the UWB signal receiving antennas, based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, the second UWB coordinate data and the second UWB velocity data of the UAV are calculated to obtain N sets of target data. Each set of target data includes the second UWB coordinate data and the second UWB velocity data of the UAV determined based on the UWB signal received by one of the UWB signal receiving antennas. The N sets of target data are weighted and calculated to obtain the first UWB coordinate data and the first UWB velocity data of the UAV. Replace the coordinate and velocity data of the UAV corresponding to the GPS positioning technology in the ECL control library with the first UWB coordinate and velocity data of the UAV. When the Extended Kalman Filter (EKF) is in the fusion calculation state, according to the preset first Jacobian observation matrix, the EKF is used to perform fusion calculation on the first UWB coordinate data, the first UWB velocity data, the auxiliary coordinate data and the auxiliary velocity data of the UAV to obtain the target coordinate data and the target velocity data. The drone's landing is controlled based on the target coordinate data and target speed data.

2. The fusion navigation method according to claim 1, characterized in that, The step of calculating the first UWB coordinate data and the first UWB velocity data of the UAV based on the arrival time corresponding to each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal includes: The received M UWB signals are divided to obtain a first UWB signal and M-1 second UWB signals, wherein the first UWB signal is a reference UWB signal among the M UWB signals; Calculate the difference between the arrival time of the first UWB signal and the arrival time of each second UWB signal to obtain M-1 time differences; Based on the M-1 time differences and the propagation speed of the UWB signal, calculate the distance difference between the UWB base station corresponding to the first UWB signal and the UWB base station corresponding to each second UWB signal and the UAV, and obtain M-1 distance differences; Based on the signal arrival time difference positioning algorithm, according to the M-1 distance differences and the preset coordinate data of each of the M UWB base stations, the first UWB coordinate data of the UAV is calculated, and the first UWB coordinate data of the UAV at at least two times is obtained. The first UWB velocity data of the UAV is calculated based on the first UWB coordinate data of the UAV at at least two time points.

3. The fusion navigation method according to claim 2, characterized in that, The positioning algorithm based on signal time difference of arrival (TSA) calculates the first UWB coordinate data of the UAV using the following formula, based on the M-1 distance differences and the preset coordinate data of each of the M UWB base stations: ； Wherein, x1 and y1 are the data in the UWB base station coordinates (x1, y1, z1) corresponding to the reference UWB signal, and x2, y2, z1 are the coordinates of the base station. n and y n Let (x2, y2, z2) be the coordinates of the UWB base stations corresponding to the M-1 second UWB signals. n y n , z n The data in ) , the r 21 ...r n1 These are the M-1 distance differences.

4. The fusion navigation method according to claim 1, characterized in that, The step of controlling the landing of the drone based on the target coordinate data and target speed data includes: Calculate the UWB yaw angle of the UAV based on the N sets of target data; According to the preset second Jacobian observation matrix, the yaw angle of the UAV corresponding to the magnetometer measurement value in the ECL is replaced with the UWB yaw angle of the UAV, and the target yaw angle is obtained by fusion calculation through the EKF. The landing of the UAV is controlled based on the target coordinate data, target speed data, and target yaw angle.

5. The fusion navigation method according to claim 4, characterized in that, When N equals 2, the formula for calculating the UWB yaw angle of the UAV based on the N sets of target data is as follows: ； Among them, the This indicates the UWB yaw angle of the drone. and The second UWB coordinate data of the two UAVs in the two sets of target data, the and stated Used to indicate respectively according to and The converted vector distance.

6. The fusion navigation method according to claim 1, characterized in that, Before fusing the first UWB coordinate data, first UWB velocity data, auxiliary coordinate data, and auxiliary velocity data of the UAV using the EKF according to the preset first Jacobian observation matrix when the Extended Kalman Filter (EKF) is in the fusion calculation state, the method further includes: Acquire GPS data, inertial measurement unit data, magnetometer data, and barometer data of the UAV; Based on the UAV's GPS data, inertial measurement unit data, magnetometer data, and barometer data, the EKF initialization parameters are controlled and the system enters the fusion calculation state.

7. A fusion navigation device, characterized in that, include: The receiving module is used to receive the UWB signal transmitted by each of the M ultra-wideband UWB base stations at the same time, where M is an integer greater than 3. The calculation module, for each of the UWB signal receiving antennas, calculates the second UWB coordinate data and the second UWB velocity data of the UAV based on the arrival time of each UWB signal, the preset coordinate data of each of the M UWB base stations, and the preset propagation speed of the UWB signal, to obtain N sets of target data. Each set of target data includes the second UWB coordinate data and the second UWB velocity data of the UAV determined based on the UWB signal received by one of the UWB signal receiving antennas. The UAV is equipped with at least N UWB signal receiving antennas, where N is an integer greater than 1. The N sets of target data are weighted and calculated to obtain the first UWB coordinate data and the first UWB velocity data of the UAV. The replacement module is used to replace the coordinate data and velocity data of the UAV corresponding to the GPS positioning technology in the Estimation and Control Library (ECL) with the first UWB coordinate data and first UWB velocity data of the UAV. The fusion calculation module is used to perform fusion calculations on the first UWB coordinate data, first UWB velocity data, auxiliary coordinate data and auxiliary velocity data of the UAV according to the preset first Jacobian observation matrix when the extended Kalman filter (EKF) is in the fusion calculation state, so as to obtain the target coordinate data and target velocity data. The first control module is used to control the landing of the UAV based on the target coordinate data and target speed data.

8. An electronic device, comprising: A transceiver, a memory, a processor, and a program stored in the memory and executable on the processor; characterized in that the processor is configured to read the program in the memory to implement the steps of the fusion navigation method as described in any one of claims 1 to 6.

9. A readable storage medium for storing a program, characterized in that, When the program is executed by the processor, it implements the steps in the fusion navigation method as described in any one of claims 1 to 6.

Images