A method, apparatus, device and storage medium for unmanned aerial vehicle (UAV) navigation.

Abstract

This invention discloses a UAV navigation method, device, equipment, and storage medium. The method constructs the LIEKF algorithm in a world coordinate system (local coordinate system), resulting in smaller numerical values ​​and ensuring the stability of the linear error kinematic matrix and observation matrix. A left-invariant error on a Lie group is defined in the world coordinate system, remaining invariant on the left group. Attitude, position, velocity, and other state variables, along with the error, are defined in the group space. Through the multiplicative closure of the Lie group and the affine properties between Lie algebras, and the corresponding nonlinear error quantities constructed therefrom, a state-independent linear state-space model is derived. Since the redefined velocity and position errors both include attitude terms, they can more accurately represent the true error.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) navigation technology, and in particular to a UAV navigation method, apparatus, device, and storage medium using a tightly coupled LIEKF system under large misalignment angles. Background Technology

[0002] Currently, commonly used algorithms in UAV integrated navigation include extended Kalman filters, particle filters, unscented Kalman filters, factor graphs, and graph-based SLAM (Simultaneous Localization and Mapping). The main methods for solving large misalignment angles include first performing analytical coarse alignment followed by standard Kalman fine alignment, or directly using nonlinear filters for initial alignment.

[0003] Traditional EKF UAV navigation algorithms rely heavily on the initial state. If the initial state estimation has a large error, it may be difficult to converge to the correct state quickly, or even produce large estimation deviations during the convergence process. At the same time, the combination of analytical coarse alignment and standard Kalman filter fine alignment greatly increases the time and complexity, while directly using nonlinear filters not only increases the computational load and complexity, but also causes numerical instability leading to filter divergence.

[0004] Unmanned aerial vehicle (UAV) systems that directly utilize GNSS positioning results require at least four satellites to guarantee performance. Furthermore, the measurement noise covariance of SPP (Single Point Positioning) is difficult to determine. Under these conditions, the raw GNSS measurement information is difficult to fully utilize.

[0005] In the ECEF framework, both the linear error dynamic matrix and the measurement matrix based on right-invariant error depend on the position vector. However, since the position vector is large, the possible error is also amplified, leading to instability of the above matrices, which in turn affects the numerical stability and accuracy of the filter. Summary of the Invention

[0006] In view of the above problems, the present invention provides a drone navigation method, apparatus, device, and storage medium to overcome or at least partially solve the above problems. It solves problems such as large misalignment angles of the drone and the reduced filter performance due to large position vector values ​​in the ECEF coordinate system.

[0007] This invention provides the following solution:

[0008] A drone navigation method, comprising:

[0009] Acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings;

[0010] The angular velocity contained in the gyroscope readings is converted into angular change, and combined with the attitude matrix of the UAV at the previous moment, the attitude matrix of the UAV at the current moment is calculated.

[0011] Based on the specific force contained in the accelerometer readings, combined with the attitude matrix and the velocity vector of the UAV at the previous moment, the velocity vector of the UAV transformed from the carrier coordinate system to the world coordinate system is obtained;

[0012] The position change is obtained by integrating the velocity vector. The position change is then added to the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system.

[0013] Embed the attitude matrix, the velocity vector, and the position vector into a matrix Lie group;

[0014] Based on the left-invariant error on the Lie group of the matrix, the error matrix under this framework is obtained, and the attitude error, velocity error and position error are differentiated based on the error matrix to obtain the error state differential equation.

[0015] The values ​​of each state variable at the current time are obtained by updating the state variables using the dynamic equation combined with the error state differential equation.

[0016] Based on the observation data of the Global Navigation Satellite System, the current state quantities are measured and updated to obtain the required adjustment amount for each of the current state quantities.

[0017] The optimal estimate of each state variable is made by using the adjustment amount required for each state variable at the current moment, so as to correct the attitude, position and velocity, sensor and satellite related parameters.

[0018] Preferably, the embedding matrix Lie group is represented by the following equation:

[0019]

[0020] In the formula: X t Represents the navigation state on the matrix Lie group. Represents the attitude matrix, Represents the velocity vector. This represents the position vector.

