CN112284415A - Odometer scale error calibration method, system and computer storage medium - Google Patents

Odometer scale error calibration method, system and computer storage medium Download PDF

Info

Publication number
CN112284415A
CN112284415A CN202011120011.5A CN202011120011A CN112284415A CN 112284415 A CN112284415 A CN 112284415A CN 202011120011 A CN202011120011 A CN 202011120011A CN 112284415 A CN112284415 A CN 112284415A
Authority
CN
China
Prior art keywords
error
coordinate system
navigation
time
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
CN202011120011.5A
Other languages
Chinese (zh)
Other versions
CN112284415B (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.)
Zhuzhou Phase Lock Photoelectric Technology Co ltd
Original Assignee
Zhuzhou Phase Lock Photoelectric Technology Co ltd
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 Zhuzhou Phase Lock Photoelectric Technology Co ltd filed Critical Zhuzhou Phase Lock Photoelectric Technology Co ltd
Priority to CN202011120011.5A priority Critical patent/CN112284415B/en
Publication of CN112284415A publication Critical patent/CN112284415A/en
Application granted granted Critical
Publication of CN112284415B publication Critical patent/CN112284415B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention relates to the technical field of metering, and discloses a method and a system for calibrating a scale error of a speedometer and a computer storage medium, which are used for realizing high-precision navigation. The method comprises the following steps: obtaining a reference track through inertial navigation and RTK combination; calculating a horizontal track of the vehicle through an odometer; and comparing the reference track with the horizontal track to estimate the scale error of the odometer.

Description

