CN111912405A - Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar - Google Patents
Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar Download PDFInfo
- Publication number
- CN111912405A CN111912405A CN201910388074.XA CN201910388074A CN111912405A CN 111912405 A CN111912405 A CN 111912405A CN 201910388074 A CN201910388074 A CN 201910388074A CN 111912405 A CN111912405 A CN 111912405A
- Authority
- CN
- China
- Prior art keywords
- data
- attitude
- vehicle
- angle
- matrix
- 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.)
- Pending
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
Abstract
The invention discloses a combined navigation method and a system based on a vehicle-mounted inertial measurement unit and a Doppler radar. The method comprises the following steps: acquiring inertial data and radar data at the current moment; carrying out navigation position calculation on the attitude and the position of the vehicle at the current moment by adopting angle increment and radar data in the inertial data set; adopting inertial group data to carry out inertial group calculation on the attitude, position and speed of the vehicle at the current moment; subtracting the attitude data corresponding to the inertial measurement unit solution from the attitude data corresponding to the dead reckoning, and subtracting the position data corresponding to the inertial measurement unit solution from the position data corresponding to the dead reckoning; according to the difference value, a system error estimation value at the current moment is obtained by adopting a Kalman filtering algorithm; adopting the system error estimation value at the current moment to carry out error correction on the data obtained by resolving; and outputting the corrected attitude data, position data and speed data resolved by the inertial measurement unit as navigation data at the current moment. The invention has strong anti-interference capability, strong independence and high navigation positioning precision.
Description
Technical Field
The invention relates to the technical field of vehicle navigation and positioning, in particular to a vehicle inertial measurement unit and Doppler radar based integrated navigation method and system.
Background
In order to realize quick response and maneuvering operation, the modern war requires that special military vehicles such as command vehicles, launching vehicles and armored vehicles need to have accurate and quick navigation and positioning capabilities, and a vehicle navigation system needs to have higher autonomy and stronger anti-interference capability in a battlefield environment.
The inertial navigation system based on the inertial navigation unit is a strong anti-interference autonomous navigation system and is very suitable for military application occasions. However, the error of the pure inertial navigation system is accumulated continuously with time, and the long-time high-precision navigation positioning cannot be completed. At present, in the field of vehicle navigation positioning, a plurality of inertial/satellite and inertial/mileometer combined navigation modes are adopted. The inertial/satellite combined navigation has high precision, low cost and good maneuverability, but satellite navigation signals are easy to interfere or shield, so the mode has poor anti-interference capability and independence, and the application in the military field is limited; the inertia/milemeter combined navigation has strong autonomy, good anti-interference performance and high precision, but if the vehicle slips or slides, the larger error is brought to the measurement of the milemeter, thereby influencing the navigation positioning precision of the system.
Disclosure of Invention
Therefore, it is necessary to provide a vehicle navigation method and system with strong anti-interference capability, strong independence and high navigation positioning precision.
In order to achieve the purpose, the invention provides the following scheme:
a combined navigation method based on a vehicle-mounted inertial measurement unit and a Doppler radar comprises the following steps:
acquiring inertial data and radar data at the current moment; the inertial measurement data are velocity increment and angle increment acquired by the inertial measurement unit; the radar data is the speed of the vehicle carrier collected by the Doppler radar;
performing navigation position calculation on the attitude and the position of the vehicle at the current moment by adopting the angle increment and the radar data to obtain a first attitude matrix and first position data;
performing inertial measurement solution on the attitude, position and speed of the vehicle at the current moment by using the inertial measurement data to obtain a second attitude matrix, second position data and speed data;
subtracting the first attitude data from the second attitude data to obtain a first difference value, and subtracting the first position data from the second position data to obtain a second difference value; the first attitude data is obtained according to the first attitude matrix; the second attitude data is obtained according to the second attitude matrix;
obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the first difference value and the second difference value;
error correction is carried out on the first attitude matrix, the first position data, the second attitude matrix, the second position data and the speed data by adopting a system error estimation value at the current moment to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data and speed correction data;
and determining the second posture correction data, the second position correction data and the speed correction data as navigation data output at the current moment.
Optionally, the performing the dead reckoning on the attitude and the position of the vehicle at the current time by using the angle increment and the radar data to obtain a first attitude matrix and first position data specifically includes:
obtaining a first attitude matrix of the vehicle loading at the current moment by adopting an equivalent rotation vector method according to the angle increment;
and performing navigation position calculation on the position of the vehicle at the current moment according to the radar data and the first attitude matrix to obtain first position data.
Optionally, obtaining a first attitude matrix of the vehicle at the current moment by using an equivalent rotation vector method according to the angle increment specifically includes:
calculating the attitude quaternion of the vehicle at the current moment by adopting an equivalent rotation vector method according to the angle increment; the formula of the attitude quaternion is
Wherein the content of the first and second substances,
represents tjThe posture quaternion of the vehicle is carried at any moment,
represents tj-1The posture quaternion of the vehicle is carried at any moment,
indicating that vehicle system b is from tj-1Time tjTime of day transformation quaternion, ηjIs denoted by tj-1To tjThe time vehicle system b is an equivalent rotation vector relative to the navigation system n;
determining an attitude matrix of the vehicle carrying at the current moment according to the attitude quaternion of the vehicle carrying at the current moment; the attitude matrix of the vehicle carrier is a first attitude matrix.
Optionally, the performing the dead reckoning on the position of the vehicle at the current time according to the radar data and the first attitude matrix to obtain first position data specifically includes:
performing navigation position calculation on the position of the vehicle at the current moment by adopting the first attitude matrix and the radar data to obtain the latitude of the position of the vehicle, the longitude of the position and the height of the position of the vehicle at the current moment;
hj＝hj-1+ΔSUj，
wherein L isjIs shown at tjLatitude, lambda of the location of the vehicle at any momentjIs shown at tjLongitude of the location of the vehicle at that moment, hjIs shown at tjHeight of vehicle at all times, Lj-1Is shown at tj-1Latitude, lambda of the location of the vehicle at any momentj-1Is shown at tj-1Longitude of the location of the vehicle at that moment, hj-1Is shown at tj-1The height of the vehicle at the moment, Delta SEj、ΔSNj、ΔSUjAre each [ tj-1,tj]Projection components of vehicle driving distance increment in three axes of northeast geographic coordinate system in time period, RMDenotes the principal radius of curvature, R, of the local meridianNExpressing the main curvature radius of the prime circle perpendicular to the meridian plane
Wherein the content of the first and second substances,
represents tj-1The radar output data at the time of day,
represents tjRadar output data of time, T-tableShowing the refresh period, T ═ Tj-tj-1，
Represents tjA first attitude matrix corresponding to the vehicle loading at any moment;
and determining the latitude of the position of the carrier vehicle, the longitude of the position of the carrier vehicle and the height of the position of the carrier vehicle as first position data.
Optionally, the obtaining, according to the first difference and the second difference, a system error estimation value at the current time by using a kalman filtering algorithm specifically includes:
constructing a measurement vector according to the first difference and the second difference; the measurement vector
Z＝[ψS-ψD,θS-θD,γS-γD,LS-LD,λS-λD,hS-hD]T，
Wherein psiS,θS,γSRespectively resolving and outputting a course angle, a pitch angle and a roll angle psi of the vehicle loader by the inertial measurement unitD,θD,γDRespectively resolving a course angle, a pitch angle and a rolling angle of the output carrier vehicle for the navigation position; l isS,λS,hSRespectively resolving and outputting the latitude, longitude and height L of the position of the vehicle load by the inertial measurement unitD,λD,hDRespectively resolving and outputting the latitude of the position of the carrier vehicle, the longitude of the position and the height of the position for the navigation position;
constructing a system state; the system state
X＝[φE,φN,φU,vE,vN,vU,L,λ,h,bx,by,bz,▽bx,▽by,▽bz,φDE,φDN,φDU,LD,λD,hD]T，
Wherein phi isEEast misalignment angle, phi, of a mathematical platform solved for an inertial measurement unitNIs the north misalignment angle phiUIs the angle of vertical misalignment, vEEast velocity error, v, resolved for the inertial setNIs a north velocity error, vUIs the error of the speed in the sky, L is the latitude error of the inertial unit, lambda is the longitude error, h is the altitude error,bxis the constant drift of the inertial set x-axis gyroscope,byis the constant drift of the inertial y-axis gyroscope,bzis a constant value drift of a z-axis gyro of an inertial measurement unitbxIs a constant error of an inertial measurement unit x-axis accelerometerbyIs a constant error of the inertial set y-axis accelerometerbzIs a constant error of z-axis accelerometer of inertial measurement unitDEEast misalignment angle of mathematical platform, phi, for dead reckoningDNNorth misalignment angle, phi, resolved for dead reckoningDUAngle of day misalignment, L, resolved for dead reckoningDLatitude error, λ, resolved for dead reckoningDLongitude error resolved for dead reckoning, hDHeight error resolved for the dead reckoning;
obtaining a measurement equation according to the measurement vector and the system state; the measurement equation is a relation equation between the measurement vector and the system state; the measurement equation is
Wherein, TwvRepresenting a w-th row and a v-th column of the attitude matrix of the vehicle obtained by the inertial measurement unit through calculation, namely a w-th row and a v-th column of the second attitude matrix; cwvRepresenting a w-th row and a v-th column of a posture matrix of the vehicle obtained by the navigation position calculation, namely a w-th row and a v-th column of the first posture matrix;
and obtaining a system error estimated value at the current moment by adopting a Kalman filtering algorithm according to the measurement equation.
Optionally, the performing error correction on the first attitude matrix, the first position data, the second attitude matrix, the second position data, and the speed data by using the system error estimation value at the current time to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data, and speed correction data specifically includes:
error correction is carried out on the first position data, the second position data and the speed data by adopting a system error estimation value at the current moment to obtain first position correction data, second position correction data and speed correction data;
correcting the first attitude matrix by using a first attitude correction matrix to obtain a corrected first attitude matrix; the first attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the inertial set,
is an estimate of the north misalignment angle,
is an estimated value of the antenna misalignment angle;
correcting the second attitude matrix by using a second attitude correction matrix to obtain a corrected second attitude matrix; the second attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the position,
an estimate of the north misalignment angle resolved for the position,
an estimated value of a sky-direction misalignment angle calculated for the navigation position;
determining first attitude correction data and second attitude correction data; the first attitude correction data is attitude data after corresponding correction of dead reckoning; the second attitude correction data is corresponding corrected attitude data calculated by the inertial measurement unit; the attitude data is a course attitude angle of the vehicle; the course attitude angle of the vehicle carrier comprises a course angle, a pitch angle and a rolling angle of the vehicle carrier.
The invention also provides a combined navigation system based on the vehicle-mounted inertial measurement unit and the Doppler radar, which comprises the following components:
the data acquisition module is used for acquiring the inertial data and the radar data at the current moment; the inertial measurement data are velocity increment and angle increment acquired by the inertial measurement unit; the radar data is the speed of the vehicle carrier collected by the Doppler radar;
the navigation position resolving module is used for performing navigation position resolving on the attitude and the position of the vehicle at the current moment by adopting the angle increment and the radar data to obtain a first attitude matrix and first position data;
the inertial measurement unit resolving module is used for performing inertial measurement unit resolving on the attitude, the position and the speed of the vehicle at the current moment by adopting the inertial measurement unit data to obtain a second attitude matrix, second position data and speed data;
the difference making module is used for making a difference between the first attitude data and the second attitude data to obtain a first difference value, and making a difference between the first position data and the second position data to obtain a second difference value; the first attitude data is obtained according to the first attitude matrix; the second attitude data is obtained according to the second attitude matrix;
the filtering estimation module is used for obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the first difference value and the second difference value;
a correction module, configured to perform error correction on the first attitude matrix, the first position data, the second attitude matrix, the second position data, and the speed data by using a system error estimation value at a current time to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data, and speed correction data;
and the output data determining module is used for determining the second posture correction data, the second position correction data and the speed correction data as navigation data output at the current moment.
Optionally, the dead reckoning module specifically includes:
the attitude calculation unit is used for obtaining a first attitude matrix of the vehicle load at the current moment by adopting an equivalent rotation vector method according to the angle increment;
and the position calculating unit is used for carrying out navigation position calculation on the position of the vehicle at the current moment according to the radar data and the first attitude matrix to obtain first position data.
Optionally, the filtering estimation module specifically includes:
the first construction unit is used for constructing a measurement vector according to the first difference and the second difference; the measurement vector
Z＝[ψS-ψD,θS-θD,γS-γD,LS-LD,λS-λD,hS-hD]T，
Wherein psiS,θS,γSRespectively resolving and outputting a course angle, a pitch angle and a roll angle psi of the vehicle loader by the inertial measurement unitD,θD,γDRespectively resolving a course angle, a pitch angle and a rolling angle of the output carrier vehicle for the navigation position; l isS,λS,hSRespectively resolving and outputting the latitude, longitude and height L of the position of the vehicle load by the inertial measurement unitD,λD,hDRespectively resolving and outputting the latitude of the position of the carrier vehicle, the longitude of the position and the height of the position for the navigation position;
the second construction unit is used for constructing a system state; the system state
X＝[φE,φN,φU,vE,vN,vU,L,λ,h,bx,by,bz,▽bx,▽by,▽bz,φDE,φDN,φDU,LD,λD,hD]T，
Wherein phi isEEast misalignment angle, phi, of a mathematical platform solved for an inertial measurement unitNIs the north misalignment angle phiUIs the angle of vertical misalignment, vEEast velocity error, v, resolved for the inertial setNIs a north velocity error, vUIs the error of the speed in the sky, L is the latitude error of the inertial unit, lambda is the longitude error, h is the altitude error,bxis the constant drift of the inertial set x-axis gyroscope,byis the constant drift of the inertial y-axis gyroscope,bzis a constant value drift of a z-axis gyro of an inertial measurement unitbxIs a constant error of an inertial measurement unit x-axis accelerometerbyIs a constant error of the inertial set y-axis accelerometerbzIs a constant error of z-axis accelerometer of inertial measurement unitDEEast misalignment angle of mathematical platform, phi, for dead reckoningDNNorth misalignment angle, phi, resolved for dead reckoningDUAngle of day misalignment, L, resolved for dead reckoningDLatitude error, λ, resolved for dead reckoningDLongitude error resolved for dead reckoning, hDHeight error resolved for the dead reckoning;
the third construction unit is used for obtaining a measurement equation according to the measurement vector and the system state; the measurement equation is a relation equation between the measurement vector and the system state; the measurement equation is
Wherein, TwvRepresenting a w-th row and a v-th column of the attitude matrix of the vehicle obtained by the inertial measurement unit through calculation, namely a w-th row and a v-th column of the second attitude matrix; cwvRepresenting a w-th row and a v-th column of a posture matrix of the vehicle obtained by the navigation position calculation, namely a w-th row and a v-th column of the first posture matrix;
and the error estimation unit is used for obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the measurement vector.
Optionally, the correction module specifically includes:
a first correcting unit, configured to perform error correction on the first position data, the second position data, and the speed data by using a system error estimation value at a current time to obtain first position correction data, second position correction data, and speed correction data;
the second correction unit is used for correcting the first attitude matrix by using the first attitude correction matrix to obtain a corrected first attitude matrix; the first attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the inertial set,
is an estimate of the north misalignment angle,
is an estimated value of the antenna misalignment angle;
the third correction unit is used for correcting the second attitude matrix by using the second attitude correction matrix to obtain a corrected second attitude matrix; the second attitude correction matrix
Wherein the content of the first and second substances,
east direction of math platform for dead reckoningAn estimate of the angle of misalignment is determined,
an estimate of the north misalignment angle resolved for the position,
an estimated value of a sky-direction misalignment angle calculated for the navigation position;
an attitude data determination unit for determining first attitude correction data and second attitude correction data; the first attitude correction data is attitude data after corresponding correction of dead reckoning; the second attitude correction data is corresponding corrected attitude data calculated by the inertial measurement unit; the attitude data is a course attitude angle of the vehicle; the course attitude angle of the vehicle carrier comprises a course angle, a pitch angle and a rolling angle of the vehicle carrier.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides a combined navigation method and a system based on a vehicle-mounted inertial measurement unit and a Doppler radar. The combined navigation method based on the vehicle-mounted inertial group and the Doppler radar adopts the combination of the vehicle-mounted Doppler velocity measurement radar and the inertial group system, realizes high-precision and high-performance combined navigation, fully utilizes the outstanding advantages of high precision, no velocity measurement error accumulation, continuous output, good anti-interference performance, strong autonomy and the like of the vehicle-mounted Doppler radar to assist the inertial group system to correct errors, realizes quick and accurate positioning and orientation, and has strong anti-interference capability and high autonomy; the invention can avoid the measurement error caused by the skidding or sliding of the vehicle, and effectively reduce the adverse effect of external factors on the vehicle navigation; the invention has the advantages of easy realization of engineering and strong practicability, and is very suitable for special application fields of military affairs, armed police, public transportation and the like.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
Fig. 1 is a flowchart of a combined navigation method based on a vehicle-mounted inertial measurement unit and a doppler radar according to an embodiment of the present invention.
Detailed Description
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 only a part of the embodiments of the present invention, and not all of the embodiments. 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.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Fig. 1 is a flowchart of a combined navigation method based on a vehicle-mounted inertial measurement unit and a doppler radar according to an embodiment of the present invention.
Referring to fig. 1, the combined navigation method based on the vehicle-mounted inertial measurement unit and the doppler radar of the embodiment includes:
step S1: acquiring inertial data and radar data at the current moment; the inertial measurement data are velocity increment and angle increment acquired by the inertial measurement unit; the radar data is the speed of the vehicle carrier collected by the Doppler radar.
Step S2: and performing navigation position calculation on the attitude and the position of the vehicle at the current moment by adopting the angle increment and the radar data to obtain a first attitude matrix and first position data.
The step S2 specifically includes:
21) because the equivalent rotation vector rule can adopt a multi-subsample algorithm to realize effective compensation of the non-exchangeable error, the algorithm relationship is simple, the operation is easy, and the precision is high, the first attitude matrix of the vehicle load at the current moment is obtained by adopting the equivalent rotation vector method according to the angle increment. The method specifically comprises the following steps:
a. and calculating the attitude quaternion of the vehicle load at the current moment by adopting an equivalent rotation vector method according to the angle increment. Let the attitude update period be T (T ═ T)j-tj-1)，tjThe attitude quaternion of the vehicle at any moment is
tj-1The quaternion of the time attitude is
Vehicle system b is from tj-1To tjThe change quaternion of the time of day is
The attitude quaternion updating formula of dead reckoning can be obtained according to the quaternion multiplication rule
Wherein eta isjIs tj-1To tjThe time frame b is an equivalent rotation vector relative to the navigation frame n,
for the projection of the angular speed of the vehicle in the vehicle body system,
is the projection of the rotation angle rate of the navigation system relative to the inertia space in the navigation system.
Due to the fact that
Ratio of change of
Much more violent, so it can be considered that
And
at tj-1≤t≤tjThe equivalent rotation vector eta is basically unchanged in timejIs approximated to
Wherein v isE,vNThe speed of the vehicle is east and north, L and h are latitude and height, RMDenotes the principal radius of curvature, R, of the local meridianNThe main curvature radius of the unitary and mortise ring perpendicular to the meridian plane is shown.
Note tj-1To tjEquivalent rotation vector of time vehicle system relative to inertia space
Due to the fact that in the attitude updating period of the vehicle
Slowly changing along with time, the first-order polynomial can be adopted to approximately describe the angular speed of the vehicle carrying vehicle
Thereby deriving phijIs calculated by
Wherein, Delta thetaj1At t for the gyroscope in the inertial setj+t1Angular increment of the time output, Delta thetaj2At t for the gyroscope in the inertial setj+t1+t2Angular increment of the time output, wherein
Thus, completing the dead reckoning attitude quaternion
After real-time updating, order
The attitude matrix of the vehicle carrier can be determined by the following formula
Namely a first attitude matrix and a heading attitude angle of the vehicle.
ψD＝tan-1(C12/C22)
θD＝sin-1(C32)
γD＝tan-1(-C31/C33)
Wherein psiD,θD,γDRespectively resolving and outputting a course angle, a pitch angle and a rolling angle of the vehicle carrier, CwvAnd representing the w-th row and the v-th column of the vehicle attitude matrix obtained by the dead reckoning, namely the w-th row and the v-th column of the first attitude matrix.
22) And performing navigation position calculation on the position of the vehicle at the current moment according to the radar data and the first attitude matrix to obtain first position data. The method specifically comprises the following steps:
by usingAnd performing navigation position calculation on the position of the vehicle at the current moment by using the first attitude matrix and the radar data to obtain the latitude, the longitude and the height of the position of the vehicle at the current moment. The position updating period and the attitude updating period of the navigation position calculation are consistent, namely T. Let tjThe latitude of the position of the vehicle, the longitude of the position of the vehicle and the height of the position of the vehicle are respectively Lj、λj、hjThen the position update formula of dead reckoning is
hj＝hj-1+ΔSUj，
Wherein L isj-1Is shown at tj-1Latitude, lambda of the location of the vehicle at any momentj-1Is shown at tj-1Longitude of the location of the vehicle at that moment, hj-1Is shown at tj-1The height of the vehicle at the moment, Delta SEj、ΔSNj、ΔSUjAre each [ tj-1,tj]Projection components of vehicle driving distance increment in three axes of northeast geographic coordinate system in time period, RMDenotes the principal radius of curvature, R, of the local meridianNExpressing the main curvature radius of the prime circle perpendicular to the meridian plane
According to tj-1、tjVehicle carrying speed output by Doppler radar at any moment
Approximate description using a first order polynomial [ t ]j-1,tj]The speed of the vehicle in the time period can be obtained
Is approximately calculated as
Wherein the content of the first and second substances,
represents tjAnd carrying a first attitude matrix corresponding to the vehicle all the time.
And determining the latitude of the position of the carrier vehicle, the longitude of the position of the carrier vehicle and the height of the position of the carrier vehicle as first position data.
Step S3: and performing inertial set calculation on the attitude, the position and the speed of the vehicle at the current moment by adopting the inertial set data to obtain a second attitude matrix, second position data and speed data.
Step S4: and subtracting the first attitude data from the second attitude data to obtain a first difference value, and subtracting the first position data from the second position data to obtain a second difference value. The first attitude data is obtained according to the first attitude matrix; the second attitude data is obtained according to the second attitude matrix.
Step S5: and obtaining a system error estimated value at the current moment by adopting a Kalman filtering algorithm according to the first difference value and the second difference value.
The step S5 specifically includes:
51) because the attitude and position information of the vehicle can be output in real time by the inertial measurement and the dead reckoning, the attitude/position combination can be selected as the measurement of the integrated navigation Kalman filtering, and the attitude information is introduced into the measurement equation, which is favorable for effectively improving the course and the attitude precision of the integrated navigation. Therefore, the vehicle loading attitude and position information output by the inertial measurement unit through calculation is subtracted from the corresponding information output by the dead reckoning to be used as measurement, and a measurement vector is constructed according to the first difference and the second difference; the measurement vector
Z＝[ψS-ψD,θS-θD,γS-γD,LS-LD,λS-λD,hS-hD]T，
Wherein psiS,θS,γSRespectively resolving and outputting a course angle, a pitch angle and a roll angle psi of the vehicle loader by the inertial measurement unitD,θD,γDRespectively resolving a course angle, a pitch angle and a rolling angle of the output carrier vehicle for the navigation position; l isS,λS,hSRespectively resolving and outputting the latitude, longitude and height L of the position of the vehicle load by the inertial measurement unitD,λD,hDAnd respectively resolving and outputting the latitude of the position of the carrier vehicle, the longitude of the position and the height of the position.
52) Constructing a system state; the system state
X＝[φE,φN,φU,vE,vN,vU,L,λ,h,bx,by,bz,▽bx,▽by,▽bz,φDE,φDN,φDU,LD,λD,hD]T，
Wherein phi isEEast misalignment angle, phi, of a mathematical platform solved for an inertial measurement unitNIs the north misalignment angle phiUIs the angle of vertical misalignment, vEEast velocity error, v, resolved for the inertial setNIs a north velocity error, vUIs the error of the speed in the sky, L is the latitude error of the inertial unit, lambda is the longitude error, h is the altitude error,bxis the constant drift of the inertial set x-axis gyroscope,byis the constant drift of the inertial y-axis gyroscope,bzis a constant value drift of a z-axis gyro of an inertial measurement unitbxIs a constant error of an inertial measurement unit x-axis accelerometerbyIs a constant error of the inertial set y-axis accelerometerbzIs a constant error of z-axis accelerometer of inertial measurement unitDEEast misalignment angle of mathematical platform, phi, for dead reckoningDNNorth misalignment angle, phi, resolved for dead reckoningDUAngle of day misalignment, L, resolved for dead reckoningDLatitude error, λ, resolved for dead reckoningDLongitude error resolved for dead reckoning, hDHeight error resolved for dead reckoning.
53) Obtaining a measurement equation based on the measurement vector and the system state according to the relation between the attitude angle error resolved by the inertial measurement unit and the attitude error of the mathematical platform; the measurement equation is a relation equation between the measurement vector and the system state; the measurement equation is
Wherein, TwvRepresenting a w-th row and a v-th column of the attitude matrix of the vehicle obtained by the inertial measurement unit through calculation, namely a w-th row and a v-th column of the second attitude matrix; cwvAnd representing the w-th row and the v-th column of the vehicle attitude matrix obtained by the dead reckoning, namely the w-th row and the v-th column of the first attitude matrix.
According to the measurement equation, filtering calculation is carried out by adopting a Kalman filtering algorithm, and a real-time estimation value of the system state X (namely the error between inertial measurement solution and dead reckoning) can be recursively calculated through the filtering calculation, so that the estimation value of the system error at the current moment is obtained.
Step S6: and carrying out error correction on the first attitude matrix, the first position data, the second attitude matrix, the second position data and the speed data by adopting the system error estimation value at the current moment to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data and speed correction data.
Step S7: and determining the second posture correction data, the second position correction data and the speed correction data as navigation data output at the current moment.
Because the errors of the inertial measurement unit solution and the dead reckoning are gradually dispersed along with the time, in order to effectively improve the integrated navigation precision, the results of the inertial measurement unit solution and the dead reckoning need to be respectively subjected to error correction in real time by using the system error estimation value. As an optional implementation manner, the step S6 specifically includes:
61: and carrying out error correction on the first position data, the second position data and the speed data by adopting the system error estimation value at the current moment to obtain first position correction data, second position correction data and speed correction data. Specifically, for the position data and speed data errors, the second position data output by the inertial measurement unit, the speed data and the first position data output by the dead reckoning are digital quantities, so that the corresponding error estimation values can be directly subtracted from the position and speed outputs of the inertial measurement unit and the dead reckoning to correct the errors.
62: for the attitude error of the mathematical platform, the attitude correction matrix is needed to be respectively utilized
Vehicle-carrying attitude matrix obtained by correcting inertial measurement unit solution and navigation position solution
The specific correction method is as follows.
Setting the navigation coordinate systems actually established by inertial measurement and navigation position calculation as n' system and
the attitude matrix obtained by actual solution of the two systems is respectively
Wherein n' is an error angle phi with respect to nn＝[φEφNφU]T，
Is at an error angle with respect to n
(all considered to be small angles). Thus, a mathematical platform attitude error estimate obtained using a filtering calculation
And
can respectively calculate the conversion matrix from n' system to n system
And
conversion matrix tied to n series
Namely, it is
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the inertial set,
is an estimate of the north misalignment angle,
is an estimated value of the antenna misalignment angle;
wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the position,
an estimate of the north misalignment angle resolved for the position,
an estimate of the antenna misalignment angle resolved for the position.
And real vehicle carrying attitude matrix
Can be calculated by the following formula:
therefore, by using the formula, the attitude matrixes of inertial measurement solution and dead reckoning can be respectively corrected to obtain a corrected first attitude matrix and a corrected second attitude matrix, namely, the real-time correction of the attitude error of the system can be realized.
63: determining first attitude correction data and second attitude correction data; the first attitude correction data is attitude data after corresponding correction of dead reckoning; the second attitude correction data is corresponding corrected attitude data calculated by the inertial measurement unit; the attitude data is a course attitude angle of the vehicle; the course attitude angle of the vehicle carrier comprises a course angle, a pitch angle and a rolling angle of the vehicle carrier.
The navigation method based on the vehicle-mounted inertial measurement unit has the following advantages:
1) the method has the advantages of high speed, high precision, strong anti-interference capability, good autonomy, easy realization of the engineering and the like.
2) The problem of divergence of the navigation error of the pure inertial navigation unit is effectively restrained, the navigation positioning error caused by vehicle slipping or sliding can be avoided, the long-time navigation positioning accuracy is high, the performance is good, the engineering practicability is strong, and the influence of the external environment is not easy to cause.
3) As can be seen from step S2 of this embodiment, the method uses the angular increment output by the gyroscope in the inertial measurement unit and the speed of the vehicle-carrying vehicle output by the doppler radar to implement independent calculation of the vehicle-carrying attitude, and can improve the attitude calculation accuracy by designing a high-precision attitude update algorithm, which provides more accurate and beneficial measurement information for integrated navigation.
4) In the embodiment, the errors of inertial set solution and dead reckoning are taken as states together, which is beneficial to effectively estimating the inertial set solution errors and the dead reckoning errors by using the integrated navigation kalman filtering technology, so that not only the inertial set solution errors can be corrected, but also the dead reckoning errors can be effectively corrected, which is beneficial to improving the whole integrated navigation precision.
5) In addition to the position information, the method of the embodiment also introduces the attitude information output by inertial measurement solution and dead reckoning into the measurement of the integrated navigation to improve the observability of the attitude error of inertial measurement solution, thereby effectively solving the prominent problem of low course accuracy commonly existing in the traditional integrated navigation method and also being beneficial to further improving the whole integrated navigation accuracy.
6) In practical application, the internal structure and output parameters of the inertial unit system do not need to be changed, only a set of Doppler speed measurement radar needs to be carried on a vehicle to complete necessary installation position calibration and relevant parameter testing, high-precision navigation positioning can be carried out by using the method provided by the embodiment, the engineering is easy to realize, the using effect is good, and the application field is wide.
The invention also provides a combined navigation system based on the vehicle-mounted inertial measurement unit and the Doppler radar, which comprises the following components:
the data acquisition module is used for acquiring the inertial data and the radar data at the current moment; the inertial measurement data are velocity increment and angle increment acquired by the inertial measurement unit; the radar data is the speed of the vehicle carrier collected by the Doppler radar.
And the navigation position resolving module is used for performing navigation position resolving on the attitude and the position of the vehicle at the current moment by adopting the angle increment and the radar data to obtain a first attitude matrix and first position data.
And the inertial measurement unit calculation module is used for carrying out inertial measurement unit calculation on the attitude, the position and the speed of the vehicle at the current moment by adopting the inertial measurement unit data to obtain a second attitude matrix, second position data and speed data.
And the difference making module is used for making a difference between the first attitude data and the second attitude data to obtain a first difference value, and making a difference between the first position data and the second position data to obtain a second difference value. The first attitude data is obtained according to the first attitude matrix; the second attitude data is obtained according to the second attitude matrix.
And the filtering estimation module is used for obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the first difference value and the second difference value.
And the correction module is used for performing error correction on the first attitude matrix, the first position data, the second attitude matrix, the second position data and the speed data by adopting the system error estimation value at the current moment to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data and speed correction data.
And the output data determining module is used for determining the second posture correction data, the second position correction data and the speed correction data as navigation data output at the current moment.
As an optional implementation manner, the dead reckoning module specifically includes:
and the attitude calculation unit is used for obtaining a first attitude matrix of the vehicle load at the current moment by adopting an equivalent rotation vector method according to the angle increment. The attitude calculation unit specifically comprises:
calculating the attitude quaternion of the vehicle at the current moment by adopting an equivalent rotation vector method according to the angle increment; the formula of the attitude quaternion is
Wherein the content of the first and second substances,
represents tjThe posture quaternion of the vehicle is carried at any moment,
represents tj-1The posture quaternion of the vehicle is carried at any moment,
indicating that vehicle system b is from tj-1Time tjTime of day transformation quaternion, ηjIs denoted by tj-1To tjThe time vehicle system b is an equivalent rotation vector relative to the navigation system n;
determining an attitude matrix of the vehicle carrying at the current moment according to the attitude quaternion of the vehicle carrying at the current moment; the attitude matrix of the vehicle carrier is a first attitude matrix.
And the position calculating unit is used for carrying out navigation position calculation on the position of the vehicle at the current moment according to the radar data and the first attitude matrix to obtain first position data. The position calculating unit specifically comprises:
performing navigation position calculation on the position of the vehicle at the current moment by adopting the first attitude matrix and the radar data to obtain the latitude of the position of the vehicle, the longitude of the position and the height of the position of the vehicle at the current moment;
hj＝hj-1+ΔSUj，
wherein L isjIs shown at tjLatitude, lambda of the location of the vehicle at any momentjIs shown at tjLongitude of the location of the vehicle at that moment, hjIs shown at tjHeight of vehicle at all times, Lj-1Is shown at tj-1Latitude, lambda of the location of the vehicle at any momentj-1Is shown at tj-1Longitude of the location of the vehicle at that moment, hj-1Is shown at tj-1The height of the vehicle at the moment, Delta SEj、ΔSNj、ΔSUjAre each [ tj-1,tj]Projection components of vehicle driving distance increment in three axes of northeast geographic coordinate system in time period, RMDenotes the principal radius of curvature, R, of the local meridianNExpressing the main curvature radius of the prime circle perpendicular to the meridian plane
Wherein the content of the first and second substances,
represents tj-1The radar output data at the time of day,
represents tjRadar output data at time, T represents updating period, and T is Tj-tj-1，
Represents tjA first attitude matrix corresponding to the vehicle loading at any moment;
and determining the latitude of the position of the carrier vehicle, the longitude of the position of the carrier vehicle and the height of the position of the carrier vehicle as first position data.
Optionally, the filtering estimation module specifically includes:
the first construction unit is used for constructing a measurement vector according to the first difference and the second difference; the measurement vector
Z＝[ψS-ψD,θS-θD,γS-γD,LS-LD,λS-λD,hS-hD]T，
Wherein psiS,θS,γSRespectively resolving and outputting a course angle, a pitch angle and a roll angle psi of the vehicle loader by the inertial measurement unitD,θD,γDRespectively resolving a course angle, a pitch angle and a rolling angle of the output carrier vehicle for the navigation position; l isS,λS,hSRespectively resolving and outputting the latitude, longitude and height L of the position of the vehicle load by the inertial measurement unitD,λD,hDRespectively resolving and outputting the latitude of the position of the carrier vehicle, the longitude of the position and the height of the position for the navigation position;
the second construction unit is used for constructing a system state; the system state
X＝[φE,φN,φU,vE,vN,vU,L,λ,h,bx,by,bz,▽bx,▽by,▽bz,φDE,φDN,φDU,LD,λD,hD]T，
Wherein phi isEEast misalignment angle, phi, of a mathematical platform solved for an inertial measurement unitNIs the north misalignment angle phiUIs the angle of vertical misalignment, vEEast velocity error, v, resolved for the inertial setNIs a north velocity error, vUIs the error of the speed in the sky, L is the error of the latitude resolved by the inertial set, lambda is the error of the longitude, h is the error of the altitude,bxis the constant drift of the inertial set x-axis gyroscope,byis the constant drift of the inertial set y-axis gyroscope,bzis a constant value drift of a z-axis gyro of an inertial measurement unitbxIs a constant error of an inertial measurement unit x-axis accelerometerbyIs a constant error of the inertial set y-axis accelerometerbzAs z-axis acceleration of the inertial massError of the constant value phiDEEast misalignment angle of mathematical platform, phi, for dead reckoningDNNorth misalignment angle, phi, resolved for dead reckoningDUAngle of day misalignment, L, resolved for dead reckoningDLatitude error, λ, resolved for dead reckoningDLongitude error resolved for dead reckoning, hDHeight error resolved for the dead reckoning;
the third construction unit is used for obtaining a measurement equation according to the measurement vector and the system state; the measurement equation is a relation equation between the measurement vector and the system state; the measurement equation is
Wherein, TwvRepresenting a w-th row and a v-th column of a vehicle attitude matrix obtained by inertial measurement, namely a w-th row and a v-th column of a second attitude matrix; cwvRepresenting a w-th row and a v-th column of a vehicle attitude matrix obtained by the navigation position calculation, namely a w-th row and a v-th column of the first attitude matrix;
and the error estimation unit is used for obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the measurement equation.
As an optional implementation manner, the correction module specifically includes:
a first correcting unit, configured to perform error correction on the first position data, the second position data, and the speed data by using a system error estimation value at a current time to obtain first position correction data, second position correction data, and speed correction data;
the second correction unit is used for correcting the first attitude matrix by using the first attitude correction matrix to obtain a corrected first attitude matrix; the first attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the inertial set,
is an estimate of the north misalignment angle,
is an estimated value of the antenna misalignment angle;
the third correction unit is used for correcting the second attitude matrix by using the second attitude correction matrix to obtain a corrected second attitude matrix; the second attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the position,
an estimate of the north misalignment angle resolved for the position,
an estimated value of a sky-direction misalignment angle calculated for the navigation position;
an attitude data determination unit for determining first attitude correction data and second attitude correction data; the first attitude correction data is attitude data after corresponding correction of dead reckoning; the second attitude correction data is corresponding corrected attitude data calculated by the inertial measurement unit; the attitude data is a course attitude angle of the vehicle; the course attitude angle of the vehicle carrier comprises a course angle, a pitch angle and a rolling angle of the vehicle carrier.
According to the combined navigation system based on the vehicle-mounted inertial unit and the Doppler radar, the combined navigation method based on the vehicle-mounted inertial unit and the Doppler radar adopts the combination of the vehicle-mounted Doppler velocity measurement radar and the inertial unit system, so that high-precision and high-performance combined navigation is realized, the outstanding advantages of high precision, no velocity measurement error accumulation, continuous output, good anti-interference performance, strong autonomy and the like of the vehicle-mounted Doppler radar are fully utilized, the inertial unit system is assisted to correct errors, and not only is quick and accurate positioning and orientation realized, but also strong anti-interference performance and high autonomy are realized; the invention can avoid the measurement error caused by the skidding or sliding of the vehicle, and effectively reduce the adverse effect of external factors on the vehicle navigation; the invention has the advantages of easy realization of engineering and strong practicability, and is very suitable for special application fields of military affairs, armed police, public transportation and the like.
For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.
Claims (10)
1. A combined navigation method based on a vehicle-mounted inertial measurement unit and a Doppler radar is characterized by comprising the following steps:
acquiring inertial data and radar data at the current moment; the inertial measurement data are velocity increment and angle increment acquired by the inertial measurement unit; the radar data is the speed of the vehicle carrier collected by the Doppler radar;
performing navigation position calculation on the attitude and the position of the vehicle at the current moment by adopting the angle increment and the radar data to obtain a first attitude matrix and first position data;
performing inertial measurement solution on the attitude, position and speed of the vehicle at the current moment by using the inertial measurement data to obtain a second attitude matrix, second position data and speed data;
subtracting the first attitude data from the second attitude data to obtain a first difference value, and subtracting the first position data from the second position data to obtain a second difference value; the first attitude data is obtained according to the first attitude matrix; the second attitude data is obtained according to the second attitude matrix;
obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the first difference value and the second difference value;
error correction is carried out on the first attitude matrix, the first position data, the second attitude matrix, the second position data and the speed data by adopting a system error estimation value at the current moment to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data and speed correction data;
and determining the second posture correction data, the second position correction data and the speed correction data as navigation data output at the current moment.
2. The combined navigation method based on the vehicle-mounted inertial measurement unit and the doppler radar according to claim 1, wherein the performing the dead reckoning on the attitude and the position of the vehicle at the current time by using the angular increment and the radar data to obtain a first attitude matrix and first position data specifically comprises:
obtaining a first attitude matrix of the vehicle loading at the current moment by adopting an equivalent rotation vector method according to the angle increment;
and performing navigation position calculation on the position of the vehicle at the current moment according to the radar data and the first attitude matrix to obtain first position data.
3. The combined navigation method based on the vehicle-mounted inertial measurement unit and the doppler radar according to claim 2, wherein the obtaining of the first attitude matrix of the vehicle at the current time by using an equivalent rotation vector method according to the angle increment specifically comprises:
calculating the attitude quaternion of the vehicle at the current moment by adopting an equivalent rotation vector method according to the angle increment; the formula of the attitude quaternion is
Wherein the content of the first and second substances,
represents tjThe posture quaternion of the vehicle is carried at any moment,
represents tj-1The posture quaternion of the vehicle is carried at any moment,
indicating that vehicle system b is from tj-1Time tjTime of day transformation quaternion, ηjIs denoted by tj-1To tjThe time vehicle system b is an equivalent rotation vector relative to the navigation system n;
determining an attitude matrix of the vehicle carrying at the current moment according to the attitude quaternion of the vehicle carrying at the current moment; the attitude matrix of the vehicle carrier is a first attitude matrix.
4. The combined navigation method based on the vehicle-mounted inertial measurement unit and the doppler radar according to claim 3, wherein the performing the dead reckoning on the position of the vehicle at the current time according to the radar data and the first attitude matrix to obtain the first position data specifically comprises:
performing navigation position calculation on the position of the vehicle at the current moment by adopting the first attitude matrix and the radar data to obtain the latitude of the position of the vehicle, the longitude of the position and the height of the position of the vehicle at the current moment;
hj＝hj-1+ΔSUj，
wherein L isjIs shown at tjLatitude, lambda of the location of the vehicle at any momentjIs shown at tjLongitude of the location of the vehicle at that moment, hjIs shown at tjHeight of vehicle at all times, Lj-1Is shown at tj-1Latitude, lambda of the location of the vehicle at any momentj-1Is shown at tj-1Longitude of the location of the vehicle at that moment, hj-1Is shown at tj-1The height of the vehicle at the moment, Delta SEj、ΔSNj、ΔSUjAre each [ tj-1,tj]Projection components of vehicle driving distance increment in three axes of northeast geographic coordinate system in time period, RMDenotes the principal radius of curvature, R, of the local meridianNExpressing the main curvature radius of the prime circle perpendicular to the meridian plane
Wherein the content of the first and second substances,
represents tj-1The radar data at the time of day is,
represents tjRadar data at time, T represents an update period, and T is Tj-tj-1，
Represents tjA first attitude matrix corresponding to the vehicle loading at any moment;
and determining the latitude of the position of the carrier vehicle, the longitude of the position of the carrier vehicle and the height of the position of the carrier vehicle as first position data.
5. The integrated navigation method based on the vehicle-mounted inertial measurement unit and the doppler radar according to claim 1, wherein the obtaining of the system error estimation value at the current time by using a kalman filtering algorithm according to the first difference value and the second difference value specifically comprises:
constructing a measurement vector according to the first difference and the second difference; the measurement vector
Z＝[ψS-ψD,θS-θD,γS-γD,LS-LD,λS-λD,hS-hD]T，
Wherein psiS,θS,γSRespectively resolving and outputting a course angle, a pitch angle and a roll angle psi of the vehicle loader by the inertial measurement unitD,θD,γDRespectively resolving a course angle, a pitch angle and a rolling angle of the output carrier vehicle for the navigation position; l isS,λS,hSRespectively resolving and outputting the latitude, longitude and height L of the position of the vehicle load by the inertial measurement unitD,λD,hDRespectively resolving and outputting the latitude of the position of the carrier vehicle, the longitude of the position and the height of the position for the navigation position;
constructing a system state; the system state
X＝[φE,φN,φU,vE,vN,vU,L,λ,h,bx,by,bz,▽bx,▽by,▽bz,φDE,φDN,φDU,LD,λD,hD]T，
Wherein phi isEEast misalignment angle, phi, of a mathematical platform solved for an inertial measurement unitNIs the north misalignment angle phiUIs the angle of vertical misalignment, vEEast velocity error, v, resolved for the inertial setNIs a north velocity error, vUIs the error of the speed in the sky, L is the error of the latitude resolved by the inertial set, lambda is the error of the longitude, h is the error of the altitude,bxis the constant drift of the inertial set x-axis gyroscope,byis the constant drift of the inertial set y-axis gyroscope,bzis a constant value drift of a z-axis gyro of an inertial measurement unitbxIs a constant error of an inertial measurement unit x-axis accelerometerbyIs a constant error of the inertial set y-axis accelerometerbzIs a constant error of z-axis accelerometer of inertial measurement unitDEEast misalignment angle of mathematical platform, phi, for dead reckoningDNNorth misalignment angle, phi, resolved for dead reckoningDUAngle of day misalignment, L, resolved for dead reckoningDLatitude error, λ, resolved for dead reckoningDLongitude error resolved for dead reckoning, hDHeight error resolved for the dead reckoning;
obtaining a measurement equation according to the measurement vector and the system state; the measurement equation is a relation equation between the measurement vector and the system state; the measurement equation is
Wherein, TwvRepresenting a w-th row and a v-th column of the attitude matrix of the vehicle obtained by the inertial measurement unit through calculation, namely a w-th row and a v-th column of the second attitude matrix; cwvRepresenting a w-th row and a v-th column of a posture matrix of the vehicle obtained by the navigation position calculation, namely a w-th row and a v-th column of the first posture matrix;
and obtaining a system error estimated value at the current moment by adopting a Kalman filtering algorithm according to the measurement equation.
6. The integrated navigation method based on the vehicle-mounted inertial measurement unit and the doppler radar according to claim 1, wherein the error correction is performed on the first attitude matrix, the first position data, the second attitude matrix, the second position data, and the velocity data by using a system error estimation value at a current time to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data, and velocity correction data, and specifically includes:
error correction is carried out on the first position data, the second position data and the speed data by adopting a system error estimation value at the current moment to obtain first position correction data, second position correction data and speed correction data;
correcting the first attitude matrix by using a first attitude correction matrix to obtain a corrected first attitude matrix; the first attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the inertial set,
is an estimate of the north misalignment angle,
is an estimated value of the antenna misalignment angle;
correcting the second attitude matrix by using a second attitude correction matrix to obtain a corrected second attitude matrix; the second attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the position,
an estimate of the north misalignment angle resolved for the position,
an estimated value of a sky-direction misalignment angle calculated for the navigation position;
determining first attitude correction data and second attitude correction data; the first attitude correction data is attitude data after corresponding correction of dead reckoning; the second attitude correction data is corresponding corrected attitude data calculated by the inertial measurement unit; the attitude data is a course attitude angle of the vehicle; the course attitude angle of the vehicle carrier comprises a course angle, a pitch angle and a rolling angle of the vehicle carrier.
7. A combined navigation system based on a vehicle-mounted inertial measurement unit and a Doppler radar is characterized by comprising:
the data acquisition module is used for acquiring the inertial data and the radar data at the current moment; the inertial measurement data are velocity increment and angle increment acquired by the inertial measurement unit; the radar data is the speed of the vehicle carrier collected by the Doppler radar;
the navigation position resolving module is used for performing navigation position resolving on the attitude and the position of the vehicle at the current moment by adopting the angle increment and the radar data to obtain a first attitude matrix and first position data;
the inertial measurement unit resolving module is used for performing inertial measurement unit resolving on the attitude, the position and the speed of the vehicle at the current moment by adopting the inertial measurement unit data to obtain a second attitude matrix, second position data and speed data;
the difference making module is used for making a difference between the first attitude data and the second attitude data to obtain a first difference value, and making a difference between the first position data and the second position data to obtain a second difference value; the first attitude data is obtained according to the first attitude matrix; the second attitude data is obtained according to the second attitude matrix;
the filtering estimation module is used for obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the first difference value and the second difference value;
a correction module, configured to perform error correction on the first attitude matrix, the first position data, the second attitude matrix, the second position data, and the speed data by using a system error estimation value at a current time to obtain first attitude correction data, first position correction data, second attitude correction data, second position correction data, and speed correction data;
and the output data determining module is used for determining the second posture correction data, the second position correction data and the speed correction data as navigation data output at the current moment.
8. The integrated navigation system based on the vehicle-mounted inertial measurement unit and the doppler radar according to claim 7, wherein the dead reckoning module specifically comprises:
the attitude calculation unit is used for obtaining a first attitude matrix of the vehicle load at the current moment by adopting an equivalent rotation vector method according to the angle increment;
and the position calculating unit is used for carrying out navigation position calculation on the position of the vehicle at the current moment according to the radar data and the first attitude matrix to obtain first position data.
9. The integrated navigation system based on the vehicle-mounted inertial measurement unit and the doppler radar of claim 7, wherein the filtering estimation module specifically comprises:
the first construction unit is used for constructing a measurement vector according to the first difference and the second difference; the measurement vector
Z＝[ψS-ψD,θS-θD,γS-γD,LS-LD,λS-λD,hS-hD]T，
Wherein psiS,θS,γSRespectively resolving and outputting a course angle, a pitch angle and a roll angle psi of the vehicle loader by the inertial measurement unitD,θD,γDRespectively resolving a course angle, a pitch angle and a rolling angle of the output carrier vehicle for the navigation position; l isS,λS,hSRespectively resolving and outputting the latitude, longitude and height L of the position of the vehicle load by the inertial measurement unitD,λD,hDRespectively resolving and outputting the latitude of the position of the carrier vehicle, the longitude of the position and the height of the position for the navigation position;
the second construction unit is used for constructing a system state; the system state
X＝[φE,φN,φU,vE,vN,vU,L,λ,h,bx,by,bz,▽bx,▽by,▽bz,φDE,φDN,φDU,LD,λD,hD]T，
Wherein phi isEEast misalignment angle, phi, of a mathematical platform solved for an inertial measurement unitNIs the north misalignment angle phiUIs the angle of vertical misalignment, vEEast velocity error, v, resolved for the inertial setNIs a north velocity error, vUIs the error of the speed in the sky, L is the latitude error of the inertial unit, lambda is the longitude error, h is the altitude error,bxis the constant drift of the inertial set x-axis gyroscope,byis the constant drift of the inertial y-axis gyroscope,bzis a constant value drift of a z-axis gyro of an inertial measurement unitbxIs a constant error of an inertial measurement unit x-axis accelerometerbyIs a constant error of the inertial set y-axis accelerometerbzIs a constant error of z-axis accelerometer of inertial measurement unitDEEast misalignment angle of mathematical platform, phi, for dead reckoningDNNorth misalignment angle, phi, resolved for dead reckoningDUAngle of day misalignment, L, resolved for dead reckoningDLatitude error resolved for dead reckoning，λDLongitude error resolved for dead reckoning, hDHeight error resolved for the dead reckoning;
the third construction unit is used for obtaining a measurement equation according to the measurement vector and the system state; the measurement equation is a relation equation between the measurement vector and the system state; the measurement equation is
Wherein, TwvRepresenting a w-th row and a v-th column of the attitude matrix of the vehicle obtained by the inertial measurement unit through calculation, namely a w-th row and a v-th column of the second attitude matrix; cwvRepresenting a w-th row and a v-th column of a posture matrix of the vehicle obtained by the navigation position calculation, namely a w-th row and a v-th column of the first posture matrix;
and the error estimation unit is used for obtaining a system error estimation value at the current moment by adopting a Kalman filtering algorithm according to the measurement vector.
10. The integrated navigation system based on vehicle-mounted inertial measurement unit and doppler radar of claim 7, wherein the correction module specifically comprises:
a first correcting unit, configured to perform error correction on the first position data, the second position data, and the speed data by using a system error estimation value at a current time to obtain first position correction data, second position correction data, and speed correction data;
the second correction unit is used for correcting the first attitude matrix by using the first attitude correction matrix to obtain a corrected first attitude matrix; the first attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the inertial set,
is an estimate of the north misalignment angle,
is an estimated value of the antenna misalignment angle;
the third correction unit is used for correcting the second attitude matrix by using the second attitude correction matrix to obtain a corrected second attitude matrix; the second attitude correction matrix
Wherein the content of the first and second substances,
an estimate of the east misalignment angle of the mathematical platform solved for the position,
an estimate of the north misalignment angle resolved for the position,
an estimated value of a sky-direction misalignment angle calculated for the navigation position;
an attitude data determination unit for determining first attitude correction data and second attitude correction data; the first attitude correction data is attitude data after corresponding correction of dead reckoning; the second attitude correction data is corresponding corrected attitude data calculated by the inertial measurement unit; the attitude data is a course attitude angle of the vehicle; the course attitude angle of the vehicle carrier comprises a course angle, a pitch angle and a rolling angle of the vehicle carrier.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910388074.XA CN111912405A (en) | 2019-05-10 | 2019-05-10 | Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910388074.XA CN111912405A (en) | 2019-05-10 | 2019-05-10 | Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111912405A true CN111912405A (en) | 2020-11-10 |
Family
ID=73242619
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910388074.XA Pending CN111912405A (en) | 2019-05-10 | 2019-05-10 | Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111912405A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20210049724A (en) * | 2020-07-20 | 2021-05-06 | 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. | Dead reckoning method and apparatus for vehicle, device and storage medium |
Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6037893A (en) * | 1998-07-31 | 2000-03-14 | Litton Systems, Inc. | Enhanced motion compensation technique in synthetic aperture radar systems |
| JP2009192325A (en) * | 2008-02-13 | 2009-08-27 | Furuno Electric Co Ltd | Satellite navigation/estimated navigation integration positioning device |
| CN102608596A (en) * | 2012-02-29 | 2012-07-25 | 北京航空航天大学 | Information fusion method for airborne inertia/Doppler radar integrated navigation system |
| CN102607595A (en) * | 2012-03-07 | 2012-07-25 | 北京航空航天大学 | Method for testing dynamic random drifting of strap-down flexible gyroscope by aid of laser Doppler velocimeter |
| US20120326918A1 (en) * | 2011-06-21 | 2012-12-27 | Honeywell International Inc. | Motion-based adaptive frequency estimation of a doppler velocity sensor |
| CN103217159A (en) * | 2013-03-06 | 2013-07-24 | 郭雷 | SINS/GPS/polarized light combination navigation system modeling and dynamic pedestal initial aligning method |
| CN103776449A (en) * | 2014-02-26 | 2014-05-07 | 北京空间飞行器总体设计部 | Moving base initial alignment method for improving robustness |
| CN105021212A (en) * | 2015-07-06 | 2015-11-04 | 中国人民解放军国防科学技术大学 | Initial orientation information assisted rapid transfer alignment method for autonomous underwater vehicle |
| CN105509738A (en) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | Inertial navigation/Doppler radar combination-based vehicle positioning and orientation method |
| CN108088443A (en) * | 2016-11-23 | 2018-05-29 | 北京自动化控制设备研究所 | A kind of positioning and directing device rate compensation method |
| CN108106635A (en) * | 2017-12-15 | 2018-06-01 | 中国船舶重工集团公司第七0七研究所 | Inertia defends the anti-interference posture course calibration method of long endurance for leading integrated navigation system |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN108345021A (en) * | 2018-01-19 | 2018-07-31 | 东南大学 | A kind of Doppler radar assistant GPS/INS vehicle speed measuring methods |
| CN109443393A (en) * | 2018-12-11 | 2019-03-08 | 中国人民解放军火箭军工程大学 | A kind of inertial navigation method for extracting signal and system based on blind separation algorithm |
| CN109556631A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares |
| CN109556632A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering |
-
2019
- 2019-05-10 CN CN201910388074.XA patent/CN111912405A/en active Pending
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6037893A (en) * | 1998-07-31 | 2000-03-14 | Litton Systems, Inc. | Enhanced motion compensation technique in synthetic aperture radar systems |
| JP2009192325A (en) * | 2008-02-13 | 2009-08-27 | Furuno Electric Co Ltd | Satellite navigation/estimated navigation integration positioning device |
| US20120326918A1 (en) * | 2011-06-21 | 2012-12-27 | Honeywell International Inc. | Motion-based adaptive frequency estimation of a doppler velocity sensor |
| CN102608596A (en) * | 2012-02-29 | 2012-07-25 | 北京航空航天大学 | Information fusion method for airborne inertia/Doppler radar integrated navigation system |
| CN102607595A (en) * | 2012-03-07 | 2012-07-25 | 北京航空航天大学 | Method for testing dynamic random drifting of strap-down flexible gyroscope by aid of laser Doppler velocimeter |
| CN103217159A (en) * | 2013-03-06 | 2013-07-24 | 郭雷 | SINS/GPS/polarized light combination navigation system modeling and dynamic pedestal initial aligning method |
| CN103776449A (en) * | 2014-02-26 | 2014-05-07 | 北京空间飞行器总体设计部 | Moving base initial alignment method for improving robustness |
| CN105021212A (en) * | 2015-07-06 | 2015-11-04 | 中国人民解放军国防科学技术大学 | Initial orientation information assisted rapid transfer alignment method for autonomous underwater vehicle |
| CN105509738A (en) * | 2015-12-07 | 2016-04-20 | 西北工业大学 | Inertial navigation/Doppler radar combination-based vehicle positioning and orientation method |
| CN108088443A (en) * | 2016-11-23 | 2018-05-29 | 北京自动化控制设备研究所 | A kind of positioning and directing device rate compensation method |
| CN108106635A (en) * | 2017-12-15 | 2018-06-01 | 中国船舶重工集团公司第七0七研究所 | Inertia defends the anti-interference posture course calibration method of long endurance for leading integrated navigation system |
| CN108180925A (en) * | 2017-12-15 | 2018-06-19 | 中国船舶重工集团公司第七0七研究所 | A kind of odometer assists vehicle-mounted dynamic alignment method |
| CN108345021A (en) * | 2018-01-19 | 2018-07-31 | 东南大学 | A kind of Doppler radar assistant GPS/INS vehicle speed measuring methods |
| CN109556631A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares |
| CN109556632A (en) * | 2018-11-26 | 2019-04-02 | 北方工业大学 | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering |
| CN109443393A (en) * | 2018-12-11 | 2019-03-08 | 中国人民解放军火箭军工程大学 | A kind of inertial navigation method for extracting signal and system based on blind separation algorithm |
Non-Patent Citations (3)
| Title |
|---|
| YANG BO等: "Accurate integrated position and", 《MEASUREMENT AND CONTROL》 * |
| 丛佃伟著: "《GNSS高动态定位性能检定理论及关键技术研究》", 31 January 2017, 测绘出版社 * |
| 潘鸿飞等: "捷联惯导系统级余度技术研究(上)", 《上海航天》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20210049724A (en) * | 2020-07-20 | 2021-05-06 | 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. | Dead reckoning method and apparatus for vehicle, device and storage medium |
| US11435186B2 (en) * | 2020-07-20 | 2022-09-06 | Beijing Baidu Netcom Science and Technology Co., Ltd | Dead reckoning method and apparatus for vehicle, device and storage medium |
| KR102454882B1 (en) | 2020-07-20 | 2022-10-17 | 베이징 바이두 넷컴 사이언스 앤 테크놀로지 코., 엘티디. | Dead reckoning method and apparatus for vehicle, device and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108226980B (en) | Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit | |
| US8311739B2 (en) | Inertial navigation system error correction | |
| CN109459044B (en) | GNSS dual-antenna assisted vehicle-mounted MEMS inertial navigation combined navigation method | |
| US6859727B2 (en) | Attitude change kalman filter measurement apparatus and method | |
| CN112097763B (en) | Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination | |
| CN105606094B (en) | A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems | |
| CN108387227B (en) | Multi-node information fusion method and system of airborne distributed POS | |
| CN110926468B (en) | Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN112504275B (en) | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN111141273A (en) | Combined navigation method and system based on multi-sensor fusion | |
| CN113503892B (en) | Inertial navigation system moving base initial alignment method based on odometer and retrospective navigation | |
| CN113252038B (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
| CN108303120B (en) | Real-time transfer alignment method and device for airborne distributed POS | |
| CN108151765B (en) | Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error | |
| CN111912427B (en) | Method and system for aligning motion base of strapdown inertial navigation assisted by Doppler radar | |
| CN114812545A (en) | Combined navigation method and device based on dual-laser Doppler velocimeter and inertial navigation system | |
| CN110514200B (en) | Inertial navigation system and high-rotation-speed rotating body attitude measurement method | |
| CN109059913A (en) | A kind of zero-lag integrated navigation initial method for onboard navigation system | |
| RU2277696C2 (en) | Integrated satellite inertial-navigational system | |
| CN111912405A (en) | Combined navigation method and system based on vehicle-mounted inertial measurement unit and Doppler radar | |
| CN112083425A (en) | SINS/LBL tight combination navigation method introducing radial velocity | |
| CN112525204A (en) | Spacecraft inertia and solar Doppler velocity combined navigation method | |
| CN114674313B (en) | Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20201110 |