Initial alignment method based on navigational speed optimization and integrated navigation system

Abstract

The invention discloses an initial alignment method based on navigational speed optimization and an integrated navigation system, and the initial alignment method comprises the following steps: moving the integrated navigation system according to a preset rule in a dynamic window involved in initialization; obtaining a relational expression between the GNSS velocity solution between adjacent epochs in the dynamic window and the observed values of the acceleration and the angular velocity; acquiring a rough initial value of a to-be-estimated parameter of an initial attitude parameter according to the preset rule and the relational expression; and constructing a global optimization problem by using the rough initial value, and solving the global optimization problem to obtain the initial attitude parameters. According to the invention, GNSS / INS rapid initial alignment in a complex environment can be realized. And a reliable and accurate initial alignment result can be provided in a complex and shielded environment.

Description

Technical Field

[0001] The invention relates to an initial alignment method based on speed optimization and an integrated navigation system. Background Art

[0002] In recent years, inclination measurement based on inertial measurement units has been gradually promoted and applied. Through the inertial orientation positioning navigation system (INS) built into the GNSS receiver, the GNSS receiver attitude data is output in real time, and then the direction angle, tilt angle and tilt direction angle of the centering pole in the tilted state are calculated. Combined with the obtained GNSS receiver antenna phase center coordinates, the coordinates of the ground point at the bottom of the tilt centering pole can be calculated.

[0003] With the development and application of technologies such as high-precision engineering measurement and high-precision navigation, the accuracy consistency and reliability of sensor estimation systems are receiving more and more attention. In this industry, GNSS / INS combined navigation algorithms, visual VIO algorithms, and lidar LIO algorithms, as representative fusion navigation technologies, have been widely used. In recent years, in order to further improve the reliability of the system, the tight combination of GNSS, INS, vision, and lidar has gradually become a mainstream trend. Scientific research and engineering applications have proved that this type of multi-sensor tight combination system can provide the system with robust pose estimation output in complex environments to meet a wider range of application conditions.

[0004] The initial heading alignment under large misalignment angle is an important factor to ensure the performance of the GNSS / INS integrated navigation estimator. Initial alignment refers to estimating the heading angle of the integrated navigation system in the initial state. Generally speaking, for low-cost IMU devices, when only a single GNSS antenna is used, only dynamic alignment can be used for initial alignment, that is, using the dynamic position and velocity information of GNSS to assist INS in heading estimation.

[0005] In complex environments, due to problems such as signal blocking and interference, the stability of the GNSS positioning solution is often poor within a short period of time, especially during the initialization period after power-on. Summary of the invention

[0006] The technical problem to be solved by the present invention is to overcome the defect of poor stability of GNSS positioning solution of the combined navigation system in the prior art, and to provide an initial alignment method and an integrated navigation system based on speed optimization, which can realize fast initial alignment of GNSS / INS in complex environments and provide reliable and accurate initial alignment results in complex and obscured environments.

[0007] The present invention solves the above technical problems through the following technical solutions:

[0008] An initial alignment method based on speed optimization is used for an integrated navigation system, wherein the initial alignment method comprises:

[0009] Moving the integrated navigation system according to preset rules within the dynamic window involved in the initialization;

[0010] Obtain the relationship between the GNSS velocity solution and the acceleration and angular velocity observations between adjacent epochs in the dynamic window;

[0011] Obtaining rough initial values ​​of the parameters to be estimated of the initial posture parameters according to the preset rules and the relationship;

[0012] A global optimization problem is constructed using the rough initial values, the global optimization problem is solved to obtain fine initial values ​​of the parameters to be estimated, and the initial posture parameters are obtained according to the fine initial values.

[0013] The design focus of this application is:

[0014] 1. Speed ​​optimization

[0015] Traditional initial alignment methods often rely more on position than speed, see the figure in the specification. Figure 1 , which explains the difference between speed and position in the initialization phase, and uses speed optimization, which is the basis for the present invention to achieve higher robustness.

[0016] See also Figure 1 In complex environments, due to problems such as signal blocking and interference, the stability of the GNSS positioning solution is often poor within a short period of time, especially during the initialization period after power-on. Figure 1 The upper part of the figure shows the change of the positioning solution during GNSSRTK initialization.