Odometer scale error calibration method, system and computer storage medium
Technical Field
The invention relates to the technical field of metering, in particular to a method and a system for calibrating scale errors of a speedometer and a computer storage medium.
Background
On autonomous vehicles, a combination of inertial navigation and odometer is often used to provide higher navigation accuracy, avoiding the increase in pure inertial navigation error over time. The odometer can realize complete autonomous navigation by outputting position increment information and utilizing a dead reckoning algorithm, but the odometer is influenced by factors such as vehicle tire abrasion, tire air pressure, vehicle load and the like, so that scale errors are changed, the precision of combined navigation is influenced, and the scale errors of the odometer need to be calibrated.
The traditional calibration method mainly comprises zero-speed correction, track similarity correction and GPS auxiliary calibration. The zero-speed correction is to use inertial navigation as a position reference and to calibrate the odometer scale factor by using inertial navigation information subjected to zero-speed correction. However, the method needs periodic parking to realize calibration, is inconvenient to use and cannot meet the requirements of practical application; the track similarity calibration method is to compare the tracks of the odometer by using a high-precision reference track so as to realize calibration of scale factor errors, and the method has good precision but has the difficulty that the high-precision reference track is obtained; the GPS-assisted calibration uses the position information provided by GPS with high accuracy as a reference, but is limited by the independency, and is not effective in the places where the GPS signal is weak or absent.
Disclosure of Invention
The invention mainly aims to disclose a method and a system for calibrating the scale error of a speedometer and a computer storage medium so as to realize high-precision navigation.
In order to achieve the purpose, the invention discloses a method for calibrating the scale error of a speedometer, which comprises the following steps:
obtaining a reference track by combining inertial navigation and RTK (Real-time kinematic, carrier-phase differential technology);
calculating a horizontal track of the vehicle through an odometer;
and comparing the reference track with the horizontal track to estimate the scale error of the odometer.
Preferably, the solving process of the reference trajectory comprises:
defining the transverse rolling axis of the inertial navigation carrier as ybAxis pointing directly in front of inertial navigation and pitch axis xbAxis pointing to the right of inertial navigation and course axis zbAxis, pointing in the direction of gravity, x, towards the skyb、yb、zbThree axes conform to the right hand coordinate system, xb、yb、zbIs marked as b series;
defining a navigation coordinate system xnThe axis pointing east, ynThe axis pointing north, znThe axis points to the sky, xn、yn、znIs marked as n series;
the measuring coordinate system of the mileage gauge isA right-front-upper right-hand rectangular coordinate system fixedly connected with the vehicle body and recorded as an m system, oymThe shaft is arranged in a plane contacted with the vehicle-carrying wheels and points to the right front of the vehicle body; ozmThe axis is positive perpendicular to the ground plane; oxmThe axis points to the right;
selecting system state parameters, and recording the state quantity as:
Figure BDA0002731658840000021
delta r is inertial navigation position error, delta v is inertial navigation speed error, phi is inertial navigation attitude angle error,
Figure BDA0002731658840000022
is zero offset, δ f, of the gyrobIs the accelerometer zero offset;
establishing an inertial navigation attitude error equation as follows:
Figure BDA0002731658840000023
wherein,
Figure BDA0002731658840000024
is the derivative of phi and is,
Figure BDA0002731658840000025
in order to navigate the gyro measurement error under the coordinate system,
Figure BDA0002731658840000026
the error is calculated for the navigation coordinate system,
Figure BDA0002731658840000027
representing a rotation of the navigation coordinate system relative to the inertial navigation coordinate system;
Figure BDA0002731658840000028
the rotation of the navigation coordinate system due to the rotation of the earth,
Figure BDA0002731658840000029
moving a navigation coordinate system rotation caused by earth surface curvature near the earth surface for the system;
the velocity error equation is:
Figure BDA00027316588400000210
wherein v isn、δvnRespectively the speed and the speed error in the navigation coordinate system,
Figure BDA00027316588400000211
is δ vnThe derivative of (a) of (b),
Figure BDA00027316588400000212
to navigate the accelerometer measurements in the coordinate system,
Figure BDA00027316588400000213
in order to navigate the accelerometer measurement error in the coordinate system,
Figure BDA00027316588400000214
δgnrespectively calculating errors of the spin angular velocity of the earth under the navigation coordinate system, rotation calculation errors and gravity errors;
the inertial navigation position error equation is as follows:
Figure BDA00027316588400000215
l, λ and h denote latitude, longitude and altitude, respectively, δ L, δ λ and δ h denote latitude error, longitude error and altitude error, respectively,
Figure BDA00027316588400000216
and
Figure BDA00027316588400000217
derivatives of δ L, δ λ and δ h, RMIs the radius of the meridian principal curvature; rNThe radius of main curvature of the mortise and unitary ring;
recording inertial navigation velocity component vn＝[vE vN vU]T；
Velocity error component δ vn＝[δvEδvNδvU]T；
Wherein subscripts E, N, U represent the east, north, and sky directions, respectively;
under a navigation coordinate system, the state equation of inertial navigation and RTK combined navigation is expressed as follows:
Figure BDA0002731658840000031
x (t) for the selected combined navigational coordinate system state vector,
Figure BDA0002731658840000032
is the derivative of X (t), F (t) is the system state transition matrix, W (t) is the system noise vector, G (t) is the system noise driving matrix;
Figure BDA0002731658840000033
wherein,
Figure BDA0002731658840000034
the measured value of the lower accelerometer is denoted as b,
Figure BDA0002731658840000035
representing an attitude transformation matrix from n to b, and (the) x represents an antisymmetric matrix;
Figure BDA0002731658840000036
Figure BDA0002731658840000037
Figure BDA0002731658840000038
Figure BDA0002731658840000041
Figure BDA0002731658840000042
Figure BDA0002731658840000043
Figure BDA0002731658840000044
Figure BDA0002731658840000045
wherein, WaAnd WgRespectively representing the noise of the gyroscope and the accelerometer, R is the radius of the earth when viewed as a sphere, wieIs the earth rotation rate;
the measurement equation is obtained by respectively subtracting the position and the velocity measured by RTK and the position and the velocity measured by inertial navigation, and is as follows:
Figure BDA0002731658840000046
in the above formula, Z (t) is a measurement matrix at time t, B represents longitude, L represents latitude, H represents altitude, VERepresenting east velocity, VNRepresenting north velocity, VURepresenting the antenna speed, and subscripts RTK and INS represent RTK and inertial navigation respectively; v (t) a noise sequence representing the position and velocity of the RTK;
and performing optimal estimation by adopting Kalman filtering, wherein the calculation steps are as follows:
one-step prediction equation: xk,k―1＝Φk,k―1Xk―1(ii) a Wherein phik,k―1Is the system transfer matrix from time k-1 to time k, Xk―1Is the system state vector at time k-1;
the state estimation equation is: xk＝Xk,k―1+Kk(Zk―Zk,k―1) (ii) a Wherein, XkIs the system state vector at time K, KkIs the gain matrix at time k, ZkIs a measure of the time k, Zk,k―1Measuring the quantity from the time k-1 to the time k;
the method for obtaining the gain matrix comprises the following steps:
Figure BDA0002731658840000051
wherein, Pk,k―1Is the mean square error matrix from time k-1 to time k, HkIs a measurement matrix at time k, RkMeasuring a noise variance matrix for the system k moment;
one-step prediction of mean square error:
Figure BDA0002731658840000052
wherein, gamma isk―1Is the noise driving matrix at time k-1, Γk,k―1Is the noise drive matrix from time k-1 to time k, Qk-1A noise variance matrix at the k-1 moment of the system;
estimating the mean square error: pk＝(I―KkHk)Pk,k―1(ii) a Wherein p iskA mean square error matrix at time k;
the position trajectory calculated by combining the above-described equations is referred to as S1 as a reference trajectory.
Preferably, the solving of the horizontal track of the vehicle carrier comprises:
the velocity output of the odometer is expressed on the odometer coordinate system as:
Figure BDA0002731658840000053
wherein v isDThe forward speed measured by the mileage meter, the right-direction speed and the sky-direction speed are both zero, the speed is regarded as a speed constraint condition when the vehicle runs normally, and the superscripts m, n and b respectively represent corresponding coordinate systems;
the output of the speed of the odometer under the navigation coordinate system is as follows:
Figure BDA0002731658840000054
considering that there is a small amount of installation deviation angle from m-system to b-system, the deviation angle vector alpha is recorded as [ alpha ]θ αγ αψ]TAnd scale coefficient error δ KDThen, the actual output of the odometer on the navigation coordinate system is:
Figure BDA0002731658840000055
wherein phi isDSubscripts psi, gamma and theta represent pitch angle error, roll angle error and course angle error from m system to b system respectively for attitude misalignment angle of dead reckoning, and phiD＝[φDE φDN φDU]T(ii) a Neglecting the effect of horizontal attitude error, i.e. making an approximation of phiDE≈φDNAnd 0, obtaining:
Figure BDA0002731658840000061
αθ、φDU+αψand δ KDBe the constant small quantity, and carry the car and go in the little within range of geographical position change, the rotation change of whole navigation in-process navigation coordinate system is not big promptly, handles as the plane, and the integration is got simultaneously on the two sides of formula:
Figure BDA0002731658840000062
wherein,
Figure BDA0002731658840000063
respectively, in a time period [0, T]The real displacement vector of the inner carrier vehicle, the calculated displacement vector and the driving mileage; u. ofU＝[0 0 1]TIs a vector of units in the direction of the sky;
the formula is decomposed into a horizontal part and a vertical part to obtain:
Figure BDA0002731658840000064
Figure BDA0002731658840000065
wherein,
Figure BDA0002731658840000066
Figure BDA0002731658840000067
subscript H represents a projection on a horizontal plane;
true displacement
Figure BDA0002731658840000068
Around the zenith axis uUAngle of rotation phiDU+αψThen expand by 1+ delta KDMultiplying to obtain the calculated displacement
Figure BDA0002731658840000069
Thereby calculating a horizontal trajectory S2; further, the scale error delta K of the mileage meter is estimated by comparing the traces S1 and S2D。
Corresponding to the method, the invention also discloses a system for calibrating the scale error of the odometer, which comprises a memory, a processor and a computer program which is stored on the memory and can run on the processor, wherein the steps of the method are realized when the processor executes the computer program.
Correspondingly, the invention also discloses a computer storage medium, on which a computer program is stored, wherein the program realizes the steps of the method when being executed by a processor.
The method can scientifically calculate the scale error of the odometer, thereby effectively improving the positioning precision of the odometer and providing guarantee for realizing high-precision navigation.
The present invention will be described in further detail below with reference to the accompanying drawings.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
FIG. 1 is a schematic flow chart of a method for calibrating the odometer scale error disclosed in the embodiment of the invention.
FIG. 2 is a schematic diagram of inertial navigation and RTK combined navigation according to an embodiment of the present invention.
Fig. 3 is a schematic diagram comparing a reference route and an actual route of an odometer according to an embodiment of the present invention.
Detailed Description
The embodiments of the invention will be described in detail below with reference to the drawings, but the invention can be implemented in many different ways as defined and covered by the claims.
Example 1
The embodiment discloses a method for calibrating scale errors of a odometer, which comprises the steps of respectively establishing an inertial navigation coordinate system, a navigation coordinate system and a mileage measuring coordinate system, mapping data in the inertial navigation coordinate system and the mileage measuring coordinate system to the navigation coordinate system, and solving the following reference track, horizontal track and scale errors.
As shown in fig. 1, the method of the present invention comprises:
and step S1, acquiring a reference track through inertial navigation and RTK combination.
In this step, the preferred solution process of the reference trajectory includes:
defining the transverse rolling axis of the inertial navigation carrier as ybAxis pointing directly in front of inertial navigation and pitch axis xbAxis pointing to the right of inertial navigation and course axis zbAxis, pointing in the direction of gravity, x, towards the skyb、yb、zbThree axes conform to the right hand coordinate system, xb、yb、zbIs marked as b series;
defining a navigation coordinate system xnShaft fingerEast, ynThe axis pointing north, znThe axis points to the sky, xn、yn、znIs marked as n series;
the measuring coordinate system of the mileage gauge is a right-front-upper right-hand rectangular coordinate system fixedly connected with the vehicle body and recorded as an m system, oy systemmThe shaft is arranged in a plane contacted with the vehicle-carrying wheels and points to the right front of the vehicle body; ozmThe axis is positive perpendicular to the ground plane; oxmThe axis points to the right;
selecting system state parameters, and recording the state quantity as:
Figure BDA0002731658840000071
delta r is inertial navigation position error, delta v is inertial navigation speed error, phi is inertial navigation attitude angle error,
Figure BDA0002731658840000072
is zero offset, δ f, of the gyrobIs the accelerometer zero offset;
establishing an inertial navigation attitude error equation as follows:
Figure BDA0002731658840000073
wherein,
Figure BDA0002731658840000074
is the derivative of phi and is,
Figure BDA0002731658840000075
in order to navigate the gyro measurement error under the coordinate system,
Figure BDA0002731658840000076
the error is calculated for the navigation coordinate system,
Figure BDA0002731658840000077
representing a rotation of the navigation coordinate system relative to the inertial navigation coordinate system;
Figure BDA0002731658840000081
the rotation of the navigation coordinate system due to the rotation of the earth,
Figure BDA0002731658840000082
moving a navigation coordinate system rotation caused by earth surface curvature near the earth surface for the system;
the velocity error equation is:
Figure BDA0002731658840000083
wherein v isn、δvnRespectively the speed and the speed error in the navigation coordinate system,
Figure BDA0002731658840000084
is δ vnThe derivative of (a) of (b),
Figure BDA0002731658840000085
to navigate the accelerometer measurements in the coordinate system,
Figure BDA0002731658840000086
in order to navigate the accelerometer measurement error in the coordinate system,
Figure BDA0002731658840000087
δgnrespectively calculating errors of the spin angular velocity of the earth under the navigation coordinate system, rotation calculation errors and gravity errors;
the inertial navigation position error equation is as follows:
Figure BDA0002731658840000088
l, λ and h denote latitude, longitude and altitude, respectively, δ L, δ λ and δ h denote latitude error, longitude error and altitude error, respectively,
Figure BDA0002731658840000089
and
Figure BDA00027316588400000810
are respectively delta L, delta lambda and delta hDerivative of (A), RMIs the radius of the meridian principal curvature; rNThe radius of main curvature of the mortise and unitary ring;
recording inertial navigation velocity component vn＝[vE vN vU]T；
Velocity error component δ vn＝[δvE δvN δvU]T；
Wherein subscripts E, N, U represent the east, north, and sky directions, respectively;
under a navigation coordinate system, the state equation of inertial navigation and RTK combined navigation is expressed as follows:
Figure BDA00027316588400000811
x (t) for the selected combined navigational coordinate system state vector,
Figure BDA00027316588400000812
is the derivative of X (t), F (t) is the system state transition matrix, W (t) is the system noise vector, G (t) is the system noise driving matrix;
Figure BDA00027316588400000813
wherein,
Figure BDA0002731658840000091
the measured value of the lower accelerometer is denoted as b,
Figure BDA0002731658840000092
representing an attitude transformation matrix from n to b, and (the) x represents an antisymmetric matrix;
Figure BDA0002731658840000093
Figure BDA0002731658840000094
Figure BDA0002731658840000095
Figure BDA0002731658840000096
Figure BDA0002731658840000097
Figure BDA0002731658840000101
Figure BDA0002731658840000102
Figure BDA0002731658840000103
wherein, WaAnd WgRespectively representing the noise of the gyroscope and the accelerometer, R is the radius of the earth when viewed as a sphere, wieIs the earth rotation rate;
the measurement equation is obtained by respectively subtracting the position and the velocity measured by RTK and the position and the velocity measured by inertial navigation, and is as follows:
Figure BDA0002731658840000104
in the above formula, Z (t) is a measurement matrix at time t, B represents longitude, L represents latitude, H represents altitude, VERepresenting east velocity, VNRepresenting north velocity, VURepresenting the antenna speed, and subscripts RTK and INS represent RTK and inertial navigation respectively; v (t) a noise sequence representing the position and velocity of the RTK;
as shown in fig. 2, in the present embodiment, kalman filtering is used to perform the optimal estimation, and the calculation steps are as follows:
one-step prediction equation: xk,k―1＝Φk,k―1Xk―1(ii) a Wherein phik,k―1Is the system transfer matrix from time k-1 to time k, Xk―1Is the system state vector at time k-1;
the state estimation equation is: xk＝Xk,k―1+Kk(Zk―Zk,k―1) (ii) a Wherein, XkIs the system state vector at time K, KkIs the gain matrix at time k, ZkIs a measure of the time k, Zk,k―1Measuring the quantity from the time k-1 to the time k;
the method for obtaining the gain matrix comprises the following steps:
Figure BDA0002731658840000105
wherein, Pk,k―1Is the mean square error matrix from time k-1 to time k, HkIs a measurement matrix at time k, RkMeasuring a noise variance matrix for the system k moment;
one-step prediction of mean square error:
Figure BDA0002731658840000111
wherein, gamma isk―1Is the noise driving matrix at time k-1, Γk,k―1Is the noise drive matrix from time k-1 to time k, Qk-1A noise variance matrix at the k-1 moment of the system;
estimating the mean square error: pk＝(I―KkHk)Pk,k―1(ii) a Wherein p iskA mean square error matrix at time k;
the position trajectory calculated by combining the above-described equations is referred to as S1 as a reference trajectory.
As a variation, in the process of obtaining the reference trajectory, the method for performing the optimal estimation based on the kalman filter shown in fig. 2 may be replaced by another filtering method that is easily conceivable by those skilled in the art.
And step S2, calculating the horizontal track of the vehicle through the odometer.
Preferably, the solving of the horizontal trajectory of the vehicle carrier in the step includes:
the velocity output of the odometer is expressed on the odometer coordinate system as:
Figure BDA0002731658840000112
wherein v isDThe forward speed measured by the mileage meter, the right-direction speed and the sky-direction speed are both zero, the speed is regarded as a speed constraint condition when the vehicle runs normally, and the superscripts m, n and b respectively represent corresponding coordinate systems;
the output of the speed of the odometer under the navigation coordinate system is as follows:
Figure BDA0002731658840000113
considering that there is a small amount of installation deviation angle from m-system to b-system, the deviation angle vector alpha is recorded as [ alpha ]θ αγ αψ]TAnd scale coefficient error δ KDThen, the actual output of the odometer on the navigation coordinate system is:
Figure BDA0002731658840000114
wherein phi isDSubscripts psi, gamma and theta represent pitch angle error, roll angle error and course angle error from m system to b system respectively for attitude misalignment angle of dead reckoning, and phiD＝[φDE φDN φDU]T(ii) a Neglecting the effect of horizontal attitude error, i.e. making an approximation of phiDE≈φDNAnd 0, obtaining:
Figure BDA0002731658840000115
αθ、φDU+αψand δ KDAll are constant and small, and the vehicle is driven in the range of small change of the geographic position, namely, the rotation change of the navigation coordinate system in the whole navigation process is small, and the vehicle is treated as a planeThe two sides of the formula are integrated simultaneously:
Figure BDA0002731658840000116
wherein,
Figure BDA0002731658840000117
respectively, in a time period [0, T]The real displacement vector of the inner carrier vehicle, the calculated displacement vector and the driving mileage; u. ofU＝[0 0 1]TIs a vector of units in the direction of the sky;
the formula is decomposed into a horizontal part and a vertical part to obtain:
Figure BDA0002731658840000121
Figure BDA0002731658840000122
wherein,
Figure BDA0002731658840000123
Figure BDA0002731658840000124
subscript H represents a projection on a horizontal plane;
true displacement
Figure BDA0002731658840000125
Around the zenith axis uUAngle of rotation phiDU+αψThen expand by 1+ delta KDMultiplying to obtain the calculated displacement
Figure BDA0002731658840000126
Thereby calculating a horizontal trajectory S2; further, the scale error delta K of the mileage meter is estimated by comparing the traces S1 and S2D。
And step S3, comparing the reference track with the horizontal track, and estimating the scale error of the odometer.
In this step, as shown in fig. 3, the reference trajectory S1 is broken as indicated by the broken line OA in the figure, and the horizontal trajectory S2 is broken as indicated by the broken line OB in the figure.
Example 2
Corresponding to the above method, the present embodiment discloses a system for calibrating an odometer scale error, which includes a memory, a processor, and a computer program stored on the memory and operable on the processor, wherein the steps of the above method are implemented when the computer program is executed by the processor.
Example 3
Similarly, corresponding to the above method embodiments, the present embodiment discloses a computer storage medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the steps of the above method.
In summary, the odometer scale error calibration method, the odometer scale error calibration system and the computer storage medium respectively disclosed in the embodiments of the invention can scientifically calculate the scale error of the odometer, thereby effectively improving the positioning accuracy of the odometer and providing guarantee for realizing high-accuracy navigation.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (7)

