CN114964222A - Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device - Google Patents

Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device Download PDF

Info

Publication number
CN114964222A
CN114964222A CN202210398274.5A CN202210398274A CN114964222A CN 114964222 A CN114964222 A CN 114964222A CN 202210398274 A CN202210398274 A CN 202210398274A CN 114964222 A CN114964222 A CN 114964222A
Authority
CN
China
Prior art keywords
angle
representing
imu
error
initial
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210398274.5A
Other languages
Chinese (zh)
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.)
Guangzhou Hi Target Surveying Instrument Co ltd
Original Assignee
Guangzhou Hi Target Surveying Instrument 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 Guangzhou Hi Target Surveying Instrument Co ltd filed Critical Guangzhou Hi Target Surveying Instrument Co ltd
Priority to CN202210398274.5A priority Critical patent/CN114964222A/en
Publication of CN114964222A publication Critical patent/CN114964222A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a vehicle-mounted IMU attitude initialization method and an installation deviation angle estimation method, which are applied to vehicle-mounted GNSS/INS integrated navigation equipment, and comprise the steps of acquiring measurement data of an accelerometer by keeping a vehicle still for several seconds to obtain a pitch angle and a roll angle, and respectively recording the pitch angle and the roll angle as an initial pitch angle and an initial roll angle of the GNSS/INS integrated navigation equipment; and then, when the vehicle starts to move, acquiring the speed increment of the IMU module and the speed increment of the GNSS receiver, calculating a course angle according to a pre-integration principle, and recording the course angle as an initial course angle of the GNSS/INS combined navigation so as to apply the initial course angle to subsequent GNSS/INS fine alignment and realize the estimation of the installation deviation angle of the IMU relative to the vehicle body. According to the invention, the accurate installation deviation angle of the vehicle body can be given without considering the installation position of the IMU module. The invention also discloses GNSS/INS combined navigation equipment and a storage medium.

