CN111435258B - Unmanned aerial vehicle drift compensation method and device and unmanned aerial vehicle - Google Patents
Unmanned aerial vehicle drift compensation method and device and unmanned aerial vehicle Download PDFInfo
- Publication number
- CN111435258B CN111435258B CN201911010314.9A CN201911010314A CN111435258B CN 111435258 B CN111435258 B CN 111435258B CN 201911010314 A CN201911010314 A CN 201911010314A CN 111435258 B CN111435258 B CN 111435258B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- value
- acceleration
- bias
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000001133 acceleration Effects 0.000 claims abstract description 169
- 238000012937 correction Methods 0.000 claims abstract description 84
- 238000005259 measurement Methods 0.000 claims abstract description 45
- 230000033001 locomotion Effects 0.000 claims abstract description 27
- 238000006073 displacement reaction Methods 0.000 claims description 23
- 238000003860 storage Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses an unmanned aerial vehicle drift compensation method and device and an unmanned aerial vehicle. The method comprises the following steps: s10, detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode; s20, changing at least one acceleration offset correction coefficient from an initial value to a first target value when entering a rotation mode, wherein the first target value is larger than the initial value for each acceleration offset correction coefficient; s30, when the normal mode is entered, restoring the at least one acceleration bias correction coefficient to the initial value; s40, calculating a deviation value of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on measurement data of a preset sensor; s50, updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient; and S60, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the acceleration offset value and the acceleration actual measurement value, and performing motion compensation on the unmanned aerial vehicle. The invention can enable the rotation radius of the unmanned aerial vehicle to be reduced more quickly in the rotation mode.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicles, in particular to an unmanned aerial vehicle drift compensation method and device and an unmanned aerial vehicle.
Background
At present, unmanned aerial vehicles are increasingly widely applied, and the importance of the unmanned aerial vehicles is increasingly highlighted in the fields of civil use, commercial use and even military use. Along with the increasing wide application field of unmanned aerial vehicles, the requirements on unmanned aerial vehicles are more and more, at present, most unmanned aerial vehicles are provided with various sensors, speed information, position information and the like, when hovering on a certain horizontal plane, the unmanned aerial vehicles are required to be stopped at a certain point in an ideal state, but in practice, due to the influence of errors of the sensors or wind speeds, the gravity center of the unmanned aerial vehicles deviate from other factors, and the like, certain deviation exists in measurement data of the sensors, so that errors can be generated when the unmanned aerial vehicle system calculates the speed, and the unmanned aerial vehicles generate certain drift, so that the drift needs to be continuously processed, the unmanned aerial vehicles can return to the fixed point position, and the unmanned aerial vehicles can generate the following motions: the movement from the fixed point position to a certain position generates offset, and the movement from the offset position moves along the direction back to the fixed point position, still deviates from the fixed point position again due to the drift, and the movement needs to be continued back to the fixed point position direction, so that the vibration movement with almost negligible drift distance can be performed by the unmanned aerial vehicle when the stable state is reached (generally, when the drift is negligible, the unmanned aerial vehicle can be considered to be in the stable state). The position and the speed of the unmanned aerial vehicle need to be known in real time when the drift of the unmanned aerial vehicle is calculated, but the data acquisition frequency of the sensor is not real-time, so that the speed and the position information are generally obtained by integrating and resolving the acceleration under a geographic coordinate system when the unmanned aerial vehicle is positioned at present, the accuracy degree of the data is obtained by subtracting an acceleration offset value from the acceleration measured by the machine body and converting the acceleration offset value into the geographic coordinate system, and the offset value is used for compensating the error caused by the machine body measurement. However, the speed and position information obtained by integrating the acceleration may further diverge as time goes by, resulting in an increasing error with the actual value, and thus, it is also necessary to correct the acceleration offset value by feedback through the measured value of the sensor at a fixed time to reduce the error of the solution result. At present, there is such a problem in unmanned aerial vehicle's motion, when unmanned aerial vehicle when hovering carries out fixed point rotation, because the influence of centrifugal force, the influence of the error that causes when its organism is measured can be more obvious to system speed solution, from this, if still carry out drift adjustment with the mode when non-is rotatory, unmanned aerial vehicle's radius of rotation (i.e. unmanned aerial vehicle's distance that deviates from the fixed point position) can be comparatively big when beginning rotatory, need adjust for a longer time and just can converge the radius, reaches steady state.
Disclosure of Invention
Based on the above-mentioned current situation, the main purpose of the invention is to provide an unmanned aerial vehicle drift compensation method, device and unmanned aerial vehicle, which can solve the problems of large rotation radius and long convergence time after the unmanned aerial vehicle fixed-point rotation starts to rotate in the prior art.
In order to achieve the above purpose, the invention provides an unmanned aerial vehicle drift compensation method, which comprises the following steps: s10, detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode; s20, changing at least one acceleration offset correction coefficient from an initial value to a first target value when entering a rotation mode, wherein the first target value is larger than the initial value for each acceleration offset correction coefficient; s30, when the normal mode is entered, restoring the at least one acceleration bias correction coefficient to the initial value; s40, calculating a deviation value of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on measurement data of a preset sensor; s50, updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient; and S60, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the acceleration offset value and the acceleration actual measurement value, and performing motion compensation on the unmanned aerial vehicle.
Preferably, after step S20, the method further comprises: and when the rotation mode is entered, taking the current acceleration offset value of the unmanned aerial vehicle as a first offset value. Correspondingly, after step S30, the method further comprises: and when the unmanned aerial vehicle enters the common mode, recovering the current acceleration offset value of the unmanned aerial vehicle to the first offset value.
Preferably, step S60 includes: s601, the acceleration measured value of the unmanned aerial vehicle measured by an inertial sensor is read at fixed time; s602, compensating the acceleration actual measurement value of the unmanned aerial vehicle through the acceleration offset value, and carrying out coordinate conversion to obtain the corrected acceleration of the unmanned aerial vehicle under the geodetic coordinate; s603, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the corrected acceleration of the unmanned aerial vehicle; s604, determining the drift amount of the unmanned aerial vehicle according to the calculated drift amount and the deviation value; and S605, performing motion compensation on the unmanned aerial vehicle according to the drift amount, and controlling the unmanned aerial vehicle to return to a fixed point position.
Preferably, step S604 includes the steps of: s6041, when the deviation value is updated, correcting the calculated drift amount through the deviation value to obtain the drift amount of the unmanned aerial vehicle; and S6042, when the deviation value is not updated, taking the calculated drift amount as the drift amount of the unmanned aerial vehicle.
Preferably, the preset sensor is a speed sensor, and step S40 includes: reading speed sensor measurement data to calculate a first speed; and calculating according to the first speed and the calculated drift amount to obtain the deviation value.
Preferably, the calculating the drift amount includes calculating a speed and a calculating a position, the deviation value includes a speed deviation value, and step S50 includes:
calculating a first acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝v_correct*Q*dt；
wherein a_bias_ef is a first acceleration bias value, v_correction is a velocity bias value, Q is an acceleration bias correction coefficient, and Q is a constant;
and converting the calculated first speed offset value into an updated acceleration offset value of the unmanned aerial vehicle under a machine body coordinate system.
Preferably, the preset sensor is a GPS, and step S40 includes: reading GPS measurement data to obtain first position data; and calculating according to the first speed and the calculated drift amount to obtain the deviation value.
Preferably, the calculated drift amount includes a calculated position, the deviation value includes a displacement deviation value, and step S50 includes:
calculating a second acceleration offset value under the geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝P_correct*Q*dt；
wherein a_bias_ef is a second acceleration bias value, P_correction is a displacement bias value, Q is an acceleration bias correction coefficient, and Q is a constant;
and converting the calculated second speed offset value into an updated acceleration offset value of the unmanned aerial vehicle under the machine body coordinate system.
Preferably, the preset sensor is a GPS and a speed sensor, the calculated drift amount includes a calculated speed and a calculated position, the deviation value includes a speed deviation value and a displacement deviation value, and step S40 includes: respectively reading GPS and speed sensor measurement data to obtain an actual drift amount, wherein the actual drift amount comprises an actual speed and an actual position; and calculating according to the actual drift amount and the calculated drift amount to obtain a speed deviation value and a displacement deviation value.
Preferably, step S50 includes:
calculating a third acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝P_correct*Q 1 *dt+v_correct*Q 2 *dt；
wherein a_bias_ef is the third acceleration bias value, P_correction is the displacement bias value, v_correction is the velocity bias value, Q 1 And Q 2 The acceleration bias correction coefficient is a constant;
and converting the calculated third acceleration offset value into an organism coordinate system to obtain an updated acceleration offset value of the unmanned aerial vehicle.
In order to achieve the above object, the present invention further provides an unmanned aerial vehicle drift compensation device, the device comprising: the mode switching module is used for detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode; the dynamic correction module is used for changing at least one acceleration bias correction coefficient from an initial value to a first target value when entering a rotation mode, wherein the first target value is larger than the initial value for each acceleration bias correction coefficient; when entering the normal mode, restoring the at least one acceleration bias correction coefficient to the initial value; the deviation calculation module is used for calculating deviation values of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on measurement data of a preset sensor; the bias correction module is used for updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient; and the motion compensation module is used for calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the acceleration offset value and the acceleration actual measurement value and performing motion compensation on the unmanned aerial vehicle.
To achieve the above object, the present invention also provides a drone, including a processor and a computer-readable storage medium storing a drone drift compensation program, which when executed by the processor, implements the drone drift compensation method as described above.
The beneficial effects are that:
according to the unmanned aerial vehicle drift compensation method, the drift amount of the unmanned aerial vehicle is calculated in real time according to the acceleration of the unmanned aerial vehicle after being corrected by the acceleration offset value by changing the acceleration offset correction coefficient in the rotating state, and the unmanned aerial vehicle is subjected to motion compensation; and correcting the acceleration bias value of the unmanned aerial vehicle according to the drift amount of the unmanned aerial vehicle and the acceleration bias correction coefficient at regular time. When the unmanned aerial vehicle hovers and rotates at a fixed point, the influence of errors caused by the measurement of the unmanned aerial vehicle on the system speed is reduced, so that the acceleration offset value can be converged faster when the acceleration offset value is corrected through drift quantity feedback, and the rotation radius of the unmanned aerial vehicle can be reduced faster.
Other advantages of the present invention will be described in the detailed description of the specific technical features and technical solutions, and those skilled in the art should understand the advantages of the technical features and technical solutions.
Drawings
Hereinafter, preferred embodiments according to the present invention will be described with reference to the accompanying drawings. In the figure:
fig. 1 is a flow chart of a unmanned aerial vehicle drift compensation method according to a first embodiment of the present invention;
fig. 2 is a program block diagram of a unmanned aerial vehicle drift compensation device according to a fourth embodiment of the present invention;
Detailed Description
For a more detailed description of the technical solutions of the present invention, to facilitate a further understanding of the present invention, specific embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood that all of the illustrative embodiments and descriptions thereof are presented for purposes of illustration and are not intended to be a limitation on the invention.
In the invention, the unmanned aerial vehicle is defined to be in a common mode when hovering on a certain plane or moving horizontally, such as moving back and forth and left and right, and is defined to be in a rotating mode when hovering on a certain height and rotating in situ (namely rotating), and at the moment, the distance of the unmanned aerial vehicle deviating from a rotating target point is defined as a rotating radius. When the unmanned aerial vehicle is in a stable state, the distance between the unmanned aerial vehicle rotation and the target point is within an acceptable range, and the acceptable range is determined according to user setting. It will be appreciated that drift due to system speed resolution will always exist as long as the drone is in motion, and thus compensation movement for drift will also continue.
First embodiment:
referring to fig. 1, a flow chart of a method for unmanned aerial vehicle drift compensation according to a first embodiment of the present invention is shown. In this embodiment, the unmanned aerial vehicle drift compensation method includes the following steps:
step S10, detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode;
step S20, when entering a rotation mode, changing at least one acceleration offset correction coefficient from an initial value to a first target value, wherein the first target value is larger than the initial value for each acceleration offset correction coefficient;
step S30, when entering a normal mode, restoring the at least one acceleration bias correction coefficient to the initial value;
step S40, calculating the deviation value of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on the measurement data of a preset sensor;
s50, updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient;
and S60, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the acceleration offset value and the acceleration actual measurement value, and performing motion compensation on the unmanned aerial vehicle.
Specifically, when the unmanned aerial vehicle moves, the real-time position and the speed can respectively obtain a calculated value as an estimated value through the secondary integration and the primary integration of the acceleration, and in this embodiment, the real-time drift amount of the unmanned aerial vehicle from the fixed point position can be obtained by calculating the calculated speed and the calculated position through the above speed. And the measured data of each sensor is used as feedback quantity to correct the estimated value, and an acceleration offset value can be set for the feedback quantity to be used for representing the compensation of the feedback quantity after the last sensor measured value is integrated for a certain time. Assuming that the data reading period of the sensor is T, the time of each reading of the data by the sensor is 0, T,2T, … … nT, in the time of 0 to T, the acceleration offset value can be subtracted as the acceleration a1 according to the acceleration read by the inertial sensor at the time 0 to obtain the position data and the velocity data, after the integration calculation at the time T, the data of the preset sensor such as the velocity sensor is read, and the integration calculation at the time T is corrected according to the deviation value between the read data and the integration calculation result at the time T, that is, the deviation value between the actual drift amount and the calculated drift amount. In this way, the velocity calculation is performed at the time T to 2T by using the corrected acceleration a2, so that the velocity is closer to the actual value than the velocity calculated by using the acceleration a1, and the velocity calculation result is more accurate than the previous velocity calculation result after correcting the acceleration offset value according to the sensor reading data. After the position data and the speed data are obtained through calculation, the drift amount of the unmanned aerial vehicle relative to the fixed point position can be determined, and at the moment, the unmanned aerial vehicle can be controlled to return to the fixed point position according to the drift amount.
In this embodiment, the correction amplitude of the sensor measurement data with respect to the acceleration bias value depends on the acceleration bias correction coefficient, and the larger the acceleration bias correction coefficient is, the larger the correction amplitude of the sensor measurement data with respect to the acceleration bias value is. For example, assuming that the feedback amount obtained by processing the sensor measurement data is a_correction, the correction amplitude is 0.5a_correction every time if the correction coefficient is 0.5, and the correction amplitude is 0.8a_correction every time if the correction coefficient is 0.8. Therefore, in the rotation mode, the acceleration bias correction coefficient is adjusted so that the acceleration bias correction coefficient is increased from the initial value in the normal mode to the first target value, the change range of the acceleration bias value is increased, the acceleration bias value is converged faster, and when the speed of the unmanned aerial vehicle is calculated, the time required for the calculation result of the unmanned aerial vehicle to approach the actual value is shorter, so that even if the error of the machine body measurement in the rotation mode generates larger influence due to the existence of centrifugal force, the rotation radius of the unmanned aerial vehicle can be reduced faster and the unmanned aerial vehicle approaches the fixed point position faster.
It can be appreciated that for the unmanned aerial vehicle, the solution result can be corrected by a plurality of sensors, and the feedback quantity of each sensor corresponds to one and the speed bias correction coefficient. In the rotation mode, as long as at least one acceleration offset correction coefficient is larger than the acceleration offset correction coefficient in the normal mode, the influence of centrifugal force can be weakened to a certain extent by correcting the calculation result in the rotation mode, so that the rotation radius of the unmanned aerial vehicle is reduced more rapidly.
After the unmanned aerial vehicle returns to the normal mode from the rotation mode, the influence of centrifugal force is eliminated, so that the adjustment of the acceleration offset value can be smaller.
Further, in the present embodiment, after step S20, the method includes the steps of:
step S21, when the rotation mode is entered, taking the current acceleration offset value of the unmanned aerial vehicle as a first offset value;
correspondingly, after step S30, the method further includes the following steps:
and when the unmanned aerial vehicle enters the common mode, recovering the current acceleration offset value of the unmanned aerial vehicle to the first offset value.
In this embodiment, before the unmanned aerial vehicle enters the rotation mode, the current acceleration bias value is first used as the first bias value to backup. Therefore, after the rotating mode is finished and returns to the normal mode, the current acceleration bias value can be directly restored to the first bias value, the original state can be directly restored, the process of learning to gradually adjust the acceleration bias value to the first bias value again is reduced, and meanwhile unmanned aerial vehicle drift after the rotating is finished can be reduced.
Further, in the present embodiment, step S60 includes the steps of:
s601, the acceleration measured value of the unmanned aerial vehicle measured by an inertial sensor is read at fixed time;
s602, compensating the acceleration actual measurement value of the unmanned aerial vehicle through the acceleration offset value, and carrying out coordinate conversion to obtain the corrected acceleration of the unmanned aerial vehicle under the geodetic coordinate;
s603, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the corrected acceleration of the unmanned aerial vehicle; s604, determining the drift amount of the unmanned aerial vehicle according to the calculated drift amount and the deviation value;
and S605, performing motion compensation on the unmanned aerial vehicle according to the drift amount, and controlling the unmanned aerial vehicle to return to a fixed point position.
As described above, the data acquisition frequency of the sensor is not real-time, and thus, in this example, the data of the sensor is periodically read by a certain period. When the real-time position and speed of the unmanned aerial vehicle are calculated, the measured acceleration value of the unmanned aerial vehicle measured by the inertial sensor is read at fixed time, and the measured value of the inertial sensor at the moment t is assumed to be acc0_b, and the acc0_b has deviation as described above, so that the unmanned aerial vehicle needs to be corrected according to the acceleration offset value, and the acceleration offset value is assumed to be a_bias_bf, it can be understood that the acc0_b and the a_bias_bf are relative to the machine body coordinate system, and the corrected unmanned aerial vehicle acceleration after the acceleration offset value compensation at the moment t is as follows:
acc1_b＝acc0_b-a_bias_bf；
assuming that the attitude rotation matrix is R, after the attitude rotation matrix is converted into a geodetic coordinate system, the corrected unmanned aerial vehicle acceleration is as follows:
acc1_e＝R*acc1_b；
when the system carries out real-time speed calculation, the speed can be obtained through acceleration primary integration, the displacement can be obtained through acceleration secondary integration, and the speed and the displacement obtained through calculation at the moment t are as follows:
wherein, the liquid crystal display device comprises a liquid crystal display device,calculating the obtained speed and displacement for the time t-1;
the method comprises the steps of regularly reading data of a preset sensor such as a speed sensor or a position sensor, calculating a difference value between measured data of the sensor and a system resolving speed to serve as an initial correction value, and correcting the resolving speed or displacement after weighting the initial correction value (in the embodiment, the correcting speed is taken as an example):
wherein P is a constant, and the corrected speed is obtainedThen, the calculated displacement may be corrected at the same time. In this embodiment, the calculating the drift amount includes calculating the speed and the position, and according to the calculated speed and position, the displacement and the current speed of the unmanned aerial vehicle at the offset fixed point position can be determined by combining the fixed point position, so as to perform motion compensation on the unmanned aerial vehicle, and control the unmanned aerial vehicle to return to the fixed point position from the offset position.
Further, in the present embodiment, step S604 includes the steps of:
s6041, when the deviation value is updated, correcting the calculated drift amount through the deviation value to obtain the drift amount of the unmanned aerial vehicle;
and S6042, when the deviation value is not updated, taking the calculated drift amount as the drift amount of the unmanned aerial vehicle.
It will be appreciated that the speed calculation is performed in real time, and the correction is performed only when the sensor data is read at regular time, that is, the deviation value is updated only when the sensor data is read, so that when the sensor speed is not read during real-time drift control, the deviation value is not updated, the result of the calculation is directly used as the drift amount to perform motion compensation on the unmanned aerial vehicle in real time, and when the sensor speed is read, the deviation value is updated, the corrected result of the calculation is used as the drift amount to perform motion compensation on the unmanned aerial vehicle.
The unmanned aerial vehicle is subjected to motion compensation in real time through the speed resolving result, so that the timeliness of unmanned aerial vehicle control is ensured, and meanwhile, the resolved result is corrected through the read sensor data at regular time, so that the compensation is more accurate.
Further, in the present embodiment, the preset sensor may be a speed sensor that measures a speed, such as an optical flow sensor, and the result of the calculation and the acceleration bias value are corrected by data of the speed sensor as feedback amounts. In the present embodiment, step S40 includes the steps of:
reading speed sensor measurement data to calculate a first speed;
and calculating according to the first speed and the calculated drift amount to obtain the deviation value.
Taking an optical flow sensor as an example, the first speed obtained by calculating by reading the measurement data of the optical flow sensor is the current actual speed sensor_v of the unmanned aerial vehicle, and the unmanned aerial vehicle calculates the speed by calculating the real-time speed of the system at the momentError +.>The error is the speed deviation value, so the calculated speed can be corrected according to the obtained speed deviation value, and the corrected calculated speed is obtained as follows:
wherein, P is a constant, and the calculated position is corrected by the error, so as to obtain:
accordingly, in the present embodiment, step S50 includes the following steps 1-2:
step 1, calculating a first acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝v_correct*Q*dt；
wherein a_bias_ef is a first acceleration bias value obtained under a geodetic coordinate system, v_correction is a velocity bias value, Q is an acceleration bias correction coefficient, and Q is a constant;
and 2, converting the calculated first speed offset value into an updated acceleration offset value of the unmanned aerial vehicle under a machine body coordinate system.
Taking an optical flow sensor as an example, as described above, v_correct is the difference between the measured speed and the calculated speed of the unmanned aerial vehicle,it will be appreciated that the initial value of Q is deterministic for the same drone, and is dependent on the nature of the drone itself.
Similarly, the corrected acceleration offset value in the body coordinate system is:
a_bias_bf＝a_bias_ef*R T ；
in this embodiment, the acceleration offset value is corrected by regularly reading the speed sensor data, so that the acceleration of the unmanned aerial vehicle obtained after each correction is closer to the actual value, and the result of unmanned aerial vehicle speed calculation is more accurate.
Second embodiment:
in this embodiment, the unmanned aerial vehicle is configured with a GPS, so that the longitude and latitude coordinates and the altitude of the unmanned aerial vehicle, that is, the three-dimensional position information of the unmanned aerial vehicle, can be measured, and the calculation result and the acceleration offset value can be corrected by using the measurement data of the GPS as the feedback quantity. In the present embodiment, step S40 includes the steps of:
reading GPS measurement data to obtain first position data;
and calculating according to the first speed and the calculated drift amount to obtain the deviation value.
The current actual position of the unmanned aerial vehicle is sensor_P by reading GPS measurement data, and the position of the unmanned aerial vehicle is calculated by the real-time speed of the system at the momentError +.> The error is a displacement deviation value, so that the calculated position can be corrected by the displacement deviation value, and the corrected calculated position is obtained as follows:
the first speed obtained by reading the GPS measurement data is the current actual speed of the unmanned aerial vehicle which is sensor_v, and the difference value between the actual speed and the calculated speed is the speed deviation value Correcting the calculated speed by the deviation value to obtain:
wherein N and C are both constants.
Accordingly, in the present embodiment, step S50 includes the following steps 1-2:
step 1, calculating a second acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝P_correct*Q*dt；
wherein a_bias_ef is a second acceleration bias value obtained under a geodetic coordinate system, P_correction is a displacement bias value between an actual measurement position and a resolving position of the unmanned aerial vehicle, Q is an acceleration bias correction coefficient, and Q is a constant;
and 2, converting the calculated second speed offset value into an updated acceleration offset value of the unmanned aerial vehicle under a machine body coordinate system.
As described above, taking GPS as an example, P_correction is the difference between the measured position and the solution of the unmanned aerial vehicle, and the displacement deviation valueIt will be appreciated that the initial value of Q is deterministic for the same drone, and is dependent on the nature of the drone itself.
Similarly, the corrected acceleration offset value in the body coordinate system is:
a_bias_bf＝a_bias_ef*R T 。
in this embodiment, the acceleration bias value is corrected by regularly reading the GPS measurement data, so that the acceleration of the unmanned aerial vehicle obtained after each correction is closer to the actual value, and the result of unmanned aerial vehicle speed calculation is more accurate.
Third embodiment:
in this embodiment, the unmanned aerial vehicle is equipped with a GPS and a speed sensor, and when the preset sensor is the GPS and the speed sensor, step S40 includes:
respectively reading GPS and speed sensor measurement data to obtain an actual drift amount, wherein the actual drift amount comprises an actual speed and an actual position;
and calculating according to the actual drift amount and the calculated drift amount to obtain a speed deviation value and a displacement deviation value.
In the present embodiment, the feedback amount includes the actual speed and the actual position obtained from the GPS measurement data and the speed sensor measurement data, and the difference between the actual speed and the actual position and the speed calculation result is used as the deviation value, and at this time, the deviation value includes the speed deviation value and the displacement deviation value, and the calculated calculation speed and the calculated position are corrected by these two deviation values.
Further, in the present embodiment, step S50 includes the following steps 1-2:
step 1, calculating a third acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝P_correct*Q 1 *dt+v_correct*Q 2 *dt；
wherein a_bias_ef is a third acceleration offset value obtained under the geodetic coordinate system, P_correction is a displacement offset value, v_correction is a velocity offset value, Q 1 And Q 2 The acceleration bias correction coefficient is a constant;
and 2, converting the calculated third acceleration offset value into an updated acceleration offset value of the unmanned aerial vehicle under a machine body coordinate system.
As described above, p_correct is the difference between the measured position and the calculated position of the unmanned aerial vehicle, the measured position of the unmanned aerial vehicle may be obtained by GPS measurement, v_correct is the difference between the measured speed and the calculated speed of the unmanned aerial vehicle, and the measured speed of the unmanned aerial vehicle may be obtained by a speed sensor such as an optical flow sensor or calculated by GPS measurement. It will be appreciated that for the same drone, Q 1 And Q 2 The initial value is fixed and is related to the characteristics of the unmanned aerial vehicle itself.
After obtaining the corrected acceleration offset value in the geodetic coordinate system, it needs to be converted into the corrected acceleration offset value in the machine body coordinate system, and as described above, the posture rotation matrix down-converted from the machine body coordinate system to the geodetic coordinate system is R, and then the posture rotation matrix down-converted from the geodetic coordinate system to the machine body coordinate system is the transposed matrix of R, that is, R T Thus, the corrected acceleration offset value a_bias_bf in the body coordinate system is:
a_bias_bf＝a_bias_ef*R T 。
it is understood that when there are a plurality of acceleration bias correction coefficients, in the rotation mode, any acceleration bias correction coefficient may be changed to have a first target value larger than an initial value. For example, modified Q in rotational mode 1 And Q 2 ，Q 1 Become larger, Q 2 Unchanged; or Q 1 Become larger, Q 2 Becoming large; or Q 1 Unchanged, Q 2 Become large, etc.
In this embodiment, the acceleration bias value is corrected by the sensor data read at regular time, so that the acceleration of the unmanned aerial vehicle obtained after each correction is closer to the actual value, and the result of unmanned aerial vehicle speed calculation is more accurate.
Referring to fig. 2, a functional block diagram of a drift compensation device for a unmanned aerial vehicle according to a fourth embodiment of the present invention is shown. In the present embodiment, the unmanned aerial vehicle brake control apparatus 100 includes a mode switching module 110, a dynamic correction module 120, a deviation calculation module 130, a bias correction module 140, and a motion compensation module 150.
The mode switching module is used for detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode;
the dynamic correction module is used for changing at least one acceleration bias correction coefficient from an initial value to a first target value when entering a rotation mode, wherein the first target value is larger than the initial value for each acceleration bias correction coefficient; when entering the normal mode, restoring the at least one acceleration bias correction coefficient to the initial value;
the deviation calculation module is used for calculating deviation values of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on measurement data of a preset sensor;
the bias correction module is used for updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient;
and the motion compensation module is used for calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the acceleration offset value and the acceleration actual measurement value and performing motion compensation on the unmanned aerial vehicle.
The specific implementation process is described in the foregoing embodiments, and will not be described herein.
The fifth embodiment of the present invention further provides a drone, comprising a processor and a computer readable storage medium storing a drone drift compensation program, which when executed by the processor, implements a drone drift compensation method as described above.
Those skilled in the art will appreciate that the above-described preferred embodiments can be freely combined and stacked without conflict.
It will be understood that the above-described embodiments are merely illustrative and not restrictive, and that all obvious or equivalent modifications and substitutions to the details given above may be made by those skilled in the art without departing from the underlying principles of the invention, are intended to be included within the scope of the appended claims.
Claims (12)
1. A method of unmanned aerial vehicle drift compensation, the method comprising the steps of:
s10, detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode;
s20, changing at least one acceleration offset correction coefficient from an initial value to a first target value when entering a rotation mode, wherein the first target value is larger than the initial value for each acceleration offset correction coefficient;
s30, when the normal mode is entered, restoring the at least one acceleration bias correction coefficient to the initial value;
s40, calculating a deviation value of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on measurement data of a preset sensor; specifically, analyzing the data read by the preset sensor to obtain position data and speed data, and obtaining the deviation value through integral calculation;
s50, updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient; specifically, compensating and correcting an integral calculation result through measurement data of the preset sensor, and correcting the acceleration offset value by utilizing corrected position information;
and S60, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the corrected and updated acceleration offset value and the acceleration actual measurement value, and performing motion compensation on the unmanned aerial vehicle.
2. The unmanned aerial vehicle drift compensation method of claim 1, wherein after S20, the method further comprises:
when entering the rotation mode, taking the current acceleration offset value of the unmanned aerial vehicle as a first offset value;
correspondingly, after S30, the method further comprises:
and when the unmanned aerial vehicle enters the common mode, recovering the current acceleration offset value of the unmanned aerial vehicle to the first offset value.
3. The unmanned aerial vehicle drift compensation method of any of claims 1-2, wherein step S60 comprises:
s601, the acceleration measured value of the unmanned aerial vehicle measured by an inertial sensor is read at fixed time;
s602, compensating the acceleration actual measurement value of the unmanned aerial vehicle through the acceleration offset value, and carrying out coordinate conversion to obtain the corrected acceleration of the unmanned aerial vehicle under the geodetic coordinate;
s603, calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the corrected acceleration of the unmanned aerial vehicle;
s604, determining the drift amount of the unmanned aerial vehicle according to the calculated drift amount and the deviation value;
and S605, performing motion compensation on the unmanned aerial vehicle according to the drift amount, and controlling the unmanned aerial vehicle to return to a fixed point position.
4. A method of unmanned aerial vehicle drift compensation according to claim 3, wherein step S604 comprises the steps of:
s6041, when the deviation value is updated, correcting the calculated drift amount through the deviation value to obtain the drift amount of the unmanned aerial vehicle;
and S6042, when the deviation value is not updated, taking the calculated drift amount as the drift amount of the unmanned aerial vehicle.
5. The unmanned aerial vehicle drift compensation method of claim 1, wherein the predetermined sensor is a speed sensor, and step S40 comprises:
reading speed sensor measurement data to calculate a first speed;
and calculating according to the first speed and the calculated drift amount to obtain the deviation value.
6. The unmanned aerial vehicle drift compensation method of claim 5, wherein the resolving the drift amount comprises calculating a velocity and resolving a position, the deviation value comprises a velocity deviation value, and step S50 comprises:
calculating a first acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝v_correct*Q*dt；
wherein a_bias_ef is a first acceleration bias value, v_correction is a velocity bias value, Q is an acceleration bias correction coefficient, and Q is a constant;
and converting the calculated first speed offset value into an updated acceleration offset value of the unmanned aerial vehicle under a machine body coordinate system.
7. The unmanned aerial vehicle drift compensation method of claim 1, wherein the predetermined sensor is a GPS, step S40 comprising:
reading GPS measurement data to obtain first position data;
calculating a first speed according to the first position data, wherein the first speed is the current actual speed of the unmanned aerial vehicle;
and calculating according to the first speed and the calculated drift amount to obtain the deviation value.
8. The unmanned aerial vehicle drift compensation method of claim 7, wherein the resolved drift amount comprises a resolved position, the offset value comprises a displacement offset value, and step S50 comprises:
calculating a second acceleration offset value under the geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝P_correct*Q*dt；
wherein a_bias_ef is a second acceleration bias value, P_correction is a displacement bias value, Q is an acceleration bias correction coefficient, and Q is a constant;
and converting the calculated second speed offset value into an updated acceleration offset value of the unmanned aerial vehicle under the machine body coordinate system.
9. The unmanned aerial vehicle drift compensation method of claim 1, wherein the predetermined sensors are GPS and speed sensors, the resolving drift amount comprises resolving speed and resolving position, the deviation value comprises a speed deviation value and a displacement deviation value, and step S40 comprises:
respectively reading GPS and speed sensor measurement data to obtain an actual drift amount, wherein the actual drift amount comprises an actual speed and an actual position;
and calculating according to the actual drift amount and the calculated drift amount to obtain a speed deviation value and a displacement deviation value.
10. The unmanned aerial vehicle drift compensation method of claim 9, wherein step S50 comprises:
calculating a third acceleration offset value under a geodetic coordinate system according to a preset formula, wherein the preset formula is as follows:
a_bias_ef＝P_correct*Q 1 *dt+v_correct*Q 2 *dt；
wherein a_bias_ef is the third acceleration bias value, P_correction is the displacement bias value, v_correction is the velocity bias value, Q 1 And Q 2 The acceleration bias correction coefficient is a constant;
and converting the calculated third acceleration offset value into an organism coordinate system to obtain an updated acceleration offset value of the unmanned aerial vehicle.
11. An unmanned aerial vehicle drift compensation device, the device comprising:
the mode switching module is used for detecting a system instruction in real time, entering a rotation mode when receiving the instruction for entering the rotation mode, and entering the common mode when receiving the instruction for entering the common mode;
the dynamic correction module is used for changing at least one acceleration bias correction coefficient from an initial value to a first target value when entering a rotation mode, wherein the first target value is larger than the initial value for each acceleration bias correction coefficient; when entering the normal mode, restoring the at least one acceleration bias correction coefficient to the initial value;
the deviation calculation module is used for calculating deviation values of the calculated drift amount and the actual drift amount of the unmanned aerial vehicle at regular time based on measurement data of a preset sensor; specifically, analyzing the data read by the preset sensor to obtain position data and speed data, and obtaining the deviation value through integral calculation;
the bias correction module is used for updating the acceleration bias value of the unmanned aerial vehicle according to the bias value and the acceleration bias correction coefficient; specifically, compensating and correcting an integral calculation result through measurement data of the preset sensor, and correcting the acceleration offset value by utilizing corrected position information;
and the motion compensation module is used for calculating the calculated drift amount of the unmanned aerial vehicle in real time according to the corrected and updated acceleration offset value and the acceleration actual measurement value and performing motion compensation on the unmanned aerial vehicle.
12. A drone comprising a processor and a computer readable storage medium, wherein the storage medium stores a drone drift compensation program, which when executed by the processor, implements the drone drift compensation method of any one of claims 1-10.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911010314.9A CN111435258B (en) | 2019-10-23 | 2019-10-23 | Unmanned aerial vehicle drift compensation method and device and unmanned aerial vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911010314.9A CN111435258B (en) | 2019-10-23 | 2019-10-23 | Unmanned aerial vehicle drift compensation method and device and unmanned aerial vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111435258A CN111435258A (en) | 2020-07-21 |
| CN111435258B true CN111435258B (en) | 2023-08-11 |
Family
ID=71580945
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911010314.9A Active CN111435258B (en) | 2019-10-23 | 2019-10-23 | Unmanned aerial vehicle drift compensation method and device and unmanned aerial vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111435258B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1763477A (en) * | 2005-11-04 | 2006-04-26 | 北京航空航天大学 | Mixed calibration method for Inertial measurement unit capable of eliminating gyro constant drift |
| CN102538792A (en) * | 2012-02-08 | 2012-07-04 | 北京航空航天大学 | Filtering method for position attitude system |
| RU2603767C1 (en) * | 2015-07-31 | 2016-11-27 | Акционерное общество "Специальное конструкторское бюро приборов подземной навигации" | Method for self-compensation of gyroscopic device drifts independent of acceleration |
| CN106403999A (en) * | 2016-09-23 | 2017-02-15 | 西安电子科技大学 | GNSS-based real-time compensation method for inertial navigation accelerometer drifting |
| CN108333938A (en) * | 2018-02-06 | 2018-07-27 | 贵州电网有限责任公司 | A kind of compound gyrocontrol control method of more closed loops |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2988868B1 (en) * | 2012-03-30 | 2015-04-24 | Parrot | METHOD FOR CONTROLLING A MULTI-ROTOR ROTOR SAILING DRONE WITH ESTIMATION AND SIDE WIND COMPENSATION |
-
2019
- 2019-10-23 CN CN201911010314.9A patent/CN111435258B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1763477A (en) * | 2005-11-04 | 2006-04-26 | 北京航空航天大学 | Mixed calibration method for Inertial measurement unit capable of eliminating gyro constant drift |
| CN102538792A (en) * | 2012-02-08 | 2012-07-04 | 北京航空航天大学 | Filtering method for position attitude system |
| RU2603767C1 (en) * | 2015-07-31 | 2016-11-27 | Акционерное общество "Специальное конструкторское бюро приборов подземной навигации" | Method for self-compensation of gyroscopic device drifts independent of acceleration |
| CN106403999A (en) * | 2016-09-23 | 2017-02-15 | 西安电子科技大学 | GNSS-based real-time compensation method for inertial navigation accelerometer drifting |
| CN108333938A (en) * | 2018-02-06 | 2018-07-27 | 贵州电网有限责任公司 | A kind of compound gyrocontrol control method of more closed loops |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111435258A (en) | 2020-07-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106959110B (en) | Cloud deck attitude detection method and device | |
| CN110221332B (en) | Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation | |
| CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
| US7844397B2 (en) | Method and apparatus for high accuracy relative motion determination using inertial sensors | |
| CN103994763B (en) | The SINS/CNS deep integrated navigation system of a kind of Marsokhod and its implementation | |
| JP4199553B2 (en) | Hybrid navigation device | |
| KR101739390B1 (en) | Method for improving the accuracy of self-alignment about the inertial navigation system through gravitational error compensation | |
| CN110631574B (en) | inertia/odometer/RTK multi-information fusion method | |
| EP2598904A1 (en) | System and method for moving-base rtk measurements | |
| US9534900B2 (en) | Inertial navigation sculling algorithm | |
| RU2380656C1 (en) | Integrated strapdown inertial and satellite navigation system on coarse sensors | |
| KR102360465B1 (en) | Inertial sensor unit caliberation method for navigation | |
| JP2012207919A (en) | Abnormal value determination device, positioning device, and program | |
| JP2012173190A (en) | Positioning system and positioning method | |
| CN108627152B (en) | Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion | |
| CN113203429B (en) | Online estimation and compensation method for temperature drift error of gyroscope | |
| CN112946681B (en) | Laser radar positioning method fusing combined navigation information | |
| CN103674059A (en) | External measured speed information-based horizontal attitude error correction method for SINS (serial inertial navigation system) | |
| CN114877915B (en) | Device and method for calibrating g sensitivity error of laser gyro inertia measurement assembly | |
| CN110702106A (en) | Unmanned aerial vehicle, course alignment method and device thereof and storage medium | |
| CN116399351A (en) | Vehicle position estimation method | |
| CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
| CN106403999A (en) | GNSS-based real-time compensation method for inertial navigation accelerometer drifting | |
| CN204255368U (en) | A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod | |
| CN111197994B (en) | Position data correction method, position data correction device, computer device, and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |