A fast heading initialization method and navigation system for low-speed scenarios

Abstract

The present invention relates to a method and a navigation system for rapid heading initialization in low-speed scenarios, wherein the method comprises: obtaining strapdown inertial navigation system parameters, and entering an inertial navigation solution mode when the strapdown inertial navigation system parameters meet preset conditions; when a GNSS positioning result is reliable, determining the current motion state of the navigation system, selecting different strategies according to the motion state of the navigation system to determine an initial heading angle, and executing system initialization; and continuously performing background heading angle monitoring before the heading angle reaches a stable state. The present invention can shorten the initialization time of a GNSS / SINS integrated navigation system and simultaneously improve heading alignment accuracy.

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation technology, and in particular to a heading rapid initialization method and a navigation system for low-speed scenarios. Background Art

[0002] Before entering the navigation solution state, the strapdown inertial navigation system requires initial alignment to obtain accurate initial information as the basis for subsequent solutions. High-precision inertial navigation can calculate the heading angle based on gyro output. However, MEMS-IMUs generally require external information to determine the initial heading angle due to the high gyro noise and the Earth's rotational angular velocity being drowned out by the sensor noise.

[0003] Existing solutions for low-precision IMUs generally adopt a single strategy, such as using only the carrier velocity or only the carrier position change information to calculate the initial heading angle. Due to the use of only a single strategy, it is difficult to meet the rapid alignment requirements in specific usage scenarios. When the navigation system is in a low-speed motion state, the velocity result is generally less accurate, which will cause a large error in the heading angle calculation; calculating the heading angle based on position information has certain requirements for the system's motion trajectory and duration. Some technologies consider relying on Kalman filtering to converge the initial heading angle error, but in the absence of sufficient maneuvering, the convergence time may be too long, affecting the accuracy of the combined solution. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a fast heading initialization method and navigation system for low-speed scenarios, which can shorten the initialization time of a global navigation satellite system (GNSS) / strapdown inertial navigation system (SINS) combined navigation system and improve the heading alignment accuracy.

[0005] The technical solution adopted by the present invention to solve the technical problem is to provide a method for rapid heading initialization for low-speed scenarios, comprising the following steps:

[0006] Acquiring strapdown inertial navigation system parameters, and entering an inertial navigation solution mode when the strapdown inertial navigation system parameters meet preset conditions;

[0007] When the GNSS positioning result is reliable, determine the current motion state of the navigation system;

[0008] When the navigation system is currently in a stationary state, the attitude horizontal angle is calculated based on the accelerometer output; when the navigation system is currently in a turning state, the heading angle is converged based on the obtained heading angle information; when the navigation system is currently in a straight-ahead state, the heading alignment phase is entered;

[0009] During the heading alignment phase, it is determined whether the GNSS velocity information meets a preset standard. If so, the heading angle and uncertainty are calculated using the GNSS velocity information; otherwise, the heading angle and uncertainty are calculated based on a number of epoch displacements.

[0010] When the GNSS positioning result is unreliable, heading initialization is performed based on the strapdown inertial navigation system parameters.

[0011] When the GNSS positioning result is reliable, the current motion state of the navigation system is determined as follows:

[0012] Calculating a standard deviation of outputs of several groups of accelerometers within a window time, and determining whether the navigation system is in a stationary state based on the standard deviation;

[0013] When the navigation system is not in a stationary state, the angle change rate is calculated by the gyro output within the window time, and whether the navigation system is in a turning state is determined according to the angle change rate.

[0014] The calculation formula for calculating the horizontal attitude angle based on the accelerometer output is: Among them, θ is the pitch angle, γ is the roll angle, and are the x-axis, y-axis, and z-axis outputs of the accelerometer respectively; g is the component of the gravity acceleration vector on the z-axis of the navigation coordinate system n.

[0015] The calculation formula for calculating the heading angle and uncertainty using the GNSS velocity information is: in, is the heading angle, is the eastward velocity result of GNSS, is the north velocity result of GNSS, is the standard deviation of the GNSS eastward velocity results, is the standard deviation of the GNSS north velocity results.

[0016] The calculation formula for calculating the heading angle and uncertainty based on several epoch displacements is: in, is the heading angle, is the sum of the eastward components of the GNSS displacements over n epochs, is the sum of the north component of the GNSS displacement in n epochs, is the standard deviation of the easting position result of GNSS at the i-th epoch, is the standard deviation of the GNSS north position result at the i-th epoch.

[0017] The method for rapid heading initialization for low-speed scenarios also includes a heading angle detection mode, specifically: judging whether the heading angle has reached a stable state; when the heading angle has not reached a stable state, continuously detecting the heading angle; and adopting three independent strategies during continuous detection, namely: calculating the heading angle by GNSS speed, calculating the heading angle by displacement vector, and calculating the heading angle by large heading misalignment angle strategy. The weights of the three independent strategies are dynamically adjusted as the carrier speed and motion state change. When the heading angle comprehensively judged by the three independent strategies reaches a stable state, the heading information is adjusted and the detection of the heading angle is ended.

[0018] When the strapdown inertial navigation system parameters meet the saving condition, the strapdown inertial navigation system parameters are saved so that the navigation system can read the information when it is powered on next time.

[0019] The technical solution adopted by the present invention to solve its technical problem is: providing a navigation system, including a processor, a memory and a computer program, wherein the computer program is stored in the memory and is configured to be executed by the processor, and the computer program includes a method for executing the above-mentioned heading rapid initialization method for low-speed scenarios.

[0020] Beneficial effects

[0021] Due to the adoption of the above-mentioned technical solution, the present invention has the following advantages and positive effects compared with the prior art: the present invention adopts different strategies to calculate and monitor the heading angle, shortens the initial alignment time, and when the initialization accuracy is low, performs a combined solution and continuously monitors the heading angle through the background. According to the motion state of the system, the weights of the three independent strategies are dynamically adjusted, and the heading angle accuracy is comprehensively evaluated, thereby completing error convergence faster and improving the accuracy of the early results. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 is a schematic diagram of the navigation system software framework on which the embodiments of the present invention rely;

[0023] Figure 2 is a flow chart of an embodiment of the present invention;

[0024] Figure 3 This is the GNSS trajectory of a bicycle in a low-speed riding scenario;

[0025] Figure 4 This is a schematic diagram of the bicycle's movement speed in the first 30 seconds of the GNSS result trajectory. DETAILED DESCRIPTION

[0026] Below in conjunction with specific embodiment, further set forth the present invention.Should be understood that these embodiments are only used to illustrate the present invention and are not used in limiting the scope of the present invention.In addition, should be understood that after reading the content taught by the present invention, those skilled in the art can make various changes or modifications to the present invention, and these equivalent forms fall equally within the scope limited by the appended claims of the application.

[0027] Generally speaking, MEMS-level inertial navigation systems in related technical solutions require GNSS position and velocity results, as well as certain maneuvering requirements, to complete initial alignment. Specifically, they require the carrier's speed to meet a certain threshold. Initial alignment cannot be completed when GNSS struggles to provide reliable results or the carrier fails to meet maneuvering requirements. Vehicles such as bicycles, scooters, and unmanned lawn mowers struggle to achieve high speeds, and there's a high probability of obstruction by trees or tall buildings in their use scenarios. Therefore, existing navigation systems struggle to meet fast initialization requirements, hindering the system's ability to enter the combined solution phase.

[0028] In the following description, "first initialization" and "second initialization" represent different initialization methods and do not limit other attributes such as order and accuracy.

[0029] The inertial navigation solution mode is based on the carrier position, velocity, and attitude obtained by inertial solution. It is supplemented by different virtual observation information according to different carrier maneuvers. For example, Zero Velocity Update (ZUPT) is used when the carrier is stationary, and Non-holonomic Constraint (Non-holonomic Constraint) is used when the carrier is moving on the ground. Based on the Kalman Filter (KF), the state equation and corresponding measurement equation are established to estimate the state quantity.

[0030] The GNSS / SINS loosely combined solution mode uses the position and velocity results obtained from GNSS and SINS, respectively, and combines them using Kalman filtering to calculate navigation parameters while correcting for inertial navigation errors. When GNSS is stable and available, it provides the system with position and velocity results and their associated uncertainties. When unavailable, the GNSS / SINS loosely combined solution mode degenerates to the inertial navigation solution mode.