Description

Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
Technical Field
The invention relates to the field of vehicle navigation, in particular to a vehicle IMU attitude initialization method, an installation deviation angle estimation method, combined navigation equipment and a storage medium.
Background
In the field of vehicle-mounted technology, both autonomous driving and conventional vehicle-mounted mobile measurement systems require a combination of a GNSS (Global Navigation Satellite System) receiver + an IMU (Inertial measurement unit) module to provide high-precision position and attitude information. Meanwhile, under the condition that satellite signals are blocked or completely unlocked, other sensors are used for assisting the GNSS/INS combined navigation positioning, such as an odometer.
When the IMU module is mounted on the vehicle body, a mounting deviation angle is formed between the IMU coordinate system and the vehicle body coordinate system due to the mounting position or other operation factors, and at present, the mounting deviation angle is mainly formed by the following two aspects:
1) the IMU module is as follows: the front-right-lower or right-front-upper is regularly installed, but when the installation is carried out, a user can introduce an installation deviation angle due to a naked eye observation error, at the moment, the X axis (Y axis) of the visual IMU coordinate system is approximately parallel to the advancing direction of the vehicle body coordinate system, and the installation deviation angle is smaller;
2) the user does not mount according to the above rule, but mounts the IMU module at will, without considering the relationship between the IMU coordinate system axial direction and the vehicle body coordinate system axial direction, which may result in a large mounting deviation angle.
No matter the installation deviation angle is large or small, the user needs to calibrate and compensate the installation deviation angle if obtaining high-precision position and posture information. Meanwhile, when the installation deviation angle is larger, the error introduced by directly solving the course angle by using GNSS speed information as an INS strapdown calculation initial value is larger, and at the moment, if the installation deviation angle is directly used as a state parameter estimation, the convergence time of filtering is influenced or the filtering is caused to converge to an error value, and finally the positioning error is larger.
The existing solution is that when an IMU module is installed, the X axis (Y axis) of an IMU coordinate system is coaxial with the advancing direction of a vehicle body so as to reduce an installation deviation angle; and the other method is that the position relation between the IMU coordinate system and the advancing direction of the vehicle body is not considered during installation, then the installation deviation angle of the IMU coordinate system and the vehicle body coordinate system is determined by the aid of a theodolite measurement system after the IMU module is installed, and then the course angle is determined according to the determined installation deviation angle. However, the first method has limitations on installation conditions, and requires a user to pay attention to the fact that the axial direction of the IMU is as parallel as possible to the advancing direction of the vehicle body when the IMU is installed; in the second mode, in the actual using process, a user needs to adopt the whole set of theodolite measuring system to measure the installation deviation angle every time, the cost is high, calibration needs to be carried out again after the installation of the IMU module is changed every time, the operation is complex, and the method is not suitable for mass users.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention aims to provide a vehicle-mounted IMU posture initialization method which can solve the problems of complex calibration conditions, high cost, strict installation requirements and the like in the prior art.
The second objective of the present invention is to provide a method for estimating an installation deviation angle, which can solve the problems of complicated calibration conditions, high cost, strict installation requirements, etc. in the prior art.
The invention also aims to provide GNSS/INS combined navigation equipment which can solve the problems of complex calibration conditions, higher cost, strict installation requirements and the like in the prior art.
The fourth objective of the present invention is to provide a storage medium, which can solve the problems of complicated calibration conditions, high cost, strict installation requirements, etc. in the prior art.
One of the purposes of the invention is realized by adopting the following technical scheme:
a vehicle IMU attitude initialization method is applied to vehicle-mounted GNSS/INS integrated navigation equipment, wherein a gyroscope and an accelerometer are mounted on the GNSS/INS integrated navigation equipment; the posture initialization method comprises the following steps:
and (3) pitch angle and roll angle calculation: the vehicle is stationary for several seconds, and a pitch angle and a roll angle are calculated according to the observation data of the accelerometer and are respectively recorded as an initial pitch angle and an initial roll angle of the GNSS/INS combined navigation;
course angle calculation: when the vehicle starts to move, acquiring the speed increment of an IMU module and the speed increment of a GNSS receiver, calculating to obtain a course angle by utilizing the observation data of a gyroscope and an accelerometer, an initial pitch angle and an initial roll angle according to a pre-integration principle, and recording as an initial course angle of GNSS/INS combined navigation; the speed increment of the IMU module refers to a difference between speeds of the IMU module at two different times, and the speed increment of the GNSS receiver refers to a difference between speeds of the GNSS receiver at two different times.
Further, the calculation formula of the initial roll angle is as follows:
Figure BDA0003598359200000031
the calculation formula of the initial pitch angle is as follows:
Figure BDA0003598359200000032
wherein, theta 0 To a pitch angle, λ 0 In order to form a transverse rolling angle,
Figure BDA0003598359200000033
represents the acceleration in the x direction under the b system,
Figure BDA0003598359200000034
represents the acceleration in the y direction under b,
Figure BDA0003598359200000035
represents the acceleration in the z direction under b;
the calculation formula of the initial course angle is as follows:
Figure BDA0003598359200000036
wherein the content of the first and second substances,
Figure BDA0003598359200000037
representing speed increment deltaV of GNSS receiver gnss Horizontal plane projection under the coordinate system of northeast; Δ V imu,x ，ΔV imu,y Representing IMU module speed increment Δ V imu Projection of the horizontal plane in the IMU coordinate system.
Further, the initial heading angle calculating step specifically includes:
firstly, respectively constructing a measurement model equation according to the gyroscope and the accelerometer, wherein the measurement model equation is specifically expressed as follows:
Figure BDA0003598359200000041
Figure BDA0003598359200000042
wherein the content of the first and second substances,
Figure BDA0003598359200000043
an angular velocity value, an acceleration value representing a measurement of the IMU;
Figure BDA0003598359200000044
a n representing the real angular velocity value and the acceleration value; b g 、b a Respectively representing the zero offset of a gyroscope and the zero offset of an accelerometer; eta g 、η a Respectively representing the noise of a gyroscope and the noise of an accelerometer;
Figure BDA0003598359200000045
representing the rotation matrix of the carrier coordinate system relative to the navigation coordinate system, g n Representing gravity in a navigation coordinate system; b represents a carrier coordinate system; n represents a navigation coordinate system;
then rotating the matrix
Figure BDA0003598359200000046
Is integrated with the velocity differential equation of the IMU, as follows:
Figure BDA0003598359200000047
Figure BDA0003598359200000048
and selecting a time period k epsilon [ i, j ], and transforming the formula (3) and the formula (4) to obtain the following formula:
Figure BDA0003598359200000049
Figure BDA00035983592000000410
then, the following equations are obtained by transforming equations (5) and (6):
Figure BDA00035983592000000411
Figure BDA00035983592000000412
considering the zero offset of the gyroscope and the zero offset of the accelerometer as being unchanged, and setting the noise of the gyroscope and the noise of the accelerometer to be zero, then in equation (8):
Figure BDA0003598359200000051
updated by formula (7);
Figure BDA0003598359200000052
representing a rotation matrix at an initial time; wherein the content of the first and second substances,
Figure BDA0003598359200000053
Figure BDA0003598359200000054
for the speed increment of the GNSS receiver, noted Δ V gnss ，
Figure BDA0003598359200000055
Is a pre-integration quantity of the IMU module, and is recorded as delta V imu ；
After the speed is subjected to the leveling processing, the initial course angle is obtained by the formula (8):
Figure BDA0003598359200000056
the second purpose of the invention is realized by adopting the following technical scheme:
a mounting deviation angle estimation method, comprising the steps of:
coarse alignment step: according to the vehicle-mounted IMU posture initialization method adopted by one purpose of the invention, the initial posture is obtained; the initial attitude comprises an initial pitch angle, an initial roll angle and an initial course angle; meanwhile, acquiring an initial position and an initial speed of the GNSS receiver;
an initialization step: taking the initial position, the initial speed and the initial posture as initial values updated by an INS strapdown algorithm;
and (3) updating an INS strapdown algorithm: obtaining the position, the speed and the attitude at the current moment according to the position, the speed and the attitude at the previous moment and the angular rate and the acceleration output by the IMU module at the current moment;
a state equation creating step: an INS position error equation, an INS speed error equation and an INS attitude error equation are constructed according to the position, the speed and the attitude at the current moment, a state error equation is constructed according to the INS position error equation, the INS speed error equation, the INS attitude error equation and an IMU device error equation, and the state error equation is discretized;
a measurement equation construction step: acquiring position quantity measurement, speed quantity measurement and odometer speed measurement of a GNSS receiver and respectively constructing corresponding measurement equations;
a Kalman filtering step: respectively carrying out time updating and measurement updating on the state equation and the measurement equation according to a Kalman filtering formula to obtain each state parameter, and correcting each state parameter in the system according to each state parameter obtained by calculation; and substituting each corrected state parameter into an INS strapdown algorithm to calculate the position, the speed and the posture of the next moment until the calculation is finished.
Further, the step of updating the INS strapdown algorithm specifically includes: the method comprises the steps of solving differential equations of the position, the speed and the attitude at the previous moment to predict the position, the speed and the attitude at the current moment; wherein, the total differential equation of the position, the speed and the attitude is as follows:
Figure BDA0003598359200000061
wherein the content of the first and second substances,
Figure BDA0003598359200000062
Figure BDA0003598359200000063
represents the angular velocity of the projection of b under b relative to n;
Figure BDA0003598359200000064
representing the posture transformation quaternion from b system rotation to n system rotation;
Figure BDA0003598359200000065
to represent
Figure BDA0003598359200000066
A conjugate quaternion of (a);
Figure BDA0003598359200000067
representing the specific force output by the accelerometer;
Figure BDA0003598359200000068
v n ，g n respectively representing a velocity vector and a gravity vector under a local navigation system;
Figure BDA0003598359200000069
R N representing the curvature radius of the prime movel circle; r M Representing the radius of curvature of the meridian.
Further, the state parameters comprise position error delta r, speed error delta v and attitude angle error
Figure BDA00035983592000000610
A gyroscope zero offset correction term delta bg, an accelerometer zero offset correction term delta ba, a gyroscope scale factor correction term delta sfg, an accelerometer scale factor correction term delta sfa, an installation angle error delta Ab2v from an IMU coordinate system to a vehicle body coordinate system, and a tire coefficient error term delta Tsf; and the mounting angle error from the IMU coordinate system to the vehicle body coordinate system comprises a roll angle error, a pitch angle error and a course angle error.
Further, the measurement equation comprises a GNSS position error measurement equation, a GNSS speed error measurement equation and an odometer speed error measurement equation;
the GNSS position error measurement equation is expressed as follows:
Figure BDA0003598359200000071
wherein the content of the first and second substances,
Figure BDA0003598359200000072
indicating that the strapdown algorithm updates the derived antenna phase center position,
Figure BDA0003598359200000073
indicating the position of the GNSS receiver output, H p A matrix of position error coefficients is represented,
Figure BDA0003598359200000074
representing an attitude error coefficient matrix;
the GNSS velocity error measurement equation is expressed as follows:
Figure BDA0003598359200000075
wherein the content of the first and second substances,
Figure BDA0003598359200000076
representing the velocity of the antenna phase center estimated by updating the strapdown algorithm under the local navigation system,
Figure BDA0003598359200000077
representing the speed of the output of the GNSS receiver,
Figure BDA0003598359200000078
the odometer velocity error measurement equation is expressed as follows:
Figure BDA0003598359200000079
wherein the content of the first and second substances,
Figure BDA00035983592000000710
representing the speed of the vehicle body coordinate system calculated by the strapdown algorithm,
Figure BDA00035983592000000711
indicating the speed of the odometer output, H v ，
Figure BDA00035983592000000712
H bg ，H δAb2v ，H δTsf Coefficient matrices each representing a corresponding error state quantity.
Further, the kalman filter formula is:
Figure BDA00035983592000000713
wherein the content of the first and second substances,
Figure BDA00035983592000000714
respectively representing the state vectors at time k and k +1,
Figure BDA00035983592000000715
respectively representing the mean square error matrix at the time of k and k +1,
Figure BDA00035983592000000716
the predicted state quantity is represented by a quantity of the predicted state,
Figure BDA00035983592000000717
representing the predicted mean square error matrix, phi k+1/k The state transition matrix, Γ, representing the time k-k +1 k+1/k Representing the system noise matrix at time k-k +1, H k+1 A measurement matrix representing the time k +1, Z k+1 Represents the measurement vector at time k +1, Q k Non-negative definite variance matrix, R, representing system noise at time k k+1 Positive definite variance matrix, K, representing the observed noise at time K +1 k+1 Denotes a gain matrix, and I denotes a unit matrix.
The third purpose of the invention is realized by adopting the following technical scheme:
a combined navigation device being a GNSS/INS combined navigation device for performing the steps of an on-board IMU attitude initialization method as employed for one of the purposes of the present invention or performing the steps of an installation deviation angle estimation method as employed for the second of the purposes of the present invention.
The fourth purpose of the invention is realized by adopting the following technical scheme:
a storage medium which is a computer-readable storage medium having stored thereon a computer program for executing by a processor the steps of an in-vehicle IMU attitude initialization method as employed for one of the objects of the present invention or an installation deviation angle estimation method as employed for a second of the objects of the present invention.
Compared with the prior art, the invention has the beneficial effects that:
according to the method, the initial pitch angle and the initial roll angle of the vehicle are obtained when the vehicle is stationary for several seconds, then the speed of a GNSS receiver and the data of an IMU (inertial measurement unit) module are obtained in the motion process of the vehicle, the speed increment obtained by the GNSS receiver is connected with the speed increment of the IMU module by utilizing the principle of pre-integration to calculate the initial course angle of the vehicle, and further the initial attitude of the vehicle is determined; meanwhile, the invention does not need to adopt a professional theodolite measuring system to measure the installation deviation angle.
Drawings
FIG. 1 is a flow chart of a vehicle IMU attitude initialization method provided by the present invention;
FIG. 2 is a flow chart of a method for estimating an installation deviation angle according to the present invention;
FIG. 3 is a schematic coordinate diagram of the IMU module according to the present invention, wherein the coordinate system is in the same direction as the advancing direction of the vehicle body;
fig. 4 is a schematic coordinate diagram of an included angle between a coordinate system of the IMU module and a vehicle body advancing direction provided by the present invention.
Detailed Description
The present invention will be further described with reference to the accompanying drawings and the detailed description, and it should be noted that any combination of the embodiments or technical features described below can be used to form a new embodiment without conflict.
The invention is applied to vehicle-mounted GNSS/INS combined navigation equipment, does not need to consider the installation position of an IMU module, obtains a pitch angle and a roll angle according to accelerometer data by keeping a vehicle still for a plurality of seconds, then, when the vehicle moves, the speed increment of the IMU module and the speed increment of the GNSS receiver are introduced into the calculation process of the heading angle by using the pre-integration principle to determine the initial heading angle of the GNS/INS combination, the initial course angle is applied to subsequent precise alignment to estimate an installation deviation angle (also called an installation angle) between the IMU module and the vehicle body, and the method does not need to rely on any external equipment to calibrate the installation deviation angle of the IMU coordinate system and the vehicle body coordinate system, thereby solving the problems of high equipment cost, complex operation and the like caused by the need of adopting a complex measurement calibration system to measure the installation deviation angle in the prior art; meanwhile, the invention does not need to consider the installation axial direction of the IMU module, has simple operation and improves the flexibility of installing the IMU by a user.
Preferably, the invention provides a preferred embodiment, a vehicle-mounted IMU attitude initialization method, which is applied to vehicle-mounted GNSS/INS integrated navigation equipment, wherein a gyroscope and an accelerometer are installed on the GNSS/INS integrated navigation equipment; as shown in fig. 1, the method comprises the following steps:
and step S11, the vehicle is stopped for several seconds, and the pitch angle and the roll angle are calculated according to the observation data of the accelerometer and recorded as the initial pitch angle and the initial roll angle of the GNSS/INS combination.
Wherein the initial roll angle λ 0 The calculation formula of (2) is as follows:
Figure BDA0003598359200000101
initial pitch angle θ 0 The calculation formula of (c) is:
Figure BDA0003598359200000102
and step S12, when the vehicle starts to move, acquiring the speed increment of the IMU module and the speed increment of the GNSS receiver, calculating a course angle by utilizing the observation data of the gyroscope and the accelerometer, the initial pitch angle and the initial roll angle according to a pre-integration principle, and recording the course angle as the initial course angle of the GNSS/INS combination.
Generally, the initial heading angle is determined by using the east and north speeds output by the GNSS receiver, and the axial direction of the IMU module is required to be parallel to the longitudinal axis (the heading direction) of the vehicle coordinate system, as shown in fig. 3: wherein the EON coordinate system represents a projection of the northeast sky coordinate system on a horizontal plane, the XOY coordinate system represents a projection of the IMU coordinate system on the horizontal plane, and OA represents a forward direction of the vehicle body.
Then, at this time, the heading angle psi 0 Comprises the following steps:
Figure BDA0003598359200000103
wherein, V GNSS,E 、V GNSS,N Respectively representing the projections of the speed output by the GNSS receiver on an E axis and an N axis under an EON coordinate system.
When the axial direction of the IMU module is approximately parallel to the longitudinal axis (the advancing direction of the vehicle body) of the vehicle body coordinate system, the installation deviation angle between the IMU module and the advancing direction of the vehicle body is small, the initial course angle can be calculated by directly utilizing the speed of the GNSS receiver, and the installation deviation angle can be directly regarded as small amount to be directly estimated in the subsequent fine alignment process. However, when the axial direction of the IMU module and the advancing direction of the vehicle body have a large installation deviation angle, as shown in fig. 4, if the horizontal speed output by the GNSS receiver is continuously and independently used to calculate the heading angle, the obtained heading angle is not a real heading angle, and has a large deviation from a real value. Therefore, in the coarse alignment stage, the initial course angle is calculated by using the pre-integration principle in the visual field to connect the data increment of the IMU module with the speed increment of the GNSS receiver so as to obtain the course angle closer to the true value, thereby determining the initial course angle of the vehicle.
Preferably, the invention provides a specific derivation process of the heading angle, which specifically comprises the following steps:
A. firstly, respectively constructing corresponding measurement model equations according to a gyroscope and an accelerometer which are installed on GNSS/INS integrated navigation equipment, wherein the equations are as follows:
Figure BDA0003598359200000111
Figure BDA0003598359200000112
wherein，
Figure BDA0003598359200000113
An angular velocity value, an acceleration value representing a measurement of the IMU;
Figure BDA0003598359200000114
a n representing the real angular velocity value and the acceleration value; b g 、b a Respectively representing the zero offset of a gyroscope and the zero offset of an accelerometer; eta g 、η a Respectively representing the noise of a gyroscope and the noise of an accelerometer;
Figure BDA0003598359200000115
representing the rotation matrix of the carrier coordinate system relative to the navigation coordinate system, g n Representing gravity in a navigation coordinate system; b represents a carrier coordinate system; n denotes a navigation coordinate system.
B. Rotating matrix
Figure BDA0003598359200000116
The differential equation of (a) and the velocity differential equation of the IMU are integrated, the following equation can be obtained:
Figure BDA0003598359200000117
Figure BDA0003598359200000118
C. selecting a time period k e [ i, j ], the formula (3) and the formula (4) can be converted into:
Figure BDA0003598359200000119
Figure BDA00035983592000001110
D. a simple transformation of equations (5) and (6) yields the following equations:
Figure BDA0003598359200000121
Figure BDA0003598359200000122
as can be seen from equations (7) and (8):
the zero offset of the gyroscope and the zero offset of the accelerometer can be obtained by being static and can be regarded as constant in a short time, meanwhile, the noise of the gyroscope and the noise of the accelerometer are ignored,
Figure BDA0003598359200000123
can be obtained by updating through the formula (7),
Figure BDA0003598359200000124
represents a rotation matrix at an initial time and
Figure BDA0003598359200000125
Figure BDA0003598359200000126
Figure BDA0003598359200000127
for the speed increment of the GNSS receiver, noted as Δ V gnss ，
Figure BDA0003598359200000128
Is a pre-integration quantity of the IMU module, noted as Δ V imu 。
Then equation (8) is converted to the following equation:
Figure BDA0003598359200000129
E. the speed is processed by the process of the leveling treatment,and will be
Figure BDA00035983592000001210
Substituting into the formula (9) to obtain the initial course angle as:
Figure BDA00035983592000001211
wherein the content of the first and second substances,
Figure BDA00035983592000001212
to represent
Figure BDA00035983592000001213
I.e., the projection of the GNSS velocity increment on the N-axis in the EON coordinate system, and the like.
The speed leveling treatment refers to projecting the speed increment of the GNSS and IMU module into a horizontal coordinate system.
After the initial attitude is determined, GNSS/INS combined filtering solution can be realized, that is, another embodiment of the present invention provides a method for estimating an installation deviation angle, as shown in fig. 2, including the following steps:
and step S21, acquiring the initial attitude and the initial position and the initial speed of the GNSS receiver.
Wherein the initial attitude comprises an initial pitch angle, an initial roll angle and an initial heading angle. The initial position and initial velocity may be directly output by the GNSS receiver.
And step S22, taking the initial position, the initial speed and the initial posture as initial values updated by the INS strapdown algorithm in the running process of the vehicle.
And step S23, obtaining the position, the speed and the attitude at the current moment according to the position, the speed and the attitude at the previous moment and the angular rate and the acceleration output by the IMU module at the current moment.
Preferably, the INS strapdown algorithm update is performed by solving differential equations for position, velocity, and attitude to predict the position, velocity, and attitude for the next epoch.
Wherein, the total differential equation of the position, the speed and the attitude is as follows:
Figure BDA0003598359200000131
wherein the content of the first and second substances,
Figure BDA0003598359200000132
Figure BDA0003598359200000133
represents the angular velocity of the projection of b under b relative to n;
Figure BDA0003598359200000134
representing the posture transformation quaternion from b system rotation to n system rotation;
Figure BDA0003598359200000135
to represent
Figure BDA0003598359200000136
A conjugate quaternion of (a);
Figure BDA0003598359200000137
representing the specific force output by the accelerometer;
Figure BDA0003598359200000138
v n ，g n respectively representing a velocity vector and a gravity vector under a local navigation system;
Figure BDA0003598359200000141
R N representing the curvature radius of the prime zone;
R M representing the radius of curvature of the meridian.
And S24, constructing an INS position error equation, an INS speed error equation and an INS attitude error equation according to the position, the speed and the attitude of the current moment, constructing a state error equation according to the INS position error equation, the INS speed error equation, the INS attitude error equation and the IMU device error equation, and discretizing the state error equation.
Preferably, the state parameter error term of the system obtained after discretization processing of the constructed state parameter error term equation can be represented by a 26-dimensional vector, which is as follows:
Figure BDA0003598359200000142
wherein, δ r is a position error (3 dimension); δ v is the velocity error (3 dimensions);
Figure BDA0003598359200000143
attitude angle error (3-dimensional, roll, pitch, course); δ bg is a gyroscope zero-bias correction term (3-dimension); δ ba is the accelerometer zero offset correction term (3-dimensional); δ sfg is the gyroscope scale factor correction term (3-dimensional); δ sfa is the accelerometer scale factor correction term (3 dimensions); delta Ab2v is the mounting angle error (3-dimensional, roll, pitch and heading) from the IMU coordinate system to the vehicle body coordinate system; δ Tsf is a tire coefficient error term (left and right tire coefficients).
Therefore, the modeling of the position error, the speed error and the attitude error can be modeled into a first-order Gaussian Markov process mainly according to respective INS error equations, a gyroscope zero-bias correction term, an accelerometer zero-bias correction term, a gyroscope scale factor correction term and an accelerometer scale factor correction term, and the mounting deviation angle of an IMU coordinate system relative to a vehicle body coordinate system and the tire scale coefficient can be modeled into constant values.
The continuous-type kinetic equation can be simply expressed as follows:
Figure BDA0003598359200000144
wherein the content of the first and second substances,
Figure BDA0003598359200000151
G n (t) denotes a noise matrix.
And step S25, obtaining the position quantity measurement, the speed quantity measurement and the odometer speed measurement of the GNSS receiver and respectively constructing corresponding measurement equations.
Considering the lever arm influence, the position error measurement equation is derived as follows:
Figure BDA0003598359200000152
therefore, the position measurement equation of the GNSS/INS integrated navigation device is expressed as follows:
Figure BDA0003598359200000153
wherein the content of the first and second substances,
Figure BDA0003598359200000154
the antenna phase center position calculated by the strapdown calculation algorithm is shown,
Figure BDA0003598359200000155
indicating the position of the GNSS/INS integrated navigation device output, H p A matrix of position error coefficients is represented,
Figure BDA0003598359200000156
representing a matrix of attitude error coefficients.
Similarly, the velocity error measurement equation is derived by considering the influence of the lever arm value:
Figure BDA0003598359200000157
wherein the content of the first and second substances,
Figure BDA0003598359200000158
antenna phase representing INS estimationThe speed of the center under the local navigation system,
Figure BDA0003598359200000159
representing the speed of the output of the GNSS receiver,
Figure BDA00035983592000001510
during the speed measurement of the odometer, the influence of the installation deviation angle and the tire coefficient between the IMU coordinate system and the vehicle body coordinate system needs to be considered, and the speed measurement equation of the odometer is derived to obtain:
Figure BDA00035983592000001511
the velocity measurement equation for the odometer is therefore expressed as follows:
Figure BDA0003598359200000161
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003598359200000162
representing the speed calculated by the strapdown calculation algorithm in the vehicle body coordinate system,
Figure BDA0003598359200000163
indicating the speed of the odometer output, H v 、
Figure BDA0003598359200000164
H bg 、H δAb2v 、H δTsf Coefficient matrices each representing a corresponding error state quantity.
Step S26, respectively carrying out time updating and measurement updating on the state equation and the measurement equation according to a Kalman filtering formula to obtain each state parameter, and correcting each state parameter in the system according to each state parameter obtained by calculation; and substituting each corrected state parameter into an INS strapdown algorithm to calculate the position, the speed and the posture of the next moment until the calculation is finished.
The formula of the kalman filter is specifically:
Figure BDA0003598359200000165
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003598359200000166
respectively representing the state vectors at time k and k +1,
Figure BDA0003598359200000167
respectively representing the mean square error matrix at the time of k and k +1,
Figure BDA0003598359200000168
the predicted state quantity is represented by a quantity of the predicted state,
Figure BDA0003598359200000169
representing the predicted mean square error matrix, phi k+1/k The state transition matrix, Γ, representing the time k-k +1 k+1/k Representing the system noise matrix at time k-k +1, H k+1 A measurement matrix representing the time k +1, Z k+1 Represents the measurement vector at time k +1, Q k Non-negative-constant variance matrix, R, representing system noise at time k k+1 Positive definite variance matrix, K, representing the observed noise at time K +1 k+1 Denotes a gain matrix, and I denotes a unit matrix.
Preferably, based on the two foregoing embodiments, the present invention further provides a combined navigation device, which is a GNSS/INS combined navigation device, and is configured to execute an on-vehicle IMU attitude initialization method or an installation deviation angle estimation method.
Preferably, the present invention also provides a storage medium which is a computer readable storage medium having stored thereon a computer program, which when executed by a processor, performs the steps of a vehicle-mounted IMU attitude initialization method or a mounting deviation angle estimation method according to the present invention.
The above embodiments are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited thereby, and any insubstantial changes and substitutions made by those skilled in the art based on the present invention are within the protection scope of the present invention.

Claims (10)

1. A vehicle IMU attitude initialization method is applied to vehicle-mounted GNSS/INS integrated navigation equipment, wherein a gyroscope and an accelerometer are mounted on the GNSS/INS integrated navigation equipment; the method for initializing the posture is characterized by comprising the following steps of:
and (3) pitch angle and roll angle calculation: the vehicle is stationary for several seconds, and a pitch angle and a roll angle are calculated according to the observation data of the accelerometer and are respectively recorded as an initial pitch angle and an initial roll angle of the GNSS/INS combined navigation;
course angle calculation: when the vehicle starts to move, acquiring the speed increment of an IMU module and the speed increment of a GNSS receiver, calculating to obtain a course angle by utilizing the observation data of a gyroscope and an accelerometer, an initial pitch angle and an initial roll angle according to a pre-integration principle, and recording as an initial course angle of GNSS/INS combined navigation; the speed increment of the IMU module refers to a difference between speeds of the IMU module at two different times, and the speed increment of the GNSS receiver refers to a difference between speeds of the GNSS receiver at two different times.
2. The method of initializing IMU attitude of claim 1, wherein the initial roll angle is calculated as:
Figure FDA0003598359190000011
the calculation formula of the initial pitch angle is as follows:
Figure FDA0003598359190000012
wherein, theta 0 Is a pitch angle，λ 0 In order to form a transverse rolling angle,
Figure FDA0003598359190000013
represents the acceleration in the x direction under the b system,
Figure FDA0003598359190000014
represents the acceleration in the y direction under b,
Figure FDA0003598359190000015
represents the acceleration in the z direction under b;
the calculation formula of the initial course angle is as follows:
Figure FDA0003598359190000016
wherein the content of the first and second substances,
Figure FDA0003598359190000021
representing the velocity delta deltaV of a GNSS receiver gnss Horizontal plane projection under the coordinate system of northeast; Δ V imu,x ，ΔV imu,y Indicating IMU module speed increment Δ V imu Projection of the horizontal plane in the IMU coordinate system.
3. The vehicle-mounted IMU attitude initialization method according to claim 2, wherein the initial heading angle calculation step specifically includes:
firstly, respectively constructing a measurement model equation according to the gyroscope and the accelerometer, wherein the measurement model equation is specifically expressed as follows:
Figure FDA0003598359190000022
Figure FDA0003598359190000023
wherein the content of the first and second substances,
Figure FDA0003598359190000024
an angular velocity value, an acceleration value representing a measurement of the IMU;
Figure FDA0003598359190000025
a n representing the real angular velocity value and the acceleration value; b is a mixture of g 、b a Respectively representing the zero offset of a gyroscope and the zero offset of an accelerometer; eta g 、η a Respectively representing the noise of a gyroscope and the noise of an accelerometer;
Figure FDA0003598359190000026
representing the rotation matrix of the carrier coordinate system relative to the navigation coordinate system, g n Representing gravity in a navigation coordinate system; b represents a carrier coordinate system; n represents a navigation coordinate system;
then rotating the matrix
Figure FDA0003598359190000027
The differential equation of (a) and the velocity differential equation of the IMU are integrated, and are specifically expressed as follows:
Figure FDA0003598359190000028
Figure FDA0003598359190000029
and selecting a time period k epsilon [ i, j ], and transforming the formula (3) and the formula (4) to obtain the following formula:
Figure FDA00035983591900000210
Figure FDA00035983591900000211
then, the following equations are obtained by transforming equations (5) and (6):
Figure FDA0003598359190000031
Figure FDA0003598359190000032
considering the zero offset of the gyroscope and the zero offset of the accelerometer as being unchanged, and setting the noise of the gyroscope and the noise of the accelerometer to be zero, then in equation (8):
Figure FDA0003598359190000033
updated by formula (7);
Figure FDA0003598359190000034
representing a rotation matrix at an initial time; wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003598359190000035
Figure FDA0003598359190000036
for the speed increment of the GNSS receiver, noted as Δ V gnss ，
Figure FDA0003598359190000037
Is a pre-integration quantity of the IMU module, noted as Δ V imu ；
After the speed is subjected to the leveling processing, the initial course angle is obtained by the formula (8):
Figure FDA0003598359190000038
4. a mounting deviation angle estimation method, characterized by comprising the steps of:
coarse alignment step: the vehicle IMU attitude initialization method according to any one of claims 1-3, obtaining an initial attitude; the initial attitude comprises an initial pitch angle, an initial roll angle and an initial course angle; meanwhile, acquiring an initial position and an initial speed of the GNSS receiver;
an initialization step: taking the initial position, the initial speed and the initial posture as initial values updated by an INS strapdown algorithm;
and (3) updating an INS strapdown algorithm: obtaining the position, the speed and the attitude at the current moment according to the position, the speed and the attitude at the previous moment and the angular rate and the acceleration output by the IMU module at the current moment;
a state equation creating step: an INS position error equation, an INS speed error equation and an INS attitude error equation are constructed according to the position, the speed and the attitude at the current moment, a state error equation is constructed according to the INS position error equation, the INS speed error equation, the INS attitude error equation and an IMU device error equation, and the state error equation is discretized;
a measurement equation construction step: acquiring position quantity measurement, speed quantity measurement and odometer speed measurement of a GNSS receiver and respectively constructing corresponding measurement equations;
a Kalman filtering step: respectively carrying out time updating and measurement updating on the state equation and the measurement equation according to a Kalman filtering formula to obtain each state parameter, and correcting each state parameter in the system according to each state parameter obtained by calculation; and substituting each corrected state parameter into an INS strapdown algorithm to calculate the position, the speed and the posture of the next moment until the calculation is finished.
5. The installation deviation angle estimation of claim 4, wherein the INS strapdown algorithm updating step specifically comprises: the method comprises the steps of solving differential equations of the position, the speed and the attitude at the previous moment to predict the position, the speed and the attitude at the current moment; wherein, the total differential equation of the position, the speed and the attitude is as follows:
Figure FDA0003598359190000041
wherein the content of the first and second substances,
Figure FDA0003598359190000042
Figure FDA0003598359190000043
represents the angular velocity of the projection of b under b relative to n;
Figure FDA0003598359190000044
representing the posture transformation quaternion from b system rotation to n system rotation;
Figure FDA0003598359190000045
to represent
Figure FDA0003598359190000046
A conjugate quaternion of (a);
Figure FDA0003598359190000047
representing the specific force output by the accelerometer;
Figure FDA0003598359190000048
v n ，g n respectively representing a velocity vector and a gravity vector under a local navigation system;
Figure FDA0003598359190000051
R N representing the curvature radius of the prime movel circle; r M Representing the radius of curvature of the meridian.
6. The installation deviation angle estimation method according to claim 4, wherein the state parameters include a position error δ r, a velocity error δ v, an attitude angle error Φ, a gyro zero bias correction term δ bg, an accelerometer zero bias correction term δ ba, a gyro scale factor correction term δ sfg, an accelerometer scale factor correction term δ sfa, an installation angle error δ Ab2v of an IMU coordinate system to a vehicle body coordinate system, a tire coefficient error term δ Tsf; and the mounting angle error from the IMU coordinate system to the vehicle body coordinate system comprises a roll angle error, a pitch angle error and a course angle error.
7. The method of claim 4, wherein the measurement equations include a GNSS position error measurement equation, a GNSS velocity error measurement equation, and an odometer velocity error measurement equation;
the GNSS position error measurement equation is expressed as follows:
Figure FDA0003598359190000052
wherein the content of the first and second substances,
Figure FDA0003598359190000053
indicating that the strapdown algorithm updates the derived antenna phase center position,
Figure FDA0003598359190000054
indicating the position of the GNSS receiver output, H p A matrix of position error coefficients is represented,
Figure FDA0003598359190000055
representing an attitude error coefficient matrix;
the GNSS velocity error measurement equation is expressed as follows:
Figure FDA0003598359190000056
wherein the content of the first and second substances,
Figure FDA0003598359190000057
representing the velocity of the antenna phase center estimated by updating the strapdown algorithm under the local navigation system,
Figure FDA0003598359190000058
representing the speed of the output of the GNSS receiver,
Figure FDA0003598359190000059
the odometer velocity error measurement equation is expressed as follows:
Figure FDA00035983591900000510
wherein the content of the first and second substances,
Figure FDA00035983591900000511
representing the speed of the vehicle body coordinate system calculated by the strapdown algorithm,
Figure FDA00035983591900000512
indicating the speed of the odometer output, H v ，
Figure FDA00035983591900000513
H bg ，H δAb2v ，H δTsf Coefficient matrices each representing a corresponding error state quantity.
8. The installation deviation angle estimation method of claim 4, wherein the Kalman filter formula is:
Figure FDA0003598359190000061
wherein the content of the first and second substances,
Figure FDA0003598359190000062
respectively representing the state vectors at the time points k and k +1,
Figure FDA0003598359190000063
respectively representing the mean square error matrix at the time of k and k +1,
Figure FDA0003598359190000064
the predicted state quantity is represented by a quantity of the predicted state,
Figure FDA0003598359190000065
representing the predicted mean square error matrix, phi k+1/k Representing the state transition matrix at time k-k +1, Γ k+1/k Representing the system noise matrix at time k-k +1, H k+1 A measurement matrix representing the time k +1, Z k+1 Represents the measurement vector at time k +1, Q k Non-negative-constant variance matrix, R, representing system noise at time k k+1 Positive definite variance matrix, K, representing the observed noise at time K +1 k+1 Denotes a gain matrix, and I denotes a unit matrix.
9. A combined navigation device, which is a GNSS/INS combined navigation device, wherein the GNSS/INS combined navigation device is configured to perform the steps of an on-board IMU attitude initialization method according to any one of claims 1 to 3 or perform the steps of an installation deviation angle estimation method according to any one of claims 4 to 8.
10. A storage medium being a computer readable storage medium having a computer program stored thereon, wherein the computer program is executed by a processor to perform a method of vehicle-mounted IMU pose initialization according to any of claims 1-3 or to perform the steps of a method of droop angle estimation according to any of claims 4-8.
CN202210398274.5A 2022-04-15 2022-04-15 Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device Pending CN114964222A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210398274.5A CN114964222A (en) 2022-04-15 2022-04-15 Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210398274.5A CN114964222A (en) 2022-04-15 2022-04-15 Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device