[0021] Preferably, the error state differential equation is expressed by the following equation:

[0022]

[0023] In the formula: η LX represents the left-invariant error. t -1 The inverse matrix representing the navigation state. This represents an estimate of the navigation state. This represents the attitude estimate. This represents the speed estimate. This represents the estimated location.

[0024] Preferably: The dynamic update is obtained by updating the state variables using the dynamic equation combined with the error state differential equation, and is expressed by the following equation:

[0025] x=[φvv w δp w b g b a δd trG δd ISB δd trd ]

[0026]

[0027] In the formula: φ is the deviation between the actual rotation matrix and the calculated rotation matrix of the world system and the carrier system; δv w For velocity error in the world frame; δp w b is the position error in the world frame; g For gyroscope zero bias, b a To achieve zero bias in the accelerometer; For GPS receiver clock bias; These are the inter-system deviations of GLONASS, Galileo, BDS, and QZSS relative to GPS, respectively. This is due to GPS receiver clock drift.

[0028] Preferably, the state variables at the current moment are measured and updated using the gain matrix and covariance matrix based on the observation data of the Global Navigation Satellite System.

[0029] Preferably: The measurement update of each of the state quantities at the current time is expressed by the following formula:

[0030] Z k =H k x k +v k

[0031] In the formula: Z k H represents the measurement vector. k Let x represent the observation matrix. k Represents the state vector, v k This represents the measurement noise vector.

[0032] Preferably: The optimal estimate of each state variable is made by using the adjustment amount required for each state variable at the current time, as expressed by the following formula:

[0033]

[0034]

[0035] In the formula: Indicates the updated posture. Let I represent the attitude estimate, and φ represent the identity matrix. b Indicates attitude error, v w Indicates the speed after the update. This represents the speed estimate. p represents the speed error. w Indicates the updated position. This represents the estimated location. Indicates position error, b k,g This indicates that the gyroscope has zero bias after the update. b represents the zero-bias estimate of the gyroscope. k-1,g b indicates that the gyroscope was at zero bias at the previous moment. k,a This indicates that the accelerometer has zero bias after the update. b represents the zero bias estimate of the accelerometer. k-1,a This indicates that the accelerometer had zero bias at the previous moment. This represents the estimated clock bias value of the GPS receiver. This represents the estimated inter-system bias of GLONASS relative to GPS. This represents the estimated inter-system bias of Galileo relative to GPS. This represents the estimated inter-system bias of BDS relative to GPS. This represents the estimated inter-system bias of QZSS relative to GPS. This represents the estimated clock drift value of the GPS receiver.

[0036] A drone navigation device for performing the above-described drone navigation method, the device comprising:

[0037] The measurement data acquisition unit is used to acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings.

[0038] The current attitude matrix calculation unit is used to convert the angular velocity contained in the gyroscope reading into angular change, and combine it with the attitude matrix of the UAV at the previous moment to calculate the attitude matrix of the UAV at the current moment.

[0039] The velocity vector calculation unit in the world coordinate system is used to obtain the velocity vector of the UAV from the carrier coordinate system to the world coordinate system by combining the specific force contained in the accelerometer reading, the attitude matrix, and accumulating the velocity vector of the UAV at the previous moment.

[0040] The position vector calculation unit in the world coordinate system is used to obtain the position change based on the velocity vector integration, and to accumulate the position change with the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system.

[0041] A matrix Lie group embedding unit is used to embed the attitude matrix, the velocity vector, and the position vector into a matrix Lie group;

[0042] The error state differential unit is used to obtain the error matrix under the framework based on the left invariant error on the Lie group of the matrix, and to differentiate the attitude error, velocity error and position error based on the error matrix to obtain the error state differential equation.

[0043] The dynamics update unit is used to update the state variables by combining the dynamic equation with the error state differential equation to obtain the values ​​of each state variable at the current time after dynamic update.

[0044] The measurement update unit is used to perform measurement updates on each of the state quantities at the current time based on the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current time.

