Online estimation method, device and medium for GNSS antenna pole arm and odometer pole arm

Abstract

The present invention relates to an online estimation method, device and medium for a GNSS antenna arm and an odometer arm. The method expands the GNSS antenna arm error and the odometer arm error into a Kalman filter state vector. During navigation settlement, the arm vector is estimated in real time without manual operation, thus avoiding the difficulty of operation for non-professionals and reducing the generation of measurement errors. The method can simultaneously estimate the GNSS antenna arm and the odometer arm, and can also estimate the GNSS antenna arm and the odometer arm vector separately, thus having good practicality.

Description

Technical Field

[0001] The present invention belongs to the field of integrated navigation, and in particular relates to an online estimation method, device and medium for a GNSS antenna arm and an odometer arm. Background Art

[0002] The GNSS / INS integrated navigation system is a classic combination model. GNSS offers high continuous positioning accuracy but is susceptible to environmental factors. INS uses recursive navigation and positioning, but this can lead to cumulative errors. Therefore, leveraging the complementary strengths of GNSS and INS can achieve high-precision positioning. However, due to the high noise levels of low-cost MEMS IMUs, INS navigation operates independently in tunnels, boulevards, and other areas where satellite signals are obscured, causing errors to rapidly diverge. In vehicle-mounted navigation systems, using an odometer (ODO) to assist INS navigation can suppress the divergence of INS errors, thereby achieving continuous, high-precision positioning in complex scenarios.

[0003] However, in the GNSS / INS / ODO combined navigation system, since the position coordinates calculated by the GNSS and the position coordinates calculated by the inertial navigation mechanical arrangement do not overlap, it is necessary to compensate for the arm effect when performing data fusion and solving on the two to improve the positioning accuracy of the combined navigation. In the process of odometer-assisted inertial navigation combined navigation, due to the position mismatch with the IMU, the speed measured by the vehicle will be inconsistent with the actual speed when turning. In particular, the greater the distance between the two, the greater the speed error. The arm in the combined navigation system is usually measured with a ruler in the X, Y, and Z directions, and then filled in the code to compensate for the arm error. This approach cannot be commercialized on a large scale, and it is easy to generate arm measurement errors during the measurement process. In response to this problem, the present invention proposes a method for online real-time estimation of the GNSS antenna arm and the odometer arm of the combined navigation system. Summary of the Invention

[0004] Invention Objective: To overcome the problems of low pole arm acquisition efficiency and the susceptibility to measurement errors in GNSS / INS / ODO integrated navigation systems, the present invention provides an online estimation method for GNSS antenna pole arm and odometer pole arm. This method can simultaneously estimate the GNSS antenna pole arm and odometer pole arm vectors, or independently estimate the GNSS antenna pole arm and odometer pole arm vectors.

[0005] The present invention is as follows:

[0006] An online estimation method for GNSS antenna pole arm and odometer pole arm,

[0007] Construct three Kalman filter models. The first model includes both GNSS lever arm error and odometer lever arm error. The second model only includes GNSS lever arm error. The third model only includes odometer lever arm error.

[0008] Acquire GNSS, INS and ODO data;

[0009] Select the corresponding system state transfer matrix and observation matrix according to different requirements; the selection of the filter model is controlled by a global variable;

[0010] The GNSS lever arm and the odometry lever arm are estimated online using the prediction and update process of the Kalman filter.

[0011] As a preferred method, the specific construction process of the Kalman filter model includes:

[0012] Constructing system state equations and observation equations

[0013]

[0014] Where x k 、x k-1 are the state vectors of the system at time k and time k-1 respectively, Φ k,k-1 represents the state one-step transfer matrix from k-1 to k time, Γ k-1 is the noise correlation matrix, z k is the observed value at time k, H k is the observation matrix at time k, w k-1 is the system noise, v k is the observation noise; where w k-1 and v k is white noise and uncorrelated;

[0015] Constructing the state vector

[0016] x k =[x0 lever GNSS T lever ODO T ] T (2)

[0017] Among them, x0 represents the position error, velocity error, attitude error and zero bias error of gyroscope and accelerometer of integrated navigation, lever GNSS T 、lever ODO T They represent the GNSS antenna arm error and the odometer arm error respectively.