[0031] This embodiment relates to a method for rapid heading initialization for low-speed scenarios, which relies on Figure 1The navigation system shown in the figure is implemented, including a MEMS-IMU that collects inertial navigation data, a GNSS positioning chip that receives GNSS signals and carries an algorithm unit. Among them, the sensor mainly provides GNSS and IMU binary data. The driver layer reads the data in the sensor buffer space at a fixed frequency and converts the data into a specific form according to the definition of the sensor register. After pre-processing at the algorithm layer, the data is used for initial alignment and subsequent combined solution. When the solution result is reliable, it is recorded in the storage module in real time. When the system is powered on for initialization, if there is no GNSS data input to the sensor layer due to signal blocking or other reasons, the navigation parameters can be read from the storage module for rapid initialization. As shown in the figure, the navigation parameters are read from the storage module for rapid initialization. Figure 2 As shown, it specifically includes:

[0032] Step 1: When the navigation system is powered on, read the SINS parameters in the storage module;

[0033] Step 2: When the SINS parameters meet the preset conditions, the first initialization is completed according to the SINS parameters, and the navigation system enters the inertial navigation solution mode;

[0034] The SINS parameters include but are not limited to SINS position parameters and posture parameters.

[0035] The SINS parameters satisfying the preset conditions means that the SINS parameters include at least the position parameter and the attitude parameter, and the parameter values ​​are within a preset range. For example, the preset range of longitude and latitude for the position parameter is consistent with the longitude and latitude range of the earth. If the attitude parameter read is not within this range, the preset condition is not satisfied. The preset range of the attitude parameter is 0-360°. If the attitude parameter read is not within this range, the preset condition is not satisfied.

[0036] After entering the inertial navigation solution mode, the initialization content of the navigation system also includes:

[0037] Step 3: Determine whether the navigation system can obtain a reliable GNSS positioning result. When the GNSS positioning result is reliable, perform a second initialization based on the GNSS information.

[0038] The GNSS positioning result is reliable if the result meets preset standards, for example, the position accuracy of the result is higher than 1 meter and the velocity accuracy is higher than 0.3 m / s.

[0039] Step 4: Determine the motion state of the navigation system, where the motion state includes stationary, straight-moving, and turning.

[0040] The principle of judging the motion state of the navigation system is as follows:

[0041] The accelerometer output of the IMU tends to be stable when the system is stationary, but fluctuates greatly when it is in motion. Therefore, the standard deviation (STD) of the accelerometer's raw output is used to determine whether the system is in motion. If there are N sets of data within the window time, the standard deviation of one axis of these N sets of data can be expressed as

[0042]

[0043] Where A i is the summed single-axis output of the i-th epoch, and μ is the average value of N sets of data for the corresponding axis.

[0044] The system is considered stationary when the standard deviation meets the preset conditions, for example, the standard deviation is less than 0.05m / s 2 .

[0045] The rotational motion of the system can be reflected in the gyro output of the IMU. The angle change calculated by the gyro output within the window time, that is, the angle change rate, is used to determine whether the system is turning. The conditions that need to be met to determine whether the carrier is turning can be expressed as

[0046]

[0047] Where δ is the angle change rate, N is the total amount of data within the window time, g(i) is the gyroscope single-axis output, dt is the sampling interval, and T is the window length, for example, 1 second.

[0048] The system is considered to be turning when the angle change rate meets the preset conditions, for example, the angle change rate is greater than 2° / s.

[0049] Step 5: When the system is determined to be in a stationary state, the horizontal attitude angle is calculated based on the accelerometer output; when the system is determined to be in a turning state, if the system has obtained the heading angle information, the heading angle can be converged; when the system is determined to be in a straight-ahead state, the heading alignment stage is entered.

[0050] In step 5, the principle of calculating the horizontal attitude angle based on the accelerometer output is as follows:

[0051] The accelerometer is only affected by the earth's gravity when it is stationary. Since the accelerometer outputs specific force, the projection of the output in the navigation coordinate system n when it is stationary is -g n , which can be related to the projection of the IMU in the coordinate system b as follows:

[0052]

[0053] Where, f b is the accelerometer output vector, g is the local gravity acceleration vector, The rotation matrix from the navigation coordinate system to the inertial coordinate system.

[0054] The navigation coordinate system n is a local horizontal coordinate system with its origin at the center of mass of the carrier. The X-axis of the coordinate system points to the geographic north along the meridian, the Y-axis points to the east along the local latitude, and the Z-axis points to the ground along the geographic vertical line, forming a right-handed rectangular coordinate system with the X-axis and Y-axis.

[0055] The origin of the coordinate system b of the inertial navigation is located at the center of mass of the inertial device, the X-axis points to the forward direction of the carrier, the Y-axis points to the right side of the carrier, and the Z-axis points to the ground direction of the carrier. The three axes follow the right-hand rule.

[0056] Therefore, the relationship between the inertial navigation output and gravity at rest can be expressed as:

[0057]

[0058] in, and are the x-axis, y-axis, and z-axis outputs of the accelerometer respectively; g is the component of the gravity acceleration vector on the z-axis of the navigation coordinate system n.

[0059] There is a conversion relationship between the rotation matrix and the Euler angle, which can be expressed as:

[0060]

[0061] in, θ and γ are heading angle, pitch angle and roll angle respectively.

[0062] Therefore, the relationship between the inertial navigation output and gravity at rest can be expressed in the form of Euler angles:

[0063]

[0064] Solve the pitch and roll angles of the inertial navigation:

[0065]

[0066] After entering the heading alignment phase, the navigation system initialization further includes:

[0067] Step 6: Determine whether the GNSS speed meets the preset standard. If it does, calculate the heading angle and uncertainty using the speed provided by the GNSS.

[0068] When the preset criteria are not met, the heading angle and uncertainty are calculated based on several epoch displacements.

[0069] Meeting the preset standard means that the carrier speed reflected by the GNSS speed result reaches a certain threshold, for example, the speed exceeds 1m / s.

[0070] The principle of GNSS velocity calculation of heading angle and uncertainty is as follows:

[0071]

[0072] Where, is the eastward velocity result of GNSS, is the north velocity result of GNSS.

[0073] According to the error propagation law, the heading angle uncertainty can be expressed as:

[0074]

[0075] Where, is the standard deviation of the GNSS eastward velocity results, is the standard deviation of the GNSS north velocity results.

[0076] The principle of calculating the heading angle and uncertainty based on several epoch displacements is as follows:

[0077]

[0078] Where, is the sum of the eastward components of the GNSS displacements over n epochs, is the sum of the north component of the GNSS displacement over n epochs.

[0079] According to the error propagation law, the heading angle uncertainty can be expressed as:

[0080]

[0081] Where, is the standard deviation of the easting position result of GNSS at the i-th epoch, is the standard deviation of the GNSS north position result at the i-th epoch.

[0082] Step 7: Enter the GNSS / SINS loose combination solution mode and heading angle detection mode at the same time. While outputting the solution results, determine whether the heading angle has reached a stable state. If the heading angle has not reached a stable state, the background will continue to detect the heading angle.

[0083] The heading angle reaching the preset standard means that the heading angle uncertainty is less than the preset value, for example, 5°;

[0084] Continuous background detection includes calculating the heading angle based on GNSS velocity, calculating the heading angle based on accumulated displacement, and calculating the heading angle using a Kalman filter strategy based on large heading misalignment angles. The weights of the three strategies are dynamically adjusted as the carrier speed and motion state change. When the heading angle result determined by the three combined methods reaches a stable state, the heading result in the combined solution is corrected based on the heading angle calculated in the background, and the background monitoring ends.

[0085] The weights of the three strategies are dynamically adjusted based on the vehicle's speed and motion state. This includes: The weights of the three strategies in the overall result vary depending on the vehicle's maneuvering state. For example, when the vehicle is traveling straight at speeds greater than 3 m / s, the velocity-based heading angle strategy is weighted 80%, while the cumulative displacement-based heading angle strategy and the Kalman filter-based heading angle strategy, which calculates the heading angle based on large heading misalignment angles, each receive 10%.

[0086] The principle of the heading angle calculation strategy based on the Kalman filter with large heading misalignment angle is as follows:

[0087] The maximum heading misalignment error angle is defined as: ψ e =[ψ x ψ y sinψ z cosψ z -1] T , the first three terms are defined as ψ′, and the position error is δr c =[δr N δr E δr D ] T , which is composed of position errors in the north, east and south directions; the velocity error is δv c =[δv N δv E δv D ] T , which is composed of velocity errors in three directions: north, east and ground;

[0088] The position error equation is expressed as

[0089]

[0090] Where, δr c is the position error in the calculation system C, which is the local horizontal coordinate system at the SINS solution position; Δv c Defined as velocity error; v is the projection of the angular velocity of the calculation system relative to the earth-fixed system in the calculation system; c is the velocity of system c. And (v c ×) is v c The antisymmetric matrix of c ×) e+ Calculated by the following formula:

[0091]

[0092] Where, and are the first two components of velocity.

[0093] The velocity error equation is expressed as:

[0094]

[0095] Where, is the direction cosine matrix from the inertial navigation coordinate system to the platform system p, where the platform system p is the navigation system directly calculated by SINS; g c and δg c They represent the gravitational acceleration and its error in the c system respectively, and the gravity error is expressed as follows:

[0096]

[0097] Where R N ,R M They represent the curvature radii of the meridian circle and the meridian circle respectively.

[0098] δf b and are the output errors of the accelerometer and gyroscope in frame b respectively; is the projection of the Earth's rotational angular velocity in the C system, and are the components in the north and east directions respectively; is the component of the C-frame velocity in the earth direction. and are the projection vector of the angular velocity of system c relative to system e at c, and the components in the east and north directions respectively. c (ψ′,v c ) is defined as follows:

[0099]

[0100] The attitude error equation is expressed as:

[0101]

[0102] Where, To calculate the projection of the angular velocity of system c relative to the inertial system in system c, and The weight of the first two items.

[0103] Step 8: During the navigation system combined solution process, when the SINS parameters meet the saving conditions, the SINS parameters are saved to the storage module in real time so that the initial information can be read the next time the system is powered on;

[0104] The SINS parameters are considered reliable if they meet the preservation conditions, for example, the position standard deviation of the filtered results is less than 1m, the velocity standard deviation is less than 0.3m / s, and the attitude standard deviation is less than 1°.

[0105] Some examples of the experimental effects of the present invention are as follows: Figure 3 and Figure 4 shown. Figure 3 This is a GNSS trajectory of a bicycle riding at low speed. The point marked "S" is the starting point of the trajectory, and the point marked "E" is the end point of the trajectory. The sampling interval between each trajectory point is 1 second. After being stationary for several epochs at the starting point, the bicycle begins to move. Figure 4 The figure shows the bicycle's speed during the first 30 seconds of the trajectory, showing that it was entirely at a relatively low speed. Conventional methods for calculating heading angle based on speed struggle to meet the required threshold. Forcibly lowering the threshold can result in significant heading angle errors, reaching as much as 32° in this experiment. Using cumulative displacement to calculate heading angle reduces the error to 4°, while maintaining a relatively short alignment time. This demonstrates the necessity of using different strategies for calculating heading angle.

[0106] The present invention also relates to a navigation system, comprising a processor, a memory and a computer program, wherein the computer program is stored in the memory and configured to be executed by the processor, and the computer program includes instructions for executing the above-mentioned method for fast heading initialization for low-speed scenarios.

[0107] It is not difficult to find that the present invention adopts different strategies to calculate and monitor the heading angle, shortens the initial alignment time, and when the initialization accuracy is low, performs a combined solution and continuously monitors the heading angle through the background. According to the motion state of the system, the weights of the three independent strategies are dynamically adjusted, and the heading angle accuracy is comprehensively evaluated, thereby completing error convergence faster and improving the accuracy of the early results.

Claims

1. A method for rapid heading initialization for low-speed scenarios, characterized in that: The following steps are involved: Acquiring strapdown inertial navigation system parameters, and entering an inertial navigation solution mode when the strapdown inertial navigation system parameters meet preset conditions; When the GNSS positioning result is reliable, determine the current motion state of the navigation system; When the navigation system is currently in a stationary state, the attitude horizontal angle is calculated based on the accelerometer output; when the navigation system is currently in a turning state, the heading angle is converged based on the obtained heading angle information; when the navigation system is currently in a straight-ahead state, the heading alignment phase is entered; During the heading alignment phase, determining whether the GNSS velocity information meets a preset standard, and when the preset standard is met, calculating the heading angle and uncertainty using the GNSS velocity information; Otherwise, the heading angle and uncertainty are calculated based on the displacement of several epochs. The calculation formula for calculating the heading angle and uncertainty based on the displacement of several epochs is: in, is the heading angle, is the sum of the eastward components of the GNSS displacements over n epochs, is the sum of the north component of the GNSS displacement in n epochs, is the standard deviation of the easting position result of GNSS at the i-th epoch, is the standard deviation of the GNSS north position result at the i-th epoch; The system simultaneously enters the GNSS / SINS loose combination solution mode and the heading angle detection mode, outputs the solution results, and determines whether the heading angle has reached a stable state. The heading angle detection mode specifically comprises: determining whether the heading angle has reached a stable state. When the heading angle has not reached a stable state, continuously detecting the heading angle. Three independent strategies are adopted during the continuous detection, namely: calculating the heading angle by GNSS velocity, calculating the heading angle by displacement vector, and calculating the heading angle by a large heading misalignment angle strategy. The weights of the three independent strategies are dynamically adjusted as the carrier speed and motion state change. When the heading angle determined by the three independent strategies reaches a stable state, the heading information is adjusted and the detection of the heading angle is terminated. The weights of the three independent strategies are dynamically adjusted as the carrier speed and motion state change. Specifically, the weights of the three independent strategies are dynamically adjusted as the carrier speed and motion state change. Specifically, under different carrier maneuvering states, the results of the three strategies account for different proportions of the comprehensive result. The large heading misalignment angle strategy calculates the heading angle based on Kalman filtering.

2. The method for rapid heading initialization for low-speed scenarios according to claim 1, characterized in that: When the GNSS positioning result is unreliable, heading initialization is performed based on the strapdown inertial navigation system parameters.

3. The method for rapid heading initialization for low-speed scenarios according to claim 1, characterized in that: When the GNSS positioning result is reliable, the current motion state of the navigation system is determined as follows: Calculating a standard deviation of outputs of several groups of accelerometers within a window time, and determining whether the navigation system is in a stationary state based on the standard deviation; When the navigation system is not in a stationary state, the angle change rate is calculated by the gyro output within the window time, and whether the navigation system is in a turning state is determined according to the angle change rate.

4. The method for rapid heading initialization for low-speed scenarios according to claim 1, characterized in that: The calculation formula for calculating the horizontal attitude angle based on the accelerometer output is: Among them, θ is the pitch angle, γ is the roll angle, and are the x-axis, y-axis, and z-axis outputs of the accelerometer respectively; g is the component of the gravity acceleration vector on the z-axis of the navigation coordinate system n.

5. The method for rapid heading initialization for low-speed scenarios according to claim 1, characterized in that: The calculation formula for calculating the heading angle and uncertainty using the GNSS velocity information is: in, is the heading angle, is the eastward velocity result of GNSS, is the north velocity result of GNSS, is the standard deviation of the GNSS eastward velocity results, is the standard deviation of the GNSS north velocity results.

6. The method for rapid heading initialization for low-speed scenarios according to claim 1, characterized in that: When the strapdown inertial navigation system parameters meet the saving condition, the strapdown inertial navigation system parameters are saved so that the navigation system can read the information when it is powered on next time.

7. A navigation system, characterized in that: The invention comprises a processor, a memory and a computer program, wherein the computer program is stored in the memory and is configured to be executed by the processor, and the computer program includes a method for executing the heading fast initialization method for a low-speed scenario according to any one of claims 1 to 6.

Images