Single-antenna gnss and imu integrated navigation initial alignment method and device

Abstract

This invention discloses an initial alignment method and apparatus for single-antenna GNSS and IMU combined navigation. The method includes: real-time acquisition of IMU measurement data and GNSSRTK navigation data; performing a moving average on the high-frequency acceleration values ​​in the IMU measurement data to obtain a smoothed longitudinal acceleration value; determining the relative relationship between the carrier velocity and the carrier's longitudinal direction based on the velocity information and the smoothed longitudinal acceleration value in the GNSSRTK navigation data; determining the heading angle based on the velocity information and track angle in the GNSSRTK navigation data; performing continuity processing and smoothing filtering on the heading angle to obtain a smoothed filtered heading angle; and correcting the deviation of the smoothed filtered heading angle based on the relative relationship between the carrier velocity and the carrier's longitudinal direction to obtain an initial heading value. This invention features a simple installation structure, low cost, and strong anti-magnetic interference capability. It can obtain a high-precision initial heading angle over a wide speed range, further improving the automation and intelligence of agricultural machinery operations, reducing material and maintenance costs, and increasing economic benefits.

Description

Technical Field

[0001] This invention relates to the field of positioning and navigation technology, and in particular to an initial alignment method and apparatus for single-antenna GNSS and IMU combined navigation. Background Technology

[0002] The combined navigation technology of Global Navigation Satellite System (GNSS) and Inertial Measurement Unit (IMU) has been extensively studied and applied in agriculture. GNSS provides a wide range of high-precision absolute position and timing information that is unaffected by time, but it is susceptible to weather, signal blockage, and multipath effects. IMU, on the other hand, provides attitude and relative positioning information that is unaffected by external factors. The combination of the two can compensate for each other's shortcomings, providing more stable, reliable, accurate, and redundant navigation information.

[0003] Initial alignment is a key technology in integrated navigation. It provides initial position, velocity, and attitude information (including heading angle). Initial position and velocity are generally provided directly by GNSS, while horizontal attitude is calculated from the accelerometer output of the IMU. Obtaining a high-precision initial heading angle is more difficult and complex, and is a major task and objective of initial alignment. Different initial alignment methods will determine the composition, cost, and usage of the integrated navigation system.

[0004] In existing technologies, obtaining the initial heading angle during initial alignment generally includes: using heading information from dual-antenna GNSS as the initial heading angle; calculating the initial heading angle by measuring the Earth's magnetic field using a magnetometer; and obtaining the initial heading angle using information from a single-antenna GNSS. These three methods have the following shortcomings:

[0005] 1) Using the heading information of dual-antenna GNSS as the initial heading angle: It requires the installation of two antennas with a sufficiently long relative distance and a high-precision positioning and orientation board to be used in conjunction with them. The installation structure is complicated, and the addition of an extra antenna increases the cost. Moreover, the positioning and orientation board of dual-antenna GNSS is expensive, resulting in a high overall cost.

[0006] 2) Calculating the initial heading angle using a magnetometer to measure the Earth's magnetic field: This method calculates the magnetic heading angle by measuring the Earth's magnetic field with a triaxial magnetometer, and then compensates for magnetic declination to obtain the true north heading. The magnetometer needs to be calibrated before use. Because agricultural machinery is easily magnetized, in addition to the Earth's magnetic field, the magnetic field environment in which the magnetometer operates also includes changing interfering magnetic fields, and the error of the magnetometer's calibration parameters changes over time. Therefore, this method is susceptible to interference, leading to a large heading angle error, and requires frequent calibration of the magnetometer parameters, resulting in cumbersome use and high maintenance costs.

[0007] 3) Obtaining the initial heading angle using single-antenna GNSS information: The conventional approach is to directly use the track angle output by GNSS as the initial heading angle. However, at low speeds, the deviation between the track angle and the heading angle is large, and directly using the track angle as the initial heading angle will result in low accuracy of the initial heading angle.

[0008] In addition, the relative relationship between the longitudinal direction of the carrier (head direction) and the speed direction is also crucial for obtaining the initial heading angle. In the existing technology, manually specifying the heading direction of the agricultural machinery will reduce the level of automation and intelligence. The publicly available methods for judging the heading direction are not yet mature, especially in that they cannot accurately judge the heading direction of the agricultural machinery at low speeds. Summary of the Invention