[0017] Generally, the GNSS algorithm uses the SPP positioning mode when starting positioning. When the differential signal is received, the RTK solution is started, and the positioning solution is changed to a floating point solution state accordingly. After a short period of convergence, it reaches a fixed solution state. During this period, the positioning solution will change from large random noise to large range fluctuations, and then jump to a high-precision positioning solution.

[0018] In contrast, the Doppler velocity solution has higher accuracy, and the error and amplitude of the solution do not change significantly with the GNSS positioning state.

[0019] Its changes during the initialization of the GNSS positioning algorithm are as follows Figure 1 As shown in the lower part, it can be seen that the position information integrated from the velocity solution of GNSS has stronger stability. Therefore, the present invention will focus on Doppler speed and use an optimization algorithm to achieve initial alignment to provide higher initial alignment effect and reliability.

[0020] 2. From coarse to fine

[0021] Based on the speed and IMU raw observations, a discrete differential equation that does not depend on the position between adjacent epochs in the dynamic window is constructed, and the heading is used as the only estimated parameter to make a rough estimate of the initial value. Then a global optimization problem is constructed to obtain a refined initial value, thereby obtaining more accurate initial attitude parameters.

[0022] Preferably, the preset rules include maintaining a stationary or uniform linear motion state at the initial moment, then performing an accelerated motion for a window time period, using the state at the initial moment to obtain the deviation parameters of the inertial measurement unit, and using the accelerated motion in the window time period to obtain the initial pitch angle and initial roll angle of the combined navigation system.

[0023] Preferably, the relationship based on the GNSS velocity solution is:

[0024] ,

[0025] in, is the rotation matrix of the world coordinate system to the carrier coordinate system at the initial moment, and From the initial time to and The posture changes at all times, and According to the integration of the angular velocity observation value, The world coordinate system is The speed of time, The world coordinate system is The speed of time, is the change in the carrier coordinate system at time k measured by the sensor rotated to the carrier coordinate system at time i, is the acceleration of the carrier at time i measured by the sensor, is the time interval from i to i+1, and T is the transposed sign.

[0026] Preferably, the initial alignment method comprises:

[0027] The relationship is linearized to obtain the equation based on the GNSS velocity solution:

[0028] ,

[0029] in, Including the parameters to be estimated, I unit matrix, is the angle change, is the cross product matrix.

[0030] Preferably, the initial alignment method comprises:

[0031] Get the Jacobian matrix of the equation with respect to the parameters to be estimated:

[0032] ;

[0033] The parameters to be estimated are solved in an iterative manner to obtain rough initial values ​​of the parameters to be estimated.

[0034] Preferably, the initial alignment method comprises:

[0035] Establish the global optimization problem of the dynamic window:

[0036] ,

[0037] in, is the pre-integrated error of the inertial measurement unit, are all the parameters to be estimated for the dynamic window, represents the robust kernel function, Robust kernel functions for marginalization parameters;

[0038] The global optimization problem is solved to obtain the initial posture parameters.

[0039] Preferably, the initial alignment method comprises:

[0040] The initial attitude parameters are used to perform initial heading alignment of the integrated navigation system.

[0041] The present invention also provides an integrated navigation system, which is characterized in that the integrated navigation system is used to implement the initial alignment method as described above.

[0042] The present invention also provides a GNSS receiver, which is characterized in that the GNSS receiver is used in the integrated navigation system as described above.

[0043] On the basis of being in accordance with the common sense in the art, the above-mentioned preferred conditions can be arbitrarily combined to obtain the preferred embodiments of the present invention.

[0044] The positive and progressive effects of the present invention are:

[0045] The present invention can realize the rapid initial alignment of GNSS / INS in complex environment. Figure 2 A performance comparison between the algorithm of the present invention and the traditional initial alignment method commonly used in the industry in different environments is given, wherein the upper left, upper right, lower left, and lower right are open sky, tree-lined path, single-sided building occlusion, and double-sided building occlusion environments, respectively.

[0046] It can be seen that the traditional method cannot provide effective initial alignment in complex environments, while the initial alignment method of the present invention can provide reliable and accurate initial alignment results in all environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is the position change during the initialization of the RTK algorithm in the prior art.

[0048] Figure 2 This is a schematic diagram of the effect of the combined navigation system of Example 1 of the present invention.

[0049] Figure 3 This is a flow chart of the initial alignment method of embodiment 1 of the present invention. DETAILED DESCRIPTION

[0050] The present invention is further described below by way of examples, but the present invention is not limited to the scope of the examples.

[0051] Example 1

[0052] This embodiment provides an integrated navigation system, which includes a GNSS receiver and an IMU module. In other embodiments, the integrated navigation system is a receiver including a GNSS module and an IMU module.

[0053] Integrated navigation systems are used for:

[0054] Moving the integrated navigation system according to preset rules within the dynamic window involved in the initialization;

[0055] Obtain the relationship between the GNSS velocity solution and the acceleration and angular velocity observations between adjacent epochs in the dynamic window;

[0056] Obtaining rough initial values ​​of the parameters to be estimated of the initial posture parameters according to the preset rules and the relationship;

[0057] A global optimization problem is constructed using the rough initial values, the global optimization problem is solved to obtain fine initial values ​​of the parameters to be estimated, and the initial posture parameters are obtained according to the fine initial values.

[0058] The preset rule includes maintaining a stationary or uniform linear motion state at the initial moment, then performing an accelerated motion for a window time period, using the state at the initial moment to obtain the deviation parameter of the inertial measurement unit, and using the accelerated motion in the window time period to obtain the initial pitch angle and initial roll angle of the integrated navigation system.

[0059] See also Figure 1In complex environments, due to problems such as signal blocking and interference, the stability of the GNSS positioning solution is often poor within a short period of time, especially during the initialization period after power-on. Figure 1 The upper part of the figure shows the change of the positioning solution during GNSSRTK initialization.

[0060] Generally, the GNSS algorithm uses the SPP positioning mode when starting positioning. When the differential signal is received, the RTK solution is started, and the positioning solution is changed to a floating point solution state accordingly. After a short period of convergence, it reaches a fixed solution state. During this period, the positioning solution will change from large random noise to large range fluctuations, and then jump to a high-precision positioning solution.

[0061] In contrast, the Doppler velocity solution has higher accuracy, and the error and amplitude of the solution do not change significantly with the GNSS positioning state.

[0062] Its changes during the initialization of the GNSS positioning algorithm are as follows Figure 1 As shown in the lower part, it can be seen that the position information integrated from the velocity solution of GNSS has stronger stability. Therefore, this embodiment will focus on Doppler speed and use an optimization algorithm to achieve initial alignment to provide higher initial alignment effect and reliability.

[0063] This embodiment relies on the GNSS Doppler velocity and uses an optimization algorithm to construct the GNSS / INS initial alignment problem, so as to achieve high-precision and high-robustness GNSS / INS initial alignment in complex environments by leveraging the stability of the Doppler velocity and the optimization algorithm's ability to handle strong nonlinear problems.

[0064] Specifically, the relationship based on the GNSS velocity solution is:

[0065] ,

[0066] in, is the rotation matrix of the world coordinate system to the carrier coordinate system at the initial moment, and From the initial time to and The posture changes at all times, and According to the integration of the angular velocity observation value, The world coordinate system is The speed of time, The world coordinate system is The speed of time, is the change in the carrier coordinate system at time k measured by the sensor rotated to the carrier coordinate system at time i, is the acceleration of the carrier at time i measured by the sensor, is the time interval from i to i+1, and T is the transposed sign.

[0067] Assume that the user's initial state is stationary or in uniform linear motion, that is, the carrier acceleration and angular velocity are both small values, that is, the user can start initialization in motion, but it is necessary to ensure that the initial acceleration and angular velocity are not too large.

[0068] The initial stationary or uniform linear motion state only needs to be maintained for a moment, not a period of time. After that, the user needs to perform a certain acceleration movement and ensure that the carrier maintains sufficient maneuverability within a window period of time (in the experiment of the embodiment, this window is set to 0.5 seconds).

