RTK + TDCP / INS integrated navigation method for accurately controlling dynamic displacement of vehicle in urban complex environment

Abstract

The invention provides an RTK + TDCP / INS integrated navigation method for accurately controlling the dynamic displacement of a vehicle in an urban complex environment, which comprises the following steps: acquiring GNSS original observation values including a carrier phase observation value and a pseudo-range observation value, and simultaneously performing MEMS IMU data acquisition to extract INS information; vehicle-mounted dynamic measurement displacement detection: establishing a TDCP observation equation based on a carrier phase observation value, and obtaining a three-dimensional displacement amount between adjacent epochs through least square estimation; dynamically adjusting the noise in the filtering process, namely extracting information for the next step of integrated navigation according to a result obtained by displacement detection and a measurement updating matrix of RTK (Real-Time Kinematic); data fusion and filtering: inputting the position, speed and attitude information of the INS and the GNSS into a robust Kalman filter, when the number of visible satellites is greater than a preset value, isolating abnormal data, otherwise, performing data fusion by adopting an innovation-based adaptive Kalman filtering mode; and outputting the dynamic displacement of the vehicle, including realizing accurate control of the dynamic displacement of the vehicle in the urban complex environment through a data filtering result.

Description

Technical Field

[0001] The present invention belongs to the field of navigation and positioning technology, and relates to an RTK+TDCP / INS combined navigation technology solution for precise control of vehicle dynamic displacement in complex urban environments. Background Art

[0002] With the continuous development of multi-mode, multi-frequency GNSS RTK technology and the continued growth of ground-based augmentation systems, GNSS has achieved centimeter-level dynamic positioning accuracy in open environments. However, in complex urban environments such as tree-lined avenues, canyons with tall buildings, and tunnels, GNSS signals are frequently affected by fading, obstruction, and reflection, resulting in a sharp decline in GNSS observation quality and, consequently, poor GNSS positioning accuracy. To improve GNSS positioning performance, GNSS and INS are often combined to leverage their complementary advantages, thereby enhancing navigation and positioning accuracy, availability, and reliability. However, in complex environments, satellite signals are often obstructed, leading to insufficient or interrupted observations. This can cause the performance of MEMS INS / GNSS combined navigation to plummet, even degenerating to that of a single MEMS INS. Errors can quickly accumulate, even rendering the navigation solution ineffective, making it difficult to meet the high-precision navigation and positioning requirements of vehicles. Summary of the Invention

[0003] In order to achieve precise control and optimization of vehicle dynamic displacement in complex urban environments, the present invention provides an RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in complex urban environments.

[0004] The above technical problems of the present invention are mainly solved by the following technical solutions: An RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in complex urban environments performs the following process: Collecting GNSS raw observations, including carrier phase observations and pseudorange observations, while also collecting MEMS IMU data to extract INS information; On-board dynamic measurement displacement detection, including establishing the TDCP observation equation based on carrier phase observations and obtaining the three-dimensional displacement between adjacent epochs through least squares estimation; Dynamic adjustment of the noise in the filtering process, including combining the results of vehicle-mounted dynamic measurement displacement detection with the RTK measurement update matrix to extract information for the next step of integrated navigation; Data fusion and filtering, including inputting the position, velocity, and attitude information of INS and GNSS into the robust Kalman filter. When the number of visible satellites exceeds a preset value, abnormal data is isolated. Otherwise, data fusion is performed using an adaptive Kalman filter based on innovation information. Output vehicle dynamic displacement, including through data filtering results, to achieve precise control of vehicle dynamic displacement in complex urban environments.

[0005] Furthermore, whether the object under test has undergone significant dynamic displacement is determined based on the three-dimensional displacement between adjacent epochs.

[0006] Moreover, when the amplitude of the three-dimensional displacement is greater than the corresponding preset threshold C, it is considered that rapid displacement has occurred; when the amplitude of the three-dimensional displacement is less than the corresponding preset threshold C0, it is considered as noise in the dynamic measurement result; the setting of the preset thresholds C and C0 is based on the accuracy of the dynamic measurement solution.

[0007] Moreover, when dynamically adjusting the filtering process noise, the filtering process noise adjustment factor is calculated, and the process noise variance-covariance matrix in the ENU coordinate system is dynamically adjusted to match the actual dynamic changes of the vehicle.

[0008] Moreover, when the number of visible satellites is greater than 4, abnormal data is isolated; when the number of satellites is less than or equal to 4, an adaptive Kalman filtering method based on new information is used for data fusion.

[0009] Furthermore, isolating abnormal data includes identifying and isolating GNSS measurement data anomalies caused by multipath effects or external interference.

[0010] Furthermore, the innovation-based adaptive Kalman filtering method adjusts filter parameters to improve navigation accuracy through real-time updating of innovation covariance.

[0011] On the other hand, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and run on the processor. When the processor executes the program, it implements the RTK+TDCP / INS combined navigation method for precise control of vehicle dynamic displacement in complex urban environments as described above.

[0012] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that when the computer program is executed by a processor, the RTK+TDCP / INS combined navigation method for precise control of vehicle dynamic displacement in a complex urban environment as described above is implemented.

[0013] On the other hand, the present invention also provides a computer program product, including a computer program, which, when executed by a processor, implements the RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in a complex urban environment as described above.

[0014] To address the challenges of precisely controlling and optimizing vehicle dynamic displacement in complex urban environments, a robust Kalman filter-based RTK+TDCP / INS integrated navigation solution was proposed. This solution uses TDCP estimation within the on-board dynamic measurement system to reflect the dynamic position changes of the measured vehicle between epochs. This robust Kalman filter, combined with INS information, adaptively adjusts for system process noise. This adjustment mechanism ensures that the filtered state estimate is highly consistent with the vehicle's actual dynamic deformation state, effectively reflecting rapid dynamic changes. Combined with error detection and identification techniques, the robust Kalman model can further detect and isolate error sources, improving the accuracy and robustness of dynamic measurements and providing more reliable and accurate data support for on-board dynamic measurements.

[0015] The solution of the present invention is simple and convenient to implement and has strong practicality. It solves the problems of low practicality and inconvenience in actual application existing in related technologies, can improve user experience, and has important market value. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a flow chart of a method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0017] The following will further illustrate the concept, specific structure and technical effects of the present invention in conjunction with the accompanying drawings and embodiments, so as to fully understand the purpose, characteristics and effects of the present invention.

[0018] In order to solve the problem of precise control and optimization of vehicle dynamic displacement in complex urban environments, this paper proposes an RTK+TDCP / INS integrated navigation method based on a robust Kalman filter. This method introduces TDCP to calculate the displacement between consecutive epochs to reflect the dynamic position change of the measured vehicle between adjacent epochs. Then, the state prediction process noise of the Kalman filter is adjusted according to the size of the vehicle displacement to ensure that the process noise is coordinated with the epoch-to-epoch change of the position state of the measured object, so that the result of the Kalman filter is consistent with the actual position state measured by the on-board dynamic measurement system, and the rapid dynamic changes of the measured object can be accurately captured. This method includes two core links: (1) Dynamic adjustment of vehicle-mounted dynamic measurement displacement detection and filtering process noise. (2) A robust Kalman filter that combines error detection and recognition with adaptive filtering based on new information.

[0019] See also Figure 1, an embodiment of the present invention establishes an anti-error filtering model based on the RTK+TDCP / INS combined navigation system. The model monitors the three-dimensional position change of the vehicle between consecutive epochs through the TDCP estimation in the vehicle-mounted dynamic measurement system, thereby accurately identifying and judging the significant displacement of the vehicle. When the original process noise model is difficult to accurately match the actual dynamic state changes of the vehicle due to road conditions, load changes or other factors, the anti-error Kalman model will adaptively adjust the system process noise. This adjustment mechanism ensures that the filter state estimation result is highly consistent with the actual dynamic deformation state of the vehicle, and effectively reflects the rapid dynamic changes of the vehicle. Combined with error detection and identification technology, the anti-error Kalman model can further detect and isolate error sources, improve the accuracy and robustness of dynamic measurement, and provide more reliable and accurate data support for vehicle-mounted dynamic measurement. The specific steps are as follows: Step 1: Obtain GNSS and INS observations. Collect GNSS raw observations, including carrier phase observations, pseudorange observations, etc., and simultaneously collect MEMS IMU data. Step 2: On-board dynamic measurement displacement detection. Build the TDCP observation equation based on the carrier phase observation value, perform least squares estimation, and obtain the TDCP on-board displacement; The present invention is based on the TDCP vehicle-mounted dynamic measurement model to solve the change in the three-dimensional coordinates of the measured object between consecutive epochs. In order to further improve the accuracy and reliability of dynamic displacement solution, it is recommended to adopt a quality control scheme that combines the consistency test of pre-test measurement values with the IGGⅢ robust iteration after test. Subsequently, the inter-epoch displacement obtained by dynamic measurement is used to determine whether the measured object has undergone significant dynamic displacement: when the actual displacement is When the displacement is greater than the preset threshold C, it is considered that a rapid displacement has occurred; when the displacement is greater than the preset threshold C, it is considered that a rapid displacement has occurred. If the value is less than the preset threshold C0, it is considered as noise in the dynamic measurement result. The setting of thresholds C and C0 is mainly based on the accuracy of the dynamic measurement solution.

[0020] Step 3: Dynamic adjustment of the noise in the filtering process. Based on the TDCP displacement detection and combined with the RTK measurement update matrix, the information for the next step of integrated navigation is obtained. The implementation process of this step includes the following formulas (1) and (2); The process noise adjustment is implemented in the ENU coordinate system. Assume that the vehicle displacement in the E, N, and U directions detected by TDCP in the current epoch is (Unit: m), where R is the rotation matrix from the XYZ coordinate system to the ENU coordinate system. The process noise spectral density at the ENU position is (Unit: m·s -1 / 2 ), is the time interval (unit: s), and its filtering process noise adjustment factor (Unit: s) The calculation formula in the ENU direction is: (1) The process noise variance-covariance matrix after the current epoch position state adjustment is expressed as: (2) in, Represents the process noise variance-covariance matrix in the ENU direction.

[0021] Step 4: Data Fusion and Filtering. The inertial navigation and GNSS position, velocity, and attitude information are fed into a robust Kalman filter. When the number of satellites is greater than four, error detection and identification techniques are used to further isolate error sources, improving the quality and reliability of dynamic measurement data. When the number of satellites is four or fewer, an adaptive Kalman filter based on innovation is used to fuse and filter the data, improving navigation accuracy and reliability. In the dynamic adjustment of vehicle-mounted dynamic measurement displacement detection and filtering process noise, Kalman filtering relaxes state constraints by matching the state process noise level with the vehicle-mounted dynamic measurement displacement, effectively preventing filtering distortion caused by improper state constraints and ensuring a true and accurate vehicle-mounted dynamic measurement displacement state.

[0022] When a vehicle is in an environment where GNSS signals are susceptible to interference, such as an urban canyon, tunnel, or dense building complex, and the number of visible satellites exceeds four, traditional navigation systems may produce measurement errors due to multipath effects or external interference. To solve this problem, the robust Kalman filter model introduced in the present invention utilizes error detection and identification technology to monitor and accurately identify GNSS measurement data anomalies caused by multipath delay or interference in real time. This process not only improves the robustness of the system, but also ensures that inaccurate measurement data can be effectively distinguished and eliminated even in highly dynamic and high-noise environments. Taking into account that when there is no fault, the new information vector is zero-mean Gaussian white noise, the present invention further proposes an error detection and identification implementation method that determines whether a fault has occurred by detecting the mean of the new information vector.

[0023] The robust Kalman filter model also incorporates innovation-based adaptive Kalman filtering, which dynamically adjusts the measurement noise covariance matrix through real-time updates of the innovation covariance. This model adaptively adjusts the filter parameters based on the current GNSS signal strength and quality, effectively suppressing errors caused by partial or complete GNSS signal outages and other forms of interference, significantly improving the adaptability and reliability of the integrated navigation system in complex environments. Specifically, while traditional filters input measurement errors as fixed values, the present invention monitors GNSS observations in real time, detecting measurement innovations (the difference between state estimates and measurements) in the filter. Within the integrated navigation system, the gain matrix is used to timely adjust the weights between predicted and actual observations, thereby improving the positioning accuracy and stability of the GNSS / INS integrated navigation system.

[0024] Step 5: Output accurate vehicle dynamic displacement. Through the above data processing and fusion steps, accurate control of the vehicle's dynamic displacement can be achieved in complex urban environments, providing high-precision and reliable positioning and navigation information.

[0025] In specific implementation, the method proposed in the technical solution of the present invention can be automatically run by those skilled in the art using computer software technology. System devices that implement the method, such as computer-readable storage media that store the corresponding computer program of the technical solution of the present invention and computer equipment that runs the corresponding computer program, should also be within the scope of protection of the present invention.

[0026] The following describes the RTK+TDCP / INS combined navigation electronic device for precise control of vehicle dynamic displacement in a complex urban environment provided by the present invention. The RTK+TDCP / INS combined navigation electronic device for precise control of vehicle dynamic displacement in a complex urban environment described below and the RTK+TDCP / INS combined navigation method for precise control of vehicle dynamic displacement in a complex urban environment described above can be referenced to each other.

[0027] The electronic device may include a processor, a communications interface, a memory, and a communications bus, wherein the processor, the communications interface, and the memory communicate with each other via the communications bus. The processor may invoke logic instructions in the memory to execute an RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in complex urban environments, primarily including the software processing portion of the aforementioned steps.

[0028] Furthermore, the logical instructions in the aforementioned memory can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or a portion 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 for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage media include various media capable of storing program code, such as USB flash drives, mobile hard drives, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical disks.

[0029] On the other hand, the present invention also provides a computer program product, which includes a computer program, which can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the software processing part of the RTK+TDCP / INS combined navigation method for precise control of vehicle dynamic displacement in complex urban environments provided by the above methods.

[0030] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the software processing part of the RTK+TDCP / INS combined navigation method for precise control of vehicle dynamic displacement in complex urban environments provided by the above methods.

[0031] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0032] Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented using software plus a necessary general-purpose hardware platform, or of course, hardware. Based on this understanding, the essence of the above technical solution, or the portion that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, or an optical disk, and includes a number of instructions for causing a computer device (such as a personal computer, server, or network device) to execute the methods described in each embodiment or certain portions of the embodiments.

[0033] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. An RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in complex urban environments, characterized by: Carry out the following process, Collecting GNSS raw observations, including carrier phase observations and pseudorange observations, while also collecting MEMS IMU data to extract INS information; On-board dynamic measurement displacement detection, including establishing the TDCP observation equation based on carrier phase observations and obtaining the three-dimensional displacement between adjacent epochs through least squares estimation; Dynamic adjustment of the noise in the filtering process, including combining the results of vehicle-mounted dynamic measurement displacement detection with the RTK measurement update matrix to extract information for the next step of integrated navigation; Data fusion and filtering, including inputting the position, velocity, and attitude information of INS and GNSS into the robust Kalman filter. When the number of visible satellites exceeds a preset value, abnormal data is isolated. Otherwise, data fusion is performed using an adaptive Kalman filter based on innovation information. Output vehicle dynamic displacement, including through data filtering results, to achieve precise control of vehicle dynamic displacement in complex urban environments.

2. The integrated navigation method according to claim 1, wherein: The three-dimensional displacement between adjacent epochs is used to determine whether the object under test has undergone significant dynamic displacement.

3. The integrated navigation method according to claim 2, wherein: When the amplitude of the three-dimensional displacement is greater than the corresponding preset threshold C, it is considered that rapid displacement has occurred; when the amplitude of the three-dimensional displacement is less than the corresponding preset threshold C0, it is considered as noise in the dynamic measurement result; the setting of the preset thresholds C and C0 is based on the accuracy of the dynamic measurement solution.

4. The integrated navigation method according to claim 1, wherein: When dynamically adjusting the filtering process noise, the filtering process noise adjustment factor is calculated, and the process noise variance-covariance matrix in the ENU coordinate system is dynamically adjusted to match the actual dynamic changes of the vehicle.

5. The integrated navigation method according to claim 1, wherein: When the number of visible satellites is greater than 4, abnormal data is isolated; when the number of satellites is less than or equal to 4, data fusion is performed using an adaptive Kalman filter based on new information.

6. The integrated navigation method according to claim 1, wherein: The isolating abnormal data includes identifying and isolating GNSS measurement data anomalies caused by multipath effects or external interference.

7. The integrated navigation method according to claim 1, wherein: The innovation-based adaptive Kalman filtering method adjusts filter parameters to improve navigation accuracy through real-time updating of innovation covariance.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in a complex urban environment as described in any one of claims 1 to 7 is implemented.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in a complex urban environment as claimed in any one of claims 1 to 7 is implemented.

10. A computer program product comprising a computer program, characterized in that: When the computer program is executed by a processor, the RTK+TDCP / INS integrated navigation method for precise control of vehicle dynamic displacement in a complex urban environment as claimed in any one of claims 1 to 7 is implemented.

Images