[0009] The technical problem to be solved by the present invention is to provide an initial alignment method and apparatus for single-antenna GNSS and IMU combined navigation, which addresses the problems existing in the prior art.

[0010] To address the aforementioned technical problems, this invention provides an initial alignment method for single-antenna GNSS and IMU integrated navigation, comprising: real-time acquisition of IMU measurement data and GNSSRTK navigation data; performing a moving average on the high-frequency acceleration values ​​in the IMU measurement data to obtain a smoothed longitudinal acceleration value; determining the relative relationship between the carrier velocity and the carrier longitudinal direction based on the GNSS velocity and the smoothed longitudinal acceleration value in the GNSSRTK navigation data; determining the heading angle based on the GNSS velocity and track angle in the GNSSRTK navigation data, and performing continuity processing and smoothing filtering on the heading angle to obtain a smoothed filtered heading angle; and correcting the deviation of the smoothed filtered heading angle based on the relative relationship between the carrier velocity and the carrier longitudinal direction to obtain the initial heading value for initial alignment.

[0011] To address the aforementioned technical problems, the present invention also provides an initial alignment device for single-antenna GNSS and IMU integrated navigation, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. The device is characterized in that the processor executes the program to implement the initial alignment method for single-antenna GNSS and IMU integrated navigation provided by the aforementioned technical solution.

[0012] To address the aforementioned technical problems, the present invention also provides a readable storage medium including instructions that, when executed on a processor, cause the processor to perform the initial alignment method for single-antenna GNSS and IMU integrated navigation provided by the above-described technical solution.

[0013] The beneficial effects of this invention are as follows: This invention combines IMU measurement data and GNSS RTK navigation data. The longitudinal acceleration smoothing value is obtained through IMU measurement data. Based on the GNSS RTK navigation data and the longitudinal acceleration smoothing value, the relative relationship between the vehicle's speed and longitudinal direction when the vehicle starts to move can be accurately determined. The heading angle is determined based on the speed information and track angle. The heading angle is then processed for continuity and smoothed by filtering to obtain a smoothed and filtered heading angle. Finally, the deviation of the smoothed and filtered heading angle is corrected based on the relative relationship between the vehicle's speed and longitudinal direction, thus obtaining a high-precision initial heading angle.

[0014] Compared to directly using the heading information from a dual-antenna GNSS receiver as the initial heading angle, this invention offers advantages such as simple installation structure and low cost. Compared to calculating the initial heading angle by measuring the Earth's magnetic field with a magnetometer, it boasts strong resistance to magnetic interference, eliminates the need for frequent magnetometer parameter calibration, and is simple to use with low maintenance costs. Compared to directly using the track angle as the initial heading angle with a single-antenna GNSS receiver, this invention can obtain a high-precision initial heading angle under both low and high speed conditions. In other words, this invention can obtain a high-precision initial heading angle over a wide speed range, further improving the automation and intelligence of agricultural machinery operations, reducing material and maintenance costs, and increasing economic efficiency.

[0015] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0016] Figure 1 A flowchart of the initial alignment method for single-antenna GNSS and IMU integrated navigation provided in an embodiment of the present invention;

[0017] Figure 2 A diagram illustrating the signal processing steps of the initial alignment method for single-antenna GNSS and IMU combined navigation provided in an embodiment of the present invention.

[0018] Figure 3 A schematic diagram of a first-in-first-out (FIFO) buffer structure provided in an embodiment of the present invention;

[0019] Figure 4 A flowchart for determining the relative relationship between the carrier velocity and the carrier longitudinal direction based on velocity information and longitudinal acceleration smoothing value in GNSS RTK navigation data, provided for embodiments of the present invention;

[0020] Figure 5 The actual test results of the relative relationship between the carrier speed and the longitudinal direction of the carrier provided in the embodiments of the present invention are shown in the figure.

[0021] Figure 6A flowchart for determining the heading angle based on speed information and track angle in GNSS RTK navigation data, provided in an embodiment of the present invention;

[0022] Figure 7 A comparison chart showing the heading angle at 1 km / h and the heading angle after smoothing and filtering. Detailed Implementation

[0023] The following specific examples illustrate the implementation of this disclosure. Those skilled in the art can easily understand other advantages and effects of this disclosure from the content disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of this disclosure, and not all of them. This disclosure can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this disclosure. It should be noted that, in the absence of conflict, the following embodiments and features in the embodiments can be combined with each other. Based on the embodiments in this disclosure, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this disclosure.

[0024] It should be noted that various aspects of embodiments within the scope of the appended claims are described below. It will be apparent that the aspects described herein can be embodied in a wide variety of forms, and any particular structure and / or function described herein is merely illustrative. Based on this disclosure, those skilled in the art will understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects set forth herein can be used to implement the device and / or practice the method. Additionally, this device and / or method can be implemented using structures and / or functionalities other than one or more of the aspects set forth herein.

[0025] The initial alignment method and apparatus for single-antenna GNSS and IMU combined navigation disclosed in this invention can be applied to fields such as smart agriculture, autonomous driving, and vehicle navigation.

[0026] like Figure 1 , Figure 2 As shown, the initial alignment method for single-antenna GNSS and IMU integrated navigation provided in this embodiment of the invention includes:

[0027] S1 collects IMU measurement data and GNSS RTK navigation data in real time.

[0028] Specifically, the IMU measurement data includes the high-frequency acceleration values ​​output by the IMU, where high frequency means that the frequency of the IMU output data is not less than 50Hz.

[0029] GNSS RTK navigation data includes velocity information and track angles. Velocity information includes the horizontal ground velocity output by the GNSS receiver. When the GNSS receiver has the capability to directly output eastward and northward velocities, the velocity information also includes the eastward and northward velocities directly output by the GNSS receiver. When the GNSS receiver does not have the capability to directly output eastward and northward velocities, the eastward and northward velocities can be obtained by decomposing the horizontal ground velocity using track angles.

[0030] S2, perform a moving average on the high-frequency acceleration values ​​in the IMU measurement data to obtain a smoothed longitudinal acceleration value.

[0031] When performing a moving average on high-frequency acceleration values, the high-frequency acceleration values ​​are pushed into a calculus. Figure 3 The diagram shows a First-In-First-Out (FIFO) buffer. Once the buffer is full, the values ​​in the buffer are averaged. The moving average time ranges from 0.5s to 2s. The formula for calculating the moving average is as follows:

[0032]

[0033] In the above formula, Here, A represents the longitudinal acceleration smoothing value, n is the number of values ​​in the buffer, and A is the value of longitudinal acceleration smoothing. i Let T be the i-th high-frequency acceleration value. A T is the moving average time. s The sampling interval time. This indicates rounding down to the nearest integer.

[0034] S3. Determine the relative relationship between the vehicle velocity and the vehicle's longitudinal direction based on the velocity information and longitudinal acceleration smoothing value in the GNSS RTK navigation data.

[0035] S4. Determine the heading angle based on the speed information and track angle in the GNSS RTK navigation data, and perform continuity processing and smoothing filtering on the heading angle to obtain the smoothed heading angle.

[0036] S5, based on the relative relationship between the carrier speed and the carrier longitudinal direction, the deviation of the smoothed filtering heading angle is corrected to obtain the initial heading value for initial alignment.

[0037] This invention combines IMU measurement data and GNSS RTK navigation data. The longitudinal acceleration smoothing value is obtained through the IMU measurement data. Based on the GNSS RTK navigation data and the longitudinal acceleration smoothing value, the relative relationship between the vehicle's initial velocity and its longitudinal direction can be accurately determined. Furthermore, the heading angle is determined based on the horizontal ground velocity and track angle, and the heading angle is subjected to continuity processing and smoothing filtering to obtain a smoothed and filtered heading angle. Finally, based on the relative relationship between the vehicle's velocity and its longitudinal direction, the smoothed and filtered heading angle is corrected for deviation, thus obtaining a high-precision initial heading angle.

[0038] Compared to directly using the heading information from a dual-antenna GNSS receiver as the initial heading angle, this invention offers advantages such as simple installation structure and low cost. Compared to calculating the initial heading angle by measuring the Earth's magnetic field with a magnetometer, it boasts strong resistance to magnetic interference, eliminates the need for frequent magnetometer parameter calibration, and is simple to use with low maintenance costs. Compared to directly using the track angle as the initial heading angle with a single-antenna GNSS receiver, this invention can obtain a high-precision initial heading angle under both low and high speed conditions. In other words, this invention can obtain a high-precision initial heading angle over a wide speed range, further improving the automation and intelligence of agricultural machinery operations, reducing material and maintenance costs, and increasing economic efficiency.

[0039] Optionally, the relative relationship between the vehicle velocity and the vehicle's longitudinal direction is determined based on the velocity information and longitudinal acceleration smoothing value in the GNSS RTK navigation data, including:

[0040] S31, determine the changes in the carrier's state based on the eastward and northward velocities.

[0041] The judgment process includes four carrier states: preparation state, static state, transition state, and motion state. The preparation state is the state when the carrier state is uncertain, and the transition state is the state between the static state and the motion state.

[0042] Specifically, when the absolute values ​​of the eastward and northward speeds are both continuously less than the first speed threshold and the duration is greater than or equal to the first preset time, the carrier is determined to enter a stationary state; otherwise, it remains in a ready state. When the absolute value of the eastward or northward speed is continuously greater than the first speed threshold and the duration is greater than or equal to the first preset time, the carrier is determined to enter a transition state; otherwise, it remains in a stationary state.

[0043] S32, when the carrier state changes from a static state to a transition state, and the transition state lasts for a preset duration, the average value of the longitudinal acceleration smoothing value under the transition state is calculated. The preset duration is less than or equal to the upper limit of the moving average time when the high-frequency acceleration value is processed by moving average.

[0044] S33, determine the relative relationship between the carrier velocity and the carrier longitudinal direction based on the difference between the average value of the longitudinal acceleration smoothing value under the transition state and the longitudinal acceleration smoothing value under the stationary state.

[0045] Specifically, when the absolute value of the difference is greater than or equal to the preset acceleration threshold, and the average value of the longitudinal acceleration smoothing value in the transition state is greater than the longitudinal acceleration smoothing value in the stationary state, then it is determined that the carrier velocity is the same as the longitudinal direction of the carrier.

[0046] When the absolute value of the difference is greater than or equal to the preset acceleration threshold, and the average value of the longitudinal acceleration smoothing value in the transition state is less than the longitudinal acceleration smoothing value in the stationary state, then it is determined that the carrier velocity is opposite to the longitudinal direction of the carrier.

[0047] When the absolute value of the difference is less than the preset acceleration threshold, it indicates that the carrier velocity and the longitudinal direction of the carrier are uncertain.

[0048] S34, when the absolute value of the difference is greater than or equal to the preset acceleration threshold, set the relative relationship flag between the carrier velocity and the longitudinal direction of the carrier, and determine that the carrier state changes from the transition state to the motion state.

[0049] The following example illustrates the detailed process of determining the relative relationship between the vehicle's velocity and its longitudinal direction based on velocity information and longitudinal acceleration smoothing values ​​from GNSS RTK navigation data. Figure 4 As shown:

[0050] (1) Initially, the carrier is in a ready state, and when the eastward velocity V E，i and northbound speed V N，i The absolute values ​​of all values ​​are consistently less than the first velocity threshold V. th When the time is right, it enters a static state; otherwise, it remains in a ready state. "Continue" is: V E，i and V N，i When the condition is met, timing begins, and the timing is greater than or equal to the first time threshold T. th .Right now:

[0051]

[0052] Among them, the first velocity threshold V th The value is 0.07 m / s, and the first time threshold T is... th The value ranges from 0.04 to 0.06 s, where i represents the i-th sampling time and Ts is the sampling time interval. i T represents the duration from the start of timing to the i-th sampling moment. i+1 This represents the duration from the start of timing to the (i+1)th sampling moment.

[0053] (2) In the stationary state, the smoothed longitudinal acceleration value calculated in S2 is recorded in real time and denoted as... Then, a transition state judgment is performed: when the absolute value of the eastward velocity VE,i or the northward velocity VN,i is continuously greater than the first velocity threshold V. th When the condition is met, the system enters a transition state; otherwise, it remains stationary. "Continue" is defined as follows: when VE,i or VN,i meets the condition, timing begins, and the timing is greater than or equal to the first time threshold Tth. That is:

[0054]

[0055] (3) During the transition state, record the eastward velocity VE,i, the northward velocity VN,i, and the smoothed longitudinal acceleration value calculated in S2 at time i, denoted as And count the number of sampling points.

[0056] (4) During the transition state, monitor whether it will transition to the static state. The judgment method is the same as step (1). If the judgment condition is met, enter the static state; otherwise, maintain the transition state.

[0057] (5) During the transition state, check whether the duration of the transition state is greater than or equal to the preset duration T. If the condition is not met, maintain the transition state. The preset duration T is between 0.5s and 2s. Otherwise, proceed as follows:

[0058] ① Calculate the smoothed values ​​of the eastward velocity, northward velocity, and longitudinal acceleration respectively. Average value:

[0059]

[0060] Where k is the number of sampling points. Then, the eastward velocity V is calculated. E，i Or northward speed V N，i Greater than the first velocity threshold V th The number of points N v ,Right now:

[0061] N v =N v +1, when |V E，i |>V th or |V N，i |>V th .

[0062] ② Further confirm the transition state. When the average eastward velocity... northbound speed mean If the number of speed exceedance points Nv does not meet the following conditions, proceed to the preparation state. Otherwise, continue with the subsequent steps.

[0063]

[0064] Where R takes the value 0.7.

[0065] ③ Determining and processing the relationship between the carrier velocity and the longitudinal direction of the carrier. This involves considering the average value of the smoothed longitudinal acceleration during the transition state. Smooth value of longitudinal acceleration with rest The difference exceeds the preset acceleration threshold A th At that time, a flag (VFlag) indicating the relationship between the carrier speed and the carrier's longitudinal direction is set. That is:

[0066]

[0067] Where VFlag is 1, it indicates that the velocity direction is the same as the longitudinal direction of the vehicle (front); VFlag is -1, it indicates that the velocity direction is opposite to the longitudinal direction of the vehicle (front); and VFlag is 0, it indicates that the relationship between the velocity direction and the longitudinal direction of the vehicle (front) is uncertain. Preset acceleration threshold A th The value is 0.098 m / s 2 .

[0068] ④ Transition from the transition state. When VFlag is 0, the system transitions to the ready state; otherwise, it transitions to the motion state.

[0069] (6) When in motion, monitor whether it will transition to a stationary state. The judgment conditions are the same as in step (1). If the conditions are met, enter the stationary state and set VFlag to 0; otherwise, maintain the motion state.

[0070] In the above embodiments, after the high-frequency acceleration values ​​are averaged, the noise output by the accelerometer is significantly reduced. Under these conditions, the change in carrier acceleration is significant. Therefore, when the carrier changes from a stationary state to a moving state through acceleration, the relative relationship between the carrier's velocity and its longitudinal direction can be accurately determined. The test results are as follows... Figure 5 As shown.

[0071] In determining the relative relationship between the carrier velocity and its longitudinal direction, the high-frequency acceleration values ​​were processed using a moving average. This effectively smooths out noise from the accelerometer output, yielding useful acceleration information, improving the signal-to-noise ratio, and enhancing sensitivity to acceleration changes. In this example, the minimum acceleration reached 0.01g (where g is Earth's gravity). Furthermore, since the moving average time for the high-frequency acceleration values ​​is 0.5s to 2s, and the duration threshold of the transition state does not exceed 2s, a rapid judgment can be made when continuous acceleration changes occur, with the judgment time within 2 seconds.

[0072] In determining the relative relationship between the carrier velocity and the longitudinal direction of the carrier, the sliding average time is 0.5s to 2s. Selecting this time range ensures that the actual acceleration changes are not masked, while also preventing the smoothing out of the actual acceleration changes, thus accurately detecting the acceleration changes.

[0073] In determining the relative relationship between the carrier velocity and the carrier's longitudinal direction, the first velocity threshold V... th The value is 0.07 m / s, and the first time threshold T is... th The values ​​range from 0.04 to 0.06 s. This set of data ensures that occasional loud noises or vehicle vibrations are not misjudged as continuous motion, and also avoids delays in judging continuous motion or even missing the moment when the acceleration changes the most.

[0074] In determining the relative relationship between the carrier speed and the longitudinal direction of the carrier, the ratio value R for the high speed is set to 0.7. This value ensures that a stationary state with large speed fluctuations is not mistakenly identified as a moving state, and that a moving state at low speeds is not mistakenly identified as a stationary state.

[0075] In determining the relative relationship between the carrier velocity and the longitudinal direction of the carrier, the preset acceleration threshold Ath is set to 0.098 m / s². 2 This ensures that a stationary state with high accelerometer noise is not misjudged as a moving state, and a moving state with low acceleration is not misjudged as a stationary state.

[0076] Optionally, the heading angle is determined based on the speed information and track angle in the GNSSRTK navigation data, including:

[0077] S41, when the horizontal ground speed is continuously greater than the second speed threshold and the duration is greater than or equal to the second preset time, the current track angle is determined to be valid; the second speed threshold is greater than the first speed threshold;

[0078] S42, with the current track angle valid, smooth the corresponding eastward and northward speeds to obtain the eastward filtered speed and the northward filtered speed;

[0079] S43, determine the heading angle based on the eastward and northward filtered speeds.

[0080] The following example illustrates the detailed process of determining the heading angle based on speed information and track angle from GNSS RTK navigation data. For instance... Figure 6 As shown:

[0081] (1) After acquiring GNSS velocity information, determine the validity of the current track angle. When the horizontal ground velocity V L，i Continuously greater than the second speed threshold V Lth Then, the current track angle is determined to be valid. "Continuously greater than" means: V Li When the condition is met, timing begins, and the timing exceeds the second time threshold T. Lth .Right now:

[0082]

[0083] Among them, T VL，i T represents the duration from the start of timing to the i-th sampling moment. VL，i+1 This represents the duration from the start of timing to the (i+1)th sampling moment. Second velocity threshold V Lth The value is 0.1 m / s, and the second time threshold T is... Lth The value ranges from 0.04 to 0.06 s, T S This is the sampling time interval. Once the track angle is valid, subsequent processing steps are performed; if the track angle is invalid, the system waits for the next moment to collect GNSS velocity information.

[0084] In this embodiment of the invention, the selection of the second speed threshold and the second time threshold can ensure that the track angle with a large error is not mistakenly judged as valid, which would lead to an increase in the calculation error of the subsequent heading angle; and also avoid the track angle being judged as invalid at low speeds.

[0085] (2) Horizontal velocity decomposition and smoothing filtering. Track angle H output from GNSS. t，i and horizontal ground velocity V L，i Obtain the eastward velocity V HE，i and northbound speed V HN，i The calculation is as follows:

[0086]

[0087] Then, smooth the eastward and northward velocities to obtain the eastward filtered velocity V. FE，i and northbound filter speed V FN，i The filtering calculation is as follows:

[0088]

[0089] In the above formula, i represents the i-th calculation time, i+1 represents the (i+1)-th calculation time, and α V The first filter coefficient is set to 0.6. Choosing this first filter coefficient improves the smoothing effect, reduces velocity noise, and ensures a fast response to velocity changes.

[0090] (3) Calculate the heading angle H from the smoothed filtered velocity. The calculation range of arctan is... The following calculations assume that H ranges from 0 to 2π rad. The heading angle H at time i+1. i+1 The calculation formula is as follows:

[0091]

[0092] For example:

[0093]

[0094] (4) Heading angle continuity processing and smoothing filtering. When the heading angle is near 0 or 2π, it will exhibit abrupt changes. Direct filtering will lead to errors. In this embodiment of the invention, continuity processing is performed first, followed by smoothing filtering, thereby obtaining an accurate smoothed heading angle H. F，i+1 .

[0095]

[0096] Among them, H i+1 H is the heading angle at time i+1 after continuous processing. i+1 To continuously process the heading angle at the first i+1 time step, H F，i Let be the heading angle at time i after filtering;

[0097] The formula for calculating the heading angle filter is as follows:

[0098] H F，i+1 =α H H i+1 +(1-α H )H F，i

[0099] Among them, H F，i+1 Let α be the heading angle at time i+1 after filtering. H This is the second filter coefficient, with a value of 0.15. The selection of this second filter coefficient can both improve the smoothing filtering effect and reduce heading noise, while also ensuring a timely response to heading changes.

[0100] For example: when H i+1 =1.995π, H F，i =0.02π, assuming no continuity processing is applied to the heading angle, i.e., using H i+1 Replace H′ i+1 The calculation is performed using the heading angle filtering formula, and the result is H. F，i+1 =0.316π, this calculation result is consistent with H i+1 and H F，i The deviations are all quite large; however, after performing continuity processing on the heading angle and then filtering, the calculated result is H. F，i+1 =0.00225π. Physically, 1.995π = 1.995π - 2π = -0.005π, and 0.00225π lies between H... i+1 and H F，i The deviations between these values ​​are small, thus significantly improving accuracy.

[0101] In this embodiment of the invention, by smoothing and filtering the heading angle, a heading angle with an accuracy of less than 3° (RMS) can be obtained when the speed is low. Figure 7The image shows a comparison of the heading angle at a speed of 1 km / h and the heading angle after smoothing filtering. The variance of the heading angle before filtering is 14.45°, and the variance of the heading angle after smoothing filtering is 1.17°.

[0102] Optionally, the smoothed filtering heading angle is corrected for deviation based on the relative relationship between the carrier speed and the carrier's longitudinal direction to obtain the initial heading value for initial alignment. The calculation formula is as follows:

[0103]

[0104] In other words, when the relative relationship between the carrier speed and the carrier longitudinal direction is opposite, π is added to the filtered heading angle to obtain the initial heading value for initial alignment; when the relative relationship between the carrier speed and the carrier longitudinal direction is the same, the filtered heading angle is determined as the initial heading value for initial alignment.

[0105] With this value H o，i This is the initial heading value for initial alignment. By combining the GNSS position and velocity values ​​with the IMU's horizontal attitude values, all navigation parameters can be obtained, and initial alignment is complete.

[0106] This invention also provides an initial alignment device for single-antenna GNSS and IMU integrated navigation, including a memory, a processor, and a computer program stored in the memory and executable on the processor. The feature is that when the processor executes the program, it implements the initial alignment method for single-antenna GNSS and IMU integrated navigation provided in the above embodiments.

[0107] This invention also provides a readable storage medium including instructions that, when executed on a processor, cause the processor to perform the initial alignment method for single-antenna GNSS and IMU integrated navigation provided by the above-described technical solution.

[0108] This invention combines IMU measurement data and GNSS RTK navigation data. The longitudinal acceleration smoothing value is obtained through the IMU measurement data. Based on the GNSS RTK navigation data and the longitudinal acceleration smoothing value, the relative relationship between the vehicle's initial velocity and its longitudinal direction can be accurately determined. Furthermore, the heading angle is determined based on the horizontal ground velocity and track angle, and the heading angle is then processed for continuity and smoothed to obtain a smoothed heading angle. Finally, the deviation of the smoothed heading angle is corrected based on the relative relationship between the vehicle's velocity and its longitudinal direction, resulting in a high-precision initial heading angle. This invention has advantages such as simple installation structure, low cost, strong resistance to magnetic interference, no need for frequent magnetometer parameter calibration, ease of use, and the ability to obtain high-precision initial heading angles over a wide speed range. It can further improve the automation and intelligence of agricultural machinery operations, reduce material and maintenance costs, and improve economic efficiency.

[0109] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working process of the above-described apparatus and unit can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0110] In the several embodiments provided by this invention, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed.

[0111] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of the embodiments of the present invention, depending on actual needs.

[0112] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0113] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0114] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for initial alignment of single-antenna GNSS and IMU integrated navigation, characterized in that, The method comprises: real-time acquisition of IMU measurement data and GNSS RTK navigation data; sliding average of high-frequency acceleration values in the IMU measurement data to obtain longitudinal acceleration smooth values; determination of the relative relationship between the carrier speed and the carrier longitudinal direction according to the speed information in the GNSS RTK navigation data and the longitudinal acceleration smooth values; the determination of the relative relationship between the carrier speed and the carrier longitudinal direction according to the speed information in the GNSS RTK navigation data and the longitudinal acceleration smooth values comprises: determination of the change of the carrier state according to the eastward speed and the northward speed; when the carrier state changes from the stationary state to the transition state and the transition state lasts for a preset time length, the average value of the longitudinal acceleration smooth values in the transition state is calculated; wherein the transition state is the state between the stationary state and the moving state; and the preset time length is less than or equal to the upper limit of the sliding average time when the sliding average is performed on the high-frequency acceleration values; determination of the relative relationship between the carrier speed and the carrier longitudinal direction according to the difference between the average value of the longitudinal acceleration smooth values in the transition state and the longitudinal acceleration smooth values in the stationary state; when the absolute value of the difference is greater than or equal to a preset acceleration threshold, the relative relationship between the carrier speed and the carrier longitudinal direction is set, and it is determined that the carrier state changes from the transition state to the moving state; determination of the heading angle according to the speed information in the GNSS RTK navigation data and the track angle, and continuity processing and smooth filtering of the heading angle to obtain a smooth filtered heading angle; deviation correction of the smooth filtered heading angle according to the relative relationship between the carrier speed and the carrier longitudinal direction to obtain an initial heading value for initial alignment.

2. The single-antenna GNSS and IMU integrated navigation initial alignment method according to claim 1, characterized in that, the determination of the change of the carrier state according to the eastward speed and the northward speed comprises: when the absolute values of the eastward speed and the northward speed both last for less than a first speed threshold and the time duration is greater than or equal to a first preset time, it is determined that the carrier state enters the stationary state; otherwise, it remains in the preparation state; when the absolute value of the eastward speed or the absolute value of the northward speed lasts for greater than the first speed threshold and the time duration is greater than or equal to the first preset time, it is determined that the carrier state enters the transition state; otherwise, it remains in the stationary state.

3. The single-antenna GNSS and IMU integrated navigation initial alignment method according to claim 1, characterized in that, the determination of the relative relationship between the carrier speed and the carrier longitudinal direction according to the difference between the average value of the longitudinal acceleration smooth values in the transition state and the longitudinal acceleration smooth values in the stationary state comprises: when the absolute value of the difference is greater than or equal to the preset acceleration threshold, and the average value of the longitudinal acceleration smooth values in the transition state is greater than the longitudinal acceleration smooth values in the stationary state, it is determined that the carrier speed and the carrier longitudinal direction are the same; when the absolute value of the difference is greater than or equal to the preset acceleration threshold, and the average value of the longitudinal acceleration smooth values in the transition state is less than the longitudinal acceleration smooth values in the stationary state, it is determined that the carrier speed and the carrier longitudinal direction are opposite; when the absolute value of the difference is less than the preset acceleration threshold, it is indicated that the carrier speed and the carrier longitudinal direction are uncertain.

4. The single-antenna GNSS and IMU integrated navigation initial alignment method according to claim 2, characterized in that, the determination of the heading angle according to the speed information in the GNSS RTK navigation data and the track angle comprises: determining that the current said course angle is valid when the horizontal ground speed is greater than a second speed threshold value and the duration is greater than or equal to a second preset time; the second speed threshold value is greater than the first speed threshold value; in the case that the current said course angle is valid, performing smoothing filtering on the corresponding eastward speed and northward speed to obtain an eastward filtered speed and a northward filtered speed; and determining a heading angle according to the eastward filtered speed and the northward filtered speed.

5. The single-antenna GNSS and IMU integrated navigation initial alignment method according to claim 4, characterized in that, the determination of the heading angle according to the eastward filtered speed and the northward filtered speed is according to the following formula: where i represents the i-th calculation time point, is the heading angle, represents the east filtered velocity, represents the north filtered velocity.

6. The single-antenna GNSS and IMU integrated navigation initial alignment method according to claim 4, characterized in that, the first preset time is 0.04-0.06s, and the first speed threshold value is 0.07m / s; the second preset time is 0.04-0.06s, and the second speed threshold value is 0.1m / s.

7. The single-antenna GNSS and IMU integrated navigation initial alignment method according to claim 5, characterized in that, the continuity processing and smoothing filtering of the heading angle to obtain a smoothing filtered heading angle is according to the following formula: wherein, represents the heading angle after continuity processing at i+1 time, represents the filtered heading angle at i time; the filtering formula is as follows: wherein, represents the filtered heading angle at time i+1, is a filter coefficient.

8. The single-antenna GNSS and IMU integrated navigation initial alignment method according to any one of claims 1 to 7, characterized in that, the deviation correction of the smoothing filtered heading angle according to the relative relationship between the carrier speed and the carrier longitudinal direction to obtain an initial alignment initial heading value includes: when the relative relationship between the carrier speed and the carrier longitudinal direction is opposite, adding π to the filtered heading angle to obtain the initial alignment initial heading value; when the relative relationship between the carrier speed and the carrier longitudinal direction is the same, determining the filtered heading angle as the initial alignment initial heading value.

9. An apparatus for initial alignment of single-antenna GNSS and IMU integrated navigation, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, the processor executes the program to implement the initial alignment method of the single-antenna GNSS and IMU combined navigation according to any one of claims 1-8.

Images