A positioning method based on wheel speed inertial navigation joint pre-integration and tight integration with RTK

CN115507848BActive Publication Date: 2024-11-05SUZHOU HUAMI NAVIGATION TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202211402919.4
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-11-09
Publication Date
2024-11-05
Estimated Expiration
2042-11-09

AI Technical Summary

Technical Problem

The prior art is difficult to maintain high-precision positioning for a long time or under occlusion environment in the vehicle autonomous driving, especially due to the rapid accumulation of inertial navigation errors and the reduction of RTK positioning accuracy with the increase of distance.

Method used

Using a positioning method based on wheel speed inertia combined with pre-integration and tightly combined with RTK, the positioning method is adopted. By receiving satellite navigation data, constructing observation residuals, combining wheel speed meter and inertia sensor for pre-integration, the correction factor is generated to correct satellite positioning and extend inertia. Trusted time for navigation and improve positioning accuracy.

Benefits of technology

Effectively offset the superposition error of inertial navigation data, extend the divergence time of the INS algorithm, realize long-term high-precision positioning, and improve positioning accuracy and trusted time.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115507848B_ABST
    Figure CN115507848B_ABST
Patent Text Reader

Abstract

The present invention discloses a positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK, comprising the following steps: step 1, receiving inertial sensor and wheel speed meter data, and performing joint pre-integration to generate a pre-integration residual containing the overall error of the sensor and the wheel speed meter, receiving satellite navigation data and constructing an observation residual representing the satellite navigation error; step 2, tightly combining the pre-integration residual with the observation residual to generate a correction factor, correcting the satellite navigation data according to the correction factor and generating the motion state of the device. The present invention can tightly combine the pre-integration of the wheel speed meter with the IMU to continuously compensate for the accumulated error of the IMU, improve the performance of the pre-integration algorithm, and achieve long-term high-precision positioning.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK. Background Art

[0002] When a vehicle is driving automatically, it is essential to obtain the position coordinates, speed, attitude and other states of the unmanned vehicle, and in order to accurately control the unmanned vehicle's automatic driving, the positioning accuracy of the unmanned vehicle has high requirements. Satellite navigation performs positioning by measuring the pseudo-range between the satellite and the receiver, but satellite positioning usually has multiple error factors, such as: receiver clock error, satellite clock error, multipath effect error, channel error, ephemeris error, internal noise error, etc., which will affect the positioning accuracy of the satellite.

[0003] Conventional precision improvement is achieved by combining RTK technology to better eliminate errors in satellite navigation and improve positioning accuracy. However, the principle of RTK positioning method is to measure the distance between multiple satellites and GNSS receivers to offset the errors in single satellite positioning. However, when there is an obstruction between the receiver and the satellite, the ranging accuracy of RTK will be affected, resulting in a decrease in overall positioning accuracy. Moreover, RTK positioning technology is based on the fact that the errors of the mobile station and the base station are very similar. It uses GNSS carrier phase observations for real-time dynamic positioning. As the distance between the base station and the mobile station increases, the positioning accuracy will become lower and lower.

[0004] In order to overcome the above problems, it is proposed to combine satellite positioning technology with inertial navigation, and improve the accuracy of satellite positioning by combining satellite positioning with inertial positioning. However, inertial navigation uses an inertial measurement unit (IMU) to measure its own angular velocity and acceleration, and then uses an inertial navigation algorithm (INS) to achieve short-distance precise positioning. By combining satellite positioning and navigation technology with inertial navigation technology (correcting the satellite positioning error), the positioning accuracy can be improved.

[0005] However, the INS algorithm will diverge very quickly. The farther the vehicle moves, the greater the error of inertial navigation, resulting in a shorter INS trust time. However, if a high-precision IMU is used, it is relatively expensive. Low-cost IMUs have poor accuracy and will drift during long-term navigation and positioning, resulting in increasing error accumulation. The shorter the INS trust time, the lower the efficiency of improving positioning accuracy. Summary of the invention

[0006] In view of this, the problem to be solved by the present invention is to provide a positioning method based on the joint pre-integration of wheel speed inertial navigation and tightly combined with RTK, which can tightly combine the pre-integration of the wheel speed meter and IMU to continuously compensate for the error accumulated by the IMU, improve the performance of the pre-integration algorithm, and achieve long-term high-precision positioning.

[0007] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0008] A positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK, step 1, receiving satellite navigation data and constructing an observation residual representing the satellite navigation error;

[0009] Step 2: The mileage increment in each time period is obtained by the wheel speed meter alone, and the mileage increment is obtained by pre-integration with the inertial sensor to generate a pre-integration residual including the overall error of the sensor and the wheel speed meter.

[0010] Step 3: The pre-integrated residuals are tightly combined with the observed residuals to generate correction factors, and the satellite positioning is corrected according to the correction factors.

[0011] Furthermore, the joint pre-integration includes a velocity increment β, a displacement increment α, a posture change q and a mileage increment s, and constructs propagation equations of β, α, q and s respectively. The propagation equation of the mileage increment s is:

[0012] in: It represents the displacement between time k-1 and time i in the inertial navigation coordinate system b. represents the displacement between time k-1 and time i-1 in the inertial navigation coordinate system b. Indicates the displacement between time i-1 and time i in the inertial navigation coordinate system b, so as to obtain the mileage increment in each period separately.

[0013] The propagation equation of the mileage increment is:

[0014] in and They are all rotation matrices, used to transform the device coordinate system v to the inertial navigation coordinate system b. Indicates the displacement between time i-1 and time i in the device coordinate system v. It represents the arm correction error amount used to correct the position error between the wheel speed meter and the inertial sensor in the inertial navigation coordinate system b.

[0015] Furthermore, the observation residuals include double-difference pseudorange observation residuals and double difference phase observation residual According to the joint pre-integration, the IMU / wheel speed meter joint pre-integration residual in the satellite coordinate system is constructed The correction factor is:

[0016]

[0017] Furthermore, the IMU / wheel speed meter joint pre-integration residual in represents the covariance matrix of the IMU / wheel speedometer joint pre-integrated observation factor in the geodetic coordinate system, Represents the residual matrix including position change residual, velocity change residual, attitude change residual, accelerometer bias residual, gyroscope bias residual and wheel speed meter displacement residual.

[0018] Furthermore, an initialization strategy is added before step 1, and the initialization strategy includes obtaining initial coordinates and initial posture of the device;

[0019] The initial coordinates include obtaining several groups of satellite navigation positioning coordinate data and taking the average value thereof, obtaining accelerometer data to calculate the gravity vector, and obtaining the angle between the gravity vector and the Z axis and OXY plane of the device coordinate system to generate initial states including the roll angle and the pitch angle respectively.

[0020] The advantages and positive effects of the present invention are:

[0021] By adding the mileage increment propagation formula obtained by the wheel speed meter to the joint pre-integration of inertial navigation, and the mileage increment propagation formula obtains the mileage increment in each time period separately, and can effectively offset the superposition error caused by the superposition of inertial navigation data in that time period, so as to continuously compensate for the error accumulated by the IMU, extend the divergence time of the INS algorithm, and then improve the performance of the pre-integration algorithm, to achieve long-term and effective high-precision positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0023] Figure 1 It is an overall flow chart of a positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK according to the present invention. DETAILED DESCRIPTION

[0024] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0025] It should be noted that when a component is referred to as being "fixed to" another component, it may be directly on the other component or there may also be a component centered. When a component is considered to be "connected to" another component, it may be directly connected to the other component or there may also be a component centered. When a component is considered to be "set on" another component, it may be directly set on the other component or there may also be a component centered. The terms "vertical", "horizontal", "left", "right" and similar expressions used herein are for illustrative purposes only.

[0026] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more related listed items.

[0027] The present invention provides a positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK, such as Figure 1 As shown in the figure, a satellite sensor (GNSS receiver) for satellite positioning is installed on the device, and a double difference observation equation (double difference pseudo-range observation equation) is constructed. The double difference observation equation is used to calculate the position coordinates of the mobile device in the satellite coordinate system, and then the double difference pseudo-range observation residual is constructed based on the double difference pseudo-range observation equation. The double-difference pseudorange observation residual is used to represent the position error that will be generated when positioning through pseudorange (that is, the error generated when positioning by satellite).

[0028] In order to improve the positioning accuracy of the satellite, RTK technology is used for positioning, including constructing a double-difference carrier phase observation equation, and constructing a double-difference phase observation residual based on the double-difference carrier phase observation equation. Similarly, the double-difference phase observation residual is used to represent the amount of error generated during the positioning process.

[0029] During positioning, the GNSS receiver receives positioning data, substitutes the positioning data into the double-difference pseudorange observation residuals and the double-difference phase observation residuals to calculate the positioning error (also called the first correction amount), and substitutes the positioning data into the double-difference pseudorange observation equation and the double-difference carrier phase observation equation to generate positioning coordinates, and corrects the positioning coordinates using the error amount to improve satellite positioning accuracy.

[0030] When the device moves to an obstructed position, the accuracy of the first correction value decreases, affecting the positioning accuracy. To further improve the positioning accuracy of the device, inertial sensors (gyroscopes and accelerometers) and wheel speed meters are also installed on the device. The gyroscope is used to obtain the tilt of the device (to determine the posture of the device), the accelerometer is used to obtain the acceleration of the device's movement (to determine the moving distance of the device or the position vector of the device), and the wheel speed meter obtains the travel distance of the device. And jointly construct the IMU / wheel speed meter joint pre-integration formula:

[0031]

[0032] Among them are: the pre-integral equations representing the velocity increment β, the displacement increment α, the attitude change q and the mileage increment s, where: It represents the change of velocity between time k-1 and time i in the inertial coordinate system (obtained from the parameters of the accelerometer). Indicates the change in position between time k-1 and time i in the inertial coordinate system (obtained from the parameters of the accelerometer). It represents the change of attitude between time k-1 and time i in the inertial coordinate system (obtained from the parameters of the gyroscope).

[0033] The displacement change formula for calculating the distance displacement is added to the joint pre-integration. The displacement of the distance between time k-1 and time i in the inertial coordinate system (obtained from the parameters of the wheel speed meter) can be calculated separately between the two moments (not the overall displacement, to avoid the accumulation of errors caused by data superposition). By adding the displacement change of the device recorded by the wheel speed meter in the joint pre-integration, the error increment in the inertial positioning can be offset, the divergence time of the inertial positioning can be extended, and the pre-integration accuracy can be effectively improved.

[0034] The propagation equation is in It represents the displacement between time k-1 and time i-1 in the inertial navigation coordinate system b. It represents the displacement between time i-1 and time i in the inertial navigation coordinate system b, so as to separately obtain the displacement between each time k-1 and time i-1 and time i-1 and superimpose them. By separately obtaining the displacement between two adjacent time periods (the separate displacement of the device from time k-1 to time i-1 and time i-1 to time i), instead of separately obtaining the displacement of the device at time k-1 and the displacement of the device at time i, the displacement of the device between time k-1 and time i is calculated. This greatly reduces the superposition error of the wheel speed meter due to the superposition of the distance, and further improves the accuracy of the IMU / wheel speed meter joint pre-integration (offsetting part of the superposition error of the inertial navigation).

[0035]

[0036] in and They are all rotation matrices, used to transform the device coordinate system v to the inertial navigation coordinate system b. Indicates the displacement between time i-1 and time i in the device coordinate system v. It represents the arm correction error amount used to correct the position error between the wheel speed meter and the inertial sensor in the inertial navigation coordinate system b.

[0037] The IMU / wheel speedometer joint pre-integration residual is constructed based on the IMU / wheel speedometer joint pre-integration, and its formula is: in represents the covariance matrix of the IMU / wheel speedometer joint pre-integrated observation factor in the geodetic coordinate system, Represents the residual matrix including position change residual, velocity change residual, attitude change residual, accelerometer bias residual, gyroscope bias residual and wheel speed meter displacement residual.

[0038] The residual matrix is:

[0039]

[0040] in Represents the attitude rotation matrix between the inertial navigation coordinate system at the northeast celestial coordinate system N to the time k-1; It represents the projection of the position vector of the inertial navigation coordinate system at time k relative to the N system in the N system; It represents the projection of the position vector of the inertial navigation coordinate system at time k-1 relative to the N system in the N system; Δt k is the time between k-1 and k; g N represents the gravity vector in the N system; It is the pre-integrated position result corrected by the first-order approximation. The first column of formulas calculates the position residual between time k-1 and k.

[0041] same, It represents the projection of the velocity vector of the inertial navigation coordinate system at time k relative to the N system in the N system; It represents the projection of the velocity vector of the inertial navigation coordinate system at time k-1 relative to the N system in the N system; It is the pre-integrated velocity result corrected by the first-order approximation. The formula in the second column calculates the velocity residual between time k-1 and k.

[0042] Indicates the attitude of the inertial navigation coordinate system at time k relative to the N system; Indicates the attitude of the inertial navigation coordinate system at time k-1 relative to the N system; It is the pre-integrated attitude result corrected by the first-order approximation. The third column formula calculates the attitude residual between time k-1 and k.

[0043] b a (t k ) represents the accelerometer zero bias at time k; b a (t k-1 ) represents the accelerometer bias at time k-1, and the fourth column formula calculates the accelerometer bias residual between time k-1 and k. g (t k ) represents the gyro bias at time k, b g (t k-1 ) represents the gyroscope bias at time k-1, and the formula in the fifth column calculates the gyroscope bias residual between time k-1 and time k.

[0044] is the pre-integrated mileage result corrected by the first-order approximation. The formula in the sixth column calculates the mileage residual of the wheel speedometer between time k-1 and k, s odo (t k ) represents the displacement of the wheel speed meter at time k, s odo (t k-1 ) represents the wheel speed meter displacement at time k-1, and the formula in the seventh column calculates the wheel speed meter displacement residual between time k-1 and k.

[0045] By superimposing the double-difference pseudorange observation residuals, double-difference phase observation residuals and IMU / wheel speed meter joint pre-integration residuals to generate a total correction factor, the correction factor is used to further correct the satellite positioning coordinates. The satellite positioning accuracy can be guaranteed even when driving for a long time or moving to a location with many obstructions.

[0046] To further improve the positioning accuracy of the device, the system can be initialized before the first combined navigation. First, place the unmanned vehicle in an open space, collect 100 sets of data from RTK, use the least squares method to get 100 coordinates, and initialize the position by taking the average value (accurately determine the initial position of the device). The initial posture of the unmanned vehicle is obtained from the accelerometer data, and the roll angle and pitch angle are determined by measuring the gravity vector and the angle between the Z axis and the OXY plane of the unmanned vehicle carrier coordinate system. When the unmanned vehicle just starts to move, the heading angle of the unmanned vehicle can be obtained through the RTK and accelerometer data. The formula for getting the roll angle and pitch angle is:

[0047]

[0048] In addition, since the installation position of the IMU and GNSS antennas does not coincide with the origin of the unmanned vehicle carrier coordinate system, it is also necessary to calibrate the sensor's homogeneous transformation matrix T to transform the sensor's coordinate system to the carrier coordinate system;

[0049]

[0050] The embodiments of the present invention are described in detail above, but the contents described are only preferred embodiments of the present invention and cannot be considered to limit the scope of implementation of the present invention. All equivalent changes and improvements made within the scope of the present invention should still fall within the scope of this patent.

Claims

1. A positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK, characterized in that: The following steps are involved: Step 1, receiving satellite navigation data and constructing an observation residual representing the satellite navigation error; Step 2: The mileage increment in each period is obtained separately through the wheel speed meter, and combined with the inertial sensor for joint pre-integration to offset the superposition error caused by the superposition of inertial navigation data in this period, so as to continuously compensate for the error accumulated by the IMU and extend the divergence time of the INS algorithm to generate a pre-integrated residual containing the overall error of the sensor and the wheel speed meter; Step 3: The pre-integrated residual is tightly combined with the observed residual to generate a correction factor, and the satellite positioning is corrected according to the correction factor; The observation residuals include double-difference pseudo-range observation residuals and double difference phase observation residual According to the joint pre-integration, the IMU / wheel speed meter joint pre-integration residual in the satellite coordinate system is constructed The correction factor is:

2. A positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK according to claim 1, characterized in that: The joint pre-integration includes velocity increment β, displacement increment α, attitude change q and mileage increment s, and constructs propagation equations of β, α, q and s respectively. The propagation equation of mileage increment s is: in: It represents the displacement between time k-1 and time i in the inertial navigation coordinate system b. represents the displacement between time k-1 and time i-1 in the inertial navigation coordinate system b. Indicates the displacement between time i-1 and time i in the inertial navigation coordinate system b, so as to obtain the mileage increment in each period separately, instead of obtaining the displacement of the device at time k-1 and the displacement of the device at time i separately, and calculating the displacement of the device between time k-1 and time i, thereby reducing the superposition error of the wheel speed meter due to the superposition of the distance; The propagation equation of the mileage increment is: in and They are all rotation matrices, used to transform the device coordinate system v to the inertial navigation coordinate system b. Indicates the displacement between time i-1 and time i in the device coordinate system v. It represents the arm correction error amount used to correct the position error between the wheel speed meter and the inertial sensor in the inertial navigation coordinate system b.

3. The positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK according to claim 1 is characterized in that: The IMU / wheel speed meter joint pre-integration residual in represents the covariance matrix of the IMU / wheel speedometer joint pre-integrated observation factor in the geodetic coordinate system, Represents the residual matrix including position change residual, velocity change residual, attitude change residual, accelerometer bias residual, gyroscope bias residual and wheel speed meter displacement residual.

4. The positioning method based on wheel speed inertial navigation joint pre-integration and tightly combined with RTK according to claim 1 is characterized in that: An initialization strategy is added before step 1, and the initialization strategy includes obtaining the initial coordinates and initial posture of the device; The initial coordinates include obtaining several groups of satellite navigation positioning coordinate data and taking the average value thereof, obtaining accelerometer data to calculate the gravity vector, and obtaining the angle between the gravity vector and the Z axis and OXY plane of the device coordinate system to generate initial states including the roll angle and the pitch angle respectively.