Initial alignment method based on speed optimization, and combined navigation system

Abstract

An initial alignment method based on speed optimization, a combined navigation system, and a GNSS receiver. The method comprises: in a dynamic window involved in initialization, moving a combined navigation system on the basis of a preset rule (100); acquiring a relational expression between a GNSS velocity solution and acceleration and an angular velocity observation between adjacent epochs in the dynamic window (101); on the basis of the preset rule and the relational expression based on the GNSS velocity solution, acquiring a rough initial value of a parameter to be estimated of an initial attitude parameter (102); and using the rough initial value to construct a global optimization problem, and solving the global optimization problem to acquire the initial attitude parameter (103). The method can realize fast initial alignment of GNSS / INS in complex environments, and can provide reliable and accurate initial alignment results in complex and obstructed environments.

Description

Initial Alignment Method and Integrated Navigation System Based on Speed ​​Optimization Technical Field

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

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

[0003] With the development and application of technologies such as high-precision engineering surveying and high-precision navigation, the accuracy consistency and reliability of sensor estimation systems are receiving increasing attention. In this industry, GNSS / INS integrated navigation algorithms, visual VIO algorithms, and LiDAR LIO algorithms, as representative fusion navigation technologies, have been widely used. In recent years, to further improve system reliability, tightly integrating GNSS, INS, visual, and LiDAR sensors has gradually become a mainstream trend. Scientific research and engineering applications have proven that such tightly integrated multi-sensor systems can provide robust pose estimation outputs in complex environments, meeting a wider range of application requirements.

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

[0005] In complex environments, due to issues such as signal obstruction and interference, the stability of GNSS positioning solutions is often poor for a short period of time, especially during the initial startup period. 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 in the existing integrated navigation system, and to provide an initial alignment method and integrated navigation system based on speed optimization that can achieve rapid initial alignment of GNSS / INS in complex environments and provide reliable and accurate initial alignment results in complex and obstructed environments.

[0007] The present invention solves the above-mentioned technical problems through the following technical solution:

[0008] An initial alignment method based on airspeed optimization for integrated navigation systems, characterized in that the initial alignment method includes:

[0009] Within the dynamic window involved in initialization, the integrated navigation system is moved according to preset rules;

[0010] Obtain the relationship between GNSS velocity solutions and acceleration and angular velocity observations between adjacent epochs within a dynamic window;

[0011] According to the preset rules and the relational formula, obtain the rough initial values ​​of the parameters to be estimated for the initial attitude parameters;

[0012] A global optimization problem is constructed using the coarse initial value. The global optimization problem is solved to obtain fine initial values ​​for the parameters to be estimated. The initial attitude parameters are obtained based on the fine initial values.

[0013] The design focus of this application is:

[0014] 1. Speed-based optimization

[0015] Traditional initial alignment methods often rely heavily on position rather than velocity. See Figure 1 in the accompanying drawings of the specification, which explains the difference between velocity and position during the initialization phase. Using speed optimization is the basis for the high robustness of this invention.

[0016] Referring to Figure 1, in complex environments, due to signal obstruction, interference, and other issues, the stability of GNSS positioning solutions is often poor for a short period, especially during the initial startup phase. The upper part of Figure 1 shows the changes in positioning solutions during GNSS RTK initialization.

[0017] Generally, GNSS algorithms use SPP positioning mode when starting positioning. After receiving differential signals, RTK calculation begins, and the positioning solution becomes a floating-point solution. After a short period of convergence, a fixed solution is achieved. During this period, the positioning solution will change from large random noise to a large range of fluctuations, and then jump to a high-precision positioning solution.

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

[0019] The changes during the initialization of the GNSS positioning algorithm are shown in the lower half of Figure 1. It can be seen that the position information integrated from the GNSS velocity solution has greater stability. Therefore, this invention focuses on Doppler speed and uses an optimized algorithm to achieve initial alignment, providing higher initial alignment performance and reliability.

[0020] 2. From coarse to fine

[0021] Based on airspeed and raw IMU observations, a position-independent discrete differential equation is constructed between adjacent epochs within a dynamic window, and the initial values ​​are coarsely estimated using heading as the sole estimation parameter. Then, a global optimization problem is constructed to obtain fine initial values, 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 accelerating for a window period, using the initial state to obtain the deviation parameters of the inertial measurement unit, and using the accelerated motion during the window period to obtain the initial pitch angle and initial roll angle of the integrated navigation system.

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

