CN111380516A - Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information - Google Patents
Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information Download PDFInfo
- Publication number
- CN111380516A CN111380516A CN202010124573.0A CN202010124573A CN111380516A CN 111380516 A CN111380516 A CN 111380516A CN 202010124573 A CN202010124573 A CN 202010124573A CN 111380516 A CN111380516 A CN 111380516A
- Authority
- CN
- China
- Prior art keywords
- odometer
- observation
- representing
- pulse
- 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.)
- Granted
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 58
- 238000000034 method Methods 0.000 title claims abstract description 42
- 230000003190 augmentative effect Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 76
- 239000013598 vector Substances 0.000 claims description 33
- 238000001914 filtration Methods 0.000 claims description 24
- 230000001186 cumulative effect Effects 0.000 claims description 11
- 238000009434 installation Methods 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 239000002245 particle Substances 0.000 claims description 6
- 238000005295 random walk Methods 0.000 claims description 6
- 239000004576 sand Substances 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003416 augmentation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
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
- G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
Abstract
The invention provides an inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information, which comprises the following steps: estimating a navigation system state, comprising: and (3) an accumulative pulse observation method step: the mileage model is augmented into the existing navigation system model, the accumulated pulse count of the odometer is used as the observation information of the navigation system, and the state of the navigation system is estimated according to the sequential filter; or, the pulse velocity observation method comprises the following steps: and modeling the pulse counting time sequence in a cell, designing a Kalman filter, estimating pulse speed information by using accumulated pulse counting as observation, then estimating the state of the navigation system by using the pulse speed information as observation information of the vehicle navigation system and using a sequential filter. The invention can directly use the accumulated travel distance as the measurement information, and effectively restricts the accumulated travel distance error; the invention obtains high-precision pulse speed measurement information and has higher vehicle speed measurement precision.
Description
Technical Field
The invention relates to the technical field of integrated navigation, in particular to an inertial navigation/odometer vehicle integrated navigation method and system based on odometer measurement information.
Background
Inertial navigation and odometers (pulse encoders) are often used in vehicle autonomous position determination systems. The inertial navigation system comprises a gyroscope and an accelerometer which respectively sense the angular speed and the acceleration of the carrier. The odometer detects pulses generated by the forward motion of the vehicle and provides range observation information for the integrated navigation system. In addition, kinematic constraints (non-integrity constraints) of the vehicle itself are also an important measurement information. The current combined inertial navigation/odometer navigation system mainly uses a differential mode to obtain a forward speed as observation information, but the differential operation can cause excessive speed measurement errors. Therefore, the accuracy of the integrated navigation system using the speed as the observation is not high. In order to improve the navigation performance of the system, the accuracy of the velocity measurement needs to be improved.
The current inertial navigation/odometer combined navigation system has a single scheme, and used measurement information mainly comprises speed and distance increment. The accumulated mileage as one of the information output by the odometer can provide a vehicle travel distance constraint. However, existing navigation system frameworks cannot directly use accumulated distances as measurement information. Therefore, the system model needs to be redesigned for the accumulated mileage information.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide an inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information.
The inertial navigation/odometer vehicle combined navigation method based on odometer measurement information provided by the invention comprises the following steps:
estimating a navigation system state, comprising:
and (3) an accumulative pulse observation method step: the mileage model is augmented into the existing navigation system model, the accumulated pulse count of the odometer is used as the observation information of the navigation system, and the state of the navigation system is estimated according to the sequential filter;
or, the pulse velocity observation method comprises the following steps: and modeling the pulse counting time sequence in a cell, designing a Kalman filter, estimating pulse speed information by using accumulated pulse counting as observation, then estimating the state of the navigation system by using the pulse speed information as observation information of the vehicle navigation system and using a sequential filter.
Preferably, the step of cumulative pulse observation comprises:
the navigation system is represented as:
wherein: the body coordinate system of the gyroscope/accelerometer is represented as a b system, the inertial coordinate system is represented as an i system, the navigation coordinate system is represented as an n system, m is represented as a carrier measurement coordinate system,representing the attitude matrix of the navigational coordinate system relative to the body coordinate system,to representThe time derivative of (a) of (b),representing the attitude matrix of the body coordinate system relative to the navigation coordinate system,representing angular velocity vectors in the b-system of the gyro measurement,v represents the angular velocity vector of n system relative to b system at n systemnRepresenting the speed of the carrier under navigation, fbRepresents the specific force acceleration measurement under b system,represents the rotational angular velocity vector of the earth under n system,denotes the angular velocity vector, g, of n-system relative to e-systemnExpressing the gravity vector under the navigation system, vector p ═ λ L h]TRepresenting the longitude, latitude and altitude of the body under the navigation system,representing zero bias of gyrogThe time derivative of (a) of (b),representing accelerometer zero offset baThe time derivative of (a) of (b),represents the time derivative of the odometer scale factor,represents the time derivative of the inertial navigation with respect to the installation offset angle of the vehicle body in the longitudinal direction,represents the time derivative of the mounting offset angle of inertial navigation relative to the vehicle body along the lateral direction,indicating mounting lever arm l of inertial navigation relative to vehicle bodybThe time derivative of (a) of (b),representing the time derivative of the mileage s, as a new augmented system state, e1Representing a three-dimensional unit vector with the first element 1, the superscript T representing the matrix transpose,representing the attitude matrix of the measurement coordinate system relative to the inertial navigation system coordinate system,representing the angular velocity of the system relative to the earth system;
matrix RcThe calculation formula of (2) is as follows:
wherein R isE,RNRepresentative groundThe curvature radius of the sphere reference ellipsoid, L is the local latitude, and h is the local height;
the operation a × indicates that an arbitrary three-dimensional vector a is given by [ a ═ a1a2a3]TPerforming a cross product operation, the formula being:
the integrated navigation system observation equation based on the accumulated mileage measurement is expressed as:
in the formula: s represents the cumulative pulse count measured by the odometer, corresponding to the total mileage traveled, e2、e3Three-dimensional unit vectors respectively representing the 2 nd and 3 rd elements as 1; y issRepresents the cumulative mileage measurement, i.e., the total number of pulses output by the odometer; y isnhcRepresenting a vehicle non-integrity constraint measurement, constraining the vehicle to have an instantaneous speed of 0 in both the longitudinal and lateral directions.
Preferably, the navigation system state comprises attitude, carrier position, velocity, gyro/accelerometer zero offset, mounting offset angle, odometer scale factor, lever arm and accumulated number of pulses; the pose and carrier position are obtained by initial alignment; initial values for velocity, mounting offset angle, odometer scale factor, lever arm, gyro/accelerometer zero offset, and number of accumulated pulses are all set to zero.
Preferably, the step of cumulative pulse observation comprises:
the sequential filtering includes: extended kalman filtering, or particle filtering;
the extended Kalman filtering comprises:
defining a state error δ xsIs an estimated valueSubtract true value x, i.e.:delta represents the error of the corresponding state, and the attitude estimateTrue and true postureAnd attitude error phinThe relationship of (c) is defined as:wherein, I represents an identity matrix with dimension 3;
the corresponding state error vector is expressed as:φnTindicating an attitude error phinTransposition of, δ vnT,δpTIndicating velocity and position error deltavnThe transpose of δ p is,representing zero offset error δ b of gyro and accelerometerg,δbaTransposition of, δ lbTIndicating lever arm error δ lbThe transpose of (1), wherein delta K represents an odometer scale factor error, delta psi represents a longitudinal installation offset angle error, delta theta represents a lateral installation offset angle error, and delta s represents an accumulated pulse number error;
in the formula:represents δ xsTime derivative of (1), matrix Fs、Gs、HsRespectively representing a system matrix, a system input matrix and an observation matrix, and representing process noise asWherein,representing gyro random walk noise ngThe transpose of (a) is performed,representing random walk noise n of an accelerometeraN isKRepresenting scale factor noise, nψRepresenting longitudinal installation offset angle noise, nθIndicating the offset angle noise of the side mounting,representing lever arm noise nlTransposing; delta ysRepresenting the form of error of the observation, nsRepresenting the noise of the linearized observation model;
matrix F in models,GsAnd HsThe concrete form of (A) is as follows:
wherein 0 represents a zero matrix, and subscripts of 0 represent row and column numbers of the zero matrix; matrix FsThe calculation formula of the other elements is as follows:
wherein, ω isieRepresenting magnitude of angular velocity of rotation of the earth, vE,vNRespectively representing east-oriented speed and north-oriented speed under a navigation system;
symbol simplification is carried out, and the carrier speed in a measurement coordinate system is defined as follows:
the noise input matrix is represented as:
wherein I represents a unit matrix, and subscripts of I represent the order number of the unit matrix; the observation matrix is represented as:wherein HnhcAn observation matrix corresponding to the non-integrity-constrained measurement;
matrix HnhcThe formula for calculating the medium element is as follows:
preferably, the pulse velocity observation step comprises: modeling a pulse counting time sequence, setting pulse acceleration to be constant, estimating pulse speed by adopting a Kalman filter, and observing accumulated pulse quantity as a filter, wherein the method comprises the following steps:
wherein w is model noise and ds/dt represents the derivative of s with respect to time t;
the observation model is as follows:
yk=sk+ek
wherein s iskTo correspond to tkCumulative number of pulses measured by time odometer, ekThe error is measured for accumulated pulses.
Preferably, the pulse velocity observation step comprises: taking the pulse velocity as the measurement information, the state relation between the navigation system state and the state space modelComprises the following steps:
preferably, the pulse velocity observation step comprises: the navigation system state is estimated by adopting sequential filtering, and the sequential filtering method comprises the following steps: extended kalman filtering, or particle filtering;
when the extended Kalman filter is adopted, the method comprises the following steps:
defining state errorsIs an estimated valueSubtract true valueNamely, it isDelta represents the error of the corresponding state, and the attitude estimateTrue and true postureAnd attitude error phinThe relationship of (c) is defined as:
The corresponding matrix relation is as follows:
in the formula: matrix arrayRespectively representing a system matrix, a system input matrix and an observation matrix.
The inertial navigation/odometer vehicle combined navigation system based on odometer measurement information provided by the invention comprises:
an accumulative pulse observation method module: the mileage model is augmented into the existing navigation system model, the accumulated pulse count of the odometer is used as the observation information of the navigation system, and the state of the navigation system is estimated according to the sequential filter;
pulse velocity observation module: and modeling the pulse counting time sequence in a cell, designing a Kalman filter, estimating pulse speed information by using accumulated pulse counting as observation, then estimating the state of the navigation system by using the pulse speed information as observation information of the vehicle navigation system and using a sequential filter.
Compared with the prior art, the invention has the following beneficial effects:
1. the novel combined navigation system structure provided by the invention has the advantages that the mileage kinematics model is added into the system model, so that the system can directly use the accumulated travel distance as measurement information, and the accumulated travel distance error is effectively restrained.
2. The invention provides a method for measuring the pulse velocity by using a sequential filter to take accumulated pulse counting as observation, obtains high-precision pulse velocity measurement information, and has higher velocity measurement precision compared with a direct difference mode.
3. The invention can be popularized and applied to navigation and positioning of various wheeled or tracked vehicles.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a flow chart of the method of the present invention.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.
Referring to fig. 1, a flow chart of the method of the present invention includes:
step 1: the mileage model is augmented into the existing system state model, the accumulated pulse count of the odometer is directly used as the observation information of the navigation system, and the system state is estimated by adopting a sequential filter;
step 2: taking the accumulated pulse as observation, and adopting a sequential filter to obtain high-precision pulse speed information;
and step 3: and combining the pulse velocity information with a pulse velocity model, and estimating the state of the navigation system by using a sequential filter.
Preferably, the state space model after the state augmentation in step 1 is as follows:
the navigation system is represented as:
wherein: the body coordinate system of the gyroscope/accelerometer is represented as a b system, the inertial coordinate system is represented as an i system, the navigation coordinate system is represented as an n system, m is represented as a carrier measurement coordinate system,representing the attitude matrix of the navigational coordinate system relative to the body coordinate system,to representThe time derivative of (a) of (b),representing the attitude matrix of the body coordinate system relative to the navigation coordinate system,representing angular velocity vectors in the b-system of the gyro measurement,v represents the angular velocity vector of n system relative to b system at n systemnRepresenting the speed of the carrier under navigation, fbRepresents the specific force acceleration measurement under b system,represents the rotational angular velocity vector of the earth under n system,denotes the angular velocity vector, g, of n-system relative to e-systemnExpressing the gravity vector under the navigation system, vector p ═ λ L h]TRepresenting the longitude, latitude and altitude of the body under the navigation system,representing zero bias of gyrogThe time derivative of (a) of (b),representing accelerometer zero offset baThe time derivative of (a) of (b),represents the time derivative of the odometer scale factor,represents the time derivative of the inertial navigation with respect to the installation offset angle of the vehicle body in the longitudinal direction,represents the time derivative of the mounting offset angle of inertial navigation relative to the vehicle body along the lateral direction,indicating mounting lever arm l of inertial navigation relative to vehicle bodybThe time derivative of (a) of (b),representing the time derivative of the mileage s, as a new augmented system state, e1Representing a three-dimensional unit vector with the first element 1, the superscript T representing the matrix transpose,representing the attitude matrix of the measurement coordinate system relative to the inertial navigation system coordinate system,representing the angular velocity of the system relative to the earth system; matrix RcThe calculation formula of (2) is as follows:
wherein R isE,RNThe curvature radius of the earth reference ellipsoid is represented, L is the local latitude, and h is the local height;
the operation a × indicates that an arbitrary three-dimensional vector a is given by [ a ═ a1a2a3]TPerforming a cross product operation, the formula being:
the integrated navigation system observation equation based on the accumulated mileage measurement is expressed as:
in the formula: s represents the cumulative number of pulses measured by the odometer, corresponding to the total mileage traveled, e2、e3Three-dimensional unit vectors respectively representing the 2 nd and 3 rd elements as 1; y issRepresents the cumulative mileage measurement, i.e., the total number of pulses output by the odometer; y isnhcRepresenting a vehicle non-integrity constraint measurement, constraining the vehicle to have an instantaneous speed of 0 in both the longitudinal and lateral directions.
Preferably, the states of the state space model in step 1 include attitude, carrier position, velocity, gyro and accelerometer zero offset, mounting offset angle, odometer scale factor, lever arm and accumulated number of pulses; initial values of the attitude and the position are obtained through initial alignment; the initial values of speed, installation offset angle, odometer scale factor, lever arm, gyro and accelerometer zero offset, and the number of accumulated pulses are all set to zero.
Preferably, the step 1 comprises: estimating the state of the state space model by adopting a sequential filtering method, wherein the sequential filtering method comprises the following steps: extended kalman filtering, or particle filtering; specifically, when the extended kalman filter is adopted, the method comprises the following steps:
step 1.1: defining a state error δ xsIs an estimated valueSubtract true value x, i.e.:delta represents the error of the corresponding state, and the attitude estimateTrue and true postureAnd attitude error phinThe relationship of (c) is defined as:where I represents an identity matrix with dimension 3.
The corresponding state error vector is then expressed as:
φnTindicating an attitude error phinTransposition of, δ vnT,δpTIndicating velocity and position error deltavnThe transpose of δ p is,representing zero offset error δ b of gyro and accelerometerg,δbaTransposition of, δ lbTIndicating lever arm error δ lbδ K denotes an odometer scale factor error, δ ψ denotes a longitudinal mounting offset angle error, δ θ denotes a lateral mounting offset angle error, δ s denotes an accumulated pulse number error.
The state space model of the state error is represented as:
in the formula:represents δ xsTime derivative of (1), matrix Fs、Gs、HsRespectively representing a system matrix, a system input matrix and an observation matrix. Process noise is represented asWherein,representing gyro random walk noise ngThe transpose of (a) is performed,representing random walk noise n of an accelerometeraN isKRepresenting scale factor noise, nψRepresenting longitudinal installation offset angle noise, nθIndicating the offset angle noise of the side mounting,representing lever arm noise nlTransposing; delta ysRepresenting the form of error of the observation, nsRepresenting the noise of the linearized observation model.
Matrix F in models,GsAnd HsThe concrete form of (A) is as follows:
wherein 0 represents a zero matrix, and subscripts of 0 represent row and column numbers of the zero matrix; matrix FsThe calculation formula of the other elements is as follows:
wherein, ω isieRepresenting magnitude of angular velocity of rotation of the earth, vE,vNRespectively representing east-oriented speed and north-oriented speed under a navigation system;
to simplify the notation, the carrier speed in the measured coordinate system is defined as:
in addition, the noise input matrix is represented as:
wherein, I represents a unit matrix, and the subscript of I represents the order number of the unit matrix.
Finally, the observation matrix is represented as:wherein HnhcAn observation matrix corresponding to a non-integrity-constrained measurement. The non-integrity-constrained measurement equation in the observation equation can be expressed as:
further, matrix HnhcThe formula for calculating the medium element is as follows:
preferably, the step 2 includes: and a sequential filter is adopted to obtain high-precision pulse velocity. The Kalman filter design comprises: assuming that the pulse acceleration is constant, observing the accumulated pulse number as a filter, comprising the steps of:
wherein w is model noise, ds/dt represents the derivative of s with respect to time t, and so on;
the observation model is as follows: y isk=sk+ek
Wherein s iskTo correspond to tkCumulative number of pulses measured by time odometer, ekThe error is measured for accumulated pulses.
Preferably, the step 3 comprises: using pulse velocity as measurement information, system status and system in step 1The state relationship is as follows:
preferably, the step 3 comprises: estimating the state of the state space model by adopting a sequential filtering method, wherein the sequential filtering method comprises the following steps: extended kalman filtering, or particle filtering; specifically, when the extended kalman filter is adopted, the method comprises the following steps:
step 3.1: defining state errorsIs an estimated valueSubtract true valueNamely, it isDelta represents the error of the corresponding state, and the attitude estimateTrue and true postureAnd attitude error phinThe relationship of (c) is defined as:
wherein: noise(s)Matrix arrayAndcorresponding to the matrix F in step 2sAnd GsThe relationship is as follows:
in the formula: matrix arrayRespectively representing a system matrix, a system input matrix and an observation matrix.
Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.
Claims (8)
1. An inertial navigation/odometer vehicle combined navigation method based on odometer measurement information is characterized by comprising the following steps:
estimating a navigation system state, comprising:
and (3) an accumulative pulse observation method step: the mileage model is augmented into the existing navigation system model, the accumulated pulse count of the odometer is used as the observation information of the navigation system, and the state of the navigation system is estimated according to the sequential filter;
or, the pulse velocity observation method comprises the following steps: and modeling the pulse counting time sequence in a cell, designing a Kalman filter, estimating pulse speed information by using accumulated pulse counting as observation, then estimating the state of the navigation system by using the pulse speed information as observation information of the vehicle navigation system and using a sequential filter.
2. The integrated navigation method for inertial/odometer vehicles based on odometer measurement information according to claim 1, characterized in that said step of cumulative pulse observation comprises:
the navigation system is represented as:
wherein: the body coordinate system of the gyroscope/accelerometer is represented as a b system, the inertial coordinate system is represented as an i system, the navigation coordinate system is represented as an n system, m is represented as a carrier measurement coordinate system,representing the attitude matrix of the navigational coordinate system relative to the body coordinate system,to representThe time derivative of (a) of (b),representing the attitude matrix of the body coordinate system relative to the navigation coordinate system,representing angular velocity vectors in the b-system of the gyro measurement,v represents the angular velocity vector of n system relative to b system at n systemnRepresenting the speed of the carrier under navigation, fbRepresents the specific force acceleration measurement under b system,represents the rotational angular velocity vector of the earth under n system,denotes the angular velocity vector, g, of n-system relative to e-systemnExpressing the gravity vector under the navigation system, vector p ═ λ L h]TRepresenting the longitude, latitude and altitude of the body under the navigation system,representing zero bias of gyrogThe time derivative of (a) of (b),representing accelerometer zero offset baThe time derivative of (a) of (b),indicating odometer scale factorThe time derivative of the number of bits,represents the time derivative of the inertial navigation with respect to the installation offset angle of the vehicle body in the longitudinal direction,represents the time derivative of the mounting offset angle of inertial navigation relative to the vehicle body along the lateral direction,indicating mounting lever arm l of inertial navigation relative to vehicle bodybThe time derivative of (a) of (b),representing the time derivative of the mileage s, as a new augmented system state, e1Representing a three-dimensional unit vector with the first element 1, the superscript T representing the matrix transpose,representing the attitude matrix of the measurement coordinate system relative to the inertial navigation system coordinate system,representing the angular velocity of the system relative to the earth system;
matrix RcThe calculation formula of (2) is as follows:
wherein R isE,RNThe curvature radius of the earth reference ellipsoid is represented, L is the local latitude, and h is the local height;
the operation a × indicates that an arbitrary three-dimensional vector a is given by [ a ═ a1a2a3]TPerforming a cross product operation, the formula being:
the integrated navigation system observation equation based on the accumulated mileage measurement is expressed as:
in the formula: s represents the cumulative pulse count measured by the odometer, corresponding to the total mileage traveled, e2、e3Three-dimensional unit vectors respectively representing the 2 nd and 3 rd elements as 1; y issRepresents the cumulative mileage measurement, i.e., the total number of pulses output by the odometer; y isnhcRepresenting a vehicle non-integrity constraint measurement, constraining the vehicle to have an instantaneous speed of 0 in both the longitudinal and lateral directions.
3. The odometer measurement information-based inertial navigation/odometer vehicle combined navigation method of claim 1, wherein the navigation system states include attitude, carrier position, velocity, gyro/accelerometer zero offset, mounting offset angle, odometer scale factor, lever arm, and cumulative number of pulses; the pose and carrier position are obtained by initial alignment; initial values for velocity, mounting offset angle, odometer scale factor, lever arm, gyro/accelerometer zero offset, and number of accumulated pulses are all set to zero.
4. The integrated navigation method for inertial/odometer vehicles based on odometer measurement information according to claim 2, characterized in that said step of cumulative pulse observation comprises:
the sequential filtering includes: extended kalman filtering, or particle filtering;
the extended Kalman filtering comprises:
defining a state error δ xsIs an estimated valueSubtract true value x, i.e.:delta represents the error of the corresponding state, and the attitude estimateTrue and true postureAnd attitude error phinThe relationship of (c) is defined as:wherein, I represents an identity matrix with dimension 3;
the corresponding state error vector is expressed as:φnTindicating an attitude error phinTransposition of, δ vnT,δpTIndicating velocity and position error deltavnThe transpose of δ p is,representing zero offset error δ b of gyro and accelerometerg,δbaTransposition of, δ lbTIndicating lever arm error δ lbThe transpose of (1), wherein delta K represents an odometer scale factor error, delta psi represents a longitudinal installation offset angle error, delta theta represents a lateral installation offset angle error, and delta s represents an accumulated pulse number error;
in the formula:represents δ xsTime derivative of (1), matrix Fs、Gs、HsRespectively representing a system matrix, a system input matrix and an observation matrix, and representing process noise asWherein,representing gyro random walk noise ngThe transpose of (a) is performed,representing random walk noise n of an accelerometeraN isKRepresenting scale factor noise, nψRepresenting longitudinal installation offset angle noise, nθIndicating the offset angle noise of the side mounting,representing lever arm noise nlTransposing; delta ysRepresenting the form of error of the observation, nsRepresenting the noise of the linearized observation model;
matrix F in models,GsAnd HsThe concrete form of (A) is as follows:
wherein 0 represents a zero matrix, and subscripts of 0 represent row and column numbers of the zero matrix; matrix FsThe calculation formula of the other elements is as follows:
wherein, ω isieRepresenting magnitude of angular velocity of rotation of the earth, vE,vNRespectively representing east-oriented speed and north-oriented speed under a navigation system;
symbol simplification is carried out, and the carrier speed in a measurement coordinate system is defined as follows:
the noise input matrix is represented as:
wherein I represents a unit matrix, and subscripts of I represent the order number of the unit matrix;
wherein HnhcAn observation matrix corresponding to the non-integrity-constrained measurement;
matrix HnhcThe formula for calculating the medium element is as follows:
5. the integrated navigation method for inertial/odometer vehicles based on odometer measurement information according to claim 4, characterized in that said pulse velocity observation step comprises: modeling a pulse counting time sequence, setting pulse acceleration to be constant, estimating pulse speed by adopting a Kalman filter, and observing accumulated pulse quantity as a filter, wherein the method comprises the following steps:
wherein w is model noise and ds/dt represents the derivative of s with respect to time t;
the observation model is as follows:
yk=sk+ek
wherein s iskTo correspond to tkCumulative number of pulses measured by time odometer, ekThe error is measured for accumulated pulses.
6. The integrated navigation method for inertial/odometer vehicles based on odometer measurement information according to claim 5, characterized in that said pulse velocity observation step comprises: taking the pulse velocity as measurement information, the state relation between the navigation system state and the state space model is as follows:
7. the integrated navigation method for inertial/odometer vehicles based on odometer measurement information according to claim 6, characterized in that said pulse velocity observation step comprises: the navigation system state is estimated by adopting sequential filtering, and the sequential filtering method comprises the following steps: extended kalman filtering, or particle filtering;
when the extended Kalman filter is adopted, the method comprises the following steps:
defining state errorsIs an estimated valueSubtract true valueNamely, it isDelta represents the error of the corresponding state, and the attitude estimateTrue and true postureAnd attitude error phinThe relationship of (c) is defined as:
The corresponding matrix relation is as follows:
8. An inertial navigation/odometer vehicle integrated navigation system based on odometer measurement information, comprising:
an accumulative pulse observation method module: the mileage model is augmented into the existing navigation system model, the accumulated pulse count of the odometer is used as the observation information of the navigation system, and the state of the navigation system is estimated according to the sequential filter;
pulse velocity observation module: and modeling the pulse counting time sequence in a cell, designing a Kalman filter, estimating pulse speed information by using accumulated pulse counting as observation, then estimating the state of the navigation system by using the pulse speed information as observation information of the vehicle navigation system and using a sequential filter.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010124573.0A CN111380516B (en) | 2020-02-27 | 2020-02-27 | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010124573.0A CN111380516B (en) | 2020-02-27 | 2020-02-27 | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111380516A true CN111380516A (en) | 2020-07-07 |
CN111380516B CN111380516B (en) | 2022-04-08 |
Family
ID=71217145
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010124573.0A Active CN111380516B (en) | 2020-02-27 | 2020-02-27 | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111380516B (en) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112013843A (en) * | 2020-09-18 | 2020-12-01 | 中国人民解放军32202部队 | Mileage factor correction method for fusing inertial navigation and vehicle central inflation and deflation system |
CN112066983A (en) * | 2020-09-08 | 2020-12-11 | 中国人民解放军国防科技大学 | Inertial/odometer combined navigation filtering method, electronic equipment and storage medium |
CN112902982A (en) * | 2021-01-18 | 2021-06-04 | 惠州市德赛西威汽车电子股份有限公司 | Method and system for accurately calculating vehicle mileage and automobile |
CN114184209A (en) * | 2021-10-29 | 2022-03-15 | 北京自动化控制设备研究所 | Inertial error suppression method for low-speed detection platform system |
CN114370841A (en) * | 2022-02-22 | 2022-04-19 | 华东送变电工程有限公司 | Anti-interference method for magnetic pulse odometer |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110238308A1 (en) * | 2010-03-26 | 2011-09-29 | Isaac Thomas Miller | Pedal navigation using leo signals and body-mounted sensors |
CN105824039A (en) * | 2016-03-17 | 2016-08-03 | 孙红星 | Odometer-based GNSS/INS (Global Navigation Satellite System/Inertial Navigation System) vehicle-mounted combined positioning and orientation algorithm for overcoming satellite locking loss |
CN107588769A (en) * | 2017-10-17 | 2018-01-16 | 北京航天发射技术研究所 | A kind of vehicle-mounted inertial navigation, odometer and altimeter Combinated navigation method |
CN107589441A (en) * | 2017-09-07 | 2018-01-16 | 成都理工大学 | Pulse pile-up modification method based on Kalman filter passage |
CN108180923A (en) * | 2017-12-08 | 2018-06-19 | 北京理工大学 | A kind of inertial navigation localization method based on human body odometer |
CN108196289A (en) * | 2017-12-25 | 2018-06-22 | 北京交通大学 | A kind of train combined positioning method under satellite-signal confined condition |
CN109343095A (en) * | 2018-11-15 | 2019-02-15 | 众泰新能源汽车有限公司 | A kind of vehicle mounted guidance vehicle combination positioning device and combinations thereof localization method |
CN110411462A (en) * | 2019-07-22 | 2019-11-05 | 武汉大学 | A kind of GNSS/ inertia/lane line constraint/odometer multi-source fusion method |
CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
-
2020
- 2020-02-27 CN CN202010124573.0A patent/CN111380516B/en active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20110238308A1 (en) * | 2010-03-26 | 2011-09-29 | Isaac Thomas Miller | Pedal navigation using leo signals and body-mounted sensors |
CN105824039A (en) * | 2016-03-17 | 2016-08-03 | 孙红星 | Odometer-based GNSS/INS (Global Navigation Satellite System/Inertial Navigation System) vehicle-mounted combined positioning and orientation algorithm for overcoming satellite locking loss |
CN107589441A (en) * | 2017-09-07 | 2018-01-16 | 成都理工大学 | Pulse pile-up modification method based on Kalman filter passage |
CN107588769A (en) * | 2017-10-17 | 2018-01-16 | 北京航天发射技术研究所 | A kind of vehicle-mounted inertial navigation, odometer and altimeter Combinated navigation method |
CN108180923A (en) * | 2017-12-08 | 2018-06-19 | 北京理工大学 | A kind of inertial navigation localization method based on human body odometer |
CN108196289A (en) * | 2017-12-25 | 2018-06-22 | 北京交通大学 | A kind of train combined positioning method under satellite-signal confined condition |
CN109343095A (en) * | 2018-11-15 | 2019-02-15 | 众泰新能源汽车有限公司 | A kind of vehicle mounted guidance vehicle combination positioning device and combinations thereof localization method |
CN110411462A (en) * | 2019-07-22 | 2019-11-05 | 武汉大学 | A kind of GNSS/ inertia/lane line constraint/odometer multi-source fusion method |
CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
Non-Patent Citations (2)
Title |
---|
NING BO,等: "A Hierarchical Optimization Strategy of Trajectory Planning for Multi-UAVs", 《2017 9TH INTERNATIONAL CONFERENCE ON INTELLIGENT HUMAN-MACHINE SYSTEMS AND CYBERNETICS》 * |
周洪霞,等: "基于姿态率控制的无人机目标跟踪算法", 《信息科学与控制工程》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112066983A (en) * | 2020-09-08 | 2020-12-11 | 中国人民解放军国防科技大学 | Inertial/odometer combined navigation filtering method, electronic equipment and storage medium |
CN112066983B (en) * | 2020-09-08 | 2022-09-23 | 中国人民解放军国防科技大学 | Inertial/odometer combined navigation filtering method, electronic equipment and storage medium |
CN112013843A (en) * | 2020-09-18 | 2020-12-01 | 中国人民解放军32202部队 | Mileage factor correction method for fusing inertial navigation and vehicle central inflation and deflation system |
CN112013843B (en) * | 2020-09-18 | 2023-11-17 | 中国人民解放军32202部队 | Mileage factor correction method integrating inertial navigation and vehicle central inflation and deflation system |
CN112902982A (en) * | 2021-01-18 | 2021-06-04 | 惠州市德赛西威汽车电子股份有限公司 | Method and system for accurately calculating vehicle mileage and automobile |
CN112902982B (en) * | 2021-01-18 | 2023-11-17 | 惠州市德赛西威汽车电子股份有限公司 | Vehicle driving mileage accurate calculation method, system and automobile |
CN114184209A (en) * | 2021-10-29 | 2022-03-15 | 北京自动化控制设备研究所 | Inertial error suppression method for low-speed detection platform system |
CN114184209B (en) * | 2021-10-29 | 2023-10-13 | 北京自动化控制设备研究所 | Inertial error suppression method for low-speed detection platform system |
CN114370841A (en) * | 2022-02-22 | 2022-04-19 | 华东送变电工程有限公司 | Anti-interference method for magnetic pulse odometer |
CN114370841B (en) * | 2022-02-22 | 2024-05-14 | 华东送变电工程有限公司 | Anti-interference method for magnetic pulse odometer |
Also Published As
Publication number | Publication date |
---|---|
CN111380516B (en) | 2022-04-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111380516B (en) | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information | |
CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
CN103917850B (en) | A kind of motion alignment methods of inertial navigation system | |
CN100516775C (en) | Method for determining initial status of strapdown inertial navigation system | |
CN105300379B (en) | A kind of Kalman filtering Attitude estimation method and system based on acceleration | |
Parsa et al. | Design and implementation of a mechatronic, all-accelerometer inertial measurement unit | |
CN104061899B (en) | A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation | |
CN103090867B (en) | Error restraining method for fiber-optic gyroscope strapdown inertial navigation system rotating relative to geocentric inertial system | |
CN102519470B (en) | Multi-level embedded integrated navigation system and navigation method | |
CN103630146B (en) | The laser gyro IMU scaling method that a kind of discrete parsing is combined with Kalman filter | |
CN106507913B (en) | Combined positioning method for pipeline mapping | |
CN102087110B (en) | Miniature underwater moving vehicle autonomous attitude detecting device and method | |
CN111678514B (en) | Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation | |
CN104977002A (en) | SINS/double OD-based inertial integrated navigation system and method | |
CN113340298B (en) | Inertial navigation and dual-antenna GNSS external parameter calibration method | |
CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
CN106153073A (en) | A kind of nonlinear initial alignment method of full attitude SINS | |
CN105865452A (en) | Mobile platform pose estimation method based on indirect Kalman filtering | |
CN117053782A (en) | Combined navigation method for amphibious robot | |
CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
Bai et al. | Improved preintegration method for GNSS/IMU/in-vehicle sensors navigation using graph optimization | |
CN115200578A (en) | Polynomial optimization-based inertial-based navigation information fusion method and system | |
CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
CN102830415B (en) | Quick integrated navigation method based on Carlson filtering algorithm for reducing dimensionality | |
CN104297525A (en) | Accelerometer calibration method for inertia measurement system on basis of rocket sled test |
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 |