Combined navigation solution method and device for unmanned aerial vehicle, unmanned aerial vehicle and medium

Abstract

The present invention is applicable to the technical field of unmanned aerial vehicles, and provides a combined navigation solution method, device, unmanned aerial vehicle and medium for unmanned aerial vehicles. The method comprises: before the unmanned aerial vehicle launches a shell, the attitude parameters of the unmanned aerial vehicle are first initialized; after the unmanned aerial vehicle launches a shell, the attitude parameters of the unmanned aerial vehicle are continuously solved and updated through inertial navigation mechanical arrangement; when the unmanned aerial vehicle reaches the highest point of the trajectory, during the process of the unmanned aerial vehicle opening the parachute and descending, the attitude parameters are corrected to make the unmanned aerial vehicle enter a stable state; after the unmanned aerial vehicle cuts the parachute, the unmanned aerial vehicle is controlled to fly horizontally in the direction of the nose, and the attitude parameters are second initialized; combined navigation solution is performed on the unmanned aerial vehicle through Kalman filtering. The present invention can solve the problems of high cost of artillery-fired unmanned aerial vehicles and large attitude errors caused by high-speed rotation during the launch of unmanned aerial vehicles.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a combined navigation solution method and device for unmanned aerial vehicles, an unmanned aerial vehicle and a medium. Background Art

[0002] With the rapid development of science and technology, Unmanned Aerial Vehicle (UAV), as an efficient and flexible aerial platform, has shown great application potential in various fields. From precision agricultural operations to disaster emergency response, from environmental monitoring to target tracking, UAVs play an irreplaceable role. In these applications, the combination of the Global Navigation Satellite System (GNSS) and the Inertial Navigation System (INS) (GNSS / INS system) provides UAVs with powerful navigation and positioning capabilities.

[0003] The gun-launched UAV is a new weapon system that combines UAV technology and ammunition launching technology. It is mainly used for reconnaissance, strike and communication relay tasks in modern warfare.

[0004] Gun-launched drones place higher demands on the GNSS / INS system. First, before launch, they are located inside the launch system and cannot be positioned by GNSS. During the launch process, the drone needs to withstand a large overload and is accompanied by high-speed rotation. This process places high demands on the range of the IMU gyroscope and accelerometer. For the gyroscope, the range is usually required to be greater than 4000° / s, and for the accelerometer, the range is usually required to be greater than 100g, which greatly increases the cost of the GNSS / INS combined navigation system. Moreover, after launch, the drone is usually in a high-spin state, so the scale error and cross-coupling error of the gyroscope will cause very large errors in attitude solution. Calculated based on the rotation time of the drone after launch of 20s and the rotation speed of 6 rpm, the scale error is 5 / 10,000, and the heading error caused by the scale error is 21.6 degrees, which will seriously affect the control of the drone. Summary of the invention

[0005] In view of this, an embodiment of the present invention provides a combined navigation solution method, device, drone and medium for a drone to solve the problems of high cost of launching drones and large attitude errors caused by high-speed rotation during drone launch.

[0006] A first aspect of an embodiment of the present invention provides a combined navigation solution method for a drone, comprising:

[0007] Before the UAV launches a shell, the attitude parameters of the UAV are first initialized;

[0008] After the UAV launches the shells, the attitude parameters of the UAV are continuously calculated and updated through inertial navigation machinery arrangement;

[0009] When the UAV reaches the highest point of the trajectory, during the process of the UAV opening the parachute and descending, the attitude parameter is corrected to make the UAV enter a stable state;

[0010] After the drone cuts the parachute, controlling the drone to fly horizontally in the direction of the nose, and performing a second initialization on the attitude parameters;

[0011] The integrated navigation solution of the UAV is performed through Kalman filtering.

[0012] In a possible implementation manner of the first aspect, the first initialization of the attitude parameters of the drone includes:

[0013] according to Initializing the roll angle of the drone;

[0014] according to Initializing the pitch angle of the drone;

[0015] Obtaining and initializing the heading angle of the drone;

[0016] in, Respectively represent the roll angle and pitch angle of the drone; They represent the accelerometer of the drone under static conditions. x, y, z The output mean of the three axes; a tan is the symbol for the inverse tangent.

[0017] In a possible implementation manner of the first aspect, the correcting the posture parameter includes:

[0018] Continuously cache the gyroscope data and accelerometer data of the drone within a preset time through a sliding time window;

[0019] Determine the stability of the drone in the current sliding time window according to the gyroscope data and accelerometer data of the drone cached in the current sliding time window;

[0020] If the stability of the UAV in the current sliding time window does not meet the preset stability condition, the stability of the UAV in the next sliding time window is judged until it is determined that the stability of the UAV meets the stability condition;

[0021] After the stability of the UAV meets the stability condition, the attitude parameters are corrected according to the gyroscope data and accelerometer data of the UAV in the corresponding sliding time window.

[0022] In a possible implementation manner of the first aspect, determining the stability of the drone in the current sliding time window according to the gyroscope data and accelerometer data of the drone cached in the current sliding time window includes:

[0023] Calculating the variance of the three axes of the gyroscope respectively according to the gyroscope data;

[0024] According to the accelerometer data, respectively calculating the variance of the three axes of the accelerometer;

[0025] Calculating the sum of the variance of the three axes of the gyroscope and the variance of the three axes of the accelerometer to obtain the stability of the drone;

[0026] Accordingly, the stability condition is:

[0027] The stability of the drone is less than a preset stability threshold.

[0028] In a possible implementation manner of the first aspect, the performing a second initialization on the posture parameter includes:

[0029] During the level flight of the UAV along the nose direction, when the speed of the UAV reaches a preset speed threshold, the attitude parameters of the UAV are second initialized.

[0030] In a possible implementation manner of the first aspect, performing combined navigation solution on the drone through Kalman filtering includes:

[0031] according to Determining a state quantity of the drone;

[0032] Compensating the attitude parameters of the UAV according to the state quantity;

[0033] in, They are k and k-1 The state quantity at the moment, is the discretized state transfer matrix, is the system noise driving matrix, is the noise amount of the state quantity.

[0034] In a possible implementation manner of the first aspect, the state quantity includes at least one of the following: a position error, a velocity error, an attitude error, a gyroscope bias error, an accelerometer bias error, and a lever arm error.

[0035] A second aspect of an embodiment of the present invention provides a combined navigation solution device for an unmanned aerial vehicle, comprising:

[0036] A first initialization module, used for performing a first initialization on the attitude parameters of the UAV before the UAV launches a shell;

[0037] A first calculation module is used to continuously calculate and update the attitude parameters of the UAV through inertial navigation mechanical arrangement after the UAV launches a shell;

[0038] A correction module, used for correcting the attitude parameters when the UAV reaches the highest point of the trajectory and during the parachute opening and descent of the UAV, so as to make the UAV enter a stable state;

[0039] A second initialization module is used to control the drone to fly horizontally in the direction of the nose after the drone cuts the parachute, and perform a second initialization on the attitude parameters;

[0040] The second computing module is used to perform integrated navigation solution on the UAV through Kalman filtering.

[0041] In a possible implementation manner of the second aspect, the first initialization module is specifically configured to:

[0042] according to Initializing the roll angle of the drone;

[0043] according to Initializing the pitch angle of the drone;

[0044] Obtaining and initializing the heading angle of the drone;

[0045] in, Respectively represent the roll angle and pitch angle of the drone; They represent the accelerometer of the drone under static conditions. x, y, z The output mean of the three axes; a tan is the symbol for the inverse tangent.

[0046] In a possible implementation manner of the second aspect, the correction module is specifically configured to:

[0047] Continuously cache the gyroscope data and accelerometer data of the drone within a preset time through a sliding time window;

[0048] Determine the stability of the drone in the current sliding time window according to the gyroscope data and accelerometer data of the drone cached in the current sliding time window;

[0049] If the stability of the UAV in the current sliding time window does not meet the preset stability condition, the stability of the UAV in the next sliding time window is judged until it is determined that the stability of the UAV meets the stability condition;

[0050] After the stability of the UAV meets the stability condition, the attitude parameters are corrected according to the gyroscope data and accelerometer data of the UAV in the corresponding sliding time window.

[0051] In a possible implementation manner of the second aspect, the correction module is specifically configured to:

[0052] Calculating the variance of the three axes of the gyroscope respectively according to the gyroscope data;

[0053] According to the accelerometer data, respectively calculating the variance of the three axes of the accelerometer;

[0054] Calculating the sum of the variance of the three axes of the gyroscope and the variance of the three axes of the accelerometer to obtain the stability of the drone;

[0055] Accordingly, the stability condition is:

[0056] The stability of the drone is less than a preset stability threshold.

[0057] In a possible implementation manner of the second aspect, the second initialization module is specifically configured to:

[0058] During the level flight of the UAV along the nose direction, when the speed of the UAV reaches a preset speed threshold, the attitude parameters of the UAV are second initialized.

[0059] In a possible implementation manner of the second aspect, the second calculation module is specifically configured to:

[0060] according to Determining a state quantity of the drone;

[0061] Compensating the attitude parameters of the UAV according to the state quantity;

[0062] in, They are k and k-1 The state quantity at the moment, is the discretized state transfer matrix, is the system noise driving matrix, is the noise amount of the state quantity.

[0063] In a possible implementation manner of the second aspect, the state quantity includes at least one of the following: a position error, a velocity error, an attitude error, a gyroscope bias error, an accelerometer bias error, and a lever arm error.

[0064] A third aspect of an embodiment of the present invention provides a drone, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the steps of the method in the first aspect or any one of the implementations of the first aspect are implemented.

[0065] A fourth aspect of an embodiment of the present invention provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the steps of the method in the above-mentioned first aspect or any one of the implementation methods of the first aspect.

[0066] Compared with the prior art, the embodiments of the present invention have the following beneficial effects:

[0067] (1) Before the drone launches the projectile, the attitude parameters of the drone are initialized. That is, the initialization of the integrated navigation is completed during the deployment of the drone, thereby greatly reducing the cost of the integrated navigation system and further reducing the cost of the drone.

[0068] (2) After the drone launches the shell, the attitude parameters of the drone are continuously calculated and updated through the inertial navigation mechanical arrangement. When the drone reaches the highest point of the trajectory, the attitude parameters are corrected during the parachute opening and descent process to make the drone enter a stable state. By making high-precision corrections to the attitude parameters of the integrated navigation system during the parachute opening and descent phase, the subsequent stable flight control of the drone can be guaranteed, thereby overcoming the problem of excessive roll and pitch angles caused by high rotation.

[0069] (3) After the UAV cuts the parachute, the UAV is controlled to fly horizontally in the direction of the nose, and the attitude parameters are initialized for the second time. Finally, the Kalman filter is used to perform combined navigation solution on the UAV to provide continuous, stable and reliable position, speed and attitude information. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0071] Figure 1 It is a flow chart of a combined navigation solution method for a UAV provided by an embodiment of the present invention;

[0072] Figure 2 is a structural schematic diagram of a combined navigation solution device for a UAV provided in an embodiment of the present invention;

[0073] Figure 3 Schematic diagram of a drone provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0074] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, it should be clear to those skilled in the art that the present invention may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present invention.

[0075] In order to illustrate the technical solution of the present invention, a specific embodiment is provided below for illustration.

[0076] First, a brief introduction to the gun-launched drone technology is given.

[0077] According to the platform configuration, drones can be divided into fixed-wing drones, multi-rotor drones, parachute-wing drones, and flapping-wing drones. Among them, fixed-wing drones have advantages such as efficient aerodynamic design, long range and long flight time, high-speed flight capability, and a wide range of application fields. Modern fixed-wing drones integrate advanced navigation systems, autopilot technology, and high-performance sensors, and are able to perform various tasks in complex environments. The application of these technologies not only improves the flight performance and mission execution capabilities of drones, but also reduces the difficulty and risk of operation. Fixed-wing drones occupy an important position in drone technology and applications with their advantages such as efficient aerodynamic design, long range and long flight time, high-speed flight capability, and a wide range of application fields. With the continuous development of technology, the performance and functions of fixed-wing drones will be further improved, bringing convenience and value to more fields.

[0078] Fixed-wing UAVs have a variety of deployment methods, among which fixed-wing UAVs launched through gun barrels have significant advantages, which are mainly reflected in the following aspects:

[0079] (1) Strong penetration capability: Fixed-wing drones can quickly penetrate enemy defense lines with the help of the launch speed and flight trajectory of artillery shells or rockets, reducing the risk of being intercepted.

[0080] (2) Small target signature: When launched from the barrel, the drone is packaged in a compact form with a small target signature, making it difficult to be detected by enemy radar or air defense systems.

[0081] (3) Strong survivability and reduced exposure time: The drone stays in the barrel for a longer time, reducing the time it is exposed to the external environment, thereby reducing the risk of being attacked.

[0082] (4) Disposable use: Some gun-launched drones are designed to be used once and do not need to be recovered, which reduces the risk of being captured and reverse engineered by the enemy.

[0083] (5) Low launch cost: Compared with traditional UAV launch methods, gun-launched UAVs can be launched using existing artillery without the need for additional launch equipment, thus reducing launch costs.

[0084] (6) Large-scale deployment: Due to the low launch cost, gun-launched UAVs can be deployed on a large scale to form a numerical advantage and improve combat effectiveness.

[0085] (7) Wide range of applications and adaptability to a variety of environments: Gun-launched UAVs can adapt to different launch environments, including land, sea and air platforms.

[0086] (8) Compatible with various artillery: It can be compatible with artillery or rocket launchers of various calibers to meet different combat requirements.

[0087] (9) Strong technological innovation: Gun-launched UAVs are an innovative combination of UAV technology and traditional ammunition technology, demonstrating the diversity and integration of technology.

[0088] GNSS, as a satellite-based positioning technology, achieves accurate position and time information measurement around the world by receiving and solving signals transmitted by multiple satellites. Its main representatives include the US GPS system, Europe's Galileo system, China's BeiDou system, and Russia's GLONASS system. These systems provide high-precision navigation services for drones, but pure GNSS systems may be affected by signal shielding or multipath effects in certain complex environments (such as urban canyons, forested areas, etc.), resulting in reduced positioning accuracy.

[0089] To make up for this deficiency, the INS inertial navigation system was introduced and combined with the GNSS system to form a GNSS / INS combined navigation system. The INS system can measure the attitude and motion state of the drone in real time through built-in inertial sensors such as gyroscopes and accelerometers, and provide continuous navigation information in a short period of time. When the GNSS signal is unavailable, the INS system can take over the navigation task to ensure the continued stable flight of the drone.

[0090] The advantages of the GNSS / INS integrated navigation system are mainly reflected in the following aspects:

[0091] High-precision positioning and navigation: Through the GNSS / INS combined navigation system, drones can achieve centimeter-level high-precision positioning, ensuring the accuracy of the flight path even in complex environments. This is crucial for performing precision agriculture operations, geological exploration, power inspections and other tasks.

[0092] Autonomous obstacle avoidance and path planning: Combining the location information provided by GNSS and the attitude information provided by INS, the drone can perceive the surrounding environment in real time and perform autonomous obstacle avoidance and path planning, which greatly improves the safety and mission execution efficiency of the drone.

[0093] Emergency rescue and target tracking: In disaster emergency response, drones carrying GNSS / INS systems can quickly locate disaster-stricken areas, conduct aerial reconnaissance and target tracking. At the same time, through the high-definition cameras, infrared sensors and other equipment carried, drones can transmit on-site images and video information in real time to provide decision-making support for the rescue headquarters.

[0094] Long-term and long-distance flight: For drones that need to fly for a long time and long distance, the GNSS / INS combined navigation system can provide continuous and stable navigation information to ensure that the drone will not get lost due to signal loss during flight.

[0095] However, gun-launched UAVs place higher demands on the GNSS / INS system. On the one hand, before launch, the drone is located inside the launch system and cannot be positioned by GNSS. During the launch process, the drone needs to withstand a large overload and is accompanied by high-speed rotation. This process places high demands on the range of the IMU gyroscope and accelerometer. For the gyroscope, the range is usually required to be greater than 4000° / s, and for the accelerometer, the range is usually required to be greater than 100g, which greatly increases the cost of the GNSS / INS combined navigation system. On the other hand, after launch, the drone is usually in a high-spin state, so the scale error and cross-coupling error of the gyroscope will cause very large errors in attitude solution. Calculated based on the rotation time of the drone after launch of 20s and the rotation speed of 6 rpm, the scale error is 5 / 10,000, and the heading error caused by the scale error is 21.6 degrees, which will seriously affect the control of the drone.

[0096] In order to solve the above problems, the present invention provides a method for solving the combined navigation of a UAV. Figure 1 As shown, including:

[0097] Step S101, before the drone launches a shell, the attitude parameters of the drone are first initialized.

[0098] The attitude parameters here mainly include roll angle, pitch angle and heading angle. By completing the initialization of the integrated navigation before the drone launches the shell, such as during the deployment of the drone, the cost of the integrated navigation system can be greatly reduced, thereby reducing the cost of the drone.

[0099] As a possible implementation method, the process of first initializing the attitude parameters of the drone may include:

[0100] according to Initialize the roll angle of the drone;

[0101] according to Initialize the pitch angle of the drone;

[0102] Get and initialize the heading angle of the drone;

[0103] in, Respectively represent the roll angle and pitch angle of the drone; They represent the accelerometer of the drone under static conditions. x, y, z The output mean of the three axes; a tan is the symbol for the inverse tangent.

[0104] Step S102, after the drone launches the shell, the attitude parameters of the drone are continuously calculated and updated through the inertial navigation mechanical arrangement.

