Laser doppler velocimeter online calibration method and device based on position observation

Abstract

This application relates to an online calibration method and apparatus for a laser Doppler velocimeter based on position observation. The method divides the calibration process into two stages: coarse calibration and fine calibration. In the coarse calibration stage, an analytical method is employed. In the fine calibration stage, a robust Kalman filter is first designed to obtain accurate carrier attitude, velocity, and position information. Then, another Kalman filter is designed using the error propagation model of the laser Doppler velocimeter based on position observation to further calibrate the laser Doppler velocimeter. To reduce the impact of non-holonomic constraints being violated during vehicle turning on the calibration results, the output of an astrogyroscope and the output velocity of the laser Doppler velocimeter are used to distinguish whether the vehicle is changing direction, and lateral velocity compensation is performed on the laser Doppler velocimeter's velocity in its own coordinate system when the vehicle changes direction. This method significantly improves the robustness of the laser Doppler velocimeter during the calibration process.

Description

Technical Field

[0001] This application relates to the field of integrated navigation technology, and in particular to a robust online calibration method for a laser Doppler velocimeter based on position observation. Background Technology

[0002] Because strapdown inertial navigation systems (SINS) are autonomous dead reckoning navigation systems that utilize gyroscope and accelerometer outputs, their errors accumulate over time. Therefore, integrated navigation is currently the most mainstream navigation method. Integrated navigation systems fuse information from multiple sensors to fully leverage the strengths of each sensor. Besides the inertial measurement unit (IMU), common sensors in integrated navigation systems include Global Navigation Satellite System (GNSS), odometer (OD), acoustic Doppler log (DVL), magnetometer (MAG), camera, lidar, star sensor, and laser Doppler velocimeter (LDV).

[0003] Currently, the most common land-based integrated navigation systems are SINS / GNSS, SINS / DO, and SINS / MAG, but these systems all have limitations in practical applications. GNSS is not completely autonomous; its signals are easily blocked by tall buildings, trees, and tunnels, and it suffers from multipath effects. While OD is completely autonomous, its measurements are closely related to the condition of the vehicle's wheels. Wheel pressure, temperature, wear, and wheel slippage or bouncing all reduce the accuracy of OD measurements. MAG is also a completely autonomous sensor, but its measurements are easily interfered with by the surrounding environment, therefore it is mostly used in specific applications.

[0004] LDV, as a fully autonomous velocity sensor, measures particle velocity by detecting the Doppler frequency shift of light scattered by moving particles. It boasts advantages such as high measurement accuracy, non-contact measurement, fast dynamic response, high directional sensitivity, wide velocity range, and high spatial resolution, and has been widely adopted in land-based integrated navigation systems in recent years. However, the actual tilt angle of the LDV optical path deviates from the design value, and the actual wavelength of the LDV's internal laser is not entirely consistent with the reference value. These factors lead to scaling factor errors in the LDV's output. Furthermore, due to installation limitations, the coordinate system of the LDV is difficult to coincide with that of the IMU. In SINS / LDV integrated navigation systems, both the LDV's scaling factor error and installation error angle will affect the navigation accuracy of the integrated navigation system. Therefore, it is necessary to accurately calibrate the LDV's scaling factor error and installation error angle before using the integrated navigation system.

[0005] Most existing online LDV calibration methods rely on the assumption of non-holonomic constraints. However, vehicles inevitably experience sideslip when turning, especially at high speeds, which violates this assumption and affects the calibration results. Furthermore, most existing online LDV calibration methods use speed observations, which are susceptible to GNSS outliers, exhibit poor robustness, and cannot cope with GNSS signal interruptions caused by the vehicle passing over overpasses or tunnels.

[0006] Therefore, accurate LDV calibration is crucial for improving the accuracy of the SINS / LDV integrated navigation system. To improve the accuracy of LDV online calibration and reduce its dependence on the external environment during the online calibration process, it is necessary to study robust online calibration methods for LDV. Summary of the Invention

[0007] Therefore, it is necessary to provide an online calibration method and device for a laser Doppler velocimeter based on position observation to address the aforementioned technical problems.

[0008] An online calibration method for a laser Doppler velocimeter based on position observation, the method comprising:

[0009] Using data from a predetermined time period after initial alignment via the online calibration system, the proportional factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter were coarsely calibrated using the analytical calibration method.

[0010] The fine calibration phase begins with the results obtained from the coarse calibration as initial values. The fine calibration phase includes:

[0011] The SINS / GNSS integrated navigation phase and the laser Doppler velocimeter calibration phase.

[0012] A robust Kalman filter is designed in the SINS / GNSS integrated navigation phase to obtain accurate vehicle attitude, velocity, and position information.

[0013] During the calibration stage of the laser Doppler velocimeter, a second Kalman filter was designed using the error propagation model of the laser Doppler velocimeter. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter was continuously corrected by feedback in order to achieve the purpose of accurate calibration of the laser Doppler velocimeter.

[0014] During the calibration phase of the laser Doppler velocimeter, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process.

[0015] In one embodiment, the laser Doppler velocimeter's scaling factor error, pitch installation error angle, and heading installation error angle are coarsely calibrated using analytical calibration methods based on data from a predetermined time period after initial alignment by the online calibration system. This includes:

[0016] Using data from a predetermined time period after initial alignment via the online calibration system, the scaling factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter were coarsely calibrated using an analytical calibration method. The coarse calibration results for these three parameters are as follows:

[0017]

[0018]

[0019]

[0020] in, The scaling factor error of the LDV obtained from the coarse calibration. and These are the pitch and heading installation error angles of the LDV obtained from the coarse calibration, respectively, O(X O ,Y O Z O P is the starting point of the calibration process. GNSS (X GNSS ,Y GNSS Z GNSS () represents the output position of the GNSS after a predetermined time of carrier motion. The position calculated by the SINS / LDV integrated navigation system after a predetermined time for the carrier's movement, where D1 represents P. GNSS The distance between O and D2 represents The distance between O and O.

[0021] In one embodiment, a robust Kalman filter is designed during the SINS / GNSS integrated navigation phase to obtain accurate vehicle attitude, velocity, and position information. This robust Kalman filter is obtained by introducing an adaptive dilation factor to dilate the measurement noise covariance matrix of the filter. The dilated measurement noise covariance matrix is ​​as follows:

[0022]

[0023] Among them, R k S is the measurement noise covariance matrix before dilation. k =diag{s1 s2 … s n} is the adaptive expansion factor matrix, s i For the observation zk The inflation factor corresponding to the noise of the i-th measurement.

[0024] Observation z k The inflation factor corresponding to the noise of the i-th measurement is:

[0025]

[0026]

[0027]

[0028] C k =H k P k|k-1 H k T +R k

[0029] e k =z k -H k x k|k-1

[0030] Where, N k (i,i) and R k (i,i) represent N respectively k and R k The i-th element on the diagonal, x k|k-1 For the one-step prediction of the state, P k|k-1 To predict the state covariance matrix, H k The transformation matrix is ​​measured, and the subscript k indicates the corresponding time. η0 = 1, 0 < b < 1 is the fading factor.

[0031] In one embodiment, the Mahalanobis distance of the filter innovation vector of the robust Kalman filter is introduced to determine whether the measurement noise covariance matrix needs to be expanded.

[0032] When the innovation vector follows a Gaussian distribution, its Mahalanobis distance follows a chi-square distribution with degrees of freedom equal to the dimension of the innovation vector. The filter innovation vector and its corresponding Mahalanobis distance are:

[0033] e k =z k -H k x k|k-1

[0034] f k =e k T [H k P k|k-1 H k T +Rk ] -1 e k ~χ 2 (n)

[0035] Where x k|k-1 χ is the predicted state value. 2 (n) represents a chi-square distribution with n degrees of freedom.

[0036] If the Mahalanobis distance corresponding to the filter innovation vector is not greater than a preset value, then the measurement noise covariance matrix of the filter will not be expanded.

[0037] If the Mahalanobis distance corresponding to the filter innovation vector is greater than the preset value, then the measurement noise covariance matrix of the filter is expanded.

[0038] In one embodiment, during the laser Doppler velocimeter calibration stage, a second Kalman filter is designed using the laser Doppler velocimeter error propagation model. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter is continuously corrected through feedback to achieve accurate calibration. The error state vector of the second Kalman filter in this step is:

[0039]

[0040] in, Let be the error state vector. δp represents the attitude error residual of SINS after SINS / GNSS integrated navigation. DR =[δL DR δλ DR δh DR ] T The position error vector δφ is used to calculate the SINS / LDV track. mx and δφ mz These are the pitch and heading installation error angles of the laser Doppler velocimeter after coarse calibration, respectively, and δK′ is the scaling factor error of the laser Doppler velocimeter after coarse calibration.

[0041] In one embodiment, the error model of the second Kalman filter is:

[0042]

[0043]

[0044]

[0045]

[0046]

[0047] in, υ n υ represents the actual velocity of the carrier in the navigation coordinate system. b This represents the actual velocity of the carrier in the carrier coordinate system. The actual attitude matrix of the carrier.

[0048]

[0049] In one embodiment, the state equation of the second Kalman filter is:

[0050]

[0051]

[0052] in, The system state transition matrix is ​​9×9. Here is the system noise matrix. This is the system noise vector.

[0053] In one embodiment, the measurement equation for the second Kalman filter is:

[0054]

[0055] in, To measure the transformation matrix, To measure the noise vector, p DR p and p represent the position output of the SINS / LDV track estimation system and the position output of the SINS / GNSS integrated navigation system, respectively.

[0056] In one embodiment, during the laser Doppler velocimeter calibration stage, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process, including:

[0057] The output of the gyroscope and the output speed of the laser Doppler velocimeter are used to determine whether a vehicle is changing direction, and lateral velocity compensation is performed on the laser Doppler velocimeter's velocity in its own coordinate system when the vehicle changes direction; the output of the laser Doppler velocimeter in its own coordinate system is:

[0058]

[0059] in, A represents the actual output of a one-dimensional laser Doppler velocimeter. z T is the angular increment output by the gyroscope. NHC υ is a preset threshold. lateralTo compensate for the lateral velocity of a vehicle skidding during cornering, Where υ l(i) Let be the lateral output velocity of the SINS / GNSS integrated navigation system at time i in the coordinate system of the laser Doppler velocimeter, and N be the number of samples in the predetermined period.

[0060] An online calibration device for a laser Doppler velocimeter based on position observation, the device comprising:

[0061] The coarse calibration module is used to perform coarse calibration of the scale factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter using analytical calibration methods based on data from a predetermined time period after initial alignment of the online calibration system.

[0062] The fine calibration module is used to begin the fine calibration phase using the results obtained from the coarse calibration as initial values. The fine calibration phase includes a SINS / GNSS integrated navigation phase and a laser Doppler velocimeter calibration phase. In the SINS / GNSS integrated navigation phase, a robust Kalman filter is designed to obtain accurate carrier attitude, velocity, and position information. In the laser Doppler velocimeter calibration phase, a second Kalman filter is designed using the laser Doppler velocimeter error propagation model. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter is continuously corrected through feedback to achieve accurate calibration. In the laser Doppler velocimeter calibration phase, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process.

[0063] The above-described online calibration method and apparatus for a laser Doppler velocimeter based on position observation is presented. The method divides the calibration process into two stages: coarse calibration and fine calibration. In the coarse calibration stage, an analytical method is employed. In the fine calibration stage, a robust Kalman filter is first designed to obtain accurate carrier attitude, velocity, and position information. Then, another Kalman filter is designed using the position observation-based laser Doppler velocimeter error propagation model to further calibrate the laser Doppler velocimeter. To reduce the impact of non-holonomic constraints being violated during vehicle turning on the calibration results, the output of the gyroscope and the output velocity of the laser Doppler velocimeter are used to distinguish whether the vehicle is changing direction. Lateral velocity compensation is performed on the laser Doppler velocimeter's velocity in its own coordinate system when the vehicle changes direction. This method significantly improves the robustness of the laser Doppler velocimeter calibration process. Attached Figure Description

[0064] Figure 1 This is a diagram illustrating the installation relationship between the laser Doppler velocimeter (LDV) and the inertial measurement unit (IMU) in one embodiment, as well as the relationship between their corresponding coordinate systems.

[0065] Figure 2 This is a flowchart illustrating an online calibration method for a laser Doppler velocimeter based on position observation in one embodiment.

[0066] Figure 3 This illustrates the relationship between the GNSS trajectory and the SINS / LDV track extrapolation trajectory during the coarse calibration process in another embodiment.

[0067] Figure 4 This is a structural block diagram of an online calibration device for a laser Doppler velocimeter based on position observation in one embodiment. Detailed Implementation

[0068] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0069] For a combined navigation system consisting of a strapdown inertial navigation system (SINS) and a laser Doppler velocimeter (LDV), the inertial measurement unit (IMU) is mounted at the center of the vehicle's rear axle, and the LDV is mounted vertically below the IMU. This reduces the impact of lever arm errors between the LDV and the IMU on calibration and navigation results. Before navigation begins, the IMU needs to be calibrated to obtain the scale coefficient errors of the gyroscopes and accelerometers, installation error angles, and zero bias. System initialization is also required before entering the formal navigation process, including obtaining the initial position and velocity information of the vehicle and completing initial alignment.

[0070] Figure 1 This diagram illustrates the installation relationship between the Laser Doppler Velocimetry (LDV) and the Inertial Measurement Unit (IMU), and the relationship between their corresponding coordinate systems. In the diagram, the m-coordinate system represents the LDV's own coordinate system, and the b-coordinate system represents the IMU's coordinate system, i.e., the carrier coordinate system. Figure 1 As shown, the installation of the LDV and IMU does not overlap, resulting in an installation error angle. Furthermore, due to discrepancies between the actual and design values ​​of the LDV's laser wavelength and beam tilt angle, a scaling factor error exists. To achieve high-precision navigation in the SINS / LDV integrated navigation system, it is necessary to accurately calibrate these errors.

[0071] In one embodiment, such as Figure 2 As shown, an online calibration method for a laser Doppler velocimeter based on position observation is provided. The method includes the following steps:

[0072] Step 200: Using the data from the predetermined time period after initial alignment of the online calibration system, the proportional factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter are coarsely calibrated using the analytical calibration method.

[0073] Specifically, the calibration method includes two stages: coarse calibration and fine calibration.

[0074] As a preferred method, the coarse calibration uses the data from the first 5 minutes after the initial alignment of the online calibration system for analytical calibration, to obtain the coarse calibration results of the laser Doppler velocimeter's scale factor error, pitch installation error angle, and heading installation error angle.

[0075] Step 202: Use the results obtained from the coarse calibration as the initial values ​​to begin the fine calibration stage. The fine calibration stage includes the SINS / GNSS integrated navigation stage and the laser Doppler velocimeter calibration stage.

[0076] Step 204: Design a robust Kalman filter in the SINS / GNSS integrated navigation phase to obtain accurate vehicle attitude, velocity, and position information.

[0077] Specifically, in order to improve the robustness of the SINS / GNSS integrated navigation system and reduce the impact of GNSS outliers on the SINS / GNSS integrated navigation results, an adaptive expansion factor matrix is ​​introduced to expand the measurement noise covariance matrix of the Kalman filter.

[0078] A robust Kalman filter is a Kalman filter that expands the measurement noise covariance matrix using an adaptive expansion factor matrix.

[0079] Step 206: In the laser Doppler velocimeter calibration stage, a second Kalman filter was designed using the laser Doppler velocimeter error propagation model. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter was continuously fed back and corrected to achieve the purpose of accurate calibration of the laser Doppler velocimeter.

[0080] Specifically, another Kalman filter was designed using the error propagation model of the laser Doppler velocimeter based on position observation, which was used to further calibrate the laser Doppler velocimeter.

[0081] Step 208: During the laser Doppler velocimeter calibration stage, compensate for the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process.

[0082] Specifically, in order to reduce the impact of non-integrity constraint violations caused by vehicle turning on the calibration results, the output of the gyroscope and the output of the laser Doppler velocimeter are used to identify whether the vehicle is changing direction, and lateral velocity compensation is performed on the laser Doppler velocimeter's velocity in its own coordinate system when the vehicle changes direction.

[0083] In the aforementioned online calibration method for laser Doppler velocimeters based on position observation, the calibration process is divided into two stages: coarse calibration and fine calibration. The coarse calibration stage employs an analytical method. In the fine calibration stage, a robust Kalman filter is first designed to obtain accurate carrier attitude, velocity, and position information. Then, another Kalman filter is designed using the position observation-based laser Doppler velocimeter error propagation model to further calibrate the laser Doppler velocimeter. To reduce the impact of non-holonomic constraints being violated during vehicle turning on the calibration results, the output of the gyroscope and the output velocity of the laser Doppler velocimeter are used to distinguish whether the vehicle is changing direction. Lateral velocity compensation is performed on the laser Doppler velocimeter's velocity in its own coordinate system when the vehicle changes direction. This method significantly improves the robustness of the laser Doppler velocimeter during the calibration process.

[0084] In one embodiment, step 200 includes: using data from a predetermined time period after initial alignment by the online calibration system, and employing an analytical calibration method to coarsely calibrate the scaling factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter; the coarse calibration results for the scaling factor error, pitch installation error angle, and heading installation error angle are as follows:

[0085]

[0086]

[0087]

[0088] in, The scaling factor error of the LDV obtained from the coarse calibration. and These are the pitch and heading installation error angles of the LDV obtained from the coarse calibration, respectively, O(X O ,Y O Z O P is the starting point of the calibration process. GNSS (X GNSS ,Y GNSS Z GNSS () represents the output position of the GNSS after a predetermined time of carrier motion. The position calculated by the SINS / LDV integrated navigation system after a predetermined time for the carrier's movement, where D1 represents P. GNSS The distance between O and D2 represents The distance between O and O.

[0089] Specifically, coarse calibration utilizes data from a predetermined time period (preferably the first 5 minutes) after initial alignment of the online calibration system for analytical calibration. For example... Figure 3As shown, by utilizing the similarity between the trajectory calculated from GNSS and SINS / LDV tracks, coarse calibration results for the scale factor error, pitch installation error angle, and heading installation error angle are obtained.

[0090] In one embodiment, the robust Kalman filter in step 204 is obtained by introducing an adaptive inflation factor to inflate the measurement noise covariance matrix of the filter; the inflated measurement noise covariance matrix is:

[0091]

[0092] Among them, R k S is the measurement noise covariance matrix before dilation. k =diag{s1s2…s n} is the adaptive expansion factor matrix, s i For the observation z k The inflation factor corresponding to the i-th measurement. Observation z k The inflation factor corresponding to the i-th measurement is:

[0093]

[0094]

[0095]

[0096] C k =H k P k|k-1 H k T +R k (8)

[0097] e k =z k -H k x k|k-1 (9)

[0098] Where, N k (i,i) and R k (i,i) represent N respectively k and R k The i-th element on the diagonal, x k|k-1 For the one-step prediction of the state, P k|k-1 To predict the state covariance matrix, H k The transformation matrix is ​​measured, and the subscript k indicates the corresponding time. η0 = 1, 0 < b < 1 is the fading factor, e k This is the information vector of the filter.

[0099] In one embodiment, the Mahalanobis distance of the robust Kalman filter's innovation vector is introduced to determine whether the measurement noise covariance matrix needs to be dilated; when the innovation vector follows a Gaussian distribution, its Mahalanobis distance follows a chi-square distribution with degrees of freedom equal to the dimension of the innovation vector, and the Mahalanobis distance of the filter innovation vector is:

[0100]

[0101] Where x k|k-1 χ is the predicted state value. 2 (n) represents a chi-square distribution with n degrees of freedom.

[0102] If the Mahalanobis distance corresponding to the filter innovation vector is not greater than a preset value, the measurement noise covariance matrix of the filter is not expanded; if the Mahalanobis distance corresponding to the filter innovation vector is greater than a preset value, the measurement noise covariance matrix of the filter is expanded.

[0103] In one embodiment, the error state vector of the second Kalman filter in step 206 is:

[0104]

[0105] in, Error state vector, δp represents the attitude error residual of SINS after SINS / GNSS integrated navigation. DR =[δL DR δλ DR δh DR ] T The position error vector δφ is used to calculate the SINS / LDV track. mx and δφ mz These are the pitch and heading installation error angles of the laser Doppler velocimeter after coarse calibration, respectively, and δK′ is the scaling factor error of the laser Doppler velocimeter after coarse calibration.

[0106] In one embodiment, the error model of the second Kalman filter in step 206 is:

[0107]

[0108]

[0109]

[0110] in, υ n υ represents the actual velocity of the carrier in the navigation coordinate system. b This represents the actual velocity of the carrier in the carrier coordinate system. The actual attitude matrix of the carrier.

[0111]

[0112] In one embodiment, the state equation of the second Kalman filter in step 206 is:

[0113]

[0114]

[0115] in, The system state transition matrix is ​​9×9. Here is the system noise matrix. This is the system noise vector.

[0116] In one embodiment, the measurement equation for the second Kalman filter in step 206 is:

[0117]

[0118] in, To measure the transformation matrix, To measure the noise vector, p DR p and p represent the position output of the SINS / LDV track estimation system and the position output of the SINS / GNSS integrated navigation system, respectively.

[0119] In one embodiment, step 208 includes: using the output of the gyroscope and the output speed of the laser Doppler velocimeter to determine whether the vehicle has changed direction, and performing lateral velocity compensation on the speed of the laser Doppler velocimeter in its own coordinate system when the vehicle changes direction; the output of the laser Doppler velocimeter in its own coordinate system is:

[0120]

[0121] in, A represents the actual output of a one-dimensional laser Doppler velocimeter. z T is the angular increment output by the gyroscope. NHC υ is a preset threshold. lateral To compensate for the lateral velocity of a vehicle skidding during cornering, Where υ l(i) Let be the lateral output velocity of the SINS / GNSS integrated navigation system at time i in the coordinate system of the laser Doppler velocimeter, and N be the number of samples in the predetermined period.

[0122] In one specific embodiment, the results obtained in the coarse calibration stage are used as the initial values ​​to start the fine calibration stage. The fine calibration stage is divided into two parts: the SINS / GNSS integrated navigation stage and the LDV calibration stage.

[0123] During the SINS / GNSS integrated navigation phase, the error model for SINS is:

[0124]

[0125]

[0126]

[0127]

[0128]

[0129]

[0130]

[0131] in

[0132]

[0133]

[0134]

[0135] in This represents the attitude error of SINS. This indicates the speed error of SINS. R represents the velocity of the SINS in the navigation coordinate system. δL, δλ, and δh represent the latitude, longitude, and altitude errors of the SINS, respectively. L, λ, and h represent latitude, longitude, and altitude, respectively. M and R N These represent the radii of curvature of the Earth's meridian and circumference, respectively, where the carrier is located. n This represents the projection of the accelerometer output specific force into the navigation coordinate system. ω ie This represents the Earth's angular velocity of rotation. and These represent the measurement errors of the gyroscope and accelerometer, respectively.

[0136] According to equations (19)-(25), the error state vector of the SINS / GNSS integrated navigation system is defined as:

[0137]

[0138] Where δpSINS This indicates the positional error of the SINS.

[0139] The state equation of the system is defined as follows:

[0140]

[0141] Where F k The system state transition matrix is ​​represented as follows:

[0142]

[0143] in Represents the attitude matrix. and f n × respectively represent and f n A skew-symmetric matrix.

[0144]

[0145]

[0146]

[0147]

[0148]

[0149]

[0150] The system noise matrix is:

[0151]

[0152] The system noise vector is:

[0153]

[0154] ε wi and These represent the noise levels of the gyroscope and accelerometer, respectively.

[0155] Using the velocity and position difference between SINS and GNSS as system observations, the measurement equation can be written as:

[0156]

[0157] Where H k =[0 6×3 I60 6×6 ] is the measurement transformation matrix, v k To measure the noise vector. υ GNSS and p GNSSThese are the GNSS velocity and position outputs, respectively.

[0158] To improve the robustness of the SINS / GNSS integrated navigation system and mitigate the impact of GNSS outliers on the SINS / GNSS integrated navigation results, an adaptive dilation factor matrix is ​​introduced to dilate the measurement noise covariance matrix of the Kalman filter. The dilated measurement noise covariance matrix is ​​shown in Equation (4).

[0159] The Mahalanobis distance of the filter innovation vector is introduced to determine whether the measurement noise covariance matrix needs to be expanded. When the innovation vector follows a Gaussian distribution, its Mahalanobis distance follows a chi-square distribution with degrees of freedom equal to the dimension of the innovation vector. The filter innovation vector and its corresponding Mahalanobis distance are shown in Equations (9) and (10).

[0160] Therefore, the following conditions should be used to determine whether to expand the measurement noise covariance matrix of the filter.

[0161]

[0162] Where T D The preset value can be obtained by looking up a table based on the degrees of freedom and the required significance level.

[0163] During the LDV calibration stage, based on the LDV error model, the LDV error parameters are transformed into part of the state variables of the integrated navigation system, realizing the online calibration of the SINS / LDV integrated navigation system. For the filter design in the LDV calibration stage, the error state vector is shown in Equation (11).

[0164] After coarse calibration, the LDV output in the navigation coordinate system is:

[0165]

[0166] in The attitude matrix is ​​provided by the SINS / GNSS integrated navigation system. Based on coarse calibration results and The resulting transformation matrix, It is the scaling factor of LDV after coarse calibration. This is the output of LDV in its own coordinate system.

[0167] To reduce the impact of non-integrity constraints being violated due to vehicle turning on the calibration results, the output of LDV in its own coordinate system is shown in Equation (18).

[0168] According to (18), the output of LDV in the navigation coordinate system can be rewritten as:

[0169]

[0170] Where δφ m =[δφ mx 0 δφ mz ] T This represents the residual of the installation error angle vector after coarse calibration.

[0171] The error model for the LDV calibration stage can be expressed as:

[0172]

[0173]

[0174]

[0175] in:

[0176]

[0177]

[0178]

[0179] Among them, υ n υ represents the actual velocity of the vehicle in the navigation coordinate system. b This represents the actual velocity of the carrier in the carrier coordinate system. This represents the actual attitude matrix of the carrier.

[0180] According to (11), (44), (45), (46) and (49), the state equation of the filter in the LDV calibration stage can be expressed as:

[0181]

[0182] in, Here is the system noise matrix. Let be the system noise vector. The system state transition matrix is ​​9×9. for:

[0183]

[0184] Using the position difference between the SINS / LDV track estimation system and the SINS / GNSS integrated navigation system as the observation, the measurement equation is:

[0185]

[0186] in To measure the transformation matrix, To measure the noise vector. pDR p and p represent the position output of the SINS / LDV track estimation system and the position output of the SINS / GNSS integrated navigation system, respectively.

[0187] After filtering, based on the error state vector obtained from filtering... Perform feedback correction.

[0188] It should be understood that, although Figure 2 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 2 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0189] In one embodiment, such as Figure 4 As shown, an online calibration device for a laser Doppler velocimeter based on position observation is provided, comprising: a coarse calibration module and a fine calibration module, wherein:

[0190] The coarse calibration module is used to perform coarse calibration of the scale factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter using analytical calibration methods based on data from a predetermined time period after initial alignment of the online calibration system.

[0191] The fine calibration module uses the results obtained from the coarse calibration as initial values ​​to begin the fine calibration phase. The fine calibration phase includes: the SINS / GNSS integrated navigation phase and the laser Doppler velocimeter calibration phase. In the SINS / GNSS integrated navigation phase, a robust Kalman filter is designed to obtain accurate carrier attitude, velocity, and position information. In the laser Doppler velocimeter calibration phase, a second Kalman filter is designed using the laser Doppler velocimeter error propagation model. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter is continuously corrected through feedback to achieve accurate calibration. In the laser Doppler velocimeter calibration phase, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process.

[0192] In one embodiment, the coarse calibration module is also used to perform coarse calibration of the scale factor error, pitch installation error angle and heading installation error angle of the laser Doppler velocimeter using analytical calibration method based on the data of a predetermined time period after initial alignment of the online calibration system; the coarse calibration results of the scale factor error, pitch installation error angle and heading installation error angle are shown in equations (1)-(3).

[0193] In one embodiment, the robust Kalman filter in the fine calibration module is obtained by introducing an adaptive expansion factor to expand the measurement noise covariance matrix of the filter; the expanded measurement noise covariance matrix is ​​shown in Equation (4).

[0194] In one embodiment, the Mahalanobis distance of the filter innovation vector of the robust Kalman filter in the fine calibration module is introduced to determine whether the measurement noise covariance matrix needs to be expanded; when the innovation vector satisfies the Gaussian distribution, its Mahalanobis distance follows the chi-square distribution with the degree of freedom equal to the dimension of the innovation vector, and the filter innovation vector and its corresponding Mahalanobis distance are shown in Equations (8) and (9).

[0195] If the Mahalanobis distance corresponding to the filter innovation vector is not greater than a preset value, the measurement noise covariance matrix of the filter is not expanded; if the Mahalanobis distance corresponding to the filter innovation vector is greater than a preset value, the measurement noise covariance matrix of the filter is expanded.

[0196] In one embodiment, the error state vector of the second Kalman filter in step 206 is shown in equation (11).

[0197] In one embodiment, the error model of the second Kalman filter in the fine calibration module is shown in equations (12) to (14).

[0198] In one embodiment, the state equations of the second Kalman filter in the fine calibration module are shown in equations (15) and (16).

[0199] In one embodiment, the measurement equation for the second Kalman filter in the fine calibration module is shown in equation (17).

[0200] In one embodiment, the precision calibration module is also used to use the output of the astronomical gyroscope and the output speed of the laser Doppler velocimeter to identify whether the vehicle has changed direction, and to perform lateral speed compensation on the speed of the laser Doppler velocimeter in its own coordinate system when the vehicle changes direction; the output of the laser Doppler velocimeter in its own coordinate system is shown in equation (18).

[0201] Specific limitations regarding the online calibration device for laser Doppler velocimeters based on location observation can be found in the limitations of the online calibration method for laser Doppler velocimeters based on location observation mentioned above, and will not be repeated here. Each module in the aforementioned online calibration device for laser Doppler velocimeters based on location observation can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device, or stored in the memory of a computer device as software, so that the processor can call and execute the corresponding operations of each module.

[0202] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0203] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A method for online calibration of a laser Doppler velocimeter based on position observation, characterized in that, The method includes: Using data from a predetermined time period after initial alignment via the online calibration system, the laser Doppler velocimeter was coarsely calibrated using an analytical calibration method to determine the scale factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter. The fine calibration phase begins with the results obtained from the coarse calibration as the initial values. The fine calibration phase includes: the SINS / GNSS integrated navigation phase and the laser Doppler velocimeter calibration phase. A robust Kalman filter is designed in the SINS / GNSS integrated navigation phase to obtain accurate vehicle attitude, velocity, and position information; During the calibration stage of the laser Doppler velocimeter, a second Kalman filter was designed using the error propagation model of the laser Doppler velocimeter. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter was continuously fed back and corrected to achieve the purpose of accurate calibration of the laser Doppler velocimeter. During the calibration phase of the laser Doppler velocimeter, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process.

2. The method of claim 1, wherein, Using data from a predetermined time period after initial alignment via the online calibration system, the laser Doppler velocimeter was coarsely calibrated using analytical calibration methods to determine its scaling factor error, pitch installation error angle, and heading installation error angle. This included: Using data from a predetermined time period after initial alignment via the online calibration system, the scaling factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter were coarsely calibrated using an analytical calibration method. The coarse calibration results for these three parameters are as follows: ； ； ； wherein, is the scale factor error of the LDV obtained from the coarse calibration, and are the pitch and heading installation error angles of the LDV obtained from the coarse calibration, respectively, is the start point of the calibration process, is the output position of the GNSS after the carrier has moved for a predetermined time, is the position obtained by the SINS / LDV integrated navigation system after the carrier has moved for a predetermined time, denotes the distance between O and denotes the distance between O and 3. The method of claim 1, wherein, In the SINS / GNSS integrated navigation phase, a robust Kalman filter is designed to obtain accurate vehicle attitude, velocity, and position information. This robust Kalman filter is obtained by introducing an adaptive dilation factor to dilate the measurement noise covariance matrix of the filter. The dilated measurement noise covariance matrix is ​​as follows: ； wherein is the measured noise covariance matrix before inflation, is the adaptive inflation factor matrix, is the observation the inflation factor corresponding to the noise of the i th measurement value. Observation quantity In the middle i The inflation factor corresponding to the noise of the individual measurement value is: ； ； ； ； ； wherein, and are respectively and the element on the diagonal, i the element on the diagonal, is the one-step prediction of the state, is the predicted state covariance matrix, is the measurement transformation matrix, the subscript k denotes the corresponding time instant, , , is the fading factor.

4. The method of claim 3, wherein, The Mahalanobis distance of the filter innovation vector of the robust Kalman filter is introduced to determine whether the measurement noise covariance matrix needs to be expanded. When the innovation vector follows a Gaussian distribution, its Mahalanobis distance follows a chi-square distribution with degrees of freedom equal to the dimension of the innovation vector. The filter innovation vector and its corresponding Mahalanobis distance are: ； ； wherein is the one-step ahead forecast of the state, denotes a chi-squared distribution with n degrees of freedom; If the Mahalanobis distance corresponding to the filter innovation vector is not greater than a preset value, then the measurement noise covariance matrix of the filter will not be expanded. If the Mahalanobis distance corresponding to the filter innovation vector is greater than the preset value, then the measurement noise covariance matrix of the filter is expanded.

5. The method of claim 1, wherein, In the calibration stage of the laser Doppler velocimeter, a second Kalman filter was designed using the error propagation model of the laser Doppler velocimeter. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter was continuously corrected through feedback to achieve accurate calibration. The error state vector of the second Kalman filter in this step is: ； wherein, is the error state vector, is the SINS / GNSS integrated navigation post-SINS attitude error residual, is the SINS / LDV dead reckoning position error vector, and are the pitch and heading installation error angles of the laser Doppler velocimeter after coarse calibration, respectively, is the scale factor error of the laser Doppler velocimeter after coarse calibration.

6. The method of claim 5, wherein, The error model of the second Kalman filter is: ； ； ； wherein, , is the real velocity of the carrier in the navigation coordinate system, is the real velocity of the carrier in the carrier coordinate system, denotes the skew-symmetric matrix of denotes the skew-symmetric matrix of is the real attitude matrix of the carrier, denotes the residual of the installation error angle vector after coarse calibration, , , and respectively represent the meridian and prime vertical radii of curvature of the carrier's location on the earth.

7. The method of claim 6, wherein, The state equation of the second Kalman filter is: ； ； wherein is system state transition matrix, is a system noise matrix, is a system noise vector.

8. The method of claim 5, wherein, The measurement equation for the second Kalman filter is: ； wherein, is a measurement noise vector, is a measurement noise vector, and are the position outputs of the SINS / LDV dead reckoning system and the SINS / GNSS integrated navigation system, respectively.

9. The method of claim 1, wherein, During the calibration phase of the laser Doppler velocimeter, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process, including: The output of the gyroscope and the output speed of the laser Doppler velocimeter are used to determine whether a vehicle is changing direction, and lateral velocity compensation is performed on the laser Doppler velocimeter's velocity in its own coordinate system when the vehicle changes direction; the output of the laser Doppler velocimeter in its own coordinate system is: ； wherein is the actual output of the one-dimensional laser Doppler velocimeter, is the angular increment output by the skyward gyro, is a preset threshold value, is the lateral velocity used to compensate for side slip when the vehicle is turning, wherein is i is the lateral output velocity of the SINS / GNSS integrated navigation system in the laser Doppler velocimeter coordinate system at time N is the number of samples in a predetermined period, is the scale factor of the LDV after coarse calibration.

10. A laser Doppler velocimeter on-line calibration device based on position observation, characterized in that, The device includes: The coarse calibration module is used to coarsely calibrate the scale factor error, pitch installation error angle, and heading installation error angle of the laser Doppler velocimeter using analytical calibration methods based on data from a predetermined time period after initial alignment of the online calibration system. The fine calibration module is used to begin the fine calibration phase using the results obtained from the coarse calibration as initial values. The fine calibration phase includes a SINS / GNSS integrated navigation phase and a laser Doppler velocimeter calibration phase. In the SINS / GNSS integrated navigation phase, a robust Kalman filter is designed to obtain accurate carrier attitude, velocity, and position information. In the laser Doppler velocimeter calibration phase, a second Kalman filter is designed using the laser Doppler velocimeter error propagation model. Based on the error state of the laser Doppler velocimeter obtained after filtering by the second Kalman filter, the output of the laser Doppler velocimeter is continuously corrected through feedback to achieve accurate calibration. In the laser Doppler velocimeter calibration phase, compensation is performed on the lateral velocity of the laser Doppler velocimeter in its own coordinate system during the turning process.

Images