CN112284415B - 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
CN112284415B
CN112284415B CN202011120011.5A CN202011120011A CN112284415B CN 112284415 B CN112284415 B CN 112284415B CN 202011120011 A CN202011120011 A CN 202011120011A CN 112284415 B CN112284415 B CN 112284415B
Authority
CN
China
Prior art keywords
error
coordinate system
time
navigation
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.)
Active
Application number
CN202011120011.5A
Other languages
Chinese (zh)
Other versions
CN112284415A (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 Fisrock Photoelectric Technology Co ltd
Original Assignee
Zhuzhou Fisrock 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 Fisrock Photoelectric Technology Co ltd filed Critical Zhuzhou Fisrock 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

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 scale errors 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 y b Axis pointing directly in front of inertial navigation, pitch axis being x b Axis pointing to the right of inertial navigation and course axis z b Axis, pointing in the direction of gravity, towards the sky, x b 、y b 、z b Three axes conform to the right hand coordinate system, x b 、y b 、z b Is marked as b is;
defining a navigation coordinate system x n The axis pointing east, y n The axis pointing north, z n The axis points to the sky, x n 、y n 、z n Is 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 system m The shaft is arranged in a plane contacted with the vehicle-carrying wheels and points to the right front of the vehicle body; oz m The axis is positive perpendicular to the ground plane; ox m The 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 gyro b Is the accelerometer zero offset;
establishing an inertial navigation attitude error equation as follows:
Figure BDA0002731658840000023
wherein the content of the first and second substances,
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 is n 、δv n Respectively the speed and the speed error in the navigation coordinate system,
Figure BDA00027316588400000211
is δ v n The derivative of (a) is determined,
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
δg n respectively 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 respectively represent latitude, longitude and altitude, δ L, δ λ and δ h respectively represent latitude error, longitude error and altitude error,
Figure BDA00027316588400000216
and
Figure BDA00027316588400000217
derivatives of δ L, δ λ and δ h, R M Is the radius of the meridian principal curvature; r N The radius of main curvature of the mortise and unitary ring;
recording inertial navigation velocity component v n ＝[v E v N v U ] T ；
Velocity error component δ v n ＝[δv E δv N δv U ] 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) is 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 the content of the first and second substances,
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, W a And W g Respectively representing the noise of the gyroscope and the accelerometer, R is the radius of the earth when viewed as a sphere, w ie Is 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, V E Representing east velocity, V N Representing north velocity, V U Representing the antenna speed, and subscripts RTK and INS represent RTK and inertial navigation respectively; v (t) represents the noise sequence of 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: x k,k―1 ＝Φ k,k―1 X k―1 (ii) a Wherein phi k,k―1 Is the system transfer matrix from time k-1 to time k, X k―1 Is the system state vector at time k-1;
the state estimation equation is: x k ＝X k,k―1 +K k (Z k ―Z k,k―1 ) (ii) a Wherein, X k Is the system state vector at time K, K k Is the gain matrix at time k, Z k Is a measure of the time k, Z k,k―1 Measuring 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, P k,k―1 Is the mean square error matrix from time k-1 to time k, H k Is a measurement matrix at time k, R k Measuring a noise variance matrix for the system k moment;
one-step prediction of mean square error:
Figure BDA0002731658840000052
wherein, gamma is k―1 Is the noise driving matrix at time k-1, Γ k,k―1 Is the noise drive matrix from time k-1 to time k, Q k-1 A noise variance matrix at the k-1 moment of the system;
estimating the mean square error: p is k ＝(I―K k H k )P k,k―1 (ii) a Wherein p is k A mean square error matrix at time k;
the position trajectory calculated by each of the simultaneous parties Cheng Jie is taken as a reference trajectory and is denoted as S1.
Preferably, the solving of the horizontal track of the vehicle carrier comprises:
the speed output of the odometer is expressed on the odometer coordinate system as:
Figure BDA0002731658840000053
wherein v is D The 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 α = [ α ] θ α γ α ψ ] T And scale coefficient error δ K D Then, the actual output of the odometer on the navigation coordinate system is:
Figure BDA0002731658840000055
wherein phi is D Subscripts 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 phi D ＝[φ DE φ DN φ DU ] T (ii) a Neglecting horizontal postureInfluence of state errors, i.e. making an approximation of phi DE ≈φ DN And 0, obtaining:
Figure BDA0002731658840000061
α θ 、φ DU +α ψ and δ K D Be 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 the content of the first and second substances,
Figure BDA0002731658840000063
respectively, in the time period [0,T]The real displacement vector of the inner carrier vehicle, the calculated displacement vector and the driving mileage; u. of U ＝[0 0 1] T Is 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 the content of the first and second substances,
Figure BDA0002731658840000066
Figure BDA0002731658840000067
subscript H represents a projection on a horizontal plane;
true displacement
Figure BDA0002731658840000068
Around the zenith axis u U Angle of rotation phi DU +α ψ Then expand by 1+ delta K D Multiplying to obtain the calculated displacement
Figure BDA0002731658840000069
Thereby calculating a horizontal trajectory S2; and then the scale error delta K of the mileage meter is estimated by comparing the tracks S1 and S2 D 。
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 an inertial navigation and RTK combined navigation system 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 S1, obtaining 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 y b Axis pointing directly in front of inertial navigation and pitch axis x b Axis pointing to the right of inertial navigation and course axis z b Axis, pointing in the direction of gravity, x, towards the sky b 、y b 、z b Three axes conform to the right hand coordinate system, x b 、y b 、z b Is marked as b series;
defining a navigation coordinate system x n The axis points east, y n The axis pointing north, z n The axis points to the sky, x n 、y n 、z n Is marked as n;
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 system m The shaft is arranged in a plane contacted with the vehicle-carrying wheels and points to the right front of the vehicle body; oz m The axis is positive perpendicular to the ground plane; ox m The 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 gyro b Is the accelerometer zero offset;
establishing an inertial navigation attitude error equation as follows:
Figure BDA0002731658840000073
wherein the content of the first and second substances,
Figure BDA0002731658840000074
is the derivative of the value of phi and,
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 for the system near the earth surface;
the velocity error equation is:
Figure BDA0002731658840000083
wherein v is n 、δv n Respectively the speed and the speed error in the navigation coordinate system,
Figure BDA0002731658840000084
is δ v n The 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
δg n respectively 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:
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
derivatives of δ L, δ λ and δ h, R M Is the radius of the meridian principal curvature; r is N The radius of main curvature of the mortise and unitary ring;
recording inertial navigation velocity component v n ＝[v E v N v U ] T ；
Velocity error component δ v n ＝[δv E δv N δv U ] 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) is 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, the first and the second end of the pipe are connected with each other,
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 (a) x represents an anti-symmetric matrix;
Figure BDA0002731658840000093
Figure BDA0002731658840000094
Figure BDA0002731658840000095
Figure BDA0002731658840000096
Figure BDA0002731658840000097
Figure BDA0002731658840000101
Figure BDA0002731658840000102
Figure BDA0002731658840000103
wherein, W a And W g Respectively representing the noise of the gyroscope and the accelerometer, R is the radius of the earth when viewed as a sphere, w ie Is 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, V E Representing east velocity, V N Representing north velocity, V U Representing the antenna speed, and subscripts RTK and INS represent RTK and inertial navigation respectively; v (t) represents a noise sequence of the RTK's position and velocity;
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: x k,k―1 ＝Φ k,k―1 X k―1 (ii) a Wherein phi k,k―1 Is the system transfer matrix from time k-1 to time k, X k―1 Is the system state vector at time k-1;
the state estimation equation is: x k ＝X k,k―1 +K k (Z k ―Z k,k―1 ) (ii) a Wherein, X k Is the system state vector at time K, K k Is the gain matrix at time k, Z k Is a measure of the k time, Z k,k―1 Measuring 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, P k,k―1 Is the mean square error matrix from time k-1 to time k, H k Is a measurement matrix at time k, R k Measuring a noise variance matrix for the system k moment;
one-step prediction of mean square error:
Figure BDA0002731658840000111
wherein, gamma is k―1 Is the noise driving matrix at time k-1, Γ k,k―1 Is the noise drive matrix from time k-1 to time k, Q k-1 A noise variance matrix at the k-1 moment of the system;
estimating the mean square error: p k ＝(I―K k H k )P k,k―1 (ii) a Wherein p is k A mean square error matrix at time k;
the position locus calculated by combining the aforementioned parties Cheng Jie is denoted as a reference locus and is denoted as S1.
As a variation, in the process of obtaining the reference trajectory, the method for performing optimal estimation based on kalman filtering as shown in fig. 2 may be replaced by another filtering method that is easily conceived by those skilled in the art.
And S2, calculating a horizontal track of the vehicle through the odometer.
Preferably, the solving of the horizontal track of the vehicle loading vehicle in the step comprises the following steps:
the velocity output of the odometer is expressed on the odometer coordinate system as:
Figure BDA0002731658840000112
wherein v is D The 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 α = [ α ] θ α γ α ψ ] T And scale coefficient error δ K D Then, the actual output of the odometer on the navigation coordinate system is:
Figure BDA0002731658840000114
wherein phi is D Subscripts 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 phi D ＝[φ DE φ DN φ DU ] T (ii) a Neglecting the effect of horizontal attitude error, i.e. making an approximation of phi DE ≈φ DN And 0, obtaining:
Figure BDA0002731658840000115
α θ 、φ DU +α ψ and δ K D Be 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 BDA0002731658840000116
wherein the content of the first and second substances,
Figure BDA0002731658840000117
respectively, in a time period of [0,T]The real displacement vector of the inner carrier vehicle, the calculated displacement vector and the driving mileage; u. of U ＝[0 0 1] T Is 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, the first and the second end of the pipe are connected with each other,
Figure BDA0002731658840000123
Figure BDA0002731658840000124
subscript H represents a projection on a horizontal plane;
true displacement
Figure BDA0002731658840000125
Around the zenith axis u U Angle of rotation phi DU +α ψ Then expand by 1+ delta K D Multiplying to obtain a calculated displacement
Figure BDA0002731658840000126
Thereby calculating a horizontal trajectory S2; and then the scale error delta K of the mileage meter is estimated by comparing the tracks S1 and S2 D 。
And 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 track S1 is broken as indicated by the broken line OA in the figure, and the horizontal track 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 (3)

1. A method for calibrating scale errors 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;
comparing the reference track with the horizontal track, and estimating the scale error of the odometer;
further comprising:
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 a reference track, a horizontal track and a scale error;
in the process of obtaining the reference track, optimal estimation is carried out based on Kalman filtering;
the solving process of the reference track comprises the following steps:
defining the transverse rolling axis of the inertial navigation carrier as y b Axis pointing directly in front of inertial navigation and pitch axis x b Axis pointing to the right of inertial navigation and course axis z b Axis, pointing in the direction of gravity, x, towards the sky b 、y b 、z b Three axes conform to the right hand coordinate system, x b 、y b 、z b Is marked as b series;
defining a navigation coordinate system x n The axis pointing east, y n The axis pointing north, z n The axis points to the sky, x n 、y n 、z n Is marked as n series;
the measuring coordinate system of the mileage recorder is oneA right-front-upper right-hand rectangular coordinate system fixedly connected with the vehicle body and recorded as an m system, oy m The shaft is arranged in a horizontal plane contacted with the vehicle-carrying wheels and points to the right front of the vehicle body; oz m The axis is positive when vertical to the ground plane; ox m The axis points to the right;
selecting system state parameters, and recording the state quantity as:
Figure FDA0003801884840000011
delta r is inertial navigation position error, delta v is inertial navigation speed error, phi is inertial navigation attitude angle error,
Figure FDA0003801884840000012
is zero offset, δ f, of the gyro b Is the accelerometer zero offset;
establishing an inertial navigation attitude error equation as follows:
Figure FDA0003801884840000013
wherein the content of the first and second substances,
Figure FDA0003801884840000014
is the derivative of phi and is,
Figure FDA0003801884840000015
in order to navigate the gyro measurement error under the coordinate system,
Figure FDA0003801884840000016
the error is calculated for the navigation coordinate system,
Figure FDA0003801884840000017
representing a rotation of the navigation coordinate system relative to the inertial navigation coordinate system;
Figure FDA0003801884840000018
Figure FDA0003801884840000019
the rotation of the navigation coordinate system due to the rotation of the earth,
Figure FDA00038018848400000110
moving a navigation coordinate system rotation caused by earth surface curvature near the earth surface for the system;
the speed error equation is:
Figure FDA00038018848400000111
wherein v is n 、δv n Respectively the speed and the speed error in the navigation coordinate system,
Figure FDA00038018848400000112
is δ v n The derivative of (a) of (b),
Figure FDA00038018848400000113
to navigate the accelerometer measurements in the coordinate system,
Figure FDA00038018848400000114
in order to navigate the accelerometer measurement error in the coordinate system,
Figure FDA00038018848400000115
δg n respectively 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 FDA0003801884840000021
l, λ and h denote latitude, longitude and altitude, respectively, δ L, δ λ and δ h denote latitude error, longitude error and altitude error, respectively,
Figure FDA0003801884840000022
and
Figure FDA0003801884840000023
derivatives of δ L, δ λ and δ h, R M Is the radius of the meridian principal curvature; r is N The radius of main curvature of the mortise and unitary ring;
recording inertial navigation velocity component v n ＝[v E v N v U ] T ；
Velocity error component δ v n ＝[δv E δv N δv U ] 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 FDA0003801884840000024
x (t) is the selected combined navigational coordinate system state vector,
Figure FDA0003801884840000025
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 FDA0003801884840000026
wherein the content of the first and second substances,
Figure FDA0003801884840000027
the measured value of the lower accelerometer is denoted as b,
Figure FDA0003801884840000028
representing an attitude transformation matrix from n to b, and (the) x represents an antisymmetric matrix;
Figure FDA0003801884840000031
Figure FDA0003801884840000032
Figure FDA0003801884840000033
Figure FDA0003801884840000034
Figure FDA0003801884840000035
Figure FDA0003801884840000036
Figure FDA0003801884840000041
Figure FDA0003801884840000042
wherein, W a And W g Respectively representing the noise of the gyroscope and the accelerometer, R is the radius of the earth when viewed as a sphere, w ie Is 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 FDA0003801884840000043
in the above formula, Z (t) is a measurement matrix at time t, B represents longitude, L represents latitude, H represents altitude, V E Representing east velocity, V N Representing north-bound velocity, V U Representing the antenna speed, and subscripts RTK and INS represent RTK and inertial navigation respectively; v (t) represents a noise sequence of the RTK's position and velocity;
and performing optimal estimation by adopting Kalman filtering, wherein the calculation steps are as follows:
one-step prediction equation: x k,k-1 ＝Φ k,k-1 X k-1 (ii) a Wherein phi k,k-1 Is the system transfer matrix from time k-1 to time k, X k-1 Is the system state vector at time k-1;
the state estimation equation is: x k ＝X k,k-1 +K k (Z k -Z k,k-1 ) (ii) a Wherein, X k Is the system state vector at time K, K k Is the gain matrix at time k, Z k Is a measure of the time k, Z k,k-1 Measuring the quantity from the time k-1 to the time k;
the method for obtaining the gain matrix comprises the following steps:
Figure FDA0003801884840000044
wherein, P k,k-1 Is the mean square error matrix from time k-1 to time k, H k Is a measurement matrix at time k, R k Measuring a noise variance matrix for the system k moment;
one-step prediction of mean square error:
Figure FDA0003801884840000045
wherein, gamma is k-1 Is the noise driving matrix at time k-1, Γ k,k-1 Is the noise drive matrix from time k-1 to time k, Q k-1 A noise variance matrix at the k-1 moment of the system;
estimating the mean square error: p k ＝(I-K k H k )P k,k-1 (ii) a Wherein p is k A mean square error matrix at time k;
simultaneously taking the position track calculated by each party Cheng Jie as a reference track and recording as S1;
the solving of the horizontal track of the vehicle carrier comprises the following steps:
the speed output of the odometer is expressed on the odometer coordinate system as:
Figure FDA0003801884840000051
wherein v is D The 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 FDA0003801884840000052
considering that there is a small amount of installation deviation angle from m-system to b-system, the deviation angle vector α = [ α ] θ α γ α ψ ] T And scale coefficient error δ K D Then, the actual output of the odometer on the navigation coordinate system is:
Figure FDA0003801884840000053
wherein phi is D Subscripts 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 phi D ＝[φ DE φ DN φ DU ] T (ii) a Neglecting the effect of horizontal attitude error, i.e. making an approximation of phi DE ≈φ DN And 0, obtaining:
Figure FDA0003801884840000054
α θ 、φ DU +α ψ and δ K D Are all constant and small, and the vehicle is driven in the range with little change of the geographical position, namely the whole vehicle is navigatedThe rotation change of the navigation coordinate system in the process is small, and the two sides of the above formula are integrated at the same time as a plane processing:
Figure FDA0003801884840000055
wherein the content of the first and second substances,
Figure FDA0003801884840000056
respectively, in the time period [0,T]The real displacement vector of the inner carrier vehicle, the calculated displacement vector and the driving mileage; u. of U ＝[0 0 1] T Is 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 FDA0003801884840000057
Figure FDA0003801884840000058
wherein the content of the first and second substances,
Figure FDA0003801884840000059
Figure FDA00038018848400000510
subscript H represents a projection on a horizontal plane;
true displacement
Figure FDA0003801884840000061
Around the zenith axis u U Angle of rotation phi DU +α ψ Then expand by 1+ delta K D Multiplying to obtain the calculated displacement
Figure FDA0003801884840000062
Thereby calculating a horizontal trajectory S2; and then estimating the mileage by comparing the trajectories S1 and S2Cheng Ji Scale error δ K D 。
2. 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 claim 1 are performed when the computer program is executed by the processor.
3. A computer storage medium having a computer program stored thereon, wherein the program when executed by a processor implements the steps of the method of claim 1.
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 CN112284415A (en) 2021-01-29
CN112284415B true 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)