1. A method for calibrating the scale error of a speedometer is characterized by comprising the following steps:
obtaining a reference track through inertial navigation and RTK combination;
calculating a horizontal track of the vehicle through an odometer;
and comparing the reference track with the horizontal track to estimate the scale error of the odometer.
2. The odometer scale error calibration method of claim 1, further comprising:
and respectively establishing an inertial navigation coordinate system, a navigation coordinate system and a mileage measuring coordinate system, and mapping data in the inertial navigation coordinate system and the mileage measuring coordinate system to the navigation coordinate system to solve the reference track, the horizontal track and the scale error.
3. The odometer scale error calibration method according to claim 2, wherein in the process of obtaining the reference trajectory, an optimal estimation is performed based on kalman filtering.
4. The odometer scale error calibration method of claim 3, wherein the solving of the reference trajectory comprises:
defining the transverse rolling axis of the inertial navigation carrier as ybAxis pointing directly in front of inertial navigation and pitch axis xbAxis pointing to the right of inertial navigation and course axis zbAxis, pointing in the direction of gravity, x, towards the skyb、yb、zbThree axes conform to the right hand coordinate system, xb、yb、zbIs marked as b series;
defining a navigation coordinate system xnThe axis pointing east, ynThe axis pointing north, znThe axis points to the sky, xn、yn、znIs marked as n series;
the measuring coordinate system of the mileage gauge is a right-front-upper right-hand rectangular coordinate system fixedly connected with the vehicle body and recorded as an m system, oy systemmThe shaft is arranged in a plane contacted with the vehicle-carrying wheels and points to the right front of the vehicle body; ozmThe axis is positive perpendicular to the ground plane; oxmThe axis points to the right;
selecting system state parameters, and recording the state quantity as:
Figure FDA0002731658830000011
delta r is inertial navigation position error, delta v is inertial navigation speed error, phi is inertial navigation attitude angle error,
Figure FDA0002731658830000012
is zero offset, δ f, of the gyrobIs the accelerometer zero offset;
establishing an inertial navigation attitude error equation as follows:
Figure FDA0002731658830000013
wherein,
Figure FDA0002731658830000014
is the derivative of phi and is,
Figure FDA0002731658830000015
in order to navigate the gyro measurement error under the coordinate system,
Figure FDA0002731658830000016
the error is calculated for the navigation coordinate system,
Figure FDA0002731658830000017
representing a rotation of the navigation coordinate system relative to the inertial navigation coordinate system;
Figure FDA0002731658830000018
Figure FDA0002731658830000019
the rotation of the navigation coordinate system due to the rotation of the earth,
Figure FDA00027316588300000110
moving a navigation coordinate system rotation caused by earth surface curvature near the earth surface for the system;
the velocity error equation is:
Figure FDA00027316588300000111
wherein v isn、δvnRespectively the speed and the speed error in the navigation coordinate system,
Figure FDA0002731658830000021
is δ vnThe derivative of (a) of (b),
Figure FDA0002731658830000022
to navigate the accelerometer measurements in the coordinate system,
Figure FDA0002731658830000023
in order to navigate the accelerometer measurement error in the coordinate system,
Figure FDA0002731658830000024
δgnrespectively calculating errors of the spin angular velocity of the earth under the navigation coordinate system, rotation calculation errors and gravity errors;
the inertial navigation position error equation is as follows:
Figure FDA0002731658830000025
l, λ and h denote latitude, longitude and altitude, respectively, δ L, δ λ and δ h denote latitude error, longitude error and altitude error, respectively,
Figure FDA0002731658830000026
and
Figure FDA0002731658830000027
derivatives of δ L, δ λ and δ h, RMIs the radius of the meridian principal curvature; rNThe radius of main curvature of the mortise and unitary ring;
recording inertial navigation velocity component vn＝[vE vN vU]T；
Velocity error component δ vn＝[δvE δvN δvU]T；
Wherein subscripts E, N, U represent the east, north, and sky directions, respectively;
under a navigation coordinate system, the state equation of inertial navigation and RTK combined navigation is expressed as follows:
Figure FDA00027316588300000212
x (t) for the selected combined navigational coordinate system state vector,
Figure FDA0002731658830000028
is the derivative of X (t), F (t) is the system state transition matrix, W (t) is the system noise vector, G (t) is the system noise driving matrix;
Figure FDA0002731658830000029
wherein,
Figure FDA00027316588300000210
the measured value of the lower accelerometer is denoted as b,
Figure FDA00027316588300000211
representing an attitude transformation matrix from n to b, and (the) x represents an antisymmetric matrix;
Figure FDA0002731658830000031
Figure FDA0002731658830000032
Figure FDA0002731658830000033
Figure FDA0002731658830000034
Figure FDA0002731658830000035
Figure FDA0002731658830000036
Figure FDA0002731658830000041
Figure FDA0002731658830000042
wherein, WaAnd WgRespectively representing the noise of the gyroscope and the accelerometer, R is the radius of the earth when viewed as a sphere, wieIs the earth rotation rate;
the measurement equation is obtained by respectively subtracting the position and the velocity measured by RTK and the position and the velocity measured by inertial navigation, and is as follows:
Figure FDA0002731658830000043
in the above formula, Z (t) is a measurement matrix at time t, B represents longitude, L represents latitude, H represents altitude, VERepresenting east velocity, VNRepresenting north velocity, VURepresenting the antenna speed, and subscripts RTK and INS represent RTK and inertial navigation respectively; v (t) a noise sequence representing the position and velocity of the RTK;
and performing optimal estimation by adopting Kalman filtering, wherein the calculation steps are as follows:
one-step prediction equation: xk,k―1＝Φk,k―1Xk―1(ii) a Wherein phik,k―1Is the system transfer matrix from time k-1 to time k, Xk―1Is the system state vector at time k-1;
the state estimation equation is: xk＝Xk,k―1+Kk(Zk―Zk,k―1) (ii) a Wherein, XkIs the system state vector at time K, KkIs the gain matrix at time k, ZkIs a measure of the time k, Zk,k―1Measuring the quantity from the time k-1 to the time k;
the method for obtaining the gain matrix comprises the following steps:
Figure FDA0002731658830000044
wherein, Pk,k―1Is the mean square error matrix from time k-1 to time k, HkIs a measurement matrix at time k, RkMeasuring a noise variance matrix for the system k moment;
one-step prediction of mean square error:
Figure FDA0002731658830000045
wherein, gamma isk―1Is the noise driving matrix at time k-1, Γk,k―1Is the noise drive matrix from time k-1 to time k, Qk-1A noise variance matrix at the k-1 moment of the system;
estimating the mean square error: pk＝(I―KkHk)Pk,k―1(ii) a Wherein p iskA mean square error matrix at time k;
the position trajectory calculated by combining the above-described equations is referred to as S1 as a reference trajectory.
5. The odometer scale error calibration method of claim 4, wherein the solving of the vehicle load horizontal trajectory comprises:
the velocity output of the odometer is expressed on the odometer coordinate system as:
Figure FDA0002731658830000051
wherein v isDThe forward speed measured by the mileage meter, the right-direction speed and the sky-direction speed are both zero, the speed is regarded as a speed constraint condition when the vehicle runs normally, and the superscripts m, n and b respectively represent corresponding coordinate systems;
the output of the speed of the odometer under the navigation coordinate system is as follows:
Figure FDA0002731658830000052
considering that there is a small amount of installation deviation angle from m-system to b-system, the deviation angle vector alpha is recorded as [ alpha ]θ αγ αψ]TAnd scale coefficient error δ KDThen, the actual output of the odometer on the navigation coordinate system is:
Figure FDA0002731658830000053
wherein phi isDSubscripts psi, gamma and theta represent pitch angle error, roll angle error and course angle error from m system to b system respectively for attitude misalignment angle of dead reckoning, and phiD＝[φDE φDN φDU]T(ii) a Neglecting the effect of horizontal attitude error, i.e. making an approximation of phiDE≈φDNAnd 0, obtaining:
Figure FDA0002731658830000054
αθ、φDU+αψand δ KDBe the constant small quantity, and carry the car and go in the little within range of geographical position change, the rotation change of whole navigation in-process navigation coordinate system is not big promptly, handles as the plane, and the integration is got simultaneously on the two sides of formula:
Figure FDA0002731658830000055
wherein,
Figure FDA0002731658830000056
respectively, in a time period [0, T]The real displacement vector of the inner carrier vehicle, the calculated displacement vector and the driving mileage; u. ofU＝[0 0 1]TIs a vector of units in the direction of the sky;
the formula is decomposed into a horizontal part and a vertical part to obtain:
Figure FDA0002731658830000057
Figure FDA0002731658830000058
wherein,
Figure FDA0002731658830000059
Figure FDA00027316588300000510
subscript H represents a projection on a horizontal plane;
true displacement
Figure FDA0002731658830000061
Around the zenith axis uUAngle of rotation phiDU+αψThen expand by 1+ delta KDMultiplying to obtain the calculated displacement
Figure FDA0002731658830000062
Thereby calculating a horizontal trajectory S2; further, the scale error delta K of the mileage meter is estimated by comparing the traces S1 and S2D。
6. An odometer scale error calibration system comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the steps of the method of any one of claims 1 to 5 are carried out when the computer program is executed by the processor.
7. A computer storage medium having a computer program stored thereon, wherein the program is adapted to perform the steps of the method of any one of claims 1 to 5 when executed by a processor.
CN202011120011.5A 2020-10-19 2020-10-19 Odometer scale error calibration method, system and computer storage medium Active CN112284415B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011120011.5A CN112284415B (en) 2020-10-19 2020-10-19 Odometer scale error calibration method, system and computer storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011120011.5A CN112284415B (en) 2020-10-19 2020-10-19 Odometer scale error calibration method, system and computer storage medium

Publications (2)

Publication Number Publication Date
CN112284415A true CN112284415A (en) 2021-01-29
CN112284415B CN112284415B (en) 2023-01-03

Family

ID=74497947

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011120011.5A Active CN112284415B (en) 2020-10-19 2020-10-19 Odometer scale error calibration method, system and computer storage medium

Country Status (1)

Country Link
CN (1) CN112284415B (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112985464A (en) * 2021-05-10 2021-06-18 湖北亿咖通科技有限公司 Precision detection method of vehicle odometer, electronic device and storage medium
CN114088113A (en) * 2021-11-16 2022-02-25 北京航空航天大学 Odometer track alignment and precision evaluation method
CN114397480A (en) * 2022-01-04 2022-04-26 湖南大学 Acoustic Doppler velocimeter error estimation method, device and system
CN114396965A (en) * 2022-01-17 2022-04-26 广州导远电子科技有限公司 Auxiliary calibration method and device for combined navigation unit and electronic equipment
WO2024087478A1 (en) * 2022-10-28 2024-05-02 中煤科工集团上海有限公司 Inertial navigation precision evaluation system and evaluation method for coal mining machine, and mobile carrier

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5929753A (en) * 1997-03-05 1999-07-27 Montague; Albert Vehicle/aircraft security system based on vehicle displacement profile, with optional GPS/cellular discrimination indicator
CN104977002A (en) * 2015-06-12 2015-10-14 同济大学 SINS/double OD-based inertial integrated navigation system and method
CN105318876A (en) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 Inertia and mileometer combination high-precision attitude measurement method
CN106767894A (en) * 2015-11-20 2017-05-31 北方信息控制集团有限公司 A kind of Big Dipper/odometer combination scaling method for inertial navigation
CN108088443A (en) * 2016-11-23 2018-05-29 北京自动化控制设备研究所 A kind of positioning and directing device rate compensation method
CN108180925A (en) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 A kind of odometer assists vehicle-mounted dynamic alignment method
CN109974697A (en) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 A kind of high-precision mapping method based on inertia system

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5929753A (en) * 1997-03-05 1999-07-27 Montague; Albert Vehicle/aircraft security system based on vehicle displacement profile, with optional GPS/cellular discrimination indicator
CN105318876A (en) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 Inertia and mileometer combination high-precision attitude measurement method
CN104977002A (en) * 2015-06-12 2015-10-14 同济大学 SINS/double OD-based inertial integrated navigation system and method
CN106767894A (en) * 2015-11-20 2017-05-31 北方信息控制集团有限公司 A kind of Big Dipper/odometer combination scaling method for inertial navigation
CN108088443A (en) * 2016-11-23 2018-05-29 北京自动化控制设备研究所 A kind of positioning and directing device rate compensation method
CN108180925A (en) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 A kind of odometer assists vehicle-mounted dynamic alignment method
CN109974697A (en) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 A kind of high-precision mapping method based on inertia system

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
ZHIBIN XIAO 等: "Analysis_of_the_GNSS_Code-Carrier_Hardware_Delay_Difference_in_RTK_Process_Effect_Measurement_and_Calibration", 《IEEE ACCESS》 *
刘鹏飞: "里程计辅助的高精度车载GNSS/INS组合导航系统", 《光学精密工程》 *
张小跃等: "捷联惯导/里程计组合导航方法", 《北京航空航天大学学报》 *
李旦等: "车载惯导航位推算组合导航系统误差补偿研究", 《计算机测量与控制》 *
李治国: "基于惯导_激光雷达的无人车融合定位技术研究", 《软件》 *
李艳等: "基于车辆运动约束的里程计误差在线标定方法", 《中国惯性技术学报》 *
谢波等: "一种车载定位定向系统误差补偿方法", 《计算机测量与控制》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112985464A (en) * 2021-05-10 2021-06-18 湖北亿咖通科技有限公司 Precision detection method of vehicle odometer, electronic device and storage medium
CN114088113A (en) * 2021-11-16 2022-02-25 北京航空航天大学 Odometer track alignment and precision evaluation method
CN114088113B (en) * 2021-11-16 2023-05-16 北京航空航天大学 Odometer track alignment and precision evaluation method
CN114397480A (en) * 2022-01-04 2022-04-26 湖南大学 Acoustic Doppler velocimeter error estimation method, device and system
CN114397480B (en) * 2022-01-04 2022-10-14 湖南大学 Acoustic Doppler velocimeter error estimation method, device and system
CN114396965A (en) * 2022-01-17 2022-04-26 广州导远电子科技有限公司 Auxiliary calibration method and device for combined navigation unit and electronic equipment
WO2024087478A1 (en) * 2022-10-28 2024-05-02 中煤科工集团上海有限公司 Inertial navigation precision evaluation system and evaluation method for coal mining machine, and mobile carrier

Also Published As

Publication number Publication date
CN112284415B (en) 2023-01-03

Similar Documents

Publication Publication Date Title
CN112284415B (en) Odometer scale error calibration method, system and computer storage medium
CN110501024B (en) Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN109974697B (en) High-precision mapping method based on inertial system
CN101476894B (en) Vehicle-mounted SINS/GPS combined navigation system performance reinforcement method
US7711483B2 (en) Dead reckoning system
Fu et al. High-accuracy SINS/LDV integration for long-distance land navigation
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
CN110779521A (en) Multi-source fusion high-precision positioning method and device
Wu Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning
CN112762961B (en) On-line calibration method for integrated navigation of vehicle-mounted inertial odometer
CN109186597B (en) Positioning method of indoor wheeled robot based on double MEMS-IMU
JP2000502803A (en) Zero motion detection system for improved vehicle navigation system
CN109470276B (en) Odometer calibration method and device based on zero-speed correction
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
CN109612460B (en) Plumb line deviation measuring method based on static correction
JP7344895B2 (en) How to calibrate a gyrometer installed in a vehicle
CN109579870A (en) The automatic aligning method and combined navigation device of Strapdown Inertial Navigation System
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN117053802A (en) Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU
WO2000050917A1 (en) Vehicle navigation system with correction for selective availability
CN116337053A (en) Vehicle navigation method, device, electronic equipment and storage medium
CN112525204B (en) Spacecraft inertia and solar Doppler speed combined navigation method
CN105928519A (en) Navigation algorithm based on INS inertial navigation, GPS navigation and magnetometer
CN109059913A (en) A kind of zero-lag integrated navigation initial method for onboard navigation system

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
CB02 Change of applicant information

Address after: 420007 Building 1, group E, China Power Valley phase 3.1, no.899, Xianyue Ring Road, Tianyuan District, Zhuzhou City, Hunan Province

Applicant after: Zhuzhou fisrock Photoelectric Technology Co.,Ltd.

Address before: 420007 Building 1, group E, China Power Valley phase 3.1, no.899, Xianyue Ring Road, Tianyuan District, Zhuzhou City, Hunan Province

Applicant before: ZHUZHOU PHASE-LOCK PHOTOELECTRIC TECHNOLOGY CO.,LTD.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant