Integrated initialization method and device of inertial navigation RTK receiver

Abstract

The invention discloses an integrated initialization method of an inertial navigation RTK (Real-Time Kinematic) receiver, which comprises the following steps of: straightening a measuring rod, and enabling a rod tip of the measuring rod to be stably contacted with a certain fixed point on the ground; under the condition that the rod tip is kept to be in contact with the ground, holding the top end of the rod body by hand to enable the rod body to swing repeatedly so as to fully excite each shaft error of the IMU sensor, and acquiring angular velocity and specific force data output by the MEMS-IMU in the repeated swing process; and constructing a system state equation and a measurement updating equation based on the angular velocity and specific force data, and synchronously completing initial attitude alignment, velocity initialization and field calibration of sensor errors of the IMU through a filtering estimation algorithm. According to the method, the rod tip of the measuring rod continuously contacts with the fixed point on the ground, the rod body is controlled to swing in a specific mode, and the integrated estimation and initialization of the navigation state and the sensor error are realized by utilizing kinematics constraint generated by the rigid body rotation motion of the rod body around the fixed point.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of satellite navigation and inertial navigation technology, in particular to a fast initialization method for a MEMS IMU in a GNSS RTK receiver with tilt compensation function. BACKGROUND

[0002] The real-time kinematic (RTK) based on the carrier phase of the global navigation satellite system (GNSS) realizes centimeter-level dynamic positioning, which is widely used in navigation and positioning, digital mapping and other fields. The typical RTK is to fix the GNSS antenna and receiver on the antenna mounting rod, and in the measurement process, the tip of the antenna rod is aligned with the ground point to be measured, the antenna rod is straightened, and the positioning result of the GNSS RTK is calculated from the antenna phase center to the tip of the rod, so that the coordinates of the ground point to be measured can be obtained. However, since the antenna rod needs to be straightened during measurement, the traditional RTK is limited in measurement efficiency. And when the point to be measured is located in challenging terrain such as wall corner, steep slope, car bottom, etc., it is difficult to vertically stand the measurement rod on the point to be measured, so that the RTK cannot measure these points.

[0003] The RTK receiver with tilt compensation function (referred to as tilt RTK) usually integrates a low-cost micro-electro-mechanical system (MEMS) inertial measurement unit (IMU), and uses the attitude information provided by the inertial navigation system (INS) to perform tilt compensation, so that it is not necessary to center and level during measurement, which improves the measurement efficiency of the RTK and expands the application ability of the RTK in restricted environments.

[0004] However, as a classic dead reckoning system, the INS must be initialized before entering navigation and positioning, including the initialization of the navigation state (including position, velocity, attitude) and the error of the inertial sensor. The existing INS initialization method is usually designed for vehicle and aircraft platforms, and is performed in steps, which often needs to be corrected by continuous GNSS positioning, and needs to set static and multiple dynamic conditions to effectively initialize all parameters. The initialization process usually takes several minutes or even tens of minutes, which is low in efficiency. These methods generally cannot be effectively applied to tilt RTK, and have the problems of long time consumption and complicated operation. At present, there is still a lack of fast and comprehensive initialization scheme for the specific application scene of tilt RTK. Specifically: 1. Velocity and position initialization usually rely on GNSS, but the accuracy of GNSS velocity initialization is limited (usually no less than 0.1 m / s) in measuring the motion state of the rod; 2. In the attitude initialization, the roll and pitch angles are usually achieved by accelerometer leveling, which requires the IMU to remain stationary; the heading angle initialization needs to be completed in motion, which often takes a long time or requires sufficient travel distance; 3. The calibration of inertial sensors (gyroscopes and accelerometers) usually relies on a turntable or a multi-position stationary method, and the accuracy of the on-site calibration method is limited or has specific requirements for the motion pattern, which is not suitable for measuring rods. Online estimation (such as Kalman filtering) usually needs a long time to converge due to the coupling of multiple error sources.

[0005] Therefore, there is an urgent need in the art for an integrated initialization method that can quickly and accurately estimate all navigation states and inertial sensor errors at the same time to meet the stringent requirements of tilt RTK products for initialization efficiency. SUMMARY

[0006] To overcome the above-mentioned deficiencies of the prior art, the present application provides an integrated initialization method for an inertial navigation RTK receiver, which continuously contacts the rod tip of the measuring rod with a fixed point on the ground and manipulates the rod body to perform a specific mode of swing, utilizes the kinematic constraints generated by the rigid body rotational motion of the rod body around the fixed point, and realizes integrated estimation and initialization of navigation states and sensor errors.

[0007] According to an aspect of the present application, an integrated initialization method for an inertial navigation RTK receiver is provided, comprising: Stabilize the rod tip of the measuring rod to contact a fixed point on the ground; Under the condition of keeping the rod tip stationary, repeatedly swing the top end of the rod body to fully excite the errors of each axis of the IMU sensor, and obtain the angular velocity and specific force data output by the MEMS-IMU during the repeated swinging; Based on the angular velocity and specific force data, construct the system state equation and the measurement update equation, and simultaneously complete the initial alignment of the attitude, the velocity initialization of the IMU, and the on-site calibration of the sensor errors through a filtering estimation algorithm.

[0008] Further, under the condition of keeping the rod tip stationary, repeatedly swing the top end of the rod body to fully excite the errors of each axis of the IMU sensor, comprising: Under the condition of keeping the rod tip stationary, repeatedly swing the top end of the rod body in the left-right direction; Under the condition of keeping the rod tip stationary, repeatedly swing the top end of the rod body in the front-back direction; Under the condition of keeping the rod tip touching the ground, the rod body is tilted by a certain angle and repeatedly twisted around its vertical axis.

[0009] Further, based on the attitude error, the zero offset error, the scale factor error and the rod arm vector from the IMU center to the rod tip, a system state equation is constructed.

[0010] Further, based on the physical constraint that the rod tip contacts the ground fixed point and its acceleration is zero, a measurement update equation is constructed.

[0011] Further, the accelerometer leveling algorithm is used to determine the initial roll angle and pitch angle, and the initial attitude value is obtained, and the initial attitude value is used to initialize the filter.

[0012] Further, the initial roll angle and pitch angle are calculated by the acceleration rod arm compensation derived rod tip specific force.

[0013] Further, after the filter converges, the heading angle is calculated by matching the INS derived trajectory and the GNSS measured trajectory.

[0014] Further, after the heading initialization is completed, the initial speed is calculated by the speed rod arm compensation formula.

[0015] According to another aspect of the specification, an integrated initialization device for an inertial navigation RTK receiver is provided, comprising a measuring rod and an inertial navigation system arranged on the measuring rod, the inertial navigation system comprising an inertial sensing unit and a data processing unit, the inertial sensing unit comprising a gyroscope and an accelerometer; During initialization, under the condition of keeping the rod tip touching the ground, the rod body top end is repeatedly swung to fully excite the IMU sensor axis error; The data processing unit acquires the angular velocity and specific force data output by the MEMS-IMU during the repeated swinging, constructs the system state equation and the measurement update equation based on the angular velocity and specific force data, and synchronously completes the IMU attitude initial alignment, speed initialization and sensor error on-site calibration through the filter estimation algorithm.

[0016] Compared with the prior art, the present application has the following advantages: 1) The present application uses the rigid body fixed axis rotation motion characteristics of the measuring rod to realize the initialization of the MEMS IMU in the tilted RTK receiver. The speed initialization, attitude alignment (roll, pitch, heading) and on-site rapid calibration of all parameters of the inertial sensor error (gyroscope / accelerometer zero offset and scale factor) are completed synchronously, which changes the mode of traditional method that needs to be completed in different steps and under different conditions (such as first static and then dynamic), greatly simplifying the operation process.

[0017] 2) The method is simple and efficient. Initialization can be completed by shaking the measuring rod in place. The whole initialization process only takes about 60-90 seconds to achieve high-precision initialization of all parameters. Even as short as 20 seconds can obtain a higher-precision initial attitude and velocity.

[0018] 3) The application does not need to rely on other sensors, and can complete the initialization of the absolute heading in the absence of GNSS. It has strong adaptability in complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0019] To more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings used in the embodiments or the prior art description will be briefly introduced. Obviously, the drawings in the following description are some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without creative labor.

[0020] Figure 1 The motion step schematic diagram of the IMU in the integrated initialization process of the inertial navigation RTK receiver provided by the embodiments of the present application. DETAILED DESCRIPTION

[0021] The terms "include" and "have" and any variations thereof in the specification and claims of the present application and the above drawings are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device including a series of steps or units does not have to include all the steps or units clearly listed, but can include other steps or units not clearly listed or inherent to the process, method, product or device.

[0022] To make the purposes, technical solutions and advantages of the embodiments of the present application more clear, the technical solutions in the embodiments of the present application will be clearly and completely described below in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are some of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the present application. In addition, the technical features in each embodiment or single embodiment provided by the present application can be combined with each other to form new technical solutions. This combination is not restricted by the order of steps and / or structure composition mode, but must be based on the implementation by those skilled in the art. When the combination of technical solutions contradicts each other or cannot be implemented, it should be considered that the combination of technical solutions does not exist, and is not within the protection scope of the present application.