[0105] After the drone launches a projectile at a ground target, it enters a non-powered ascent phase due to the reaction force. During this process, the attitude parameters of the drone are updated through inertial navigation mechanical choreography. Inertial navigation mechanical choreography refers to the mathematical modeling and algorithm design of the inertial navigation system to estimate the motion state of the drone. Its main task is to calculate navigation parameters such as position, velocity and attitude through mathematical models based on the measurement values ​​output by the accelerometer and gyroscope.

[0106] For example:

[0107] (1) Speed ​​update:

[0108]

[0109] In the above formula, The centrifugal force generated by the drone's rotation around the earth and the speed increment caused by the Coriolis effect, This is due to the acceleration of the drone relative to the inertial coordinate system, that is, the specific force information.

[0110] (2) Location Update

[0111] In this embodiment, the longitude and latitude information of the drone is represented by a rotation quaternion from the n system to the e system:

[0112]

[0113] The quaternion in the above formula contains the longitude and latitude information of the drone. The position quaternion at the moment Can be The position quaternion at the moment And the speed information of the drone is obtained:

[0114]

[0115]

[0116] In the above formula and The calculation method is as follows:

[0117]

[0118]

[0119] In the above formula, Represents the rotation vector of the e system from time k-1 to time k. The velocity calculation method at the intermediate time is as follows: The position extrapolation method is the same as the velocity extrapolation method when updating.

[0120] (3) Posture update algorithm

[0121] The attitude update is also updated by the quaternion method. The attitude quaternion at time k-1 and the gyroscope angle increment from time k-1 to time k are used for updating. The calculation method is as follows:

[0122]

[0123]

[0124] For b series Time has come The change in time can be expressed by quaternion as:

[0125]

[0126] In the formula, Represents b series Time has come The rotation vector within the interval.

[0127] Step S103, when the UAV reaches the highest point of the trajectory, the attitude parameters are corrected during the process of the UAV opening the parachute and descending, so that the UAV enters a stable state.

[0128] When the drone rises to the highest point of its trajectory, it will open its own parachute to achieve a smooth descent. During the parachute opening and descent process, the attitude parameters of the combined navigation system can be corrected with high precision through accelerometer data and gyroscope data to ensure the subsequent smooth flight control of the drone, thereby overcoming the problem of excessive roll and pitch angles caused by high rotation.

[0129] As a possible implementation method, the process of correcting the posture parameters may include:

[0130] The gyroscope data and accelerometer data of the UAV within a preset time are continuously cached through a sliding time window;

[0131] Determine the stability of the drone within the current sliding time window based on the gyroscope data and accelerometer data of the drone cached within the current sliding time window;

[0132] If the stability of the UAV in the current sliding time window does not meet the preset stability condition, the stability of the UAV in the next sliding time window is judged until it is determined that the stability of the UAV meets the stability condition;

[0133] After the stability of the UAV meets the stability condition, the attitude parameters are corrected according to the gyroscope data and accelerometer data of the UAV in the corresponding sliding time window.

[0134] In this embodiment, the sliding time window can be 1 second (the specific value can be set according to actual needs), that is, the gyroscope and accelerometer data within 1 second are continuously cached and updated. x, y, z Three-axis data, and in each sliding time window, calculate the variance of the three axes of the gyroscope and the variance of the three axes of the accelerometer, which are expressed as follows: .

[0135] The reason for selecting a sliding time window with a relatively small stability to initialize is that when the UAV is in a large dynamic state, initializing the attitude is likely to result in a large attitude error. The sum of the variance of the three axes of the gyroscope and the variance of the three axes of the accelerometer is calculated as the stability of the UAV. When the stability of the UAV is less than the preset stability threshold, it means that the UAV has entered a relatively stable state, and the current attitude information can be calculated using the gyroscope data and accelerometer data of the UAV.

[0136] Step S104, after the drone cuts the parachute, the drone is controlled to fly horizontally in the direction of the nose, and the attitude parameters are second initialized.

[0137] In this embodiment, the UAV is first controlled to fly level along the direction of the nose. Since the axial direction corresponding to the heading of the combined navigation is consistent with the direction of the nose, the heading information can be obtained when the UAV reaches a certain speed after flying level. The heading information is assigned to the combined navigation as the initial heading. At the same time, the position and speed information at that moment are used as the initial information to complete the initialization of the combined navigation.

[0138] Step S105, performing integrated navigation solution on the UAV through Kalman filtering.

[0139] By using the initial information of heading and attitude obtained above, the Kalman filter algorithm established in advance is used to continuously perform cyclic calculations of the Kalman filter, thereby providing continuous and reliable navigation attitude information throughout the navigation process.

[0140] Kalman filtering needs to select the state quantity of Kalman filtering according to the scenario of navigation application. Generally speaking, the more state quantities selected, the more accurately the motion model can be reflected, and the more accurately the state quantity can be estimated. In addition, the selection of state quantities also depends on the level of inertial navigation used. For example, for low-level MEMS inertial navigation, its proportional factor and cross-axis coupling error are large. However, as the number of state quantities increases, the complexity of calculation will also increase. Therefore, comprehensive consideration is required when selecting state quantities. In this embodiment, a total of 15 errors, including position error, velocity error, attitude error, gyroscope zero bias error, accelerometer zero bias error and lever arm error, are selected as state quantities, as follows:

[0141]

[0142] in, represent the position error in the north-east direction, Represent the velocity errors in the north-east direction, Represent the roll, pitch and heading angle errors respectively, Represents the zero bias error of the gyroscope x, y, and z axes respectively, Represent the zero bias error of the accelerometer x, y, and z axes respectively.

[0143] Kalman filter prediction process:

[0144]

[0145] In the formula, They are k and k-1 The state quantity at the moment, is the discretized state transfer matrix, is the system noise driving matrix, is the noise vector of the state.

[0146] The state transfer matrix corresponding to a total of 15 state quantities: position error, velocity error, attitude error, gyroscope bias error, accelerometer bias error, and lever arm error for:

[0147]

[0148] in:

[0149]

[0150]

[0151]

[0152]

[0153]

[0154]

[0155] in, They represent the eastward speed, northward speed and skyward speed of the drone respectively. represents the angular velocity of the Earth's rotation, M, N, h They represent the radius of the earth's meridian circle, the radius of the meridian circle, and the altitude of the drone. Represents the latitude of the drone, represents the local normal gravity value, Represents the sampling interval and the correlation time of the inertial navigation noise. Represents the projection of the sensor output in the drone coordinate system and the navigation coordinate system.

[0156] Establish the measurement equation of Kalman filter:

[0157]

[0158] In the above formula, the Z matrix represents the measurement information matrix of the Kalman filter, the V matrix represents the measurement noise term, and the H matrix represents the measurement equation coefficient matrix, where the H matrix is ​​in the following form:

[0159]

[0160] in, is a 3×3 identity matrix.

[0161] The variance matrix of the measurement noise term R The array is represented as follows:

[0162]

[0163] in, It is the east position variance value, north position variance value, celestial position variance value, east velocity variance value, north velocity variance value and celestial velocity variance value of GNSS observation information.

[0164] Kalman filter update process:

[0165]

[0166] in, represents the gain matrix at time k, represents the measurement equation coefficient matrix at time k, represents the measurement noise matrix, which needs to be reasonably set in this step in combination with the satellite's altitude angle. I represents the unit matrix. is the measurement information matrix at time k.

[0167] After the prediction and update are completed, the state of the tightly combined Kalman filter is obtained, and the Kalman filter state obtained by filtering is used to compensate for each navigation information. At the same time, the position and speed information obtained by GPS time and navigation solution are output and saved until the entire Kalman filter is completed. In this way, continuous and reliable navigation attitude information is provided during the entire navigation process.

[0168] The embodiment of the present invention solves the problem that the gyroscope proportional factor angle error is accumulated continuously due to the rapid rotation of the drone during the projectile launching stage, resulting in excessive roll angle and pitch angle, which seriously affects the control of the drone. At the same time, it can avoid the high cost problem caused by using high-precision inertial navigation equipment or rotation reduction equipment to ensure the accuracy of the launching stage, and can promote the widespread application of drones.

[0169] It should be understood that the order of execution of the steps in the above embodiment does not necessarily mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention.

[0170] Figure 2 : is a schematic diagram of the structure of the combined navigation solution device 20 of the UAV provided by an embodiment of the present invention, comprising:

[0171] The first initialization module 21 is used to perform a first initialization on the attitude parameters of the UAV before the UAV launches a shell.

[0172] The first calculation module 22 is used to continuously calculate and update the attitude parameters of the UAV through inertial navigation machinery arrangement after the UAV launches the shell.

[0173] The correction module 23 is used to correct the attitude parameters when the UAV reaches the highest point of the trajectory and during the process of the UAV opening the parachute and descending, so as to make the UAV enter a stable state.

[0174] The second initialization module 24 is used to control the UAV to fly horizontally along the nose direction after the UAV cuts the parachute, and perform a second initialization on the attitude parameters.

[0175] The second calculation module 25 is used to perform integrated navigation solution on the UAV through Kalman filtering.

[0176] As a possible implementation manner, the first initialization module 21 is specifically used for:

[0177] according to Initialize the roll angle of the drone;

[0178] according to Initialize the pitch angle of the drone;

[0179] Get and initialize the heading angle of the drone;

[0180] in, Respectively represent the roll angle and pitch angle of the drone; They represent the accelerometer of the drone under static conditions. x, y, z The output mean of the three axes; a tan is the symbol for the inverse tangent.

[0181] As a possible implementation, the correction module 23 is specifically used for:

[0182] The gyroscope data and accelerometer data of the UAV within a preset time are continuously cached through a sliding time window;

[0183] Determine the stability of the drone within the current sliding time window based on the gyroscope data and accelerometer data of the drone cached within the current sliding time window;

[0184] If the stability of the UAV in the current sliding time window does not meet the preset stability condition, the stability of the UAV in the next sliding time window is judged until it is determined that the stability of the UAV meets the stability condition;

[0185] After the stability of the UAV meets the stability condition, the attitude parameters are corrected according to the gyroscope data and accelerometer data of the UAV in the corresponding sliding time window.

[0186] As a possible implementation, the correction module 23 is specifically used for:

[0187] According to the gyroscope data, the variance of the three axes of the gyroscope is calculated respectively;

[0188] According to the accelerometer data, the variance of the three axes of the accelerometer is calculated respectively;

[0189] Calculate the sum of the variance of the three axes of the gyroscope and the variance of the three axes of the accelerometer to get the stability of the drone;

[0190] Correspondingly, the stationary condition is:

[0191] The stability of the drone is less than the preset stability threshold.

[0192] As a possible implementation manner, the second initialization module 24 is specifically used for:

[0193] When the speed of the UAV reaches a preset speed threshold during the level flight of the UAV along the nose direction, the attitude parameters of the UAV are initialized for the second time.

[0194] As a possible implementation manner, the second calculation module 25 is specifically configured to:

[0195] according to Determine the state quantity of the drone;

[0196] According to the state quantity, the attitude parameters of the UAV are compensated;

[0197] in, They are k and k-1 The state quantity at the moment, is the discretized state transfer matrix, is the system noise driving matrix, is the noise amount of the state quantity.

[0198] As a possible implementation manner, the state quantity includes at least one of the following: position error, velocity error, attitude error, gyroscope bias error, accelerometer bias error and lever arm error.

[0199] The embodiment of the present invention solves the problem that the gyroscope proportional factor angle error is accumulated continuously due to the rapid rotation of the drone during the projectile launching stage, resulting in excessive roll angle and pitch angle, which seriously affects the control of the drone. At the same time, it can avoid the high cost problem caused by using high-precision inertial navigation equipment or rotation reduction equipment to ensure the accuracy of the launching stage, and can promote the widespread application of drones.

[0200] Figure 3 FIG. 3 is a schematic diagram of a drone 30 provided by an embodiment of the present invention. Figure 3As shown, the drone 30 of this embodiment includes: a processor 31, a memory 32, and a computer program 33 stored in the memory 32 and executable on the processor 31, such as a drone integrated navigation solution program. When the processor 31 executes the computer program 33, the steps in the above-mentioned embodiments of the drone integrated navigation solution method are implemented, such as Figure 1 Alternatively, when the processor 31 executes the computer program 33, the functions of each module in the above-mentioned device embodiments are realized, for example, Figure 2 The functions of modules 21 to 25 are shown.

[0201] Exemplarily, the computer program 33 may be divided into one or more modules / units, which are stored in the memory 32 and executed by the processor 31 to implement the present invention. The one or more modules / units may be a series of computer program instruction segments capable of implementing specific functions, which are used to describe the execution process of the computer program 33 in the drone 30.

[0202] The drone 30 may include, but is not limited to, a processor 31 and a memory 32. Those skilled in the art will appreciate that Figure 3 This is only an example of the drone 30 and does not constitute a limitation of the drone 30. The drone 30 may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the drone 30 may also include input and output devices, network access devices, buses, etc.

[0203] The processor 31 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0204] The memory 32 may be an internal storage unit of the drone 30, such as a hard disk or memory of the drone 30. The memory 32 may also be an external storage device of the drone 30, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the drone 30. Further, the memory 32 may include both an internal storage unit and an external storage device of the drone 30. The memory 32 is used to store the computer program and other programs and data required by the drone 30. The memory 32 may also be used to temporarily store data that has been output or is to be output.

[0205] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0206] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0207] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0208] In the embodiments provided by the present invention, it should be understood that the disclosed devices / drones and methods can be implemented in other ways. For example, the device / drone embodiments described above are only schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0209] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0210] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0211] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal and software distribution medium, etc.

[0212] The embodiments described above are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A combined navigation solution method for an unmanned aerial vehicle, characterized in that: include: Before the UAV launches a shell, the attitude parameters of the UAV are first initialized; After the UAV launches the shells, the attitude parameters of the UAV are continuously calculated and updated through inertial navigation machinery arrangement; When the UAV reaches the highest point of the trajectory, during the process of the UAV opening the parachute and descending, the attitude parameter is corrected to make the UAV enter a stable state; After the drone cuts the parachute, controlling the drone to fly horizontally in the direction of the nose, and performing a second initialization on the attitude parameters; Performing integrated navigation solution on the UAV through Kalman filtering; The step of correcting the posture parameters comprises: Continuously cache the gyroscope data and accelerometer data of the drone within a preset time through a sliding time window; Determine the stability of the drone in the current sliding time window according to the gyroscope data and accelerometer data of the drone cached in the current sliding time window; If the stability of the UAV in the current sliding time window does not meet the preset stability condition, the stability of the UAV in the next sliding time window is judged until it is determined that the stability of the UAV meets the stability condition; After the stability of the UAV meets the stability condition, the attitude parameters are corrected according to the gyroscope data and accelerometer data of the UAV in the corresponding sliding time window.

2. The integrated navigation solution method for a UAV as claimed in claim 1, characterized in that: The first initialization of the attitude parameters of the drone includes: according to Initializing the roll angle of the drone; according to Initializing the pitch angle of the drone; Obtaining and initializing the heading angle of the drone; in, Respectively represent the roll angle and pitch angle of the drone; They represent the accelerometer of the drone under static conditions. x, y, z The output mean of the three axes; a tan is the symbol for the inverse tangent.

3. The integrated navigation solution method of a UAV as claimed in claim 1, characterized in that: Determining the stability of the drone in the current sliding time window according to the gyroscope data and accelerometer data of the drone cached in the current sliding time window includes: Calculating the variance of the three axes of the gyroscope respectively according to the gyroscope data; According to the accelerometer data, respectively calculating the variance of the three axes of the accelerometer; Calculating the sum of the variance of the three axes of the gyroscope and the variance of the three axes of the accelerometer to obtain the stability of the drone; Accordingly, the stability condition is: The stability of the drone is less than a preset stability threshold.

4. The integrated navigation solution method for a UAV as claimed in claim 1, characterized in that: The second initialization of the posture parameters includes: During the level flight of the UAV along the nose direction, when the speed of the UAV reaches a preset speed threshold, the attitude parameters of the UAV are second initialized.

5. The integrated navigation solution method for a UAV as claimed in claim 1, characterized in that: The combined navigation solution of the UAV is performed by Kalman filtering, including: according to Determining a state quantity of the drone; Compensating the attitude parameters of the UAV according to the state quantity; in, They are k and k-1 The state quantity at the moment, is the discretized state transfer matrix, is the system noise driving matrix, is the noise amount of the state quantity.

6. The integrated navigation solution method for a UAV as claimed in claim 5, characterized in that: The state quantity includes at least one of the following: position error, velocity error, attitude error, gyroscope bias error, accelerometer bias error and lever arm error.

7. A combined navigation solution device for an unmanned aerial vehicle, characterized in that: include: A first initialization module, used for performing a first initialization on the attitude parameters of the UAV before the UAV launches a shell; A first calculation module is used to continuously calculate and update the attitude parameters of the UAV through inertial navigation mechanical arrangement after the UAV launches a shell; A correction module, used for correcting the attitude parameters when the UAV reaches the highest point of the trajectory and during the parachute opening and descent of the UAV, so as to make the UAV enter a stable state; A second initialization module is used to control the drone to fly horizontally in the direction of the nose after the drone cuts the parachute, and perform a second initialization on the attitude parameters; A second computing module is used to perform integrated navigation solution on the UAV through Kalman filtering; The step of correcting the posture parameters comprises: Continuously cache the gyroscope data and accelerometer data of the drone within a preset time through a sliding time window; Determine the stability of the drone in the current sliding time window according to the gyroscope data and accelerometer data of the drone cached in the current sliding time window; If the stability of the UAV in the current sliding time window does not meet the preset stability condition, the stability of the UAV in the next sliding time window is judged until it is determined that the stability of the UAV meets the stability condition; After the stability of the UAV meets the stability condition, the attitude parameters are corrected according to the gyroscope data and accelerometer data of the UAV in the corresponding sliding time window.

8. A drone comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images