CN108759845B - Optimization method based on low-cost multi-sensor combined navigation - Google Patents

Optimization method based on low-cost multi-sensor combined navigation Download PDF

Info

Publication number
CN108759845B
CN108759845B CN201810728806.0A CN201810728806A CN108759845B CN 108759845 B CN108759845 B CN 108759845B CN 201810728806 A CN201810728806 A CN 201810728806A CN 108759845 B CN108759845 B CN 108759845B
Authority
CN
China
Prior art keywords
imu
sensor
time
navigation
filter
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
Application number
CN201810728806.0A
Other languages
Chinese (zh)
Other versions
CN108759845A (en
Inventor
裴海龙
陈宏润
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN201810728806.0A priority Critical patent/CN108759845B/en
Publication of CN108759845A publication Critical patent/CN108759845A/en
Application granted granted Critical
Publication of CN108759845B publication Critical patent/CN108759845B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention discloses an optimization method based on low-cost multi-sensor combined navigation, which comprises the following steps of: and designing a Butterworth low-pass filter to filter the three-axis angular velocity and the three-axis acceleration of the IMU, and compensating according to the installation position deviation of each sensor relative to the IMU. And then establishing a state equation of 7-order quaternion and angular velocity offset, and obtaining the quaternion and the angular velocity offset by using a closed-loop extended Kalman filter. And establishing a data buffer area according to the time delay characteristics of different sensors, carrying out time synchronization by taking the time of IMU information acquisition as a reference, and establishing a 9-order closed-loop extended Kalman filter with position, speed and acceleration offset as state quantities. And performing time synchronization fusion compensation on the output of the 9-order extended Kalman filter and the result predicted by the strapdown inertial navigation equation by using a complementary filter to obtain navigation information with more accurate current time.

Description

Optimization method based on low-cost multi-sensor combined navigation
Technical Field
The invention relates to the technical field of aircraft integrated navigation, in particular to an optimization method based on low-cost multi-sensor integrated navigation.
Background
With the progress of microelectronic technology and embedded technology, the unmanned aircraft technology (aircraft for short) has been developed at a high speed, and navigation and positioning are very important parts of the aircraft technology. Conventional strapdown inertial navigation methods need to rely on high-precision, expensive IMUs as a basis and are therefore not suitable for application in civilian-grade aircraft. The cost of the consumption-level IMU is relatively low, but the precision is relatively low, the noise is large, the influence of external interference is large, and the traditional strapdown inertial navigation method is easy to lose effectiveness. Therefore, the low-cost sensor combination navigation comes to the end, the consumption-level IMU and other low-cost sensors (such as GPS and magnetometer) are subjected to data fusion, errors generated by IMU integration are eliminated by introducing innovation, and more reliable navigation positioning with relatively high precision is provided.
However, due to the improvement of the computing power of the embedded chip, in order to achieve better control performance and enable the aircraft to have better maneuvering flight capability, the control frequency (100Hz-400Hz) is often continuously increased by the flight control system. The low-cost sensor has time delay characteristics of different degrees, and a hysteresis link of different degrees is added in a feedback loop if time synchronization data fusion is not considered. The introduction of the hysteresis link will result in the degradation of the control performance and may even cause the instability of the controlled system. In addition, in the practical application process, the installation position of the sensor can not coincide with the mass center of the machine body, and different degrees of effects can be generated, so that the measurement precision is reduced, and the control effect is deteriorated.
Disclosure of Invention
The invention aims to solve the defects in the prior art, and provides an optimization method based on low-cost multi-sensor combined navigation, which is used for carrying out data fusion on the position speed of a GPS (global positioning system) acquired by an airborne flight control system, the magnetic field intensity of a magnetometer, the height of a barometer, the height of a laser radar, the three-axis angular velocity and the three-axis acceleration of an IMU (inertial measurement unit), and simultaneously compensating by considering the time delay characteristics and the installation position of each sensor.
The purpose of the invention can be achieved by adopting the following technical scheme:
an optimization method based on low-cost multi-sensor combined navigation, comprising the following steps:
s1, designing a Butterworth low-pass filter with corresponding indexes according to the vibration level of the machine body to filter the three-axis angular velocity and the three-axis acceleration of the IMU, reducing high-frequency vibration noise interference, and meanwhile, because the sensors are different in installation positions of the IMU, corresponding compensation is required to be carried out on the IMU.
S2, establishing 7 th-order quaternion [ q [ ]0 q1 q2 q3]TAngular velocity bias w _ bias ═ bp bq br]TTo the filtered triaxial acceleration acc (shown below) to obtain a filtered triaxial acceleration accb＝[ax ay az]TAnd taking the course angle psi solved by the magnetometer as an observed quantity, obtaining quaternion and angular velocity bias by utilizing a closed-loop extended Kalman filter, and obtaining the quaternion and the acceleration acc acquired by the IMUbStoring the data into a data cache region to prepare for subsequent data time synchronization;
Figure GDA0003103007340000021
Figure GDA0003103007340000022
where g is the acceleration of gravity.
S3, establishing a data buffer area according to the time delay characteristics of different sensors, and acquiring the acceleration acc by the IMUbIs synchronized with respect to time to establish a 9 th order position P ═ λ Φ h]TSpeed V ═ VN VE VD]TAcceleration bias
Figure GDA0003103007340000034
A closed-loop extended kalman filter, which is a state quantity, as follows:
Figure GDA0003103007340000031
wherein:
Figure GDA0003103007340000032
is the radius of curvature of the meridian plane,
Figure GDA0003103007340000033
in order to be the transverse radius of curvature,
R0represents the semi-major axis of the earth,
weis the rotational angular velocity of the earth.
And S4, performing time synchronization fusion compensation on the output of the 9-order extended Kalman filter and the result of the pure IMU acceleration information predicted by the strapdown inertial navigation equation by using the complementary filter, and obtaining navigation information with more accurate current time.
Further, the step S1 includes: s11, a sensor data filtering process; s12, the sensor measures a compensation process.
Further, the step S11 and the sensor data filtering process are as follows:
s111, performing discrete Fast Fourier Transform (FFT) on the collected IMU acceleration data to obtain a spectrogram;
and S112, analyzing the frequency and amplitude of the aircraft body vibration source. The Butterworth low-pass filter is designed according to sampling frequency (Hz), bandwidth frequency (Hz), vibration source attenuation amplitude (dB) indexes and hysteresis degree (°). The Butterworth low pass filter formula is as follows:
y(k)＝b0x(k)+…+bn-1x(k-n)-a0y(k)-…-an-1y(k-n)
in the above formula, x (k-n) is data read by the sensor before n time, y (k-n) is output of the Butterworth low-pass filter before n time, b0，...，bn-1，a0，...，an-1Are the coefficients of the filter.
Further, the step S12, the sensor measurement compensation process:
and S121, the laser radar measurement height is a relative height and can change along with the change of the attitude. Because the height compensation is performed according to the height difference between the laser radar and the IMU installation position, the current time attitude information must be considered.
Suppose the laser radar mounting height deviation is delta hbThen the compensated relative height is:
Figure GDA0003103007340000041
wherein:
Figure GDA0003103007340000042
for measuring the height of the laser radar in a body coordinate system,
Eulerrollto represent the roll angle of the aircraft attitude information,
Eulerpitchpitch angle, which is representative of aircraft attitude information;
s122, the actual installation position of the GPS is often not coincident with the installation position of the IMU, a lever arm effect is generated when the aircraft carries out large maneuvering flight, measurement errors are caused, the measurement precision of the GPS is reduced, and the compensation mode is as follows:
suppose the installation position deviation of the GPS and the IMU is delta rbAnd after the compensation, the position and the speed of the IMU obtained by the GPS measurement are respectively as follows:
Figure GDA0003103007340000051
Figure GDA0003103007340000052
wherein:
Figure GDA0003103007340000053
for the position information of GPS in NED coordinate system,
Figure GDA0003103007340000054
for the speed information of GPS in NED coordinate system,
Figure GDA0003103007340000055
the meridian plane curvature radius.
Figure GDA0003103007340000056
Is the transverse radius of curvature.
R0Represents the semi-major axis of the earth,
h is the altitude of the sea and the sea,
phi is the current latitude, and phi is the current latitude,
Figure GDA0003103007340000057
is an antisymmetric matrix projected by the earth rotation vector in a navigation coordinate system,
Figure GDA0003103007340000058
an anti-symmetric matrix of angular velocity vectors relative to the earth is generated for the body velocity,
Figure GDA0003103007340000059
is an antisymmetric matrix of the angular velocity of the body,
Figure GDA00031030073400000510
the rotation matrix from the body coordinate system to the navigation coordinate system.
Further, in step S2, a 7-order closed-loop extended kalman filter is used to obtain a quaternion and an angular velocity offset, which are used as inputs of a 9-order closed-loop extended kalman filter, to form a series connection, so as to reduce the time complexity and the space complexity of the entire algorithm.
Further, in step S3, a data time synchronization method is adopted.
The data time synchronization method respectively establishes FIFO data storage areas with corresponding lengths for all the sensors according to the time delay characteristics of all the sensors and the operation period of an algorithm, then stores the outputs of the sensors (such as IMU, laser radar and the like) capable of being rapidly measured in the respective data storage areas, and records the system time when the sensors output. Meanwhile, new output information received by sensors (such as GPS) with longer measuring time is also stored in respective data storage areas, and the system time is recorded and output under the condition of considering the time delay characteristics of the sensors. And then, IMU information with the same system time is searched from a storage area of the sensor for quick measurement, and input of a 9-order Kalman filtering synchronous observation value is formed.
Further, the fusion compensation method in step S4 adopts a complementary filtering concept. The specific process is as follows:
from IMU warpSearching the data buffer obtained by integrating through the strapdown inertial navigation equation for the position P obtained in the step S3EKFVelocity information VEKFThe error between the time synchronization information and the time synchronization information is subjected to complementary filtering to form compensation quantity, the compensation quantity is fed back to a data cache region, a result obtained by integrating a strapdown inertial navigation equation is corrected, and more accurate position information P of the current time is obtainedInsAnd velocity information VIns(ii) a The complementary filtering adopts a 2 nd order complementary filter, and the expression of the transfer function is as follows:
the method adopts a transfer function expression of a 2-order complementary filter as follows:
Figure GDA0003103007340000061
Figure GDA0003103007340000062
Figure GDA0003103007340000065
k for position complementary filtersP，
Figure GDA0003103007340000064
K for position complementary filtersi，
Figure GDA0003103007340000063
K for a velocity complementary filterP，
Figure GDA0003103007340000066
K for a velocity complementary filteri，
accbIs triaxial acceleration information.
Further, the vibration frequency source of the aircraft comprises body vibration generated by the operation of an electric motor or an oil-driven piston engine and the flapping of a rotor.
Compared with the prior art, the invention has the following advantages and effects:
1) the method is suitable for the field of aircraft integrated navigation, the order of the extended Kalman filter is reduced in a serial connection mode, and the time complexity and the space complexity of the algorithm are reduced.
3) The method of the invention takes the difference of the installation positions of the sensors into consideration, carries out corresponding compensation and improves the measurement precision of the sensors.
3) The method of the invention fully considers the time delay characteristic of the sensor, completes time synchronization fusion, has the advantages of high precision and small data lag, and can maintain good performance index under the condition that the aircraft carries out large-maneuvering flight.
Drawings
FIG. 1 is a flow chart of a low cost multi-sensor integrated navigation based optimization method disclosed in the present invention;
fig. 2 is a flow chart of the complementary filter for time synchronization fusion compensation in the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Examples
As shown in fig. 1, the present embodiment discloses a flow chart of an optimization method based on low-cost multi-sensor integrated navigation, which includes the following steps:
s1, designing a Butterworth low-pass filter with corresponding indexes according to the vibration level of the aircraft body to filter the three-axis angular velocity and the three-axis acceleration of the IMU, reducing high-frequency noise interference, and meanwhile, because the installation positions of the sensors on the aircraft body relative to the IMU are different, corresponding compensation is required to be carried out on the sensors.
The step S1 includes: s11, a sensor data filtering process; s12, the sensor measures a compensation process.
S11, the sensor data filtering process is as follows:
and S111, the aircraft vibration frequency source is mainly composed of the body vibration generated by the working of a motor or an oil-driven engine and the flapping of a rotor wing. And performing discrete Fast Fourier Transform (FFT) by collecting IMU acceleration data to obtain an acceleration frequency spectrogram, and analyzing the frequency and amplitude of the vibration source of the body.
And S112, designing Butterworth low-pass filters with different orders according to the IMU sampling frequency (Hz), the aircraft system bandwidth frequency (generally 20Hz to 30Hz), the vibration source attenuation amplitude (dB) index and the hysteresis degree (°). The Butterworth low pass filter formula is as follows:
y(k)＝b0x(k)+…+bn-1x(k-n)-a0y(k)-…-an-1y(k-n)
in the above formula, x (k-n) is data read by the sensor before n time, y (k-n) is output of the Butterworth low-pass filter before n time, b0，...，bn-1，a0，...，an-1Are the coefficients of the filter.
S12, sensor measurement compensation process:
and S121, the laser radar measurement height is a relative height and can change along with the change of the attitude. Because the height compensation is performed according to the height difference between the laser radar and the IMU installation position, the current time attitude information must be considered.
Suppose the laser radar mounting height deviation is delta hbThen the compensated relative height is:
Figure GDA0003103007340000081
wherein:
Figure GDA0003103007340000091
for measuring the height of the laser radar in a body coordinate system,
Eulerrollto represent the roll angle of the aircraft attitude information,
Eulerpitchpitch angle, which is representative of aircraft attitude information;
s122, the actual installation position of the GPS is often not coincident with the installation position of the IMU, a lever arm effect is generated when the aircraft performs large-maneuvering flight, measurement errors are caused, the measurement precision of the GPS is reduced, and the compensation mode is as follows:
suppose the installation position deviation of the GPS and the IMU is delta rb∈R3×1And after the compensation, the position and the speed of the IMU obtained by the GPS measurement are respectively as follows:
Figure GDA0003103007340000092
Figure GDA0003103007340000093
wherein:
Figure GDA0003103007340000094
for the position information of GPS in NED coordinate system,
Figure GDA0003103007340000095
for the speed information of GPS in NED coordinate system,
Figure GDA0003103007340000096
the meridian plane curvature radius.
Figure GDA0003103007340000097
Is the transverse radius of curvature.
R0Represents the semi-major axis of the earth,
phi is the current latitude, and phi is the current latitude,
Figure GDA0003103007340000098
is an antisymmetric matrix projected by the earth rotation vector in a navigation coordinate system,
Figure GDA0003103007340000099
an anti-symmetric matrix of angular velocity vectors relative to the earth is generated for the body velocity,
Figure GDA00031030073400000910
is an antisymmetric matrix of the angular velocity of the body,
Figure GDA0003103007340000101
the rotation matrix from the body coordinate system to the navigation coordinate system.
S2, establishing 7 th-order quaternion [ q [ ]0 q1 q2 q3]TAngular velocity bias w _ bias ═ bp bq br]TEquation of state of (1) with filtered triaxial acceleration accb＝[ax ay az]TAnd taking the course angle psi solved by the magnetometer as an observed quantity, obtaining quaternion and angular velocity bias by utilizing a closed-loop extended Kalman filter, and obtaining the quaternion and the acceleration acc acquired by the IMUbStoring the data into a data cache region to prepare for subsequent data time synchronization;
Figure GDA0003103007340000102
Figure GDA0003103007340000103
in step S2, a 7-order closed-loop extended kalman filter is used to obtain a quaternion and an angular velocity offset, which are used as inputs to a 9-order closed-loop extended kalman filter, to form a series connection, thereby reducing the time complexity and the space complexity of the entire algorithm.
S3, establishing a data buffer area according to the time delay characteristics of different sensors, and acquiring the acceleration acc by the IMUbIs synchronized with respect to time to establish a 9 th order position P ═ λ Φ h]TSpeed V ═ VN VE VD]TAcceleration bias
Figure GDA0003103007340000104
A closed loop extended kalman filter being a state quantity;
Figure GDA0003103007340000111
wherein:
Figure GDA0003103007340000112
the meridian plane curvature radius.
Figure GDA0003103007340000113
Is the transverse radius of curvature.
R0Represents the semi-major axis of the earth,
weis the rotational angular velocity of the earth.
The data time synchronization method establishes FIFO data storage areas with corresponding lengths for all the sensors respectively according to the time delay characteristics of all the sensors and the operation period of the algorithm. The outputs of the sensors (e.g., IMU, lidar, etc.) capable of rapid measurement are then stored in respective data storage areas, and the system time at which the sensor outputs is recorded. Meanwhile, new output information received by sensors (such as GPS) with longer measuring time is also stored in respective data storage areas, and the system time is recorded and output under the condition of considering the time delay characteristics of the sensors. And then, IMU information with the same system time is searched from a storage area of the sensor for quick measurement, and input of a 9-order Kalman filtering synchronous observation value is formed.
And S4, performing time synchronization fusion compensation on the output of the 9-order extended Kalman filter and the result of the pure IMU acceleration information predicted by the strapdown inertial navigation equation by using the complementary filter, and obtaining navigation information with more accurate current time.
The time synchronization fusion compensation method in the step S4 adopts a complementary filtering concept. The specific process is as follows:
searching the data buffer area obtained by IMU through strapdown inertial navigation equation integration for the position P obtained in step S3EKFVelocity information VEKFThe error between the time synchronization information and the time synchronization information is subjected to complementary filtering to form compensation quantity, the compensation quantity is fed back to a data cache region, a result obtained by integrating a strapdown inertial navigation equation is corrected, and more accurate position information P of the current time is obtainedInsAnd velocity information VIns(ii) a The complementary filtering adopts a 2 nd order complementary filter, and the expression of the transfer function is as follows:
the method adopts a transfer function expression of a 2-order complementary filter as follows:
Figure GDA0003103007340000121
Figure GDA0003103007340000122
wherein:
Figure GDA0003103007340000126
k for position complementary filtersP，
Figure GDA0003103007340000127
K for position complementary filtersi，
Figure GDA0003103007340000128
K for a velocity complementary filterP，
Figure GDA0003103007340000129
K for a velocity complementary filteri，
accbIs a three-axis acceleration.
Taking the velocity complementary filter as an example (the same applies to the position complementary filter), the above transfer function can be converted into the following form:
Figure GDA0003103007340000123
from the above formula, it can be found that the 2 nd order complementary filter acts on VEKFThe acceleration sensor comprises a second-order low-pass filter and a second-order bandwidth filter, wherein the second-order high-pass filter acts on acceleration. The frequency response of the second order complementary filter depends on the resonance frequency
Figure GDA0003103007340000124
Damping ratio
Figure GDA0003103007340000125
The damping ratio determines the overshoot at the resonance frequency. In order to smooth the complementary filter output (step response is free of oscillations), the damping ratio ζ > 1 should be set, so that the following boundary conditions exist:
Figure GDA0003103007340000131
of course, to achieve better results, ζ ≈ 2 can achieve better convergence speed.
The above embodiments are preferred embodiments of the present invention, but the present invention is not limited to the above embodiments, and any other changes, modifications, substitutions, combinations, and simplifications which do not depart from the spirit and principle of the present invention should be construed as equivalents thereof, and all such changes, modifications, substitutions, combinations, and simplifications are intended to be included in the scope of the present invention.