[0023] The present application provides a fast, comprehensive, stable and reliable integrated MEMS-IMU initialization method for a tilt RTK receiver, which continuously contacts the rod tip of a measuring rod with a fixed point on the ground, and controls the rod body to perform a specific mode of swing, uses the kinematic constraint generated by the rigid body rotation of the rod body around the fixed point to realize integrated estimation and initialization of the navigation state and sensor error.

[0024] In the application scenario of tilt RTK, the user initializes the IMU state after starting the device according to the method provided by the present application. The specific steps are as follows: Step 1: After starting the device, hold the measuring rod and stabilize the rod tip to contact a fixed point on the ground; Step 2: Under the condition of keeping the rod tip stationary, hold the top end of the rod body and make it swing repeatedly to fully stimulate the IMU sensor error; Preferably, as shown in Figure 1 , the following steps are used to fully stimulate the IMU sensor error: Step 21: Under the condition of keeping the rod tip stationary, hold the top end of the rod body and make it swing repeatedly in the left-right direction; Step 22: Under the condition of keeping the rod tip stationary, hold the top end of the rod body and make it swing repeatedly in the front-back direction; Step 23: Under the condition of keeping the rod tip stationary, tilt the rod body by a certain angle and make it twist repeatedly around its vertical axis; Step 3: After completing the movements specified in steps 21 to 23, based on the angular velocity and specific force data output by the MEMS-IMU during this process, the attitude initial alignment, velocity initialization and on-site calibration of the inertial sensor error of the IMU are simultaneously completed through a specific filtering estimation algorithm.

[0025] Further, the specific implementation method of step 3 includes the following process: Step 31: Construct the system state equation , wherein, is the attitude error, are the zero bias errors of the gyroscope and accelerometer respectively, are the scale factor errors of the gyroscope and accelerometer respectively, is the rod arm vector from the center of the IMU to the rod tip.

[0026] Step 32: Construct the acceleration measurement update equation Based on the physical constraint that the tip of the rod contacts the ground fixed point whose velocity / acceleration is zero, the measurement update equations are constructed. They include but are not limited to zero velocity and zero acceleration measurement update equations. Take the zero acceleration as an example, it is defined as follows: define the measurement vector The difference between the estimated acceleration at the tip of the rod and the zero vector , where, denotes the attitude rotation matrix from the n-frame to the b-frame, denotes the local gravity acceleration in the n-frame, is the angular velocity of the IMU in the b-frame, is the linear acceleration of the IMU in the b-frame, is the angular acceleration of the IMU.

[0027] Correspondingly, the design matrix is: , I is a 3x3 identity matrix, and diag is a function that converts a vector into a diagonal matrix, where, .

[0028] Step 33, filter initialization To initialize the filter, an initial attitude value is required. The present application uses an accelerometer leveling algorithm to determine the initial roll and pitch angles. Generally, the accelerometer leveling algorithm requires the IMU to be in a stationary state. However, when the measurement rod is performing a rigid body rotational motion, the tip of the rod remains stationary, and the derived tip of the rod specific force The roll angle and the pitch angle are calculated as follows: , denote the components of the tip of the rod specific force in the x, y, and z directions, respectively.

[0029] Step 34, velocity initialization The filter only estimates the initial roll, pitch, and sensor errors of the IMU. Specifically, the filter establishes the system state equation through step 31 and continuously "corrects" the parameters in the system state equation using the measurement update equation through step 32 to achieve the estimation of the roll, pitch, and sensor errors.

[0030] After the filter converges, the absolute heading angle can be calculated by the method disclosed in Chinese invention patent ZL 2019113803561 published on May 1, 2020. Specifically, the absolute heading angle is calculated by matching the INS derived trajectory and the GNSS measured trajectory. After the heading initialization is completed, the initial speed can be calculated by the speed lever arm compensation formula.

[0031] , wherein, is the attitude rotation matrix from b-frame to n-frame, which is calculated by the roll angle , the pitch angle , and the heading angle .

[0032] Based on the same inventive concept as the foregoing method, the application also provides an integrated initialization device for an inertial navigation RTK receiver, comprising a measuring rod and an inertial navigation system arranged on the measuring rod, wherein the inertial navigation system comprises an inertial sensing unit and a data processing unit, and the inertial sensing unit comprises a gyroscope and an accelerometer. During initialization, the top end of the rod body is repeatedly swung to fully stimulate the error of each axis of the IMU sensor under the condition that the rod tip keeps touching the ground. The data processing unit acquires the angular velocity and specific force data output by the MEMS-IMU during the repeated swinging, constructs the system state equation and the measurement update equation based on the angular velocity and specific force data, and simultaneously completes the initial alignment of the IMU attitude, the initialization of the speed, and the on-site calibration of the sensor error through a filtering estimation algorithm.