[0018] Preferably, when acquiring GNSS, INS and ODO data, the microprocessor is connected to the GNSS receiver inertial navigation unit and the odometer respectively through the serial port, serial peripheral interface and timer to acquire GNSS, INS and ODO data; GNSS data includes position, satellite signal strength and timestamp information; INS data includes angular velocity and acceleration information; ODO data includes the vehicle's forward position increment and speed information.

[0019] Preferably, the corresponding system state transfer matrix and observation matrix are selected according to different requirements.

[0020] Preferably, the online estimation process includes: a prediction process and an update process;

[0021] During the prediction process, the Kalman filter algorithm uses the vehicle's dynamic model to predict the vehicle's navigation information, GNSS lever arm vector, odometer lever arm vector, and other information, and calculates the variance of the predicted values;

[0022] During the update process, the Kalman filter algorithm uses data from GNSS, INS and ODO to calculate the Kalman gain, and uses the Kalman gain to correct the predicted value; the calculation of the Kalman gain is based on the ratio of the variance of the observed value to the variance of the predicted value. The more accurate the observation, the greater the correction weight of the predicted value; after the update process, the state vector containing the GNSS arm error and the odometer arm error is obtained, and the arm estimate of the current epoch is calculated in combination with the initial setting value or the arm estimate of the previous epoch.

[0023] An online estimation system for GNSS antenna pole arm and odometer pole arm, comprising

[0024] The first module is configured to construct three Kalman filter models, wherein the first model includes both GNSS lever arm errors and odometer lever arm errors, the second model includes only GNSS lever arm errors, and the third model includes only odometer lever arm errors;

[0025] The second module is configured to acquire GNSS, INS and ODO data;

[0026] The third module is configured to select the corresponding system state transfer matrix and observation matrix according to different requirements; the selection of the filter model is controlled by a global variable;

[0027] The fourth module is configured to estimate the GNSS lever arm and the odometer lever arm online using a prediction and update process of a Kalman filter.

[0028] As a preferred method, the specific construction process of the Kalman filter model includes:

[0029] Constructing system state equations and observation equations

[0030]

[0031] Where x k 、x k-1 are the state vectors of the system at time k and time k-1 respectively, Φ k,k-1 represents the state one-step transfer matrix from k-1 to k time, Γ k-1 is the noise correlation matrix, z k is the observed value at time k, H k is the observation matrix at time k, w k-1 is the system noise, v k is the observation noise; where w k-1 and v k is white noise and uncorrelated;

[0032] Constructing the state vector

[0033] x k =[x0 lever GNSS T lever ODO T ] T (2)

[0034] Among them, x0 represents the position error, velocity error, attitude error and zero bias error of gyroscope and accelerometer of integrated navigation, lever GNSS T 、lever ODO T They represent the GNSS antenna arm error and the odometer arm error respectively.

[0035] Preferably, when acquiring GNSS, INS, and ODO data, the microprocessor is connected to the GNSS receiver inertial navigation unit and the odometer through the serial port, serial peripheral interface, and timer, respectively, to acquire GNSS, INS, and ODO data; GNSS data includes position, satellite signal strength, and timestamp information; INS data includes angular velocity and acceleration information; and ODO data includes the vehicle's forward position increment and velocity information.

[0036] Select the corresponding system state transfer matrix and observation matrix according to different requirements;

[0037] The online estimation process includes: prediction process and update process;

[0038] During the prediction process, the Kalman filter algorithm uses the vehicle's dynamic model to predict the vehicle's navigation information, GNSS lever arm vector, odometer lever arm vector, and other information, and calculates the variance of the predicted values;

[0039] During the update process, the Kalman filter algorithm uses data from GNSS, INS and ODO to calculate the Kalman gain, and uses the Kalman gain to correct the predicted value; the calculation of the Kalman gain is based on the ratio of the variance of the observed value to the variance of the predicted value. The more accurate the observation, the greater the correction weight of the predicted value; after the update process, the state vector containing the GNSS arm error and the odometer arm error is obtained, and the arm estimate of the current epoch is calculated in combination with the initial setting value or the arm estimate of the previous epoch.

[0040] An electronic device, a computer-readable storage medium storing computer-executable instructions; and one or more processors, the one or more processors being coupled to the computer-readable storage medium and configured to execute the computer-executable instructions so that the device performs the method according to any one of claims 1-5.

[0041] A readable storage medium stores computer-executable instructions, which, when executed by a processor, configure the processor to perform the method according to any one of claims 1 to 5.

[0042] The present invention provides a method for online real-time estimation of the GNSS antenna arm and odometer arm of an integrated navigation system. The method expands the GNSS antenna arm error and the odometer arm error into a Kalman filter state vector. During navigation settlement, the arm vector is estimated in real time without manual operation to measure the arm vector, thereby avoiding operational difficulties for non-professionals and reducing the generation of measurement errors. The method can be combined with GNSS, INS and ODO data for processing, and has high positioning accuracy and real-time performance. In addition, the method can simultaneously estimate the GNSS antenna arm and the odometer arm, and can also estimate the GNSS antenna arm and the odometer arm vector separately, and has good practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 This is a flowchart for estimating the GNSS antenna arm and odometer arm.

[0044] Figure 2 It is a relationship diagram of the spatial position coordinates between the GNSS antenna phase and the IMU measurement center.

[0045] Figure 3 It is a relationship diagram of the spatial position coordinates between the wheel contact point and the IMU measurement center. DETAILED DESCRIPTION

[0046] The present invention will be further described below with reference to the accompanying drawings and specific implementations.

[0047] Example 1

[0048] As attached Figure 1The figure shows a flow chart for estimating the GNSS antenna arm and odometer arm. The specific steps of the present invention are as follows:

[0049] Step 1: Establish three Kalman filter models. The first model includes both GNSS arm error and odometer arm error, the second model only includes GNSS arm error, and the third model only includes odometer arm error. The details are as follows:

[0050] 1.1 Select an appropriate system state vector. The classic GNSS / INS integrated navigation filter model selects a 21-dimensional vector, which includes the position error, velocity error, attitude error, bias error of the gyroscope and accelerometer, and scale factor error of the integrated navigation. However, for low-cost MEMS IMU, due to the large random error, the scale factor error modeling not only has limited improvement on the filtering performance, but also increases the amount of calculation. Therefore, the present invention selects a total of 21-dimensional vectors as the system state vector, including the position error, velocity error, attitude error, bias error of the gyroscope and accelerometer, and the GNSS antenna arm error and the odometer arm error, which are specifically:

[0051] x k =[(δr n ) T (δv n ) T φ T b g T b a T lever GNSS T lever ODO T ] T (3)

[0052] Among them, (δr n ) T 、(δv n ) T 、φ T Respectively represent the position error, velocity error and attitude error of the integrated navigation, b g T 、b a T Represents the bias error of the gyroscope and accelerometer, lever GNSS T 、lever ODO T They represent the GNSS antenna arm error and the odometer arm error respectively.

[0053] 1.2 Construct the system state equation:

[0054] x k=Φ k,k-1 x k-1 +Γ k-1 w k-1 (4)

[0055] Where x k 、x k-1 are the state vectors of the system at time k and time k-1 respectively, Φ k,k-1 represents the state one-step transfer matrix from k-1 to k time, Γ k-1 is the noise correlation matrix, w k-1 is the system noise. The transfer matrices of the three Kalman filter models are expressed as:

[0056] Simultaneously estimating the transfer matrix Φ k,k-1 (1) Constructed as

[0057]

[0058] When estimating only the GNSS lever arm, the transfer matrix Φ k,k-1 (2) Constructed as

[0059]

[0060] When only estimating the odometry arm, the transfer matrix Φ k,k-1 (3) Constructed as

[0061]

[0062] in, I3 is the three-dimensional identity matrix, f n is the relative force under the n system, t gb and t ab are the gyro and accelerometer bias correlation times, respectively.

[0063] 1.3 Constructing the system observation equation:

[0064] z k =H k x k +v k (8)

[0065] Where z k is the observed value at time k, H k is the observation matrix at time k, x k is the predicted value of the state vector, v k is the observation noise. The observation matrices of the three Kalman filter models are expressed as:

[0066] When estimating simultaneously, the observation matrix H1 is constructed as

[0067]

[0068] When only estimating the GNSS lever arm, the observation matrix H2 is constructed as

[0069]

[0070] When only estimating the odometer arm, the observation matrix H3 is constructed as

[0071]

[0072] Among them, I 3×3 is the identity matrix of dimension 3, 0 3×3 is an all-zero matrix of dimension 3, is the transfer matrix from the carrier system to the navigation system, The angular velocity of the carrier system relative to the navigation system.

[0073] Step 2: Acquire GNSS, INS, and ODO data. The GNSS / INS / ODO navigation system consists of a microprocessor, a GNSS receiver module, an inertial navigation unit, and an odometer. The microprocessor connects to the GNSS receiver, the inertial navigation unit, and the odometer via a serial port, a serial peripheral interface, and a timer to acquire GNSS, INS, and ODO data. GNSS data includes information such as position, satellite signal strength, and timestamp; INS data includes information such as angular velocity and acceleration; and ODO data includes the vehicle's forward position increment and velocity information.

[0074] Step 3: Select the appropriate system state transfer matrix and observation matrix as needed. By default, the simultaneous estimation of the GNSS lever arm and the odometry lever arm model is used. If necessary, the variable macro control selects a different system state transfer matrix and observation matrix.

[0075] Step 4: Estimate the GNSS lever arm and the odometry lever arm online using the prediction and update process of the Kalman filter.

[0076] One-step prediction process:

[0077]

[0078]

[0079] Where, is the state value predicted one step ahead, is the optimal estimated value of the state at time k-1, P k,k-1 is the state covariance matrix of one-step prediction, P k-1 is the optimal estimate of the state covariance at time k-1, Q k-1 is the system noise covariance matrix at time k-1.

[0080]

[0081]

[0082]

[0083] Where K k is the gain matrix, R k is the measurement noise covariance matrix, is the optimal estimated value of the state at time k, P k is the optimal estimate of the state covariance at time k.

[0084] After the update, the state vector containing the GNSS lever arm error and the odometer lever arm error is obtained, and the lever arm estimate of the current epoch is calculated by combining the initial setting value or the lever arm estimate of the previous epoch.

[0085] Example 2

[0086] Based on the same inventive concept, this embodiment also provides an online estimation system for GNSS antenna pole arms and odometer pole arms, comprising:

[0087] The first module is configured to construct three Kalman filter models, wherein the first model includes both GNSS lever arm errors and odometer lever arm errors, the second model includes only GNSS lever arm errors, and the third model includes only odometer lever arm errors;

[0088] The second module is configured to acquire GNSS, INS and ODO data;

[0089] The third module is configured to select the corresponding system state transfer matrix and observation matrix according to different requirements; the selection of the filter model is controlled by a global variable;

[0090] The fourth module is configured to estimate the GNSS lever arm and the odometer lever arm online using a prediction and update process of a Kalman filter.

[0091] Example 3

[0092] Based on the same inventive concept, this embodiment also provides an electronic device, a computer-readable storage medium storing computer-executable instructions; and one or more processors, the one or more processors being coupled to the computer-readable storage medium and configured to execute the computer-executable instructions so that the device executes the method described in Example 1.

[0093] Example 4

[0094] Based on the same inventive concept, this embodiment further provides a readable storage medium storing computer-executable instructions. When the computer-executable instructions are executed by a processor, the processor is configured to execute the method described in Example 1.

[0095] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code. The scheme in the embodiment of the present application can be implemented in various computer languages, for example, object-oriented programming language Java and literal translation scripting language JavaScript, etc.

[0096] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the steps in the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0097] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0098] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 The steps for the function specified in one or more boxes.

[0099] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0100] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. An online estimation method for GNSS antenna arm and odometer arm, characterized in that: include Construct three Kalman filter models. The first model includes both GNSS lever arm error and odometer lever arm error. The second model only includes GNSS lever arm error. The third model only includes odometer lever arm error. Acquire GNSS, INS and ODO data; Select the corresponding system state transfer matrix and observation matrix according to different requirements; The selection of the filtering model is controlled by a global variable; Online estimation of GNSS and odometer lever arms using the Kalman filter prediction and update process; The online estimation process includes: prediction process and update process; During the prediction process, the Kalman filter algorithm uses the vehicle's dynamic model to predict the vehicle's navigation information, GNSS lever arm vector, and odometer lever arm vector information, and calculates the variance of the predicted values; During the update process, the Kalman filter algorithm uses data from GNSS, INS and ODO to calculate the Kalman gain, and uses the Kalman gain to correct the predicted value; the calculation of the Kalman gain is based on the ratio of the variance of the observed value to the variance of the predicted value. The more accurate the observation, the greater the correction weight of the predicted value; after the update process, the state vector containing the GNSS arm error and the odometer arm error is obtained, and the arm estimate of the current epoch is calculated in combination with the initial setting value or the arm estimate of the previous epoch.

2. The online estimation method for GNSS antenna pole arm and odometer pole arm according to claim 1, characterized in that: The specific construction process of the Kalman filter model includes Constructing system state equations and observation equations Where x k 、x k-1 are the state vectors of the system at time k and time k-1 respectively, Φ k,k-1 represents the state one-step transfer matrix from k-1 to k time, Γ k-1 is the noise correlation matrix, z k is the observed value at time k, H k is the observation matrix at time k, w k-1 is the system noise, v k is the observation noise; where w k-1 and v k is white noise and uncorrelated; Constructing the state vector x k =[x0 lever GNSS T lever ODO T ] T (2) Among them, x0 represents the position error, velocity error, attitude error and zero bias error of gyroscope and accelerometer of integrated navigation, lever GNSS T 、lever ODO T They represent the GNSS antenna arm error and the odometer arm error respectively.

3. The online estimation method for GNSS antenna pole arm and odometer pole arm according to claim 1, characterized in that: When acquiring GNSS, INS, and ODO data, the microprocessor connects to the GNSS receiver inertial navigation unit and odometer through the serial port, serial peripheral interface, and timer to obtain GNSS, INS, and ODO data; GNSS data includes position, satellite signal strength, and timestamp information; INS data includes angular velocity and acceleration information; and ODO data includes the vehicle's forward position increment and velocity information.

4. The online estimation method for GNSS antenna pole arm and odometer pole arm according to claim 1, characterized in that: Select the corresponding system state transfer matrix and observation matrix according to different requirements.

5. An online estimation system for GNSS antenna pole arms and odometer pole arms, characterized in that: include The first module is configured to construct three Kalman filter models, wherein the first model includes both GNSS lever arm errors and odometer lever arm errors, the second model includes only GNSS lever arm errors, and the third model includes only odometer lever arm errors; The second module is configured to acquire GNSS, INS and ODO data; The third module is configured to select the corresponding system state transfer matrix and observation matrix according to different requirements; The selection of the filtering model is controlled by a global variable; A fourth module is configured to estimate the GNSS lever arm and the odometer lever arm online using a prediction and update process of a Kalman filter; When acquiring GNSS, INS, and ODO data, the microprocessor connects to the GNSS receiver inertial navigation unit and odometer through the serial port, serial peripheral interface, and timer to obtain GNSS, INS, and ODO data. GNSS data includes position, satellite signal strength, and timestamp information; INS data includes angular velocity and acceleration information; and ODO data includes the vehicle's forward position increment and velocity information. Select the corresponding system state transfer matrix and observation matrix according to different requirements; The online estimation process includes: prediction process and update process; During the prediction process, the Kalman filter algorithm uses the vehicle's dynamic model to predict the vehicle's navigation information, GNSS lever arm vector, and odometer lever arm vector information, and calculates the variance of the predicted values; During the update process, the Kalman filter algorithm uses data from GNSS, INS and ODO to calculate the Kalman gain, and uses the Kalman gain to correct the predicted value; the calculation of the Kalman gain is based on the ratio of the variance of the observed value to the variance of the predicted value. The more accurate the observation, the greater the correction weight of the predicted value; after the update process, the state vector containing the GNSS arm error and the odometer arm error is obtained, and the arm estimate of the current epoch is calculated in combination with the initial setting value or the arm estimate of the previous epoch.

6. The system according to claim 5, characterized in that The specific construction process of the Kalman filter model includes Constructing system state equations and observation equations Where x k 、x k-1 are the state vectors of the system at time k and time k-1 respectively, Φ k,k-1 represents the state one-step transfer matrix from k-1 to k time, Γ k-1 is the noise correlation matrix, z k is the observed value at time k, H k is the observation matrix at time k, w k-1 is the system noise, v k is the observation noise; where w k-1 and v k is white noise and uncorrelated; Constructing the state vector x k =[x0 lever GNSS T lever ODO T ] T (2) Among them, x0 represents the position error, velocity error, attitude error and zero bias error of gyroscope and accelerometer of integrated navigation, lever GNSS T 、lever ODO T They represent the GNSS antenna arm error and the odometer arm error respectively.

7. An electronic device, characterized in that: A computer-readable storage medium storing computer-executable instructions; and one or more processors coupled to the computer-readable storage medium and configured to execute the computer-executable instructions so that the device performs the method according to any one of claims 1 to 4.

8. A readable storage medium, characterized in that: The computer-executable instructions are stored, which, when executed by a processor, configure the processor to perform the method according to any one of claims 1 to 4.

Images