Publications (1)

Publication Number Publication Date
CN114964222A true CN114964222A (en) 2022-08-30

Family

ID=82977487

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210398274.5A Pending CN114964222A (en) 2022-04-15 2022-04-15 Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device

Country Status (1)

Country Link
CN (1) CN114964222A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113532477A (en) * 2021-07-15 2021-10-22 青岛迈金智能科技有限公司 Riding stopwatch equipment and automatic calibration method for initial posture of riding stopwatch
CN115727842A (en) * 2022-11-04 2023-03-03 中山大学 Rapid alignment method and system for unmanned aerial vehicle, computer equipment and storage medium
CN116718196A (en) * 2023-08-09 2023-09-08 腾讯科技（深圳）有限公司 Navigation method, device, equipment and computer readable storage medium

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113532477A (en) * 2021-07-15 2021-10-22 青岛迈金智能科技有限公司 Riding stopwatch equipment and automatic calibration method for initial posture of riding stopwatch
CN115727842A (en) * 2022-11-04 2023-03-03 中山大学 Rapid alignment method and system for unmanned aerial vehicle, computer equipment and storage medium
CN116718196A (en) * 2023-08-09 2023-09-08 腾讯科技（深圳）有限公司 Navigation method, device, equipment and computer readable storage medium
CN116718196B (en) * 2023-08-09 2023-10-20 腾讯科技（深圳）有限公司 Navigation method, device, equipment and computer readable storage medium