[0033] Optionally, the inertial navigation system is arranged at the top of the measuring rod.

[0034] In summary of the above embodiments, the application realizes the integrated estimation and initialization of the navigation state and the sensor error by making the rod tip of the measuring rod continuously contact the fixed point on the ground and manipulating the rod body to perform a specific mode of swinging, and utilizing the kinematic constraint generated by the rigid body rotational motion of the rod body around the fixed point.

[0035] In this specification, the front, back, left, right and other orientation words involved are defined according to the positions of the parts in the drawings and the positions of the parts relative to each other in the drawings, only for the purpose of expressing the clarity and convenience of the technical scheme. It should be understood that the use of the orientation words should not limit the scope of the application claimed.

[0036] It should be noted that the above examples are only used to illustrate the technical solutions of the present application, and are not intended to limit the present application; although the present application has been described in detail with reference to the foregoing examples, those skilled in the art should understand that the technical solutions recorded in the foregoing examples can still be modified, or some or all of the technical features can be replaced by equivalents; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present application.

Claims

1. An integrated initialization method for an inertial navigation RTK receiver, characterized in that, The application relates to a measurement rod, which is used for initial alignment of an inertial navigation system (INS) and comprises the following steps: a measurement rod is kept in stable contact with a fixed point on the ground; under the condition that the rod tip is kept in contact with the ground, the top end of the rod body is repeatedly swung to fully stimulate errors of each axis of the IMU sensor, and angular velocity and specific force data output by the MEMS-IMU during the repeated swinging are obtained; based on the angular velocity and specific force data, system state equations and measurement update equations are constructed, and initial alignment of the attitude of the IMU, initialization of the velocity and on-site calibration of sensor errors are simultaneously completed through a filtering estimation algorithm.

2. The integrated initialization method of an inertial navigation RTK receiver according to claim 1, wherein, under the condition that the rod tip is kept in contact with the ground, the top end of the rod body is repeatedly swung to fully stimulate errors of each axis of the IMU sensor, and angular velocity and specific force data output by the MEMS-IMU during the repeated swinging are obtained; under the condition that the rod tip is kept in contact with the ground, the top end of the rod body is repeatedly swung in the left-right direction; under the condition that the rod tip is kept in contact with the ground, the top end of the rod body is repeatedly swung in the front-back direction; under the condition that the rod tip is kept in contact with the ground, the rod body is tilted by a certain angle, and the rod body is repeatedly twisted around the vertical axis.

3. The integrated initialization method for an inertial navigation RTK receiver according to claim 1, wherein, system state equations are constructed based on the attitude error, the zero offset error, the proportional factor error and a rod arm vector from the center of the IMU to the rod tip.

4. The integrated initialization method of an inertial navigation RTK receiver according to claim 1, wherein, a measurement update equation is constructed based on the physical constraint that the acceleration of the fixed point on the ground contacted by the rod tip is zero.

5. The integrated initialization method for an inertial navigation RTK receiver according to claim 1, wherein, an accelerometer leveling algorithm is used to determine initial roll and pitch angles, and initial attitude values are obtained, and the initial attitude values are used to initialize the filter.

6. The integrated initialization method of an inertial navigation RTK receiver according to claim 5, wherein, initial roll and pitch angles are derived by using rod arm compensation of specific force of the rod tip calculated by an accelerometer.

7. The integrated initialization method for an inertial navigation RTK receiver according to claim 1, wherein, after the filter converges, a heading angle is calculated by matching an INS-derived trajectory and a GNSS-measured trajectory.

8. The integrated initialization method of an inertial navigation RTK receiver according to claim 7, wherein, after the heading initialization is completed, an initial velocity is calculated by using a velocity rod arm compensation formula.

9. An integrated initialization device for an inertial navigation RTK receiver, characterized in that, The application relates to a measurement rod and an inertial navigation system (INS) arranged on the measurement rod, wherein the inertial navigation system comprises an inertial sensing unit and a data processing unit, the inertial sensing unit comprises a gyroscope and an accelerometer, and the data processing unit comprises a processor and a memory. under the condition that the rod tip is kept in contact with the ground, the top end of the rod body is repeatedly swung to fully stimulate errors of each axis of the IMU sensor during initialization; the data processing unit obtains angular velocity and specific force data output by the MEMS-IMU during the repeated swinging, constructs system state equations and measurement update equations based on the angular velocity and specific force data, and simultaneously completes initial alignment of the attitude of the IMU, initialization of the velocity and on-site calibration of sensor errors through a filtering estimation algorithm.

Images