Families Citing this family (4)

* 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
CN114088113B (en) * 2021-11-16 2023-05-16 北京航空航天大学 Odometer track alignment and precision evaluation method
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

Family Cites Families (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
CN106767894B (en) * 2015-11-20 2019-11-15 北方信息控制集团有限公司 A kind of Beidou for inertial navigation/odometer combination scaling method
CN108088443B (en) * 2016-11-23 2021-06-08 北京自动化控制设备研究所 Speed compensation method for positioning and orienting equipment
CN108180925B (en) * 2017-12-15 2020-09-01 中国船舶重工集团公司第七0七研究所 Odometer-assisted vehicle-mounted dynamic alignment method
CN109974697B (en) * 2019-03-21 2022-07-26 中国船舶重工集团公司第七0七研究所 High-precision mapping method based on inertial system

Also Published As

Publication number Publication date
CN112284415A (en) 2021-01-29

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
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
Fu et al. High-accuracy SINS/LDV integration for long-distance land navigation
Wu Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning
CN109186597B (en) Positioning method of indoor wheeled robot based on double MEMS-IMU
JP2013535669A (en) Apparatus and method for calibrating dynamic parameters of a vehicle navigation system
CN112762961B (en) On-line calibration method for integrated navigation of vehicle-mounted inertial odometer
CN109470276B (en) Odometer calibration method and device based on zero-speed correction
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
CN109612460B (en) Plumb line deviation measuring method based on static correction
CN107860399A (en) Accurate alignment method between a kind of vehicle-mounted inertial navigation based on map match is advanced
JP7344895B2 (en) How to calibrate a gyrometer installed in a vehicle
CN111678514B (en) Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation
CN109579870A (en) The automatic aligning method and combined navigation device of Strapdown Inertial Navigation System
CN111024074A (en) Inertial navigation speed error determination method based on recursive least square parameter identification
WO2000050917A1 (en) Vehicle navigation system with correction for selective availability
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN112525204B (en) Spacecraft inertia and solar Doppler speed combined navigation method
CN109059913A (en) A kind of zero-lag integrated navigation initial method for onboard navigation system
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load 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