[0045] The parameter correction unit is used to make an optimal estimate of each of the state variables using the adjustment amount required for each of the state variables at the current time, so as to correct the attitude, position and velocity, sensor and satellite related parameters.

[0046] A drone navigation device, the device including a processor and a memory:

[0047] The memory is used to store program code and transmit the program code to the processor;

[0048] The processor is used to execute the above-described UAV navigation method according to the instructions in the program code.

[0049] A computer-readable storage medium for storing program code for executing the above-described UAV navigation method.

[0050] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0051] This application provides a UAV navigation method, apparatus, device, and storage medium. The method constructs a LIEKF algorithm in a world coordinate system (local coordinate system), resulting in smaller numerical values ​​and ensuring the stability of the linear error kinematic matrix and observation matrix. A left-invariant error on a Lie group is defined in the world coordinate system, remaining invariant on the left group. Attitude, position, velocity, and other state variables, along with the error, are defined in the group space. Through the multiplicative closure of the Lie group and the affine properties between Lie algebras, and the corresponding nonlinear error quantities constructed therefrom, a state-independent linear state-space model is derived. Since the redefined velocity and position errors include attitude terms, they can more accurately represent the true error. Simultaneously, it enables the UAV to achieve rapid filter convergence under large misalignment angles. This solves the problems of low accuracy and difficulty in achieving rapid convergence, or even non-convergence, of traditional EKF loose-combination navigation under the ECEF framework for UAVs.

[0052] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

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

[0054] Figure 1 This is a flowchart of a drone navigation method provided in an embodiment of the present invention;

[0055] Figure 2 This is a flowchart of the LIEKF compact combination algorithm under the world system provided in the embodiments of the present invention;

[0056] Figure 3 This is the SINS / SPP conventional EKF tight-combination attitude error diagram provided in the embodiment of the present invention;

[0057] Figure 4 This is the SINS / SPP left-invariant EKF tight combination attitude error diagram provided in the embodiment of the present invention;

[0058] Figure 5 This is a SINS / SPP conventional EKF tight combination position error diagram provided in an embodiment of the present invention;

[0059] Figure 6 This is a diagram showing the position error of the SINS / SPP left-invariant EKF tight combination provided in an embodiment of the present invention.

[0060] Figure 7This is a speed error diagram of the SINS / SPP conventional EKF tight combination provided in an embodiment of the present invention;

[0061] Figure 8 This is a velocity error diagram of the SINS / SPP left-invariant EKF compact combination provided in an embodiment of the present invention;

[0062] Figure 9 This is a schematic diagram of a drone navigation device provided in an embodiment of the present invention;

[0063] Figure 10 This is a schematic diagram of a drone navigation device provided in an embodiment of the present invention. Detailed Implementation

[0064] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention are within the scope of protection of the present invention.

[0065] See Figure 1 This invention provides a drone navigation method, such as... Figure 1 As shown, the method may include:

[0066] S101: Acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings;

[0067] S102: Convert the angular velocity contained in the gyroscope reading into angular change, and combine it with the attitude matrix of the UAV at the previous moment to calculate the attitude matrix of the UAV at the current moment.

[0068] S103: Based on the specific force contained in the accelerometer reading, combined with the attitude matrix and the velocity vector of the UAV at the previous moment, the velocity vector of the UAV transformed from the carrier coordinate system to the world coordinate system is obtained;

[0069] S104: The position change is obtained by integrating the velocity vector, and the position change is added to the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system.

[0070] S105: Embed the attitude matrix, the velocity vector, and the position vector into a Lie group; specifically, in this application embodiment, the embedded Lie group can be represented by the following formula:

[0071]

[0072] In the formula: Xt Represents the navigation state on the matrix Lie group. Represents the attitude matrix, Represents the velocity vector. This represents the position vector.

[0073] S106: Based on the left-invariant error on the Lie group of the matrix, the error matrix under this framework is obtained, and the attitude error, velocity error, and position error are differentiated based on the error matrix to obtain the error state differential equation; in specific implementation, the embodiment of this application can provide that the error state differential equation is expressed by the following formula:

[0074]

[0075] In the formula: η L X represents the left-invariant error. t -1 The inverse matrix representing the navigation state. This represents an estimate of the navigation state. This represents the attitude estimate. This represents the speed estimate. This represents the estimated location.

[0076] S107: The state variables are updated using the dynamic equation combined with the error state differential equation to obtain the values ​​of each state variable at the current time after the dynamic update; in specific implementation, embodiments of this application can provide that the dynamic update is obtained by updating the state variables using the dynamic equation combined with the error state differential equation, expressed by the following formula:

[0077]

[0078] In the formula: φ is the deviation between the actual rotation matrix and the calculated rotation matrix of the world system and the carrier system; δv w For velocity error in the world frame; δp w b is the position error in the world frame; g For gyroscope zero bias, b a To achieve zero bias in the accelerometer; For GPS receiver clock bias; These are the inter-system deviations of GLONASS, Galileo, BDS, and QZSS relative to GPS, respectively. This is due to GPS receiver clock drift.

[0079] S108: Based on the observation data of the Global Navigation Satellite System, the state quantities at the current time are measured and updated to obtain the adjustment amount required for each of the state quantities at the current time; in specific implementation, the embodiments of this application can provide measurement and update of each of the state quantities at the current time using a gain matrix and a covariance matrix based on the observation data of the Global Navigation Satellite System.

[0080] Furthermore, the measurement update of each of the aforementioned state quantities at the current moment is represented by the following formula:

[0081] Z k =H k x k +v k

[0082] In the formula: Z k H represents the measurement vector. k Let x represent the observation matrix. k Represents the state vector, v k This represents the measurement noise vector.

[0083] S109: Optimize the state variables by using the adjustment amount required for each state variable at the current moment, so as to correct the attitude, position and velocity, sensor and satellite related parameters.

[0084] In specific implementation, embodiments of this application can provide an optimal estimate of each of the state variables using the adjustment amount required by each state variable at the current time, expressed by the following formula:

[0085]

[0086]

[0087] In the formula: Indicates the updated posture. Let I represent the attitude estimate, and φ represent the identity matrix. b Indicates attitude error, v w Indicates the speed after the update. This represents the speed estimate. p represents the speed error. w Indicates the updated position. This represents the estimated location. Indicates position error, b k,g This indicates that the gyroscope has zero bias after the update. b represents the zero-bias estimate of the gyroscope. k-1,g b indicates that the gyroscope was at zero bias at the previous moment. k,a This indicates that the accelerometer has zero bias after the update. b represents the zero bias estimate of the accelerometer. k-1,aThis indicates that the accelerometer had zero bias at the previous moment. This represents the estimated clock bias value of the GPS receiver. This represents the estimated inter-system bias of GLONASS relative to GPS. This represents the estimated inter-system bias of Galileo relative to GPS. This represents the estimated inter-system bias of BDS relative to GPS. This represents the estimated inter-system bias of QZSS relative to GPS. This represents the estimated clock drift value of the GPS receiver.

[0088] The UAV navigation method provided in this application constructs the navigation state and kinematic equations considering the Earth's rotation in a world coordinate system. Simultaneously, a left-invariant error on a matrix Lie group is defined in this coordinate system, and an observation matrix in the world coordinate system is constructed based on GNSS observations. A compact combination approach is used to reduce the impact of sensor errors on the system's navigation accuracy, thereby improving the robustness and reliability of the navigation system.

[0089] The method provided in this application includes an extended Kalman filter algorithm based on left-invariant error within a Lie group framework, realizing state error estimation and state update. It can handle nonlinear problems, converges quickly under large misalignment angles, and maintains high accuracy. The method is based entirely on a world coordinate system, which addresses the problem that a large position vector causes an increase in error, thus affecting the stability and accuracy of the filter.

[0090] The drone navigation method provided in this application will be described in detail below.

[0091] This application proposes a left-invariant extended Kalman filter and SINS / SPP tight combination algorithm based on a Lie group framework for high-precision positioning and attitude estimation of unmanned aerial vehicles (UAVs). By mapping the UAV's state vector to the tangent space of the Lie group for linearization, linearization errors are reduced, improving the robustness and accuracy of the filter. Simultaneously, inertial sensor data and GNSS pseudorange measurements are tightly combined to correct time-accumulated errors while maintaining high dynamic response capabilities. The overall algorithm flowchart is shown below. Figure 2 As shown.

[0092] The specific implementation steps are as follows:

[0093] Acquire data from the inertial measurement unit (IMU) on the UAV, including accelerometer and gyroscope readings, and preprocess outliers and missing values ​​to ensure data reliability;

[0094] Angular velocity measured by IMU gyroscope This is converted into an angle change, and combined with the attitude matrix from the previous moment, the attitude matrix of the UAV at the current moment is calculated.

[0095] Comparison force based on IMU accelerometer measurement By combining attitude information and summing the velocity vector from the previous moment, the velocity vector of the UAV in the world coordinate system after transformation is obtained.

[0096] The position change is obtained by integrating the updated velocity vector and then summing it with the position vector from the previous moment to obtain the UAV's position vector in the world coordinate system.

[0097] Embed the attitude matrix, velocity vector, and position vector obtained above into a matrix Lie group (Equation 1);

[0098]

[0099] Based on the left-invariant error on the Lie group, the error matrix under this framework is obtained (Formula 2). Based on this, the attitude error, velocity error and position error are further differentiated to obtain the error state differential equation.

[0100]

[0101] The state variables (Formula 3) are updated using the dynamic equations to obtain the values ​​of each state variable at the current time after the dynamic update.

[0102]

[0103] Where φ is the deviation between the actual rotation matrix and the calculated rotation matrix of the world system and the carrier system; δv w For velocity error in the world frame; δp w b is the position error in the world frame; g For gyroscope zero bias, b a To achieve zero bias in the accelerometer; For GPS receiver clock bias; These are the inter-system deviations of GLONASS, Galileo, BDS, and QZSS relative to GPS, respectively. This is due to GPS receiver clock drift.

[0104] Based on GNSS observation data, measurement updates are performed to determine the required adjustments for each state variable. The gain matrix and the calculation methods for the state update and covariance matrix are consistent with the traditional EKF form. The required adjustments for each state variable after dynamic update are determined as follows:

[0105] Z k =Hk x k +v k #(5)

[0106] The status feedback is based on the error status calculated in step 8 to correct the attitude, position, velocity, and related parameters of the sensors and satellite, thereby obtaining the optimal estimate.

[0107]

[0108] In the formula: Indicates the updated posture. Let I represent the attitude estimate, and φ represent the identity matrix. b Indicates attitude error, v w Indicates the speed after the update. This represents the speed estimate. p represents the speed error. w Indicates the updated position. This represents the estimated location. Indicates position error, b k,g This indicates that the gyroscope has zero bias after the update. b represents the zero-bias estimate of the gyroscope. k-1,g b indicates that the gyroscope was at zero bias at the previous moment. k,a This indicates that the accelerometer has zero bias after the update. b represents the zero bias estimate of the accelerometer. k-1,a This indicates that the accelerometer had zero bias at the previous moment. This represents the estimated clock bias value of the GPS receiver. This represents the estimated inter-system bias of GLONASS relative to GPS. This represents the estimated inter-system bias of Galileo relative to GPS. This represents the estimated inter-system bias of BDS relative to GPS. This represents the estimated inter-system bias of QZSS relative to GPS. This represents the estimated clock drift value of the GPS receiver.

[0109] Experimental results

[0110] This invention uses a Trimble R10 GNSS data acquisition system and NovAtel SPAN-CPT to acquire inertial navigation data. Post-processing results from the commercial software Inertia Explorer (IE) are used as reference values. The frequency is 100Hz, the gyroscope's zero-bias instability is 1° / h, and the random walk is... The accelerometer's zero-bias instability and random walk are 0.75 mg and 0.75 mg, respectively.

[0111] The analysis is conducted by comparing the three error models of this invention and the traditional EKF algorithm under a large misalignment angle α = [8.5° 8.5° 45°].

[0112] Table 1. RMS Errors of Attitude, Position, and Velocity

[0113]

[0114] pass Figure 3-8 As shown in Table 1, when using consumer-grade SINS for integrated navigation, the position error and velocity error are basically the same. However, in terms of attitude error, the present invention is significantly faster than the traditional algorithm in terms of convergence speed of heading angle, and the accuracy of each axis is significantly higher than that of the traditional algorithm.

[0115] In summary, the UAV navigation method provided in this application, due to the Earth's radius of approximately 6371 km and the global coordinate system position vector value of 10... 6 This method constructs the LIEKF algorithm in the world coordinate system (local coordinate system), resulting in smaller numerical values ​​and ensuring the stability of the linear error kinematic matrix and observation matrix. A left-invariant error on the Lie group is defined in the world coordinate system, remaining invariant on the left group. Attitude, position, velocity, and other state variables, along with the error, are defined in the group space. Through the multiplicative closure of the Lie group and the affine properties between Lie algebras, and the corresponding nonlinear error quantities constructed from this, a state-independent linear state-space model is derived. Since the redefined velocity and position errors include attitude terms, they can more accurately represent the true error. Simultaneously, it enables rapid filter convergence for UAVs under large misalignment angles. This solves the problems of low accuracy and difficulty in achieving rapid convergence, or even non-convergence, of traditional EKF loose-array navigation under the ECEF framework for UAVs.

[0116] See Figure 9 This application embodiment can also provide a drone navigation device, such as... Figure 9 As shown, the device for performing the above-described UAV navigation method may include:

[0117] The measurement data acquisition unit 901 is used to acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings.

[0118] The current attitude matrix calculation unit 902 is used to convert the angular velocity contained in the gyroscope reading into angular change, and combine it with the attitude matrix of the UAV at the previous moment to calculate the attitude matrix of the UAV at the current moment.

[0119] The velocity vector calculation unit 903 in the world coordinate system is used to obtain the velocity vector of the UAV from the carrier coordinate system to the world coordinate system based on the specific force contained in the accelerometer reading, combined with the attitude matrix and the velocity vector of the UAV at the previous moment.

[0120] The position vector calculation unit 904 in the world coordinate system is used to obtain the position change based on the velocity vector integration, and to accumulate the position change with the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system.

[0121] The matrix Lie group embedding unit 905 is used to embed the attitude matrix, the velocity vector and the position vector into a matrix Lie group;

[0122] Error state differential unit 906 is used to obtain the error matrix under the framework based on the left invariant error on the matrix Lie group, and differentiate the attitude error, velocity error and position error based on the error matrix to obtain the error state differential equation;

[0123] The dynamics update unit 907 is used to update the state variables by combining the dynamic equation with the error state differential equation to obtain the values ​​of each state variable at the current time after dynamic update.

[0124] The measurement update unit 908 is used to perform measurement updates on each of the state quantities at the current time based on the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current time.

[0125] The parameter correction unit 909 is used to make an optimal estimate of each of the state variables using the adjustment amount required for each of the state variables at the current time, so as to realize the correction of attitude, position velocity, sensor and satellite related parameters.

[0126] This application embodiment can also provide a drone navigation device, the device including a processor and a memory:

[0127] The memory is used to store program code and transmit the program code to the processor;

[0128] The processor is used to execute the steps of the above-described UAV navigation method according to the instructions in the program code.

[0129] like Figure 10 As shown in the figure, an embodiment of this application provides a drone navigation device, which may include: a processor 10, a memory 11, a communication interface 12, and a communication bus 13. The processor 10, the memory 11, and the communication interface 12 all communicate with each other through the communication bus 13.

[0130] In the embodiments of this application, the processor 10 may be a central processing unit (CPU), an application-specific integrated circuit, a digital signal processor, a field-programmable gate array, or other programmable logic devices.

[0131] The processor 10 can call programs stored in the memory 11. Specifically, the processor 10 can execute operations in the embodiments of the UAV navigation method.

[0132] The memory 11 is used to store one or more programs. The programs may include program code, which includes computer operation instructions. In this embodiment, the memory 11 stores at least a program for implementing the following functions:

[0133] Acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings;

[0134] The angular velocity contained in the gyroscope readings is converted into angular change, and combined with the attitude matrix of the UAV at the previous moment, the attitude matrix of the UAV at the current moment is calculated.

[0135] Based on the specific force contained in the accelerometer readings, combined with the attitude matrix and the velocity vector of the UAV at the previous moment, the velocity vector of the UAV transformed from the carrier coordinate system to the world coordinate system is obtained;

[0136] The position change is obtained by integrating the velocity vector. The position change is then added to the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system.

[0137] Embed the attitude matrix, the velocity vector, and the position vector into a matrix Lie group;

[0138] Based on the left-invariant error on the Lie group of the matrix, the error matrix under this framework is obtained, and the attitude error, velocity error and position error are differentiated based on the error matrix to obtain the error state differential equation.

[0139] The values ​​of each state variable at the current time are obtained by updating the state variables using the dynamic equation combined with the error state differential equation.

[0140] Based on the observation data of the Global Navigation Satellite System, the current state quantities are measured and updated to obtain the required adjustment amount for each of the current state quantities.

[0141] The optimal estimate of each state variable is made by using the adjustment amount required for each state variable at the current moment, so as to correct the attitude, position and velocity, sensor and satellite related parameters.

[0142] In one possible implementation, the memory 11 may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function (such as file creation or data read / write). The data storage area may store data created during use, such as initialization data.

[0143] In addition, memory 11 may include high-speed random access memory, and may also include non-volatile memory, such as at least one disk storage device or other volatile solid-state storage device.

[0144] Communication interface 12 can be an interface for the communication module, used to connect with other devices or systems.

[0145] Of course, it should be noted that, Figure 10 The structure shown does not constitute a limitation on the UAV navigation device in the embodiments of this application. In practical applications, the UAV navigation device may include more than Figure 10 More or fewer components as shown, or combinations of certain components.

[0146] This application embodiment may also provide a computer-readable storage medium for storing program code for executing the steps of the above-described UAV navigation method.

[0147] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, 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 said element.

[0148] As can be seen from the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented by means of software plus necessary general-purpose hardware platforms. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of this application.

[0149] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, for system or system embodiments, since they are basically similar to method embodiments, the description is relatively simple, and relevant parts can be referred to the descriptions in the method embodiments. The systems and system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without creative effort.

[0150] The above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A navigation method for unmanned aerial vehicles (UAVs), characterized in that, include: Acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings; The angular velocity contained in the gyroscope readings is converted into angular change, and combined with the attitude matrix of the UAV at the previous moment, the attitude matrix of the UAV at the current moment is calculated. Based on the specific force contained in the accelerometer readings, combined with the attitude matrix and the velocity vector of the UAV at the previous moment, the velocity vector of the UAV transformed from the carrier coordinate system to the world coordinate system is obtained; The position change is obtained by integrating the velocity vector. The position change is then added to the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system. Embed the attitude matrix, the velocity vector, and the position vector into a matrix Lie group; Based on the left-invariant error on the Lie group of the matrix, the error matrix under the frame is obtained, and the attitude error, velocity error and position error are differentiated based on the error matrix to obtain the error state differential equation. The state variables are updated using the dynamic equations combined with the error state differential equations to obtain the updated values ​​of each state variable at the current time; the dynamic update is expressed by the following equation: In the formula: φ is the deviation between the actual rotation matrix and the calculated rotation matrix of the world system and the carrier system; δv w For velocity error in the world frame; δp w b is the position error in the world frame; g For gyroscope zero bias, b a To achieve zero bias in the accelerometer; For GPS receiver clock bias; These are the inter-system deviations of GLONASS, Galileo, BDS, and QZSS relative to GPS, respectively. This is due to GPS receiver clock drift. Based on the observation data of the Global Navigation Satellite System, the current state quantities are measured and updated to obtain the required adjustment amount for each of the current state quantities. The optimal estimate of each state variable is obtained by using the adjustment amount required for each state variable at the current moment, so as to correct the attitude, position, velocity, sensor, and satellite-related parameters; the optimal estimate is expressed by the following formula: In the formula: Indicates the updated posture. Let I represent the attitude estimate, and φ represent the identity matrix. b Indicates attitude error, v w Indicates the speed after the update. This represents the speed estimate. p represents the speed error. w Indicates the updated position. This represents the estimated location. Indicates position error, b k,g This indicates that the gyroscope has zero bias after the update. b represents the zero-bias estimate of the gyroscope. k-1,g b indicates that the gyroscope was at zero bias at the previous moment. k,a This indicates that the accelerometer has zero bias after the update. b represents the zero bias estimate of the accelerometer. k-1,a This indicates that the accelerometer had zero bias at the previous moment. This represents the estimated clock bias value of the GPS receiver. This represents the estimated inter-system bias of GLONASS relative to GPS. This represents the estimated inter-system bias of Gali Leo relative to GPS. This represents the estimated inter-system bias of BDS relative to GPS. This represents the estimated inter-system bias of QZSS relative to GPS. This represents the estimated clock drift value of the GPS receiver.