Claims (8)

1. An optimization method based on low-cost multi-sensor combined navigation is characterized by comprising the following steps:
s1, designing the triaxial angular velocity w of the Butterworth low-pass filter with corresponding indexes to the IMU according to the vibration level of the aircraft bodyb＝[p q r]TAnd a triaxial acceleration accb＝[ax ay az]TFiltering to reduce high-frequency vibration noise interference, and correspondingly compensating each sensor according to the installation position of the aircraft body relative to the IMU;
s2, establishing 7 th-order quaternion [ q [ ]0 q1 q2 q3]TAngular velocity bias w _ bias ═ bp bq br]TEquation of state of (1) with filtered triaxial acceleration accb＝[ax ay az]TAnd taking the course angle psi solved by the magnetometer as an observed quantity, obtaining quaternion and angular velocity bias by utilizing a closed-loop extended Kalman filter, and obtaining the quaternion and the acceleration acc acquired by the IMUbStoring the data into a data cache region to prepare for subsequent data time synchronization;
s3, establishing a data buffer area according to the time delay characteristics of different sensors, and acquiring the three-axis acceleration acc by the IMUbIs synchronized with respect to time to establish a 9 th order position P ═ λ Φ h]TSpeed V ═ VN VE VD]TAcceleration bias
Figure FDA0003103007330000011
A closed loop extended kalman filter being a state quantity;
and S4, performing time synchronization fusion compensation on the output of the 9-order extended Kalman filter and the result of the pure IMU acceleration information predicted by the strapdown inertial navigation equation by using the complementary filter, and obtaining navigation information with accurate current time.
2. The method for optimizing based on low-cost multi-sensor integrated navigation as claimed in claim 1, wherein said step S1 includes:
s11, a sensor data filtering process;
s12, the sensor measures a compensation process.
3. The optimization method based on low-cost multi-sensor combined navigation of claim 2, wherein the step S11 and the sensor data filtering process are as follows:
s111, performing discrete fast Fourier transform on the collected IMU acceleration data to obtain a spectrogram, and analyzing the frequency and amplitude of the vibration source of the aircraft body;
s112, designing a Butterworth low-pass filter according to the sampling frequency, the bandwidth frequency, the vibration source attenuation amplitude index and the hysteresis degree, wherein the Butterworth low-pass filter has the following formula:
y(k)＝b0x(k)+…+bn-1x(k-n)-a0y(k-1)-…-an-1y(k-n)
in the above formula, x (k-n) is data read by the sensor before n time, y (k-n) is output of the Butterworth low-pass filter before n time, b0，...，bn-1，a0，...，an-1Are the coefficients of the filter.
4. The optimization method based on low-cost multi-sensor integrated navigation of claim 2, wherein the sensor measurement compensation process of step S12 is as follows:
s121, considering the attitude information of the aircraft at the current time, performing altitude compensation according to the altitude difference between the laser radar and the IMU installation position, and assuming that the laser radar installation altitude deviation is delta hbThe relative height of the laser radar after compensation
Figure FDA0003103007330000021
Comprises the following steps:
Figure FDA0003103007330000022
Figure FDA0003103007330000023
for measuring the height of the laser radar in a body coordinate system,
Eulerrollto represent the roll angle of the aircraft attitude information,
Eulerpitchpitch angle, which is representative of aircraft attitude information;
s122, considering that the actual installation position of the GPS is not coincident with the installation position of the IMU, a lever arm effect is generated when the aircraft body carries out large maneuvering flight, measurement errors are caused, the measurement precision of the GPS is reduced, and the GPS is compensated in the following mode: suppose the installation position deviation of the GPS and the IMU is delta rbThen the compensated GPS measurement is carried out to obtain the position of the IMU
Figure FDA0003103007330000024
Speed of rotation
Figure FDA0003103007330000025
Respectively as follows:
Figure FDA0003103007330000031
Figure FDA0003103007330000032
wherein:
Figure FDA0003103007330000033
for the position information of GPS in NED coordinate system,
Figure FDA0003103007330000034
for the speed information of GPS in NED coordinate system,
Figure FDA0003103007330000035
is the radius of curvature of the meridian plane,
Figure FDA0003103007330000036
in order to be the transverse radius of curvature,
R0represents the semi-major axis of the earth,
h is the altitude of the sea and the sea,
phi is the current latitude, and phi is the current latitude,
Figure FDA0003103007330000037
is an antisymmetric matrix projected by the earth rotation vector in a navigation coordinate system,
Figure FDA0003103007330000038
an anti-symmetric matrix of angular velocity vectors relative to the earth is generated for the body velocity,
Figure FDA0003103007330000039
is an antisymmetric matrix of the angular velocity of the body,
Figure FDA00031030073300000310
the rotation matrix from the body coordinate system to the navigation coordinate system.
5. The optimization method based on low-cost multi-sensor combined navigation is characterized in that,
in step S2, a 7-order closed-loop extended kalman filter is used to obtain a quaternion and an angular velocity offset as input of a 9-order closed-loop extended kalman filter, forming a series connection.
6. The optimization method based on low-cost multi-sensor integrated navigation of claim 1, wherein the data time synchronization method adopted in step S3 is as follows:
respectively establishing FIFO data storage areas with corresponding lengths for all the sensors according to the time delay characteristics of all the sensors and the operation period of the algorithm;
then, the output of the sensors capable of being measured quickly is stored in respective data storage areas, and the system time when the sensors output is recorded, wherein the sensors capable of being measured quickly comprise an IMU and a laser radar, and new output information received by the sensors with longer measuring time is also stored in the respective data storage areas, wherein the sensors with longer measuring time comprise a GPS, and the system time when the sensors output is recorded under the condition of considering the time delay characteristics of the sensors;
and then, IMU information with the same system time is searched from a storage area of the sensor for quick measurement, and input of a 9-order Kalman filtering synchronous observation value is formed.
7. The optimization method based on low-cost multi-sensor combined navigation as claimed in claim 1, wherein the fusion compensation method in step S4 adopts a complementary filtering idea, and the process is as follows:
searching the data buffer area obtained by IMU through strapdown inertial navigation equation integration for the position P obtained in step S3EKFVelocity information VEKFThe error between the time synchronization information and the time synchronization information is subjected to complementary filtering to form compensation quantity, the compensation quantity is fed back to a data cache region, a result obtained by integrating a strapdown inertial navigation equation is corrected, and more accurate position information P of the current time is obtainedInsAnd velocity information VIns(ii) a The complementary filtering adopts a 2 nd order complementary filter, and the expression of the transfer function is as follows:
Figure FDA0003103007330000041
Figure FDA0003103007330000042
wherein:
Figure FDA0003103007330000043
k for position complementary filtersP，
Figure FDA0003103007330000044
K for position complementary filtersi，
Figure FDA0003103007330000045
K for a velocity complementary filterP，
Figure FDA0003103007330000051
K for a velocity complementary filteri，
accbIs a three-axis acceleration.
8. The optimization method based on low-cost multi-sensor combination navigation of claim 1, wherein the vibration frequency source of the aircraft comprises a motor or an oil-driven piston engine working and body vibration generated by rotor flap.
CN201810728806.0A 2018-07-05 2018-07-05 Optimization method based on low-cost multi-sensor combined navigation Active CN108759845B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810728806.0A CN108759845B (en) 2018-07-05 2018-07-05 Optimization method based on low-cost multi-sensor combined navigation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810728806.0A CN108759845B (en) 2018-07-05 2018-07-05 Optimization method based on low-cost multi-sensor combined navigation