[0069] Assume that within the dynamic window involved in initialization, the bias parameters of the IMU are and can be considered as zero. At the initial moment, the initial pitch angle of the carrier and roll angle It can be directly obtained by the angle between the gravity vector and the accelerometer output. Since the motion state at the initial moment is restricted, the error of these two angles can generally be limited to within 5 degrees.

[0070] Therefore, for Epoch and GNSS velocity solution between epochs and And acceleration, angular velocity observations and , their relationship can be given as:

[0071] ,

[0072] in, is the initial attitude, which corresponds to the initial attitude quaternion . and From the initial time to and The attitude change at each moment can be measured using the IMU angular velocity observation value. integral Since several rough assumptions have been made above, it can be considered that in the above formula, only Contains parameters to be estimated , while the other terms are independent of the initial heading angle. Therefore, it can be linearized.

[0073] The integrated navigation system is used for:

[0074] The relationship is linearized to obtain the equation based on the GNSS velocity solution:

[0075] ,

[0076] in, Including the parameters to be estimated, I unit matrix, is the angle change, is a cross product matrix. The integrated navigation system is used for:

[0077] The initial alignment method comprises:

[0078] Get the equation relative to the parameter to be estimated The Jacobian matrix of :

[0079] ;

[0080] This is a The nonlinear least squares problem for estimation can be solved iteratively to obtain the heading estimate Rough initial value.

[0081] The integrated navigation system is used for:

[0082] The global optimization problem of the dynamic window is established to achieve a higher precision initial value estimation:

[0083] ,

[0084] in, is the pre-integrated error of the inertial measurement unit, are all the parameters to be estimated for the dynamic window, represents the robust kernel function, Robust kernel functions for marginalization parameters;

[0085] The global optimization problem is solved to obtain the initial posture parameters.

[0086] in, is the IMU pre-integration error, which builds the connection between the parameters at two moments, The parameters to be estimated include speed, attitude, acceleration deviation, and angular velocity deviation. Through the above optimization, fine initial alignment can be achieved. Generally speaking, the alignment attitude accuracy can be within 5 degrees.

[0087] The integrated navigation system is used for:

[0088] The initial attitude parameters are used to perform initial heading alignment of the integrated navigation system.

[0089] See also Figure 3, using the above-mentioned integrated navigation system, this embodiment further provides an initial alignment method based on speed optimization, including:

[0090] Step 100: moving the integrated navigation system according to a preset rule within the dynamic window involved in the initialization;

[0091] Step 101, obtaining a relationship between the GNSS velocity solution and the acceleration and angular velocity observation values ​​between adjacent epochs in a dynamic window;

[0092] Step 102: obtaining rough initial values ​​of the parameters to be estimated of the initial posture parameters according to the preset rules and the relationship;

[0093] Step 103: construct a global optimization problem using the rough initial values, solve the global optimization problem to obtain fine initial values ​​of the parameters to be estimated, and obtain the initial posture parameters according to the fine initial values.

[0094] Among them, the preset rules include maintaining a stationary or uniform linear motion state at the initial moment, then performing an accelerated motion for a window time period, using the state at the initial moment to obtain the deviation parameters of the inertial measurement unit, and using the accelerated motion in the window time period to obtain the initial pitch angle and initial roll angle of the integrated navigation system.

[0095] The relationship is:

[0096] ,

[0097] in, is the rotation matrix of the world coordinate system to the carrier coordinate system at the initial moment, and From the initial time to and The posture changes at all times, and According to the integration of the angular velocity observation value, The world coordinate system is The speed of time, The world coordinate system is The speed of time, is the change in the carrier coordinate system at time k measured by the sensor rotated to the carrier coordinate system at time i, is the acceleration of the carrier at time i measured by the sensor, is the time interval from i to i+1, and T is the transposed sign.

[0098] Step 102 specifically includes:

[0099] The relationship is linearized to obtain the equation:

[0100] ,

[0101] in, Including the parameters to be estimated, I unit matrix, is the angle change, is the cross product matrix.

[0102] Step 102 specifically includes:

[0103] Get the Jacobian matrix of the equation with respect to the parameters to be estimated:

[0104] ;

[0105] The parameters to be estimated are solved in an iterative manner to obtain rough initial values ​​of the parameters to be estimated.

[0106] Step 103 specifically includes:

[0107] Establish the global optimization problem of the dynamic window:

[0108] ,

[0109] in, is the pre-integrated error of the inertial measurement unit, are all parameters to be estimated for the dynamic window;

[0110] The global optimization problem is solved to obtain the initial posture parameters.

[0111] The initial alignment method comprises:

[0112] Step 104: Use the initial attitude parameters to perform initial heading alignment of the integrated navigation system.

[0113] This embodiment can achieve fast initial alignment of GNSS / INS in complex environments and provide reliable and accurate initial alignment results in complex and obstructed environments.

[0114] Although the specific embodiments of the present invention are described above, those skilled in the art should understand that these are only examples, and the protection scope of the present invention is defined by the appended claims. Those skilled in the art may make various changes or modifications to these embodiments without departing from the principles and essence of the present invention, but these changes and modifications all fall within the protection scope of the present invention.

Claims

1. An initial alignment method based on speed optimization for an integrated navigation system, characterized in that: The initial alignment method comprises: Moving the integrated navigation system according to preset rules within the dynamic window involved in the initialization; Obtain the relationship between the GNSS velocity solution and the acceleration and angular velocity observations between adjacent epochs in the dynamic window; Obtaining rough initial values ​​of parameters to be estimated of initial attitude parameters according to the preset rules and the relationship based on the GNSS velocity solution; A global optimization problem is constructed using the rough initial values, the global optimization problem is solved to obtain fine initial values ​​of the parameters to be estimated, and the initial posture parameters are obtained according to the fine initial values.

2. The initial alignment method based on speed optimization according to claim 1, characterized in that: The preset rule includes maintaining a stationary or uniform linear motion state at the initial moment, then performing an accelerated motion for a window time period, using the state at the initial moment to obtain the deviation parameter of the inertial measurement unit, and using the accelerated motion in the window time period to obtain the initial pitch angle and initial roll angle of the integrated navigation system.

3. The initial alignment method based on speed optimization according to claim 2, characterized in that: The relationship based on the GNSS velocity solution is: ， in, is the rotation matrix of the world coordinate system to the carrier coordinate system at the initial moment, and From the initial time to and The posture changes at all times, and According to the integration of the angular velocity observation value, The world coordinate system is The speed of time, The world coordinate system is The speed of time, is the change in the carrier coordinate system at time k measured by the sensor rotated to the carrier coordinate system at time i, is the acceleration of the carrier at time i measured by the sensor, is the time interval from i to i+1, and T is the transposed sign.

4. The initial alignment method based on speed optimization according to claim 3, characterized in that: The initial alignment method comprises: The relationship is linearized to obtain the equation based on the GNSS velocity solution: ， in, Including the parameters to be estimated, I unit matrix, is the angle change, is the cross product matrix.

5. The initial alignment method based on speed optimization according to claim 4, characterized in that: The initial alignment method comprises: Get the Jacobian matrix of the equation with respect to the parameters to be estimated: ； The parameters to be estimated are solved in an iterative manner to obtain rough initial values ​​of the parameters to be estimated.

6. The initial alignment method based on speed optimization according to claim 5, characterized in that: The initial alignment method comprises: Establish the global optimization problem of the dynamic window: ， in, is the pre-integrated error of the inertial measurement unit, are all the parameters to be estimated for the dynamic window, represents the robust kernel function, Robust kernel functions for marginalization parameters; The global optimization problem is solved to obtain the initial posture parameters.

7. The initial alignment method based on speed optimization according to claim 1, characterized in that: The initial alignment method comprises: The initial attitude parameters are used to perform initial heading alignment of the integrated navigation system.

8. An integrated navigation system, characterized in that: The integrated navigation system is used to implement the initial alignment method based on speed optimization as described in any one of claims 1 to 7.

9. A GNSS receiver, characterized in that: The GNSS receiver is used in the integrated navigation system as claimed in claim 8.

Images