2. The UAV navigation method according to claim 1, characterized in that, The Lie group of the embedding matrix is ​​represented by the following equation: In the formula: X t Represents the navigation state on the matrix Lie group. Represents the attitude matrix. Represents the velocity vector. This represents the position vector.

3. The UAV navigation method according to claim 1, characterized in that, The error state differential equation is expressed by the following equation: In the formula: η L X represents the left-invariant error. t -1 The inverse matrix representing the navigation state. This represents an estimate of the navigation state. This represents the attitude estimate. This represents the speed estimate. This represents the estimated location.

4. The UAV navigation method according to claim 1, characterized in that, Based on observation data from the Global Navigation Satellite System, the gain matrix and covariance matrix are used to measure and update the state variables at the current moment.

5. The UAV navigation method according to claim 4, characterized in that, The measurement update of each of the aforementioned state quantities at the current moment is represented by the following formula: Z k =H k x k +v k In the formula: Z k H represents the measurement vector. k Let x represent the observation matrix. k Represents the state vector, v k This represents the measurement noise vector.

6. A drone navigation device, characterized in that, The apparatus for performing the unmanned aerial vehicle navigation method according to any one of claims 1-5, the apparatus comprising: The measurement data acquisition unit is used to acquire measurement data from the inertial measurement unit on the UAV, including accelerometer readings and gyroscope readings. The current attitude matrix calculation unit is used to convert the angular velocity contained in the gyroscope reading into angular change, and combine it with the attitude matrix of the UAV at the previous moment to calculate the attitude matrix of the UAV at the current moment. The velocity vector calculation unit in the world coordinate system is used to obtain the velocity vector of the UAV from the carrier coordinate system to the world coordinate system by combining the specific force contained in the accelerometer reading, the attitude matrix, and accumulating the velocity vector of the UAV at the previous moment. The position vector calculation unit in the world coordinate system is used to obtain the position change based on the velocity vector integration, and to accumulate the position change with the position vector of the UAV at the previous moment to obtain the position vector of the UAV in the world coordinate system. A matrix Lie group embedding unit is used to embed the attitude matrix, the velocity vector, and the position vector into a matrix Lie group; The error state differential unit is used to obtain the error matrix under the framework based on the left invariant error on the Lie group of the matrix, and to differentiate the attitude error, velocity error and position error based on the error matrix to obtain the error state differential equation. The dynamics update unit is used to update the state variables by combining the dynamic equation with the error state differential equation to obtain the values ​​of each state variable at the current time after dynamic update. The measurement update unit is used to perform measurement updates on each of the state quantities at the current time based on the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current time. The parameter correction unit is used to make an optimal estimate of each of the state variables using the adjustment amount required for each of the state variables at the current time, so as to correct the attitude, position and velocity, sensor and satellite related parameters.

7. A drone navigation device, characterized in that, The device includes a processor and a memory: The memory is used to store program code and transmit the program code to the processor; The processor is configured to execute the UAV navigation method according to any one of claims 1-6 according to the instructions in the program code.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium is used to store program code for executing the UAV navigation method according to any one of claims 1-6.

Images