[0024]

[0025] in, Let be the rotation matrix that rotates the world coordinate system to the carrier coordinate system at the initial moment. and The attitude changes from the initial time to time k and k+1, and Obtained by integrating the observed angular velocity values. Let the velocity of the volume in the world coordinate system be the velocity at time k+1. Let the velocity of the volume in the world coordinate system be at time k. Let be the change in the carrier coordinate system from time k, as measured by the sensor, to the carrier coordinate system at time i. Let δt be the acceleration of the carrier at time i measured by the sensor, δt be the time interval from i to i+1, and T be the transpose sign.

[0026] Preferably, the initial alignment method includes:

[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 is the identity matrix, and θ is the change in angle. It is a cross product matrix.

[0030] Preferably, the initial alignment method includes:

[0031] Obtain the Jacobian matrix of the equation relative to the parameters to be estimated:

[0032]

[0033] The parameters to be estimated are solved iteratively to obtain rough initial values ​​for the parameters to be estimated.

[0034] Preferably, the initial alignment method includes:

[0035] The global optimization problem of establishing the aforementioned dynamic window:

[0036]

[0037] Where zi|i-1 is the pre-integration error of the inertial measurement unit, χ is all the parameters to be estimated in the dynamic window, ρ represents the robust kernel function, and ρZ is the robust kernel function of the marginalization parameter;

[0038] The initial attitude parameters are obtained by solving the global optimization problem.

[0039] Preferably, the initial alignment method includes:

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

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

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

[0043] Based on common knowledge in the field, the above-mentioned preferred conditions can be combined arbitrarily to obtain various preferred embodiments of the present invention.

[0044] The positive and progressive effects of this invention are as follows:

[0045] This invention enables rapid initial alignment of GNSS / INS in complex environments. Figure 2 in the specification shows a performance comparison between the algorithm of this invention and commonly used traditional initial alignment methods in different environments. The top left, top right, bottom left, and bottom right represent environments with open sky, a tree-lined path, single-sided building occlusion, and double-sided building occlusion, respectively.

[0046] It is evident that traditional methods cannot provide effective initial alignment in complex environments, while the initial alignment method of this invention can provide reliable and accurate initial alignment results in all environments. Attached Figure Description

[0047] Figure 1 shows the position changes during the initialization of the RTK algorithm in the prior art.

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

[0049] Figure 3 is a flowchart of the initial alignment method of Embodiment 1 of the present invention. Detailed Implementation

[0050] The present invention will be further illustrated by way of embodiments below, but the present invention is not limited to the scope of the embodiments described herein.

[0051] Example 1

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

[0053] Integrated navigation systems are used for:

[0054] Within the dynamic window involved in initialization, the integrated navigation system is moved according to preset rules;

[0055] Obtain the relationship between GNSS velocity solutions and acceleration and angular velocity observations between adjacent epochs within a dynamic window;

[0056] According to the preset rules and the relational formula, obtain the rough initial values ​​of the parameters to be estimated for the initial attitude parameters;

[0057] A global optimization problem is constructed using the coarse initial value. The global optimization problem is solved to obtain fine initial values ​​for the parameters to be estimated. The initial attitude parameters are obtained based on the fine initial values.

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

[0059] Referring to Figure 1, in complex environments, due to signal obstruction, interference, and other issues, the stability of GNSS positioning solutions is often poor for a short period, especially during the initial startup phase. The upper part of Figure 1 shows the changes in positioning solutions during GNSS RTK initialization.

[0060] Generally, GNSS algorithms use SPP positioning mode when starting positioning. After receiving differential signals, RTK calculation begins, and the positioning solution becomes a floating-point solution. After a short period of convergence, a fixed solution is achieved. During this period, the positioning solution will change from large random noise to a large range of fluctuations, and then jump to a high-precision positioning solution.

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

[0062] The changes during the initialization of the GNSS positioning algorithm are shown in the lower half of Figure 1. It can be seen that the position information integrated from the GNSS velocity solution has greater stability. Therefore, this embodiment will focus on Doppler speed and use an optimized algorithm to achieve initial alignment, providing higher initial alignment performance and reliability.

[0063] This embodiment relies on GNSS Doppler velocity and uses an optimization algorithm to construct the GNSS / INS initial alignment problem. By leveraging the stability of Doppler velocity and the optimization algorithm's ability to handle strongly nonlinear problems, it aims to achieve high-precision and highly robust GNSS / INS initial alignment in complex environments.

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

[0065]

[0066] in, Let be the rotation matrix that rotates the world coordinate system to the carrier coordinate system at the initial moment. and The attitude changes from the initial time to time k and k+1, and Obtained by integrating the observed angular velocity values. Let the velocity of the volume in the world coordinate system be the velocity at time k+1. Let the velocity of the volume in the world coordinate system be at time k. Let be the change in the carrier coordinate system from time k, as measured by the sensor, to the carrier coordinate system at time i. Let δt be the acceleration of the carrier at time i measured by the sensor, δt be the time interval from i to i+1, and T be the transpose sign.

[0067] Assuming the user's initial state is stationary or in uniform linear motion, meaning the vehicle's acceleration and angular velocity are relatively small, the user can begin initialization in dynamic conditions, but it is necessary to ensure that their initial acceleration and angular velocity are not too large.

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

[0069] Assuming that within the dynamic window involved in initialization, the IMU's bias parameter ba and b g It can be considered zero. At the initial moment, the initial pitch angle of the carrier... and roll angle It can be obtained directly from the angle between the gravity vector and the accelerometer output. Since the motion state at the initial moment is restricted, the error between these two angles can generally be limited to within 5 degrees.

[0070] Therefore, for the GNSS velocity solution between epoch k and epoch k+1 and Given the observed values ​​of acceleration and angular velocity a and ω, their relationship can be given as follows:

[0071]

[0072] in, The initial pose is given by the initial pose quaternion. and The attitude changes from the initial time to times k and k+1 can be obtained by integrating the IMU angular velocity observations ω with respect to δt. Since several rough assumptions have been made previously, it can be assumed that in the above equation, only... Includes parameters to be estimated The other terms are independent of the initial heading angle. Therefore, they 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 is the identity matrix, and θ is the change in angle. This is a cross product matrix. The integrated navigation system is used for:

[0077] The initial alignment method includes:

[0078] Obtain the equation relative to the parameters to be estimated Jacobian matrix:

[0079]

[0080] This is only for... The nonlinear least squares problem of estimation can be solved iteratively to obtain the heading estimate. These are rough initial values.

[0081] The integrated navigation system is used for:

[0082] A global optimization problem is established for the dynamic window to achieve higher accuracy in initial value estimation:

[0083]

[0084] Among them, z i|i-1 χ represents the pre-integration error of the inertial measurement unit, χ represents all the parameters to be estimated in the dynamic window, and ρ represents the robust kernel function. Z Robust kernel function for marginalizing parameters;

[0085] The initial attitude parameters are obtained by solving the global optimization problem.

[0086] Among them, z i|i-1 The IMU pre-integration error establishes a relationship between parameters at two time points. χ represents the parameters to be estimated, including velocity, attitude, acceleration deviation, and angular velocity deviation. Through the above optimization, fine initial alignment can be achieved. Generally, its alignment attitude accuracy can reach within 5 degrees.

[0087] The integrated navigation system is used for:

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

[0089] Referring to Figure 3, this embodiment also provides an initial alignment method based on speed optimization, utilizing the aforementioned integrated navigation system, including:

[0090] Step 100: Within the dynamic window involved in initialization, move the integrated navigation system according to preset rules;

[0091] Step 101: Obtain the relationship between the GNSS velocity solution and the observed values ​​of acceleration and angular velocity between adjacent epochs within the dynamic window;

[0092] Step 102: Obtain the rough initial values ​​of the parameters to be estimated for the initial attitude parameters according to the preset rules and the relational formula;

[0093] Step 103: Construct a global optimization problem using the coarse initial value, solve the global optimization problem to obtain the fine initial value of the parameter to be estimated, and obtain the initial attitude parameter based on the fine initial value.

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

[0095] The relationship is as follows:

[0096]

[0097] in, Let be the rotation matrix that rotates the world coordinate system to the carrier coordinate system at the initial moment. and The attitude changes from the initial time to time k and k+1, and Obtained by integrating the observed angular velocity values. Let the velocity of the volume in the world coordinate system be the velocity at time k+1. Let the velocity of the volume in the world coordinate system be at time k. Let be the change in the carrier coordinate system from time k, as measured by the sensor, to the carrier coordinate system at time i. Let δt be the acceleration of the carrier at time i measured by the sensor, δt be the time interval from i to i+1, and T be the transpose 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 is the identity matrix, and θ is the change in angle. It is a cross product matrix.

[0102] Step 102 specifically includes:

[0103] Obtain the Jacobian matrix of the equation relative to the parameters to be estimated:

[0104]

[0105] The parameters to be estimated are solved iteratively to obtain rough initial values ​​for the parameters to be estimated.

[0106] Step 103 specifically includes:

[0107] The global optimization problem of establishing the aforementioned dynamic window:

[0108]

[0109] Among them, z i|i-1 χ represents the pre-integration error of the inertial measurement unit, and χ represents all the parameters to be estimated in the dynamic window.

[0110] The initial attitude parameters are obtained by solving the global optimization problem.

[0111] The initial alignment method includes:

[0112] Step 104: Perform initial heading alignment of the integrated navigation system using the initial attitude parameters.

[0113] This embodiment enables rapid initial alignment of GNSS / INS in complex environments. It provides reliable and accurate initial alignment results even in complex and obstructed environments.

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

Claims

1. A method of initial alignment based on speed optimization for integrated navigation system, characterized in that, The initial alignment method comprises: moving the integrated navigation system according to a preset rule within an initialized dynamic window; obtaining a relationship between GNSS speed solution and acceleration, angular velocity observation values between adjacent epochs within the dynamic window; obtaining a rough initial value of a to-be-estimated parameter of the initial attitude parameter according to the preset rule and the relationship based on the GNSS speed solution; constructing a global optimization problem by using the rough initial value, solving the global optimization problem to obtain a fine initial value of the to-be-estimated parameter, and obtaining the initial attitude parameter according to the fine initial value.

2. The method for initial alignment based on speed optimization as claimed in claim 1, wherein, The preset rule comprises maintaining a static or uniform linear motion state at an initial time, then performing an acceleration motion for a window time period, obtaining bias parameters of the inertial measurement unit by using the state at the initial time, and obtaining an initial pitch angle and an initial roll angle of the integrated navigation system by using the acceleration motion for the window time period.

3. The method of initial alignment based on speed optimization as claimed in claim 2, wherein, The relationship based on GNSS velocity solution is: wherein rotating the world coordinate system to the rotation matrix of the carrier coordinate system at the initial time, and for the change in pose from the initial time instant to the time instants k and k+1, and According to the angular velocity observation value integration, download the velocity of the world coordinate system at time k+1, the velocity of the world coordinate system download body at time k, a change in the body coordinate system at time k measured by the sensor to the body coordinate system at time i, is a carrier i moment acceleration measured by a sensor, δt is a time interval from i to i+1, and T is a transpose symbol.

4. The method of initial alignment based on speed optimization as claimed in claim 3, wherein, The initial alignment method comprises: linearizing the relationship to obtain an equation based on the GNSS velocity solution: wherein I is an identity matrix, and θ is an angle change amount. is a cross product matrix.

5. The method of initial alignment based on speed optimization as claimed in claim 4, wherein, The initial alignment method comprises: obtaining a Jacobian matrix of the equations with respect to the parameters to be estimated: solving the to-be-estimated parameter by iteration to obtain a rough initial value of the to-be-estimated parameter.

6. The method for initial alignment based on speed optimization as claimed in claim 5, wherein, The initial alignment method comprises: establishing a global optimization problem for the dynamic window: where z i|i-1 is the pre-integration error of the inertial measurement unit, χ is the total set of parameters to be estimated for the dynamic window, p denotes a robust kernel function, p Z robust kernel function of the marginalized parameters; solving the global optimization problem to obtain the initial attitude parameter.

7. The method of initial alignment based on speed optimization as claimed in claim 1, wherein, The initial alignment method comprises: performing initial alignment of a heading of the integrated navigation system by using the initial attitude parameter.

8. A combined navigation system, characterized by The integrated navigation system is used to implement the initial alignment method based on speed optimization as claimed 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