Similar Documents

Publication Publication Date Title
CN110501024B (en) Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN110221333B (en) Measurement error compensation method of vehicle-mounted INS/OD integrated navigation system
CN108180925B (en) Odometer-assisted vehicle-mounted dynamic alignment method
CN110221332B (en) Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation
JP6094026B2 (en) Posture determination method, position calculation method, and posture determination apparatus
CN112629538A (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN101963513B (en) Alignment method for eliminating lever arm effect error of strapdown inertial navigation system (SINS) of underwater carrier
CN111121766B (en) Astronomical and inertial integrated navigation method based on starlight vector
CN113063429B (en) Self-adaptive vehicle-mounted integrated navigation positioning method
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN109596144B (en) GNSS position-assisted SINS inter-travel initial alignment method
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN111024074B (en) Inertial navigation speed error determination method based on recursive least square parameter identification
CN109612460B (en) Plumb line deviation measuring method based on static correction
CN113340298B (en) Inertial navigation and dual-antenna GNSS external parameter calibration method
CN111678514B (en) Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN114935345A (en) Vehicle-mounted inertial navigation mounting angle error compensation method based on pattern recognition
CN114526731A (en) Inertia combination navigation direction positioning method based on moped
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN107764268B (en) Method and device for transfer alignment of airborne distributed POS (point of sale)
CN114111792B (en) Vehicle-mounted GNSS/INS/odometer integrated navigation method
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN113932803B (en) Inertial/geomagnetic/satellite integrated navigation system suitable for high-dynamic aircraft

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication