Dynamic correction method for position coordinates of sensor on mobile platform carrier

Abstract

The invention discloses a dynamic correction method for position coordinates of a sensor on a mobile platform carrier. The dynamic correction method comprises the following steps: step 1, static calibration; 2, establishing a dynamic compensation model; 3, preliminarily correcting the position coordinates; 4, Kalman filtering optimization: taking the preliminarily corrected position coordinates of the sensor as observed values of a Kalman filtering algorithm, and carrying out secondary correction; and step 5, parameter adaptive adjustment: carrying out dynamic adaptive adjustment on an observation noise covariance matrix and a process noise covariance matrix in the Kalman filtering algorithm. According to the invention, the initial three-dimensional offset of the sensor is obtained through static calibration, and then the initial three-dimensional offset is subjected to primary dynamic supplementary correction and secondary Kalman filtering optimization correction, so that the high-precision position coordinates of the sensor are obtained, and the stable compensation of the position of the sensor in a dynamic scene is realized. And the accuracy and reliability of the position coordinates of the sensor are ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mobile platform carrier position correction, and particularly to a sensor position coordinate dynamic correction method on a mobile platform carrier. BACKGROUND

[0002] A mobile platform carrier, such as a ship, an airplane, a vehicle, or a water engineering platform, usually carries multiple types of sensors and positioning systems to achieve functions such as navigation positioning, environment perception, and target detection and tracking. Among them, the sensors carried include navigation radars, laser radars, and vision sensors. Due to the different physical installation positions of the sensors and the positioning systems, and the influence of attitude changes (such as roll, pitch, and yaw) and inertial forces on the mobile platform carrier during movement, the actual spatial offset between the two will dynamically change with the movement state, resulting in a significant deviation between the sensor position coordinates output by the positioning system on the mobile platform and the actual position of the sensor.

[0003] The traditional solution is usually to measure the relative position between the sensor and the positioning system when the platform is stationary, and to calculate the position of the sensor according to the coordinates provided by the positioning system during operation. However, this method does not take into account the influence of platform attitude changes and movement states on the relative position, resulting in insufficient correction accuracy and difficulty in meeting the requirements of high precision and high reliability, especially in complex environments (such as severe sea conditions, high-speed driving, complex flight attitudes, etc.), to achieve accurate correction of the sensor position.

[0004] The above-mentioned severe sea conditions are specifically: medium to large waves with wave height ≥ 3 meters, strong winds with wind force ≥ 6 levels, complex flow fields with ocean current speed > 2 knots, extreme fluctuations caused by typhoons / cold waves, etc. At this time, the navigation positioning drifts (> 1 m), the radar / laser radar target tracking is lost, and the obstacle avoidance system misjudges (such as mistaking waves as obstacles). Therefore, centimeter-level (≤ 5 cm) position correction is required to ensure the spatio-temporal synchronization of sensor data and carrier movement.

[0005] The above-mentioned high-speed driving is specifically the speed threshold: vehicle > 80 km / h, ship > 18 knots (≈ 34 km / h), airplane > 300 km / h (cruise phase). At this time, the navigation positioning drifts (> 1 m, automatic driving lane level positioning fails), the inertial force / centrifugal force offset accumulates (error increases with the square of speed), the path planning lags (such as automatic driving steering delay), and the multi-sensor fusion mismatch occurs. Therefore, centimeter-level (≤ 5 cm) dynamic correction is required to suppress the model error amplification under high-speed maneuvering.

[0006] The complex flight attitude includes the following specific attitudes: large angle tilt (pitch angle / roll angle > 30°), continuous rolling (angular velocity > 30° / s), diving / climbing (longitudinal acceleration > 2g) and other non-stationary attitudes. At this time, the visual sensor field of view deviates, the laser point cloud distorts, and the navigation system attitude solution diverges (error > 1°). Therefore, the attitude needs to be corrected by centimeter (≤10 cm) + sub-degree (≤0.1°) to ensure the spatio-temporal alignment of the sensor under high dynamics.

[0007] Therefore, there is an urgent need for a dynamic correction method that couples the kinematics of the carrier to achieve stable compensation of the sensor position in a dynamic scene and ensure the accuracy and reliability of the sensor position coordinates. SUMMARY

[0008] The technical problem to be solved by the present application is to provide a sensor position coordinate dynamic correction method on a mobile platform carrier to solve the above problems in the prior art. The sensor position coordinate dynamic correction method on the mobile platform carrier obtains an initial three-dimensional offset of the sensor through static calibration, and then performs primary dynamic supplementary correction and secondary Kalman filter optimization correction on the initial three-dimensional offset to obtain high-precision sensor position coordinates, thereby achieving stable compensation of the sensor position in a dynamic scene and ensuring the accuracy and reliability of the sensor position coordinates.

[0009] To solve the above technical problems, the technical solution adopted by the present application is as follows:

[0010] A sensor position coordinate dynamic correction method on a mobile platform carrier, comprising the following steps.

[0011] Step 1, static calibration: when the mobile platform carrier is stationary, the position coordinates of the main reference point and the sensor installation point on the mobile platform carrier are measured, and the initial three-dimensional offset of the sensor relative to the main reference point is calculated.

[0012] Step 2, establish a dynamic compensation model: a dynamic compensation model is established for the sensor on the mobile platform carrier; the dynamic compensation model includes three dynamic offsets, namely: attitude angle offset ΔG attitude , inertial force offset ΔG accel and centrifugal force offset ΔG centri .

[0013] Step 3, preliminary correction of position coordinates: the position coordinates G base of the main reference point are obtained in real time, and then the initial three-dimensional offset ΔG0 and the three dynamic offsets are superimposed to obtain the preliminary corrected sensor position coordinates

[0014] Step 4, Kalman filter optimization: the sensor position coordinates As the observation value of the Kalman filter algorithm, the Kalman filter is constructed to perform secondary correction on the sensor position coordinates to obtain accurate sensor position coordinates.

[0015] Step 5, parameter adaptive adjustment: dynamically and adaptively adjusting the observation noise covariance matrix R k and the process noise covariance matrix Q k in the Kalman filter algorithm, so that: when the GNSS signal quality decreases, the weight of the observation value is reduced by increasing R k ; when the angular velocity of the mobile platform carrier increases, the dependence of state estimation on the observation value is increased by increasing Q k .

[0016] In step 1, the longitude and latitude coordinates of the main reference point and the sensor installation point on the mobile platform carrier are measured by using the dual-frequency RTK-GNSS, and then the obtained longitude and latitude coordinates are converted into three-axis coordinates in the spatial rectangular coordinate system, so that the calculation formula of ΔG0 is obtained as follows:

[0017]

[0018] In the formula, is the initial three-axis coordinates of the main reference point in the spatial rectangular coordinate system.

[0019] is the initial three-axis coordinates of the sensor installation point in the spatial rectangular coordinate system.

[0020] Δx0, Δy0 and Δz0 are the coordinate difference values of the main reference point and the sensor installation point in the spatial rectangular coordinate system at the initial time, respectively on the x-axis, the y-axis and the z-axis.

[0021] In step 1, in step 2, the calculation method of the attitude angle offset ΔG attitude includes the following steps:

[0022] Step 2-1, real-time acquisition of the attitude angle of the mobile platform carrier, specifically including: roll angle θ around the X-axis, pitch angle φ around the Y-axis and yaw angle ψ around the Z-axis.

[0023] Step 2-2, construction of a three-axis rotation matrix, specifically: X-axis rotation matrix M x (θ), Y-axis rotation matrix M y (φ) and Z-axis rotation matrix M z (ψ).

[0024] Step 2-3, construction of an attitude angle compensation matrix M: according to the different rotation modes of the mobile platform carrier, the rotation order of the three axes is adjusted, and the corresponding rotation matrices are multiplied in the adjusted order to obtain the attitude angle compensation matrix M.

[0025] Step 2-4, calculate the attitude angle offset AG attitude The specific calculation formula is:

[0026] AG attitude = M·AG0- AG0.

[0027] In step 2-2, the expressions of M x (θ), M y (φ) and M z (ψ) are respectively:

[0028]

[0029] In step 2-3, the calculation of the attitude angle compensation matrix M depends on the dominant motion freedom of the carrier, and the attitude axis that has the greatest impact on the carrier motion is preferentially matched, so as to compensate the most active freedom and ensure that the main disturbance is preferentially corrected.

[0030] In step 2, the calculation formula of the inertial force offset AG accel is:

[0031]

[0032] In the formula, a is the real-time acceleration of the three axes of the mobile platform carrier; and Δt is the sampling time interval.

[0033] In step 2, the calculation formula of the centrifugal force offset AG centri is:

[0034] AG centri = (ω×(ω×AG0))·Δt 2

[0035] In the formula, ω is the real-time angular velocity of the three axes of the mobile platform carrier; and Δt is the sampling time interval.

[0036] In step 3, the latitude and longitude coordinates of G base before coordinate conversion are provided by a compass or GNSS; and in step 5, the adjustment method of the observation noise covariance matrix R k is:

[0037] When the latitude and longitude coordinates are provided by the compass, R k =R0; wherein R0 is the initial set value of the observation noise covariance matrix.

[0038] When the latitude and longitude coordinates are provided by the GNSS, R k is then dynamically adjusted in real time according to the number of satellites and the HDOP value fed back by the GNSS; the more the number of satellites and the smaller the HDOP value, the higher the reliability of G base and the smaller the value of R k .

[0039] In step 3, when the latitude and longitude coordinates are provided by GNSS, R k The specific adjustment method is as follows:

[0040] A. When the number of satellites > 6 and HDOP < 1.5, G base is high reliability, at this time, R k = diag [0.01 2 , 0.01 2 , 0.01 2 ].

[0041] B. When 4 ≤ the number of satellites ≤ 6 and 1.5 ≤ HDOP ≤ 2, G base is medium reliability, at this time, R k = diag [0.05 2 , 0.05 2 , 0.05 2 ].

[0042] B. When the number of satellites < 4 and HDOP > 2, G base is low reliability, at this time, R k = diag [0.5 2 , 0.5 2 , 0.5 2 ].

[0043] In step 5, the dynamic adjustment formula of the process noise covariance matrix Q k is as follows:

[0044]

[0045] In the formula, ω k is the three-axis angular velocity of the mobile platform carrier at time k.

[0046] The present application has the following beneficial effects:

[0047] 1. Significantly enhance the system anti-interference ability: block the cross-period transmission of GNSS original noise through the error isolation mechanism, combine the dynamic compensation model with Kalman filtering, effectively suppress the position drift caused by error accumulation in traditional algorithm, and improve the data anti-interference ability in complex interference environment.

[0048] 2. All-round improve the dynamic positioning accuracy: based on the rigid body kinematics equation and the multi-force coupling compensation mechanism, accurately solve the attitude angle offset, inertial force offset and centrifugal force offset, significantly weaken the dynamic error caused by model simplification in high-speed maneuvering scene, and realize centimeter-level high-precision correction.

[0049] 3. Significantly strengthen the adaptability of complex signal environment: adopt the multi-mode observation noise real-time adaptation strategy of GNSS signal quality grading, dynamically adjust the data confidence weight under extreme conditions such as satellite blocking and multipath effect, ensure the continuity and reliability of positioning, and avoid the risk of positioning failure in bad environment.

[0050] 4. Significantly optimize the stability and consistency of long-time operation: through the double-parameter cooperative adaptive mechanism of observation noise and process noise, dynamically balance the influence of environmental mutation, carrier maneuverability and signal discontinuity on the correction system, eliminate the oscillation hidden danger of traditional fixed parameter algorithm, and ensure long-term stable operation.

[0051] 5. Fully expand the application scenarios and compatibility of the system: relying on the unified spatial coordinate system conversion framework and standardized dynamic compensation model design, compatible with multiple mobile platforms and mobile carriers such as ships, vehicles, drones and robots, realize high universality and full working condition adaptation ability.

[0052] 6. High-efficiency optimization of resource utilization and real-time performance: adopt lightweight matrix operation design and patterned noise parameter matching strategy, significantly reduce the computational load of embedded platform, reduce hardware resource consumption while ensuring real-time positioning accuracy, and meet the millisecond-level response demand of high dynamic scene.

[0053] 7. Significantly enhance the robustness of extreme environment: for extreme conditions such as GNSS signal interruption, realize short-term autonomous positioning based on dynamics prediction and historical motion parameters, effectively suppress the instantaneous positioning collapse problem of traditional scheme when signal is completely lost, and ensure the emergency positioning ability in critical scenarios. BRIEF DESCRIPTION OF DRAWINGS

[0054] Figure 1 is a flowchart of a sensor position coordinate dynamic correction method on a mobile platform carrier. DETAILED DESCRIPTION

[0055] The application will be further described in detail below in combination with the drawings and specific preferred embodiments.

[0056] As shown in the figure, a sensor position coordinate dynamic correction method on a mobile platform carrier includes the following steps. Figure 1

[0057] Step 1, static calibration

[0058] The "mobile platform carrier" in the application includes but is not limited to ships, aircraft, vehicles, underwater vehicles, engineering platforms, etc.

[0059] ​When the mobile platform carrier is stationary, the longitude and latitude coordinates of the main reference point and the sensor mounting point on the mobile platform carrier are preferably measured first by using a dual-frequency RTK-GNSS or the like, and then the obtained longitude and latitude coordinates (earth longitude and latitude high coordinates in the WGS84 coordinate system) are converted into three-axis coordinates in the spatial rectangular coordinate system (ECEF).

[0060]

[0061] In the formula, is the initial three-axis coordinates of the main reference point in the spatial rectangular coordinate system.

[0062] is the initial three-axis coordinates of the sensor mounting point in the spatial rectangular coordinate system.

[0063] Δx0, Δy0 and Δz0 are the coordinate difference values of the main reference point and the sensor mounting point in the spatial rectangular coordinate system at the initial time, respectively in the x-axis, y-axis and z-axis.

[0064] The above-mentioned ECEF coordinate system (earth-centered, earth-fixed coordinate system) is a global coordinate system, the x-axis points to the intersection of the prime meridian and the equator, the y-axis points to the east 90° direction in the equatorial plane and is perpendicular to the x-axis, and the z-axis points to the north pole of the earth, which is used to describe the absolute spatial position.

[0065] Step 2, establishing a dynamic compensation model: a dynamic compensation model is established for the sensor on the mobile platform carrier; the dynamic compensation model includes three dynamic offsets, which are respectively: attitude angle offset ΔG attitude , inertial force offset ΔG accel and centrifugal force offset ΔG centri .

[0066] The calculation method of the above-mentioned attitude angle offset ΔG attitude preferably includes the following steps.

[0067] Step 2-1, real-time acquisition of the attitude angle of the mobile platform carrier, specifically including: roll angle θ rotating around the X-axis, pitch angle φ rotating around the Y-axis and yaw angle ψ rotating around the Z-axis.

[0068] The acquisition method of the above-mentioned attitude angle, acceleration and angular velocity is preferably IMU / gyroscope / GNSS or the like.

[0069] The coordinate system used for the calculation of the above-mentioned attitude angle is the carrier coordinate system, which belongs to the local coordinate system, and is usually defined as: the X-axis is along the forward direction of the carrier (the longitudinal axis), the Y-axis is perpendicular to the X-axis and points to the right side of the carrier (the transverse axis), and the Z-axis is perpendicular to the XY plane and upward (the vertical axis), which is used to describe the attitude change of the carrier relative to itself.

[0070] Correlation: The attitude angles (θ, φ, ψ) of the carrier coordinate system need to be associated with the ECEF coordinate system through the coordinate transformation matrix to realize the mapping of local attitude to global position. How to convert the two coordinate systems into each other is a prior art method, which is not described here.

[0071] Step 2-2, constructing a three-axis rotation matrix, specifically: X-axis rotation matrix M x (θ), Y-axis rotation matrix M y (φ), and Z-axis rotation matrix M z (ψ).

[0072] Step 2-3, constructing an attitude angle compensation matrix M: according to the different rotation modes of the mobile platform carrier (the rotation sequence rule is based on the main motion characteristics of the carrier and the priority of attitude coupling), adjusting the three-axis rotation sequence, and multiplying the corresponding rotation matrix according to the adjustment sequence to obtain the attitude angle compensation matrix M.

[0073] The above rotation sequence depends on the dominant motion degree of freedom of the carrier, that is, the attitude axis that has the greatest impact on the carrier motion is preferentially matched.

[0074] Core rule: preferentially compensate the most active degree of freedom to ensure that the main disturbance is preferentially corrected, that is, the main error source is preferentially compensated.

[0075]

[0076] Step 2-4, calculating the attitude angle offset ΔG attitude , the specific calculation formula is:

[0077] ΔG attitude = M·ΔG0-ΔG0.

[0078] In step 2-2, the expressions of M x (θ), M y (φ), and M z (ψ) are respectively:

[0079]

[0080] In step 2-3, the calculation method of the attitude angle compensation matrix M is:

[0081] Further, the calculation formula of the above inertial force offset ΔG accel is preferably:

[0082]

[0083] In the formula, a is the three-axis real-time acceleration of the mobile platform carrier; Δt is the sampling time interval.

[0084] Further, the calculation formula of the above centrifugal force offset ΔG centriThe calculation formula is:

[0085] Delta G centri =(omega*(omega*Delta G0))*Delta t 2

[0086] In the formula, omega is the three-axis real-time angular velocity of the mobile platform carrier.

[0087] Step 3, position coordinate preliminary correction: the position coordinate G of the main reference point is acquired in real time base Then, the initial three-dimensional offset Delta G0 and the three dynamic offsets are superimposed to obtain the preliminary corrected sensor position coordinate The specific calculation formula is:

[0088]

[0089] The above G base is the three-axis real-time coordinate of the main reference point in the space rectangular coordinate system, which is obtained by converting the longitude and latitude coordinates provided by the compass or GNSS.

[0090] Step 4, Kalman filter optimization: the sensor position coordinate obtained in step 3 is taken as the observation value of the Kalman filter algorithm, and the Kalman filter is constructed to perform secondary correction on the sensor position coordinate, so as to obtain the accurate sensor position coordinate.

[0091] In the application, the construction method of the Kalman filter is preferably as follows.

[0092] Firstly, the state vector is defined:

[0093]

[0094] Among them: (x k ,y k ,z k ) is the position coordinate of the sensor at time k in the ECEF coordinate system.

[0095] is the velocity of the sensor at time k.

[0096] is the acceleration of the sensor at time k.

[0097] The state transition matrix is as follows:

[0098]

[0099] Among them: I3 is a 3*3 unit matrix; O3 is a 3*3 zero matrix; Delta t is a sampling time interval.

[0100] The observation value is the preliminary corrected position coordinate in step three i.e. observation vector Z k is:

[0101]

[0102] where: is the position coordinate of the sensor after preliminary correction at time k.

[0103] The observation matrix H is as follows:

[0104] H = [I3 03 03]

[0105] According to the motion model of the mobile platform carrier, the predicted state X k,k-1 and the predicted covariance P k,k-1 are calculated:

[0106] X k,k-1 = FX k-1

[0107] P k,k-1 = FP k-1 F T + Q k

[0108] where: X k,k-1 is the predicted value of the state at the current time; X k-1 is the estimated value of the state at the previous time; F is the state transition matrix; P k,k-1 is the predicted value of the covariance at the current time; P k-1 is the estimated value of the covariance at the previous time; Q k is the process noise covariance matrix.

[0109] Then, the Kalman gain is calculated:

[0110] K k = P k,k-1 H k T (HP k,k-1 H T + R k ) -1

[0111] where: K k is the Kalman gain at the current time k; R k is the observation noise covariance matrix at the current time k.

[0112] State and covariance update:

[0113] X k = X k,k-1 + H k (Z k -HX k,k-1 )

[0114] P k =(I-K k H)P k,k-1

[0115] wherein: X k is the state estimation value at the current k moment; P k is the covariance estimation value at the current k moment; I is a unit matrix.

[0116] The initial values of the related parameters are set as:

[0117]

[0118] The initial values of the Kalman filtering parameters (the initial value of the state covariance matrix P0, the initial value of the observation noise covariance matrix R0, and the initial value of the process noise covariance matrix Q0) are not randomly selected, but are determined based on the sensor hardware performance index, the static calibration accuracy, the dynamic scene error characteristics, and the engineering practice experience, and the specific values are determined according to the following:

[0119] 1. The state covariance matrix P0 is used to describe the initial uncertainty of the state vector (position, velocity, acceleration). The value of P0 needs to cover the “inherent error in the static calibration stage” and “sensor zero drift noise”, and cannot be too large to cause slow convergence of the filter, nor too small to cause the initial uncertainty to be ignored, and needs to match the error range of the actual measurement scene.

[0120]

[0121]

[0122] 2. The observation noise covariance matrix R0 is used to describe the noise level of the observation value (preliminarily corrected sensor position coordinates). In the present patent, R0 is set as: R0 = diag(0.05 2 , 0.05 2 , 0.05 2 ). The value is determined according to the following:

[0123] High-precision basis of static calibration: In step 1, double-frequency RTK-GNSS is used for static calibration, and the static positioning accuracy of RTK-GNSS in an open environment can reach ±1-5 cm in plane and ±2-10 cm in height, and the error of the preliminarily corrected position coordinates is small in the static scene, and the initial observation noise should reflect this high-precision characteristic.

[0124] Error characteristics of preliminary correction: The preliminary correction in step 3 has superimposed initial three-dimensional offset and dynamic compensation (attitude angle, inertial force, centrifugal force offset), and in a low dynamic scene, the error mainly comes from the residual error of static calibration, and the typical value can be controlled within ±5 cm.

[0125] Engineering experience adaptation: the initial noise level of 0.05m (5cm) can cover the measurement fluctuation in static state and ensure the reasonable trust degree of the observation value in the initial stage of Kalman filtering (avoiding the sensitivity to noise due to small R0 or insufficient observation value weight due to large R0).

[0126] 3. Process noise covariance matrix Q0 is used to describe the uncertainty of the system model (state transition equation) (such as unmodeled dynamic error, external disturbance, etc.), and the setting of Q0 in the patent is:

[0127] Q0 = diag(0.2 2 , 0.2 2 , 0.1 2 , 0.1 2 , 0.1 2 , 0.1 2 , 0.01 2 , 0.01 2 , 0.01 2 )

[0128] The value thereof is directly related to the dynamic scene error source and model simplification error:

[0129]

[0130] Q0 needs to balance the "model adaptation ability to dynamic scene", and the initial value needs to cover the model error in low dynamic scene to provide a reasonable benchmark for subsequent parameter adaptive adjustment.

[0131] Summary: the initial value setting of the related parameters ensures the rationality through the following logic

[0132] Matching with sensor performance: all parameters refer to the factory technical indicators of core sensors such as dual-frequency RTK-GNSS (static accuracy), IMU (acceleration /

[0133] angular velocity noise), etc., to avoid disconnection with the actual performance of the hardware.

[0134] Consideration of convergence speed and stability: the initial covariance value is neither too large (to avoid slow convergence in the initial stage of filtering) nor too small (to avoid excessive sensitivity to noise leading to oscillation), which can realize rapid convergence within 3-5 sampling periods through engineering practice verification (such as vehicle and ship static test).

[0135] Adaptation to dynamic adjustment requirements: the initial values of R0 and Q0 provide a benchmark for subsequent parameter adaptive adjustment, ensuring that the parameters can be smoothly transitioned in dynamic scenes (such as Q0 as a low dynamic benchmark, and ω k amplification in high dynamic state).

[0136] In summary, the initial value of Kalman parameter is determined according to the sensor hardware characteristics, static calibration accuracy, dynamic error law and engineering practice experience, which provides a reliable initial reference for subsequent dynamic correction and ensures the stability and accuracy of the filtering algorithm in complex scenes.

[0137] Step 5, parameter adaptive adjustment: dynamically and adaptively adjusting the observation noise covariance matrix R k and the process noise covariance matrix Q k in the Kalman filtering algorithm, so that: when the GNSS signal quality decreases, the weight of the observation value is reduced by increasing R k ; when the angular velocity of the mobile platform carrier increases, the dependence of state estimation on the observation value is increased by increasing Q k .

[0138] The adjustment method of the above observation noise covariance matrix R k is preferably:

[0139] A, when the latitude and longitude coordinates are adopted by the compass, R k =R0; wherein R0 is the initial set value of the observation noise covariance matrix.

[0140] B, when the latitude and longitude coordinates are provided by GNSS, R k is then dynamically adjusted in real time according to the number of satellites and the HDOP value fed back by GNSS; the more the number of satellites and the smaller the HDOP value, the higher the credibility of G base and the smaller the value of R k .

[0141] When the latitude and longitude coordinates are provided by GNSS, the specific adjustment method of R k is:

[0142] A, when the number of satellites is greater than 6 and the HDOP is less than 1.5, G base is of high credibility, at this time, R k =diag[0.01 2 ,0.01 2 ,0.01 2 ].

[0143] B, when the number of satellites is between 4 and 6 and the HDOP is between 1.5 and 2, G base is of medium credibility, at this time, R k =diag[0.05 2 ,0.05 2 ,0.05 2 ].

[0144] B, when the number of satellites is less than 4 and the HDOP is greater than 2, G base is of low credibility, at this time, R k= diag [0.5 2 , 0.5 2 , 0.5 2 ].

[0145] The R k dynamic adjustment is shown in the following table.

[0146]

[0147]

[0148] The dynamic adjustment formula of the above process noise covariance matrix Q k is as follows:

[0149]

[0150] In the formula, ω k is the three-axis angular velocity of the mobile platform carrier at time k.

[0151] When the mobile platform carrier rotates rapidly (||ω k || increases), the centrifugal force offset error in the dynamic compensation model will significantly increase, and by increasing Q k , the dependence of state estimation on observations is improved to avoid divergence caused by model mismatch.

[0152] In addition, when the GNSS signal interruption is > 2000ms, the observation update is turned off and switched to pure inertial prediction.

[0153] The preferred embodiments of the application are described in detail above, but the application is not limited to the specific details of the above-described embodiments. Within the technical concept of the application, various equivalent transformations of the technical solutions of the application can be made, and these equivalent transformations all belong to the protection scope of the application.

Claims

1. A method for dynamically correcting the position coordinates of sensors on a mobile platform carrier, characterized in that: Includes the following steps: Step 1, Static Calibration: When the mobile platform carrier is stationary, measure the position coordinates of the main reference point and the sensor mounting point on the mobile platform carrier, and calculate the initial three-dimensional offset ΔG0 of the sensor relative to the main reference point. Step 2: Establish a dynamic compensation model: Establish a dynamic compensation model for the sensors on the mobile platform carrier; the dynamic compensation model includes three dynamic offsets, namely: attitude angle offset ΔG attitude Inertial force offset ΔG accel and centrifugal force offset ΔG centri ; Step 3, Preliminary Position Coordinate Correction: Real-time acquisition of the position coordinates G of the main reference point. base Then, the initial three-dimensional offset ΔG0 and three dynamic offsets are superimposed to obtain the initially corrected sensor position coordinates. Step 4, Kalman filter optimization: Optimize the sensor position coordinates obtained in Step 3 by initially correcting them. Using the observations of the Kalman filter algorithm, a Kalman filter is constructed to perform a secondary correction on the sensor position coordinates, thereby obtaining accurate sensor position coordinates. Step 5, Adaptive parameter adjustment: Adjust the observation noise covariance matrix R in the Kalman filter algorithm. k The process noise covariance matrix Q k Dynamic adaptive adjustment is performed so that: when the GNSS signal quality deteriorates, R is increased. k The weight of the observation is reduced by increasing the value; when the angular velocity of the mobile platform carrier increases, the weight of the observation is reduced by increasing Q. k This improves the dependence of state estimation on observations.

2. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 1, characterized in that: In step 1, the latitude and longitude coordinates of the main reference point and the sensor installation point on the mobile platform are measured using dual-frequency RTK-GNSS. The obtained latitude and longitude coordinates are then converted into three-axis coordinates in a spatial rectangular coordinate system, thus obtaining the formula for calculating ΔG0: In the formula, The initial three-axis coordinates of the principal reference point in a spatial rectangular coordinate system; The initial three-axis coordinates of the sensor mounting point in a spatial rectangular coordinate system; Δx0, Δy0, and Δz0 are the coordinate differences between the main reference point and the sensor installation point on the x-axis, y-axis, and z-axis, respectively, in the spatial rectangular coordinate system at the initial moment.

3. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 1, characterized in that: In step 1 and step 2, the attitude angle offset ΔG attitude The calculation method includes the following steps: Step 2-1: Real-time acquisition of the attitude angles of the mobile platform carrier, specifically including: roll angle θ around the X-axis, pitch angle φ around the Y-axis, and yaw angle ψ around the Z-axis; Step 2-2: Construct the three-axis rotation matrix, specifically: X-axis rotation matrix M x (θ), Y-axis rotation matrix M y (φ) and Z-axis rotation matrix M z (ψ); Steps 2-3: Construct the attitude angle compensation matrix M: Adjust the rotation order of the three axes according to the different rotation methods of the mobile platform carrier, and multiply the corresponding rotation matrices according to the adjustment order to obtain the attitude angle compensation matrix M; Steps 2-4: Calculate the attitude angle offset ΔG attitude The specific calculation formula is as follows: ΔG attitude =M·ΔG0-ΔG0.

4. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 3, characterized in that: In step 2-2, M x (θ), M y (φ) and M z The expressions for (ψ) are as follows:

5. The method for dynamically correcting sensor position coordinates on a mobile platform carrier according to claim 3, characterized in that: In steps 2-3, the rotation order in the calculation of the attitude angle compensation matrix M depends on the dominant motion degree of freedom of the carrier. Priority is given to matching the attitude axis that has the greatest impact on the carrier's motion, thereby compensating the most active degree of freedom and ensuring that the main disturbance is corrected first.

6. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 1, characterized in that: In step 2, the inertial force shifts by ΔG. accel The calculation formula is: In the formula, a is the real-time three-axis acceleration of the mobile platform carrier; Δt is the sampling time interval.

7. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 1, characterized in that: In step 2, the centrifugal force shift ΔG centri The calculation formula is: ΔG centri =(ω×(ω×ΔG0))·Δt 2 In the formula, ω is the real-time angular velocity of the three axes of the mobile platform carrier; Δt is the sampling time interval.

8. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 1, characterized in that: In step 3, G base Before coordinate transformation, the latitude and longitude coordinates are provided via compass or GNSS; then in step 5, the observation noise covariance matrix R... k The adjustment method is as follows: When latitude and longitude coordinates are provided using a compass, R k =R0; where R0 is the initial setting value of the observation noise covariance matrix; When latitude and longitude coordinates are provided using GNSS, R k The system then dynamically adjusts its performance in real time based on the number of satellites and the HDOP value fed back by GNSS; the more satellites there are and the lower the HDOP value, the better the performance of the GNSS system. base The higher the credibility of R, k The smaller the value.

9. The method for dynamically correcting the sensor position coordinates on a mobile platform carrier according to claim 7, characterized in that: In step 3, when latitude and longitude coordinates are provided using GNSS, R k The specific adjustment method is as follows: A. When the number of satellites > 6 and HDOP < 1.5, G base For high credibility, R at this time k =diag[0.01 2 0.01 2 0.01 2 ]; B. When 4 ≤ number of satellites ≤ 6 and 1.5 ≤ HDOP ≤ 2, G base For medium confidence, R at this point k =diag[0.05 2 0.05 2 0.05 2 ]; B. When the number of satellites is less than 4 and HDOP is greater than 2, G base For low confidence, R at this time k =diag[0.5 2 0.5 2 0.5 2 ].

10. The method for dynamically correcting the position coordinates of sensors on a mobile platform carrier according to claim 1, characterized in that: In step 5, the process noise covariance matrix Q k The dynamic adjustment formula is: In the formula, ω k Let be the three-axis angular velocity of the mobile platform carrier at time k.

Images