Publications (2)

Publication Number Publication Date
CN108759845A CN108759845A (en) 2018-11-06
CN108759845B true CN108759845B (en) 2021-08-10

Family

ID=63972390

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810728806.0A Active CN108759845B (en) 2018-07-05 2018-07-05 Optimization method based on low-cost multi-sensor combined navigation

Country Status (1)

Country Link
CN (1) CN108759845B (en)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109407708A (en) * 2018-12-11 2019-03-01 湖南华诺星空电子技术有限公司 A kind of accurate landing control system and Landing Control method based on multi-information fusion
CN109883426B (en) * 2019-03-08 2022-01-14 哈尔滨工程大学 Dynamic distribution and correction multi-source information fusion method based on factor graph
CN110501024B (en) * 2019-04-11 2023-03-28 同济大学 Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN113049001B (en) * 2019-12-26 2023-11-24 魔门塔(苏州)科技有限公司 Evaluation system and method for crowdsourcing map construction
CN112639399A (en) * 2020-04-27 2021-04-09 深圳市大疆创新科技有限公司 Height detection method, compensation amount determination method and device and unmanned aerial vehicle
CN111638514B (en) * 2020-06-19 2023-08-04 四川陆垚控制技术有限公司 Unmanned aerial vehicle height measurement method and unmanned aerial vehicle navigation filter
CN111721299B (en) * 2020-06-30 2022-07-19 上海汽车集团股份有限公司 Real-time positioning time synchronization method and device
CN112034885B (en) * 2020-09-08 2022-08-23 中国人民解放军海军航空大学 Unmanned aerial vehicle roll stabilizing method adopting low-cost inclinometer for measurement
CN112200240B (en) * 2020-09-30 2024-05-31 重庆长安汽车股份有限公司 Multi-sensor target data fusion method, system and computer readable storage medium
CN112629538B (en) * 2020-12-11 2023-02-14 哈尔滨工程大学 Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN113359793B (en) * 2021-06-01 2022-08-23 北京电子工程总体研究所 Compensation method and device for improving airspeed control quality of low-speed aircraft
CN113720327B (en) * 2021-06-01 2024-04-16 深圳致成科技有限公司 Method and system for improving positioning accuracy of vehicle-road cooperative vehicle positioning system
CN114459475A (en) * 2022-02-28 2022-05-10 宁波赛福汽车制动有限公司 Motorcycle body posture detection method, system, motorcycle and storage medium
CN115900646B (en) * 2023-03-08 2023-05-23 北京云圣智能科技有限责任公司 Highly-fused navigation method and device, electronic equipment and storage medium
CN116136405B (en) * 2023-04-04 2023-06-30 天津大学 Data processing method and device for inertial measurement unit introduced into magnetic fluid sensor

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101893445A (en) * 2010-07-09 2010-11-24 哈尔滨工程大学 Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
CN102830414A (en) * 2012-07-13 2012-12-19 北京理工大学 Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system)
CN102879779A (en) * 2012-09-04 2013-01-16 北京航空航天大学 Rod arm measurement and compensation method based on synthetic aperture radar (SAR) remote sensing imaging
CN103616710A (en) * 2013-12-17 2014-03-05 靳文瑞 Multi-sensor combined navigation time synchronizing system based on field programmable gate array (FPGA)

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7193559B2 (en) * 2003-01-21 2007-03-20 Novatel, Inc. Inertial GPS navigation system with modified kalman filter

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101893445A (en) * 2010-07-09 2010-11-24 哈尔滨工程大学 Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
CN102830414A (en) * 2012-07-13 2012-12-19 北京理工大学 Combined navigation method based on SINS/GPS (strapdown inertial navigation system/global position system)
CN102879779A (en) * 2012-09-04 2013-01-16 北京航空航天大学 Rod arm measurement and compensation method based on synthetic aperture radar (SAR) remote sensing imaging
CN103616710A (en) * 2013-12-17 2014-03-05 靳文瑞 Multi-sensor combined navigation time synchronizing system based on field programmable gate array (FPGA)

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
城市3D-GIS实景采集与处理技术研究;李标;《中国优秀硕士学位论文全文数据库 基础科学辑》;中国学术期刊（光盘版）电子杂志社;20140515;A008-23 *
基于ARM7飞控系统多传感器数据融合方法及实现;孙豫川等;《计算机测量与控制》;《计算机测量与控制》杂志社;20130131;第21卷(第1期);103-105、141 *

Also Published As

Publication number Publication date
CN108759845A (en) 2018-11-06

Similar Documents

Publication Publication Date Title
CN108759845B (en) Optimization method based on low-cost multi-sensor combined navigation
EP1200802B1 (en) Vibration compensation for sensors
Tseng et al. Motion and attitude estimation using inertial measurements with complementary filter
TW468035B (en) Micro inertial measurement unit
US6494093B2 (en) Method of measuring motion
US5570304A (en) Method for thermal modeling and updating of bias errors in inertial navigation instrument outputs
CN109696183A (en) The scaling method and device of Inertial Measurement Unit
KR101739390B1 (en) Method for improving the accuracy of self-alignment about the inertial navigation system through gravitational error compensation
CN109506660B (en) Attitude optimization resolving method for bionic navigation
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN109443342A (en) NEW ADAPTIVE Kalman's UAV Attitude calculation method
Chen et al. Dither signal removal of ring laser gyro POS based on combined digital filter
CN114877915B (en) Device and method for calibrating g sensitivity error of laser gyro inertia measurement assembly
Bodó et al. State estimation for UAVs using sensor fusion
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN112577514A (en) Calibration method of MEMS (micro-electromechanical system) inertial device
WO2019127092A1 (en) State estimatation
CN113932803B (en) Inertial/geomagnetic/satellite integrated navigation system suitable for high-dynamic aircraft
CN112051595A (en) Backward differential filtering method for solving motion acceleration of carrier by utilizing DGPS (differential global positioning system) position information
CN112683265B (en) MIMU/GPS integrated navigation method based on rapid ISS collective filtering
CN114993346A (en) Initial alignment method of strapdown inertial navigation system suitable for cross-sea-air medium
CN113984042B (en) Series combined navigation method applicable to high-dynamic aircraft
JP4509006B2 (en) Satellite attitude determination system
Jiachong et al. A swing online calibration method of ship-based FOG-IMU
Abbasi et al. Accuracy Improvement of GPS/INS Navigation System Using Extended Kalman Filter

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