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 PDF

Info

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
Application number
CN202010124573.0A
Other languages
Chinese (zh)
Other versions
CN111380516B (en
Inventor
武元新
欧阳威
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202010124573.0A priority Critical patent/CN111380516B/en
Publication of CN111380516A publication Critical patent/CN111380516A/en
Application granted granted Critical
Publication of CN111380516B publication Critical patent/CN111380516B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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

Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information
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:
Figure BDA0002394031570000021
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,
Figure BDA0002394031570000022
representing the attitude matrix of the navigational coordinate system relative to the body coordinate system,
Figure BDA0002394031570000023
to represent
Figure BDA0002394031570000024
The time derivative of (a) of (b),
Figure BDA0002394031570000025
representing the attitude matrix of the body coordinate system relative to the navigation coordinate system,
Figure BDA0002394031570000026
representing angular velocity vectors in the b-system of the gyro measurement,
Figure BDA0002394031570000027
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,
Figure BDA0002394031570000028
represents the rotational angular velocity vector of the earth under n system,
Figure BDA0002394031570000029
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,
Figure BDA00023940315700000210
representing zero bias of gyrogThe time derivative of (a) of (b),
Figure BDA00023940315700000211
representing accelerometer zero offset baThe time derivative of (a) of (b),
Figure BDA00023940315700000212
represents the time derivative of the odometer scale factor,
Figure BDA00023940315700000213
represents the time derivative of the inertial navigation with respect to the installation offset angle of the vehicle body in the longitudinal direction,
Figure BDA00023940315700000214
represents the time derivative of the mounting offset angle of inertial navigation relative to the vehicle body along the lateral direction,
Figure BDA00023940315700000215
indicating mounting lever arm l of inertial navigation relative to vehicle bodybThe time derivative of (a) of (b),
Figure BDA00023940315700000216
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,
Figure BDA00023940315700000217
representing the attitude matrix of the measurement coordinate system relative to the inertial navigation system coordinate system,
Figure BDA00023940315700000218
representing the angular velocity of the system relative to the earth system;
matrix RcThe calculation formula of (2) is as follows:
Figure BDA00023940315700000219
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:
Figure BDA0002394031570000031
the integrated navigation system observation equation based on the accumulated mileage measurement is expressed as:
Figure BDA0002394031570000032
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 value
Figure BDA0002394031570000033
Subtract true value x, i.e.:
Figure BDA0002394031570000034
delta represents the error of the corresponding state, and the attitude estimate
Figure BDA0002394031570000035
True and true posture
Figure BDA0002394031570000036
And attitude error phinThe relationship of (c) is defined as:
Figure BDA0002394031570000037
wherein, I represents an identity matrix with dimension 3;
the corresponding state error vector is expressed as:
Figure BDA00023940315700000310
φnTindicating an attitude error phinTransposition of, δ vnT,δpTIndicating velocity and position error deltavnThe transpose of δ p is,
Figure BDA0002394031570000038
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;
the state space model of the state error is represented as:
Figure BDA0002394031570000039
in the formula:
Figure BDA0002394031570000041
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 as
Figure BDA0002394031570000042
Wherein the content of the first and second substances,
Figure BDA0002394031570000043
representing gyro random walk noise ngThe transpose of (a) is performed,
Figure BDA0002394031570000044
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,
Figure BDA0002394031570000045
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:
Figure BDA0002394031570000046
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:
Figure BDA0002394031570000047
Figure BDA0002394031570000048
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;
Figure BDA0002394031570000049
Figure BDA00023940315700000410
symbol simplification is carried out, and the carrier speed in a measurement coordinate system is defined as follows:
Figure BDA0002394031570000051
then there are:
Figure BDA0002394031570000052
Figure BDA0002394031570000053
the partial derivatives with respect to the error state are calculated as:
Figure BDA0002394031570000054
Figure BDA0002394031570000055
Figure BDA0002394031570000056
Figure BDA0002394031570000057
Figure BDA0002394031570000058
Figure BDA0002394031570000059
the noise input matrix is represented as:
Figure BDA00023940315700000510
wherein I represents a unit matrix, and subscripts of I represent the order number of the unit matrix; the observation matrix is represented as:
Figure BDA00023940315700000511
wherein HnhcAn observation matrix corresponding to the non-integrity-constrained measurement;
the non-integrity-constrained measurement equation in the observation equation is expressed as:
Figure BDA0002394031570000061
matrix HnhcThe formula for calculating the medium element is as follows:
Figure BDA0002394031570000062
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:
define Kalman filter states as
Figure BDA0002394031570000063
The state space model is then:
Figure BDA0002394031570000064
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:
Figure BDA0002394031570000065
the observation equation is expressed as:
Figure BDA0002394031570000066
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 errors
Figure BDA0002394031570000067
Is an estimated value
Figure BDA0002394031570000068
Subtract true value
Figure BDA0002394031570000069
Namely, it is
Figure BDA00023940315700000610
Delta represents the error of the corresponding state, and the attitude estimate
Figure BDA00023940315700000611
True and true posture
Figure BDA00023940315700000612
And attitude error phinThe relationship of (c) is defined as:
Figure BDA00023940315700000613
the corresponding state error vector is expressed as:
Figure BDA00023940315700000614
the approximate linear state space model of the state error is represented as:
Figure BDA00023940315700000615
wherein: noise(s)
Figure BDA00023940315700000616
The corresponding matrix relation is as follows:
Figure BDA0002394031570000071
Figure BDA0002394031570000072
Figure BDA0002394031570000073
in the formula: matrix array
Figure BDA0002394031570000074
Respectively 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:
Figure BDA0002394031570000081
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,
Figure BDA0002394031570000082
representing the attitude matrix of the navigational coordinate system relative to the body coordinate system,
Figure BDA0002394031570000083
to represent
Figure BDA0002394031570000084
The time derivative of (a) of (b),
Figure BDA0002394031570000085
representing the attitude matrix of the body coordinate system relative to the navigation coordinate system,
Figure BDA0002394031570000086
representing angular velocity vectors in the b-system of the gyro measurement,
Figure BDA0002394031570000087
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,
Figure BDA0002394031570000088
represents the rotational angular velocity vector of the earth under n system,
Figure BDA0002394031570000089
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,
Figure BDA00023940315700000810
representing zero bias of gyrogThe time derivative of (a) of (b),
Figure BDA00023940315700000811
representing accelerometer zero offset baThe time derivative of (a) of (b),
Figure BDA00023940315700000812
represents the time derivative of the odometer scale factor,
Figure BDA00023940315700000813
represents the time derivative of the inertial navigation with respect to the installation offset angle of the vehicle body in the longitudinal direction,
Figure BDA00023940315700000814
represents the time derivative of the mounting offset angle of inertial navigation relative to the vehicle body along the lateral direction,
Figure BDA00023940315700000815
indicating mounting lever arm l of inertial navigation relative to vehicle bodybThe time derivative of (a) of (b),
Figure BDA00023940315700000816
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,
Figure BDA00023940315700000817
representing the attitude matrix of the measurement coordinate system relative to the inertial navigation system coordinate system,
Figure BDA00023940315700000818
representing the angular velocity of the system relative to the earth system; matrix RcThe calculation formula of (2) is as follows:
Figure BDA0002394031570000091
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:
Figure BDA0002394031570000092
the integrated navigation system observation equation based on the accumulated mileage measurement is expressed as:
Figure BDA0002394031570000093
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 value
Figure BDA0002394031570000094
Subtract true value x, i.e.:
Figure BDA0002394031570000095
delta represents the error of the corresponding state, and the attitude estimate
Figure BDA0002394031570000096
True and true posture
Figure BDA0002394031570000097
And attitude error phinThe relationship of (c) is defined as:
Figure BDA0002394031570000098
where I represents an identity matrix with dimension 3.
The corresponding state error vector is then expressed as:
Figure BDA0002394031570000099
φnTindicating an attitude error phinTransposition of, δ vnT,δpTIndicating velocity and position error deltavnThe transpose of δ p is,
Figure BDA00023940315700000910
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:
Figure BDA0002394031570000101
in the formula:
Figure BDA0002394031570000102
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 as
Figure BDA0002394031570000103
Wherein the content of the first and second substances,
Figure BDA0002394031570000104
representing gyro random walk noise ngThe transpose of (a) is performed,
Figure BDA0002394031570000105
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,
Figure BDA0002394031570000106
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:
Figure BDA0002394031570000107
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:
Figure BDA0002394031570000108
Figure BDA0002394031570000109
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;
Figure BDA00023940315700001010
Figure BDA0002394031570000111
to simplify the notation, the carrier speed in the measured coordinate system is defined as:
Figure BDA0002394031570000112
then there is a change in the number of,
Figure BDA0002394031570000113
Figure BDA0002394031570000114
the partial derivatives with respect to the error state are calculated as:
Figure BDA0002394031570000115
Figure BDA0002394031570000116
Figure BDA0002394031570000117
Figure BDA0002394031570000118
Figure BDA0002394031570000119
Figure BDA00023940315700001110
in addition, the noise input matrix is represented as:
Figure BDA00023940315700001111
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:
Figure BDA0002394031570000121
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:
Figure BDA0002394031570000122
further, matrix HnhcThe formula for calculating the medium element is as follows:
Figure BDA0002394031570000123
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:
step 2.1: defining the filter state as
Figure BDA0002394031570000124
The state space model is then:
Figure BDA0002394031570000125
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:
Figure BDA0002394031570000126
the observation equation:
Figure BDA0002394031570000127
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 errors
Figure BDA0002394031570000128
Is an estimated value
Figure BDA0002394031570000129
Subtract true value
Figure BDA00023940315700001210
Namely, it is
Figure BDA00023940315700001211
Delta represents the error of the corresponding state, and the attitude estimate
Figure BDA00023940315700001212
True and true posture
Figure BDA00023940315700001213
And attitude error phinThe relationship of (c) is defined as:
Figure BDA00023940315700001214
the corresponding state error vector is expressed as:
Figure BDA00023940315700001215
approximate linear state of state errorThe spatial model is represented as:
Figure BDA0002394031570000131
wherein: noise(s)
Figure BDA0002394031570000132
Matrix array
Figure BDA0002394031570000133
And
Figure BDA0002394031570000134
corresponding to the matrix F in step 2sAnd GsThe relationship is as follows:
Figure BDA0002394031570000135
Figure BDA0002394031570000136
in the formula: matrix array
Figure BDA0002394031570000137
Respectively 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:
Figure FDA0002394031560000011
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,
Figure FDA0002394031560000012
representing the attitude matrix of the navigational coordinate system relative to the body coordinate system,
Figure FDA0002394031560000013
to represent
Figure FDA0002394031560000014
The time derivative of (a) of (b),
Figure FDA0002394031560000015
representing the attitude matrix of the body coordinate system relative to the navigation coordinate system,
Figure FDA0002394031560000016
representing angular velocity vectors in the b-system of the gyro measurement,
Figure FDA0002394031560000017
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,
Figure FDA0002394031560000018
represents the rotational angular velocity vector of the earth under n system,
Figure FDA0002394031560000019
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,
Figure FDA00023940315600000110
representing zero bias of gyrogThe time derivative of (a) of (b),
Figure FDA00023940315600000111
representing accelerometer zero offset baThe time derivative of (a) of (b),
Figure FDA00023940315600000112
indicating odometer scale factorThe time derivative of the number of bits,
Figure FDA00023940315600000113
represents the time derivative of the inertial navigation with respect to the installation offset angle of the vehicle body in the longitudinal direction,
Figure FDA0002394031560000021
represents the time derivative of the mounting offset angle of inertial navigation relative to the vehicle body along the lateral direction,
Figure FDA0002394031560000022
indicating mounting lever arm l of inertial navigation relative to vehicle bodybThe time derivative of (a) of (b),
Figure FDA0002394031560000023
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,
Figure FDA0002394031560000024
representing the attitude matrix of the measurement coordinate system relative to the inertial navigation system coordinate system,
Figure FDA0002394031560000025
representing the angular velocity of the system relative to the earth system;
matrix RcThe calculation formula of (2) is as follows:
Figure FDA0002394031560000026
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:
Figure FDA0002394031560000027
the integrated navigation system observation equation based on the accumulated mileage measurement is expressed as:
Figure FDA0002394031560000028
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 value
Figure FDA0002394031560000031
Subtract true value x, i.e.:
Figure FDA0002394031560000032
delta represents the error of the corresponding state, and the attitude estimate
Figure FDA0002394031560000033
True and true posture
Figure FDA0002394031560000034
And attitude error phinThe relationship of (c) is defined as:
Figure FDA0002394031560000035
wherein, I represents an identity matrix with dimension 3;
the corresponding state error vector is expressed as:
Figure FDA0002394031560000036
φnTindicating an attitude error phinTransposition of, δ vnT,δpTIndicating velocity and position error deltavnThe transpose of δ p is,
Figure FDA0002394031560000037
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;
the state space model of the state error is represented as:
Figure FDA0002394031560000038
in the formula:
Figure FDA0002394031560000039
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 as
Figure FDA00023940315600000310
Wherein the content of the first and second substances,
Figure FDA00023940315600000311
representing gyro random walk noise ngThe transpose of (a) is performed,
Figure FDA00023940315600000312
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,
Figure FDA00023940315600000313
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:
Figure FDA00023940315600000314
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:
Figure FDA0002394031560000041
Figure FDA0002394031560000042
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;
Figure FDA0002394031560000043
Figure FDA0002394031560000044
symbol simplification is carried out, and the carrier speed in a measurement coordinate system is defined as follows:
Figure FDA0002394031560000045
then there are:
Figure FDA0002394031560000046
Figure FDA0002394031560000047
the partial derivatives with respect to the error state are calculated as:
Figure FDA0002394031560000048
Figure FDA0002394031560000049
Figure FDA00023940315600000410
Figure FDA00023940315600000411
Figure FDA00023940315600000412
Figure FDA00023940315600000413
the noise input matrix is represented as:
Figure FDA0002394031560000051
wherein I represents a unit matrix, and subscripts of I represent the order number of the unit matrix;
the observation matrix is represented as:
Figure FDA0002394031560000052
wherein HnhcAn observation matrix corresponding to the non-integrity-constrained measurement;
the non-integrity-constrained measurement equation in the observation equation is expressed as:
Figure FDA0002394031560000053
matrix HnhcThe formula for calculating the medium element is as follows:
Figure FDA0002394031560000054
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:
define Kalman filter states as
Figure FDA0002394031560000055
The state space model is then:
Figure FDA0002394031560000056
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:
Figure FDA0002394031560000061
the observation equation is expressed as:
Figure FDA0002394031560000062
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 errors
Figure FDA0002394031560000063
Is an estimated value
Figure FDA0002394031560000064
Subtract true value
Figure FDA0002394031560000065
Namely, it is
Figure FDA0002394031560000066
Delta represents the error of the corresponding state, and the attitude estimate
Figure FDA0002394031560000067
True and true posture
Figure FDA0002394031560000068
And attitude error phinThe relationship of (c) is defined as:
Figure FDA0002394031560000069
the corresponding state error vector is expressed as:
Figure FDA00023940315600000610
the approximate linear state space model of the state error is represented as:
Figure FDA00023940315600000611
wherein: noise(s)
Figure FDA00023940315600000612
The corresponding matrix relation is as follows:
Figure FDA00023940315600000613
Figure FDA00023940315600000614
Figure FDA00023940315600000615
in the formula: matrix array
Figure FDA00023940315600000616
Individual watchA system display matrix, a system input matrix, and an observation matrix.
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.
CN202010124573.0A 2020-02-27 2020-02-27 Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information Active CN111380516B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (9)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
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
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
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN102087110B (en) Miniature underwater moving vehicle autonomous attitude detecting device and method
CN102853837B (en) MIMU and GNSS information fusion method
CN111678514B (en) Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation
CN113340298B (en) Inertial navigation and dual-antenna GNSS external parameter calibration method
CN111121766A (en) Astronomical and inertial integrated navigation method based on starlight vector
CN105865452A (en) Mobile platform pose estimation method based on indirect Kalman filtering
CN117053782A (en) Combined navigation method for amphibious robot
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
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
CN113916226B (en) Minimum variance-based interference rejection filtering method for integrated navigation system
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
CN113916261B (en) Attitude error assessment method based on inertial navigation optimization alignment
CN115031727B (en) Doppler auxiliary strapdown inertial navigation system initial alignment method based on state transformation
Hong et al. Estimation of alignment errors in GPS/INS integration

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