Unmanned aerial vehicle navigation method, device, equipment and storage medium

Abstract

The invention discloses an unmanned aerial vehicle navigation method, device and equipment and a storage medium, and the method constructs an LIEKF algorithm on a world coordinate system (local coordinate system), enables the value to be small, and guarantees the stability of a linear error kinematics matrix and an observation matrix. A left invariant error on a matrix Lie group is defined under a world coordinate system, and the error is kept unchanged on a left group. According to the method, state quantities such as attitude, position and speed and errors are defined in a group space, and a linear state space model independent of the state is deduced through multiplication closure of the Lie group and affine characteristics between Lie algebra and corresponding nonlinear error quantities constructed by the multiplication closure and the Lie algebra. Because the redefined speed error and position error both comprise attitude items, the real error can be represented more accurately.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle navigation technology, and in particular to a tightly combined LIEKF unmanned aerial vehicle navigation method, device, equipment and storage medium under large misalignment angle. Background Art

[0002] At present, the commonly used algorithms in the integrated navigation of UAVs include extended Kalman filter, particle filter, unscented Kalman filter, factor graph and graph-based SLAM (Simultaneous Localization and Mapping), etc. The main methods to solve the large misalignment angle are to first use analytical coarse alignment and then use standard Kalman for fine alignment, or directly use nonlinear filters for initial alignment.

[0003] The traditional EKF UAV navigation algorithm is more dependent on the initial state. If there is a large error in the initial state estimation, it may be difficult to converge to the correct state quickly, and may even produce a large estimation deviation 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. In addition to making the calculation larger and more complex, the direct use of nonlinear filters will also cause numerical instability and filter divergence.

[0004] If the drone uses the GNSS positioning results directly, at least four satellites are needed to ensure performance. At the same time, the measurement noise covariance of SPP (Single Point Positioning) is difficult to determine. Under such conditions, the original GNSS measurement information is difficult to be fully used.

[0005] In the ECEF framework, the linear error dynamic matrix and measurement matrix based on the right invariant error both depend on the position vector. However, since the position vector is large, the possible errors are also magnified, resulting in instability of the above matrices, thus affecting 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, device, equipment and storage medium for overcoming the above problems or at least partially solving the above problems. The method solves the problems of large misalignment angle of the drone and the large position vector value in the ECEF coordinate system leading to reduced filter performance.

[0007] The present invention provides the following scheme:

[0008] A drone navigation method, comprising:

[0009] Obtaining measurement data of an inertial measurement unit on the drone, the measurement data including accelerometer readings and gyroscope readings;

[0010] Convert the angular velocity contained in the gyroscope reading into an angle change, combine it with the attitude matrix of the drone at the previous moment, and calculate the attitude matrix of the drone at the current moment;

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

[0012] A position change is obtained according to the integral of the velocity vector, and the position change is added to the position vector of the UAV at a previous moment to obtain the position vector of the UAV in the world coordinate system;

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

[0014] According to the left invariant error on the matrix Lie group, an error matrix under the framework is obtained, and the attitude error, velocity error and position error are differentiated based on the error matrix to obtain an error state differential equation;

[0015] Using a dynamic equation combined with the error state differential equation to update the state quantity to obtain the value of each state quantity at the current moment after dynamic update;

[0016] Measuring and updating each of the state quantities at the current moment according to the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment;

[0017] The optimal estimation of each state quantity is performed using the adjustment amount required for each state quantity at the current moment, so as to realize the correction of attitude, position speed, sensor, and satellite related parameters.

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

[0019]

[0020] Where: X t represents the navigation state on the matrix Lie group, represents the posture matrix, represents the velocity vector, Represents the position vector.

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

[0022]

[0023] Where: η Lrepresents the left invariant error, X t -1 represents the inverse matrix of the navigation state, represents an estimate of the navigation state, represents the estimated value of the pose, represents the estimated value of speed, Represents a position estimate.

[0024] Preferably: the dynamics update is obtained by using the dynamics equation combined with the error state differential equation to update the state quantity, which is represented by the following formula:

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

[0026]

[0027] Where: φ is the deviation between the real rotation matrix of the world system and the carrier system and the calculated rotation matrix; δv w is the velocity error in the world system; δp w is the position error in the world system; b g is the gyro bias, b a is the accelerometer zero bias; is the GPS receiver clock error; They are the inter-system biases of GLONASS, Galileo, BDS, and QZSS relative to GPS; It is the GPS receiver clock drift.

[0028] Preferably: the gain matrix and the covariance matrix are used to measure and update each of the state quantities at the current moment according to the observation data of the global navigation satellite system.

[0029] Preferably, the measurement and update of each state quantity at the current moment is expressed by the following formula:

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

[0031] Where: Z k represents the measurement vector, H k represents the observation matrix, x k represents the state vector, v k represents the measurement noise vector.

[0032] Preferably, the optimal estimation of each state quantity is performed using the adjustment amount required for each state quantity at the current moment, which is expressed by the following formula:

[0033]

[0034]

[0035] Where: Indicates the updated posture. represents the estimated value of the posture, I represents the unit matrix, φ b represents the attitude error, v w Indicates the updated speed. represents the estimated value of speed, represents the speed error, p w Indicates the updated position. represents the estimated position, represents the position error, b k,g Indicates the gyro bias after update. represents the gyro bias estimate, b k-1,g Indicates the gyro zero bias at the last moment, b k,a Indicates the accelerometer zero bias after update, represents the accelerometer bias estimate, b k-1,a Indicates the accelerometer zero bias at the last moment, represents the estimated value of GPS receiver clock error, represents the estimated inter-system bias of GLONASS relative to GPS, represents the estimated inter-system bias of Galileo relative to GPS, represents the estimated value of the inter-system bias between BDS and GPS, represents the estimated inter-system bias of QZSS relative to GPS, Represents the estimated value of GPS receiver clock drift.

[0036] A drone navigation device, used to execute the above-mentioned drone navigation method, the device comprising:

[0037] A measurement data acquisition unit, used to acquire measurement data of an inertial measurement unit on the drone, wherein the measurement data includes an accelerometer reading and a gyroscope reading;

[0038] A current moment attitude matrix calculation unit, used to convert the angular velocity contained in the gyroscope reading into an angle change, and combine the attitude matrix of the drone at the previous moment to obtain the attitude matrix of the drone at the current moment by calculation;

[0039] A velocity vector calculation unit in a world coordinate system, used to obtain a velocity vector of the UAV converted from a carrier coordinate system to a world coordinate system based on the specific force contained in the accelerometer reading, in combination with the attitude matrix and by accumulating the velocity vector of the UAV at a previous moment;

[0040] A position vector calculation unit in a world coordinate system, used to obtain a position change according to the velocity vector integral, and to accumulate the position change with the position vector of the UAV at a previous moment to obtain a position vector of the UAV in a world coordinate system;

[0041] A matrix Lie group embedding unit, used for embedding the posture matrix, the velocity vector and the position vector into a matrix Lie group;

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

[0043] A dynamic updating unit, used to update the state quantity by using a dynamic equation in combination with the error state differential equation to obtain the value of each state quantity at the current moment after dynamic updating;

[0044] A measurement and updating unit, configured to measure and update each of the state quantities at the current moment according to observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment;

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

[0046] A drone navigation device, the device comprising 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-mentioned drone navigation method according to the instructions in the program code.

[0049] A computer-readable storage medium is used to store program codes, and the program codes are used to execute the above-mentioned drone navigation method.

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

[0051] A method, device, equipment and storage medium for unmanned aerial vehicle navigation provided by an embodiment of the present application, the method constructs a LIEKF algorithm on a world coordinate system (local coordinate system), so that the value is small, and the stability of the linear error kinematic matrix and the observation matrix is ​​ensured. The left invariant error on the matrix Lie group is defined in the world coordinate system, and the error remains unchanged on the left group. The state quantities and errors such as attitude, position, and speed are defined in the group space, and the linear state space model independent of the state is derived through the multiplication closure of the Lie group and the affine characteristics between the Lie algebra, and the corresponding nonlinear error quantity constructed therefrom. Since the attitude terms are included in the redefined speed error and position error, the true error can be more accurately characterized. At the same time, the unmanned aerial vehicle can achieve rapid convergence of the filter under large misalignment angles. It solves the problem that the traditional EKF loose combination navigation accuracy of the unmanned aerial vehicle under the ECEF framework is low, and it is difficult to converge quickly or even difficult to converge under large misalignment angles.

[0052] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0054] Figure 1 is a flow chart of a drone navigation method provided by an embodiment of the present invention;

[0055] Figure 2 is a flow chart of the LIEKF tight combination algorithm in the world system provided by an embodiment of the present invention;

[0056] Figure 3 It is a SINS / SPP traditional EKF tight combination attitude error diagram provided by an embodiment of the present invention;

[0057] Figure 4 is a SINS / SPP left-invariant EKF tight combination attitude error graph provided by an embodiment of the present invention;

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

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

[0060] Figure 7It is a SINS / SPP traditional EKF tight combination velocity error diagram provided by an embodiment of the present invention;

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

[0062] Fig. 9 is a schematic diagram of a drone navigation device provided by an embodiment of the present invention;

[0063] Fig.10 Schematic diagram of a drone navigation device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0064] The technical scheme in the embodiment of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all of the embodiments. Based on the embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field belong to the scope of protection of the present invention.

[0065] See also Figure 1 , is a drone navigation method provided by an embodiment of the present invention, such as Figure 1 As shown, the method may include:

[0066] S101: Acquire measurement data of an inertial measurement unit on the drone, where the measurement data includes an accelerometer reading and a gyroscope reading;

[0067] S102: converting the angular velocity contained in the gyroscope reading into an angle change, combining it with the attitude matrix of the drone at the previous moment, and obtaining the attitude matrix of the drone at the current moment by calculation;

[0068] S103: Based on the specific force included in the accelerometer reading, combined with the attitude matrix and accumulating the velocity vector of the drone at the previous moment, obtain the velocity vector of the drone converted from the carrier coordinate system to the world coordinate system;

[0069] S104: obtaining a position change amount according to the velocity vector integral, and accumulating the position change amount with the position vector of the UAV at a previous moment to obtain a position vector of the UAV in a world coordinate system;

[0070] S105: embedding the posture matrix, the velocity vector, and the position vector into a matrix Lie group; in specific implementation, the embodiment of the present application may provide that the embedded matrix Lie group is represented by the following formula:

[0071]

[0072] Where: Xt represents the navigation state on the matrix Lie group, represents the posture matrix, represents the velocity vector, Represents the position vector.

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

[0074]

[0075] Where: η L represents the left invariant error, X t -1 represents the inverse matrix of the navigation state, represents an estimate of the navigation state, represents the estimated value of the pose, represents the estimated value of speed, Represents a position estimate.

[0076] S107: Using a dynamic equation in combination with the error state differential equation to update the state quantity to obtain the value of each state quantity at the current moment after the dynamic update; in specific implementation, the embodiment of the present application can provide that the dynamic update is obtained by using a dynamic equation in combination with the error state differential equation to update the state quantity, which is represented by the following formula:

[0077]

[0078] Where: φ is the deviation between the real rotation matrix of the world system and the carrier system and the calculated rotation matrix; δv w is the velocity error in the world system; δp w is the position error in the world system; b g is the gyro bias, b a is the accelerometer zero bias; is the GPS receiver clock error; They are the inter-system biases of GLONASS, Galileo, BDS, and QZSS relative to GPS; It is the GPS receiver clock drift.

[0079] S108: Measure and update each of the state quantities at the current moment according to the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment; in specific implementation, the embodiment of the present application can provide a gain matrix and a covariance matrix for measuring and updating each of the state quantities at the current moment according to the observation data of the global navigation satellite system.

[0080] Furthermore, the measurement and update of each state quantity at the current moment is expressed by the following formula:

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

[0082] Where: Z k represents the measurement vector, H k represents the observation matrix, x k represents the state vector, v k represents the measurement noise vector.

[0083] S109: Optimally estimating each of the state quantities using the adjustment amount required for each of the state quantities at the current moment, so as to correct the attitude, position speed, sensor, and satellite-related parameters.

[0084] In specific implementation, the embodiment of the present application can provide an optimal estimation of each state quantity by using the adjustment amount required for each state quantity at the current moment, which is expressed by the following formula:

[0085]

[0086]

[0087] Where: Indicates the updated posture. represents the estimated value of the posture, I represents the unit matrix, φ b represents the attitude error, v w Indicates the updated speed. represents the estimated value of speed, represents the speed error, p w Indicates the updated position. represents the estimated position, represents the position error, b k,g Indicates the gyro bias after update. represents the gyro bias estimate, b k-1,g Indicates the gyro zero bias at the last moment, b k,a Indicates the accelerometer zero bias after update, represents the accelerometer bias estimate, b k-1,aIndicates the accelerometer zero bias at the last moment, represents the estimated value of GPS receiver clock error, represents the estimated inter-system bias of GLONASS relative to GPS, represents the estimated inter-system bias of Galileo relative to GPS, represents the estimated value of the inter-system bias between BDS and GPS, represents the estimated inter-system bias of QZSS relative to GPS, Represents the estimated value of GPS receiver clock drift.

[0088] The drone navigation method provided in the embodiment of the present application constructs the navigation state and the kinematic equations considering the rotation of the earth in the world coordinate system. At the same time, the left invariant error on the matrix Lie group is defined in the coordinate system, and then the observation matrix in the world coordinate system is constructed based on the GNSS observation, and a tight combination method is adopted to reduce the impact of sensor errors on the navigation accuracy of the system, thereby improving the robustness and reliability of the navigation system.

[0089] The method provided in the embodiment of the present application includes an extended Kalman filter algorithm flow based on left invariant error under the Lie group framework, which realizes the estimation of state error and state update. It can handle nonlinear problems, converge quickly under large misalignment angles, and has high accuracy. The method is based on the world coordinate system as a whole, which can solve the problem that the error increases due to the large position vector, thereby affecting the stability and accuracy of the filter.

[0090] The following is a detailed introduction to the drone navigation method provided by this application.

[0091] This application proposes a tight combination algorithm of left-invariant extended Kalman filter and SINS / SPP under the Lie group framework for high-precision positioning and attitude estimation of UAVs. By mapping the state vector of the UAV into the tangent space of the Lie group for linearization, the linearization error is reduced and the robustness and accuracy of the filter are improved. At the same time, the inertial sensor data is tightly combined with the GNSS pseudo-range measurement to correct the time-accumulated error while ensuring high dynamic response capability. The overall algorithm flow chart is shown in the figure below. Figure 2 shown.

[0092] The specific implementation steps are as follows:

[0093] Obtain the inertial measurement unit (IMU) data on the drone, including accelerometer and gyroscope readings, and pre-process abnormal values ​​and missing values ​​to ensure data reliability;

[0094] Angular velocity measured by IMU gyroscope Convert it into angle change, combine it with the attitude matrix of the previous moment, and calculate the attitude matrix of the drone at the current moment

[0095] Specific force based on IMU accelerometer measurement Combined with the attitude information and the accumulated velocity vector of the previous moment, the velocity vector of the drone converted from the carrier coordinate system to the world coordinate system is obtained.

[0096] The position change is obtained by integrating the updated velocity vector and adding it to the position vector at the previous moment to obtain the position vector of the drone in the world coordinate system.

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

[0098]

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

[0100]

[0101] The dynamic equation is used to update the state quantity (Formula 3) to obtain the value of each state quantity at the current moment after the dynamic update.

[0102]

[0103] Where φ is the deviation between the real rotation matrix of the world system and the carrier system and the calculated rotation matrix; δv w is the velocity error in the world system; δp w is the position error in the world system; b g is the gyro bias, b a is the accelerometer zero bias; is the GPS receiver clock error; They are the inter-system biases of GLONASS, Galileo, BDS, and QZSS relative to GPS; It is the GPS receiver clock drift.

[0104] The adjustment required for each state quantity is determined by measuring and updating the GNSS observation data. The gain matrix is ​​consistent with the traditional EKF form, and the state update and covariance matrix calculation methods are also consistent. Determine the adjustment required for each state quantity after the dynamic update:

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

[0106] State feedback, according to the error state calculated in step 8, the attitude, position speed, and sensor and satellite related parameters are corrected to obtain the optimal estimate.

[0107]

[0108] Where: Indicates the updated posture. represents the estimated value of the posture, I represents the unit matrix, φ b represents the attitude error, v w Indicates the updated speed. represents the estimated value of speed, represents the speed error, p w Indicates the updated position. represents the estimated position, represents the position error, b k,g Indicates the gyro bias after update. represents the gyro bias estimate, b k-1,g Indicates the gyro zero bias at the last moment, b k,a Indicates the accelerometer zero bias after update, represents the accelerometer bias estimate, b k-1,a Indicates the accelerometer zero bias at the last moment, represents the estimated value of GPS receiver clock error, represents the estimated inter-system bias of GLONASS relative to GPS, represents the estimated inter-system bias of Galileo relative to GPS, represents the estimated value of the inter-system bias between BDS and GPS, represents the estimated inter-system bias of QZSS relative to GPS, Represents the estimated value of GPS receiver clock drift.

[0109] Experimental Results

[0110] The GNSS data collection in this invention uses Trimble R10 and NovAtel SPAN-CPT to collect inertial navigation data. The post-processing results of the commercial software Inertia Explorer (IE) are used as reference values. The frequency is 100Hz, the zero bias instability of the gyroscope is 1° / h, and the random walk is The bias instability and random walk of the accelerometer are 0.75 mg and

[0111] The analysis is performed by comparing the three error models simulated by the present invention and the traditional EKF algorithm under a larger misalignment angle α=[8.5°8.5°45°].

[0112] Table 1 Attitude, position, and velocity RMS errors

[0113]

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

[0115] In summary, the UAV navigation method provided by this application, since the radius of the earth is about 6371km, the position vector value of the global coordinate system is 10 6 The method constructs the LIEKF algorithm on the world coordinate system (local coordinate system) to make the value small, ensuring the stability of the linear error kinematic matrix and the observation matrix. The left invariant error on the matrix Lie group is defined in the world coordinate system, and the error remains unchanged on the left group. The state quantities and errors such as attitude, position, and velocity are defined in the group space. Through the multiplication closure of the Lie group and the affine characteristics between the Lie algebra, as well as the corresponding nonlinear error quantity constructed thereby, a linear state space model independent of the state is derived. Since the redefined velocity error and position error both contain attitude terms, the true error can be more accurately characterized. At the same time, it can enable the UAV to achieve rapid convergence of the filter under large misalignment angles. It solves the problem that the traditional EKF loose combination navigation accuracy of the UAV under the ECEF framework is low, and it is difficult to converge quickly or even difficult to converge under large misalignment angles.

[0116] See also Fig. 9 , the embodiment of the present application can also provide a drone navigation device, such as Fig. 9 As shown, for executing the above-mentioned drone navigation method, the device may include:

[0117] The measurement data acquisition unit 901 is used to acquire measurement data of an inertial measurement unit on the drone, wherein the measurement data includes an accelerometer reading and a gyroscope reading;

[0118] The current moment attitude matrix calculation unit 902 is used to convert the angular velocity contained in the gyroscope reading into an angle change, and combine it with the attitude matrix of the drone at the previous moment to obtain the attitude matrix of the drone at the current moment by calculation;

[0119] The velocity vector calculation unit 903 in the world coordinate system is used to obtain the velocity vector of the UAV converted 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 accumulated by the velocity vector of the UAV at the previous moment;

[0120] A position vector calculation unit 904 in a world coordinate system is used to obtain a position change according to the velocity vector integral, and to accumulate the position change with the position vector of the UAV at a previous moment to obtain a position vector of the UAV in a world coordinate system;

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

[0122] The error state differential unit 906 is used to obtain the error matrix under the framework according to 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] A dynamic updating unit 907, used to update the state quantity by using the dynamic equation combined with the error state differential equation to obtain the value of each state quantity at the current moment after the dynamic update;

[0124] A measurement updating unit 908 is used to measure and update each of the state quantities at the current moment according to the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment;

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

[0126] The present application embodiment may also provide a drone navigation device, the device comprising 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-mentioned drone navigation method according to the instructions in the program code.

[0129] like Fig.10 As shown, a drone navigation device provided in an embodiment of the present application 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 embodiment of the present 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 the program stored in the memory 11. Specifically, the processor 10 can execute the operations in the embodiment of the drone navigation method.

[0132] The memory 11 is used to store one or more programs, which may include program codes, and the program codes include computer operation instructions. In the embodiment of the present application, the memory 11 at least stores programs for implementing the following functions:

[0133] Obtaining measurement data of an inertial measurement unit on the drone, the measurement data including accelerometer readings and gyroscope readings;

[0134] Convert the angular velocity contained in the gyroscope reading into an angle change, combine it with the attitude matrix of the drone at the previous moment, and calculate the attitude matrix of the drone at the current moment;

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

[0136] A position change is obtained according to the integral of the velocity vector, and the position change is added to the position vector of the UAV at a previous moment to obtain the position vector of the UAV in the world coordinate system;

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

[0138] According to the left invariant error on the matrix Lie group, an error matrix under the framework is obtained, and the attitude error, velocity error and position error are differentiated based on the error matrix to obtain an error state differential equation;

[0139] Using a dynamic equation combined with the error state differential equation to update the state quantity to obtain the value of each state quantity at the current moment after dynamic update;

[0140] Measuring and updating each of the state quantities at the current moment according to the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment;

[0141] The optimal estimation of each state quantity is performed using the adjustment amount required for each state quantity at the current moment, so as to realize the correction of attitude, position speed, sensor, and satellite related parameters.

[0142] In one possible implementation, the memory 11 may include a program storage area and a data storage area, wherein the program storage area can store an operating system and application programs required for at least one function (such as a file creation function, a data reading and writing function), etc.; the data storage area can store data created during use, such as initialization data, etc.

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

[0144] The communication interface 12 may be an interface of a communication module, and is used to connect to other devices or systems.

[0145] Of course, it should be noted that Fig.10 The structure shown does not constitute a limitation on the drone navigation device in the embodiment of the present application. In actual applications, the drone navigation device may include Fig.10 More or fewer components than shown, or combinations of certain components.

[0146] An embodiment of the present application may also provide a computer-readable storage medium, wherein the computer-readable storage medium is used to store program code, and the program code is used to execute the steps of the above-mentioned drone navigation method.

[0147] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0148] It can be known from the description of the above implementation methods that those skilled in the art can clearly understand that the present application can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product can be stored in a storage medium such as ROM / RAM, a disk, an optical disk, etc., and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in the various embodiments of the present application or certain parts of the embodiments.

[0149] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can refer to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system or system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the partial description of the method embodiment. The system and system embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without creative work.

[0150] The above description is only a preferred embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention are included in the protection scope of the present invention.

Claims

1. A drone navigation method, characterized in that: include: Obtaining measurement data of an inertial measurement unit on the drone, the measurement data including accelerometer readings and gyroscope readings; Convert the angular velocity contained in the gyroscope reading into an angle change, combine it with the attitude matrix of the drone at the previous moment, and calculate the attitude matrix of the drone at the current moment; Based on the specific force contained in the accelerometer reading, combined with the attitude matrix and accumulating the velocity vector of the drone at the previous moment, the velocity vector of the drone converted from the carrier coordinate system to the world coordinate system is obtained; A position change is obtained according to the integral of the velocity vector, and the position change is added to the position vector of the UAV at a previous moment to obtain the position vector of the UAV in the world coordinate system; Embedding the attitude matrix, the velocity vector and the position vector into a matrix Lie group; According to the left invariant error on the matrix Lie group, an error matrix under the framework is obtained, and the attitude error, velocity error and position error are differentiated based on the error matrix to obtain an error state differential equation; Using a dynamic equation combined with the error state differential equation to update the state quantity to obtain the value of each state quantity at the current moment after dynamic update; Measuring and updating each of the state quantities at the current moment according to the observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment; The optimal estimation of each state quantity is performed using the adjustment amount required for each state quantity at the current moment, so as to realize the correction of attitude, position speed, sensor, and satellite related parameters.

2. The drone navigation method according to claim 1, characterized in that: The embedding matrix Lie group is represented by the following formula: Where: X t represents the navigation state on the matrix Lie group, represents the posture matrix, represents the velocity vector, Represents the position vector.

3. The drone navigation method according to claim 1, characterized in that: The error state differential equation is expressed as follows: Where: η L represents the left invariant error, X t -1 represents the inverse matrix of the navigation state, represents an estimate of the navigation state, represents the estimated value of the pose, represents the estimated value of speed, Represents a position estimate.

4. The drone navigation method according to claim 1, characterized in that: The dynamic update is obtained by updating the state quantity using the dynamic equation combined with the error state differential equation and is expressed as follows: Where: v is the deviation between the real rotation matrix of the world system and the carrier system and the calculated rotation matrix; δv w is the velocity error in the world system; δp w is the position error in the world system; b g is the gyroscope bias, b a is the accelerometer zero bias; is the GPS receiver clock error; They are the inter-system biases of GLONASS, Galileo, BDS, and QZSS relative to GPS; It is the GPS receiver clock drift.

5. The drone navigation method according to claim 1, characterized in that: The gain matrix and the covariance matrix are used to measure and update each of the state quantities at the current moment according to the observation data of the global navigation satellite system.

6. The drone navigation method according to claim 5, characterized in that: The measurement and update of each state quantity at the current moment is expressed by the following formula: Z k =H k x k +v k Where: Z k represents the measurement vector, H k represents the observation matrix, x k represents the state vector, v k represents the measurement noise vector.

7. The drone navigation method according to claim 1, characterized in that: The optimal estimation of each state quantity using the adjustment amount required for each state quantity at the current moment is expressed by the following formula: Where: Indicates the updated posture. represents the estimated value of the posture, I represents the unit matrix, φ b represents the attitude error, v w Indicates the updated speed. represents the estimated value of speed, represents the speed error, p w Indicates the updated position. represents the estimated position, represents the position error, b k,g Indicates the gyro bias after update. represents the gyro bias estimate, b k-1,g Indicates the gyro zero bias at the last moment, b k,a Indicates the accelerometer zero bias after update, represents the accelerometer zero bias estimate, b k-1,a Indicates the accelerometer zero bias at the last moment, represents the estimated value of GPS receiver clock error, represents the estimated inter-system bias of GLONASS relative to GPS, represents the estimated inter-system bias of Galileo relative to GPS, represents the estimated value of the inter-system bias between BDS and GPS, represents the estimated inter-system bias of QZSS relative to GPS, Represents the estimated value of GPS receiver clock drift.

8. A drone navigation device, characterized in that: Used to execute the drone navigation method according to any one of claims 1 to 7, the device comprises: A measurement data acquisition unit, used to acquire measurement data of an inertial measurement unit on the drone, wherein the measurement data includes an accelerometer reading and a gyroscope reading; A current moment attitude matrix calculation unit, used to convert the angular velocity contained in the gyroscope reading into an angle change, and combine the attitude matrix of the drone at the previous moment to obtain the attitude matrix of the drone at the current moment by calculation; A velocity vector calculation unit in a world coordinate system, used to obtain a velocity vector of the UAV converted from a carrier coordinate system to a world coordinate system based on the specific force contained in the accelerometer reading, in combination with the attitude matrix and by accumulating the velocity vector of the UAV at a previous moment; A position vector calculation unit in a world coordinate system, used to obtain a position change according to the velocity vector integral, and to accumulate the position change with the position vector of the UAV at a previous moment to obtain a position vector of the UAV in a world coordinate system; A matrix Lie group embedding unit, used for embedding the posture matrix, the velocity vector and the position vector into a matrix Lie group; An error state differential unit is used to obtain an error matrix under the framework according to the left invariant error on the matrix Lie group, and to differentiate the attitude error, velocity error and position error based on the error matrix to obtain an error state differential equation; A dynamic updating unit, used to update the state quantity by using a dynamic equation in combination with the error state differential equation to obtain the value of each state quantity at the current moment after dynamic updating; A measurement and updating unit, configured to measure and update each of the state quantities at the current moment according to observation data of the global navigation satellite system to obtain the adjustment amount required for each of the state quantities at the current moment; The parameter correction unit is used to make an optimal estimate of each of the state quantities using the adjustment amount required for each of the state quantities at the current moment, so as to correct the attitude, position speed, sensor, and satellite related parameters.

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

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium is used to store program code, and the program code is used to execute the drone navigation method described in any one of claims 1-7.

Images