Big truck deviation rectification method based on double-layer adaptive unscented Kalman filter

Abstract

The application discloses a large vehicle rectification method based on double-layer adaptive unscented Kalman filtering, which comprises the following steps: step one, lane line segmentation is performed on video stream data shot by a camera, and a lateral offset of a vehicle relative to a lane center line and a heading deflection angle are calculated; step two, a lateral offset of the vehicle relative to a reference trajectory and heading angle information are calculated based on GNSS satellite positioning signals; and step three, double-layer adaptive unscented Kalman filtering is adopted to fuse visual measurement results and GNSS measurement results. The application can still maintain stable positioning and rectification ability under complex illumination, rain, snow, dust and satellite signal fluctuation environment through a double-layer adaptive mechanism (first layer priori adjustment based on sensor confidence and second layer posteriori correction based on innovation sequence), and can complete driving direction correction without manual intervention based on real-time fusion processing of video stream and GNSS data, so that the safety and operation efficiency of the large vehicle are improved.

Description

Technical Field

[0002] This invention relates to the interdisciplinary fields of computer vision, GNSS positioning, and automated control, and in particular to a vision-GNSS fusion method for correcting the course of a large vehicle based on a two-layer adaptive unscented Kalman filter. Background Technology

[0004] Rubber-tired gantry cranes (RTGs) are the main machinery used in specialized container terminal yard operations. Their operational efficiency and safety directly affect the speed and safe operation of the terminal. When an RTG is in motion, the driver needs to manually correct its direction to prevent it from deviating from the lane or even scraping against electrical racks or containers, which could lead to safety accidents.

[0005] A survey of current market systems addressing this issue reveals the following conclusions: Most systems utilize a single sensor (such as a photoelectric switch or magnetic nail) for lane correction. The drawbacks of this approach are the reliance on a single, expensive sensor and the difficulty in repairing a damaged one. Furthermore, the invention disclosed in CN116692686B, which lays reflectors along the RTG's route and uses lasers to measure the distance to these reflectors for lane correction, also suffers from high costs, frequent maintenance requirements, and potential disruptions to terminal layout planning. Additionally, system malfunction occurs when a single sensor fails, impacting terminal production efficiency. Therefore, developing a highly efficient, easy-to-maintain, dual-sensor redundant lane correction method for large vehicles is crucial. Summary of the Invention

[0007] To overcome the aforementioned shortcomings of existing technologies, this invention proposes a large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering.

[0008] The technical solution adopted by this invention to solve its technical problem is as follows: a large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering, comprising the following steps:

[0009] Step 1: Perform lane line segmentation on the video stream data captured by the camera, and calculate the lateral offset and yaw angle of the vehicle relative to the lane centerline;

[0010] Step 2: Calculate the vehicle's lateral offset and heading angle relative to the reference trajectory based on the satellite positioning signal output by the GNSS receiver;

[0011] Step 3: Use a two-layer adaptive unscented Kalman filter to fuse the visual measurement results and GNSS measurement results, and output a unified state estimate.

[0012] Step 4: Calculate the correction direction and correction data based on the fused unified state variables and send them to the trolley control system.

[0013] Compared with the prior art, the positive effects of the present invention are:

[0014] This invention enables the system to maintain stable positioning and correction capabilities even under complex lighting, rain, snow, dust, and satellite signal fluctuations through a two-layer adaptive mechanism (the first layer is based on prior adjustment of sensor confidence, and the second layer is based on posterior correction of innovative sequences). Based on real-time fusion processing of video stream and GNSS data, the system can complete the driving direction correction without manual intervention, thereby significantly improving the safety of large vehicle movement and the efficiency of RTG operations. Attached Figure Description

[0016] The present invention will be described by way of example and with reference to the accompanying drawings, wherein:

[0017] Figure 1 This is a module data flow diagram of the method of the present invention;

[0018] Figure 2 This is a flowchart illustrating the execution of the method of the present invention;

[0019] Figure 3 This is a detailed flowchart of the two-layer adaptive unscented Kalman filter method. Detailed Implementation

[0021] This invention provides a vision-GNSS fusion method for large vehicle deviation correction based on dual-layer adaptive unscented Kalman filtering, comprising:

[0022] I. Methods for lane line segmentation in video stream data:

[0023] (a) Image preprocessing, including IPM preprocessing, denoising, and contrast enhancement. Based on the camera intrinsic and extrinsic parameters obtained in step one, IPM (Inverse Perspective Mapping) is used to convert the perspective view in the image into a bird's-eye view, thereby eliminating perspective distortion due to the distance and size of roads and making the lane lines appear parallel in the image; image denoising is performed to reduce environmental noise interference; and image contrast enhancement is performed to improve the grayscale difference between lane lines and the background, thereby improving the accuracy of subsequent recognition.

[0024] (ii) The preprocessed image is input into the trained deep learning instance segmentation model to perform pixel-level segmentation of the lane line region in the image, resulting in a binary segmentation map and a probability distribution map of the lane lines. The instance segmentation model uses the latest YOLO series models, which are fast and accurate.

[0025] (iii) Perform morphological processing and connected component analysis on the segmentation results, filter out the recognition results that do not conform to the prior features of the lane lines, extract only the effective area of ​​the lane lines, and fit the lane line pixels to obtain the lane line boundary or lane center line.

[0026] (iv) Calculate the lane centerline based on the extracted lane line geometry information, and calculate the lateral offset of the large vehicle relative to the lane centerline in conjunction with the vehicle reference point position. and heading deflection angle .

[0027] II. Methods for calculating lateral offset and deflection angle using GNSS:

[0028] (i) After the satellite antenna is installed, the work site of the large vehicle needs to be surveyed; the reference trajectory data of the standard driving lane of the large vehicle needs to be obtained, and a reference straight line model needs to be established;

[0029] (ii) The position coordinates of the satellite antenna installed on the top of the vehicle are obtained in real time by the GNSS receiver, and the current driving trajectory direction of the vehicle is calculated based on the positioning coordinates at continuous time.

[0030] (III) The lateral offset is obtained by calculating the vertical distance from the real-time GNSS positioning point to the reference line, and the heading deflection angle is calculated by the angle between the current driving direction and the direction of the reference line. The calculation methods for the deflection angle and lateral offset are as follows:

[0031] If the coordinates of the two antennas are:

[0032]

[0033] Then the deflection angle :

[0034]

[0035] (iv) Predicting location using GNSS data. Based on current velocity. and yaw angle and angular velocity ,but:

[0036]

[0037] in:

[0038]

[0039] It can issue corrections in advance when the predicted trajectory is about to deviate. This applies when the system is downgraded to GNSS single-source mode, and deviations can be predicted in advance.

[0040] III. A two-layer adaptive unscented Kalman filter is used to fuse visual measurement results and GNSS measurement results:

[0041] To achieve nonlinear data-level fusion of video stream measurement information and GNSS positioning information, this invention constructs a nonlinear state-space model of the vehicle's motion and employs an adaptive unscented Kalman filter algorithm for state estimation. For example... Figure 3 As shown, the process is as follows:

[0042] (I) Construction of State-Space Model

[0043] (1) Definition of state vector

[0044] Assume the trolley correction system is in the first... The state vector at time t is:

[0045]

[0046] in: Let be the system state vector. This is the lateral deviation. This represents the rate of change of the lateral deviation. For heading angle, The rate of change of heading angle, and the state dimension. .

[0047] (2) State transition equation

[0048] The system motion model is represented as follows:

[0049]

[0050] in: It is a nonlinear state transition function. For process noise, , The process noise covariance matrix

[0051] (3) Observation equation

[0052] The observation model is represented as follows:

[0053]

[0054] in: For the observation vector, For nonlinear observation functions, , To observe the noise, To observe the noise covariance matrix.

[0055] The observation vector is defined as:

[0056]

[0057] in, This indicates the actual measured lateral deviation; This represents the actual measured heading angle.

[0058] (II) Two-layer adaptive unscented Kalman filter algorithm

[0059] (1) Sigma point generation

[0060] Let the current state estimate be... covariance is .

[0061] Define scaling parameters:

[0062]

[0063] in: The scaling factor for the Sigma point distribution. It is a second-order adjustment parameter.

[0064] generate Sigma points:

[0065]

[0066]

[0067]

[0068] in: This is the square root of the matrix (Cholesky decomposition). For the first Column vector.

[0069] (2) Weight calculation

[0070] Mean weights are used to calculate the weighted average of the transformed Sigma points, serving as an estimate of the mean of the new distribution.

[0071]

[0072]

[0073] in:

[0074] This represents the mean weight of the 0th Sigma point. Represents the mean weight of the remaining Sigma points Indicates the first Sigma points.

[0075] Covariance weights are used to calculate the weighted covariance of the transformed Sigma points around the new mean, serving as an estimate of the covariance of the new distribution.

[0076]

[0077]

[0078] in: Parameters used for Gaussian distribution optimization

[0079] (3) Time update

[0080] Sigma point propagation via state function:

[0081]

[0082] Predicted state mean:

[0083]

[0084] Predicting covariance:

[0085]

[0086] (4) Observation and prediction

[0087] 1) Visual confidence is defined as:

[0088]

[0089] in:

[0090] Indicates the confidence level of the segmentation model:

[0091]

[0092] In the above formula:

[0093] The set of effective lane line pixels;

[0094] Confidence level for each pixel.

[0095] Indicates the confidence level of the segmented area:

[0096] In the above formula:

[0097] The area is divided by the lane lines;

[0098] This is the theoretical reference area.

[0099] This represents the connectivity index, when lane lines are divided into... When there are connected components:

[0100]

[0101] Ideally, the number of connected components is 1.

[0102] 2) GNSS confidence level definition:

[0103]

[0104]

[0105] in:

[0106] , , These represent the satellite quantity factor, HDOP factor, and RTK status factor, respectively. When the number of currently visible satellites is... hour,

[0107]

[0108] HDOP (horizontal dilution of precision) is the horizontal precision factor; the smaller the value, the better for horizontal positioning.

[0109] in It is a constant and .

[0110] In GPS positioning, fixed, floating, and single-point solutions represent different levels of positioning accuracy. These differences depend on the positioning accuracy of the Real-Time Kinematic (RTK) receiver. When the positioning accuracy level is fixed, the RTK state factor is set to 1; when the positioning accuracy level is floating, the RTK state factor is set to 0.6; and when the positioning accuracy level is single-point, the RTK state factor is set to 0.3. This is illustrated by the following formula:

[0111]

[0112] 3) Construct confidence level and adjust observation noise

[0113] Let the basic observation covariance be:

[0114]

[0115] The confidence-adjusted observation covariance is:

[0116]

[0117] Sigma point propagation via observation function:

[0118]

[0119] 4) Predict the observed mean:

[0120]

[0121] Observation covariance:

[0122]

[0123] 5) Calculate the cross-covariance of state observations

[0124]

[0125] 6) Kalman gain:

[0126]

[0127] 7) Status Update:

[0128]

[0129] 8) Covariance update:

[0130]

[0131] (III) Adaptive Noise Update Mechanism

[0132] To improve the system's robustness under visual obstruction or GNSS signal drift, innovative statistics are introduced for adaptive adjustment of observation noise.

[0133] Define the innovation sequence:

[0134]

[0135] Constructing a sliding window to statistically analyze covariance:

[0136]

[0137] in: Length of the sliding window

[0138] Update the observation noise covariance matrix:

[0139]

[0140] in: Forgetting factor, To predict the observed covariance

[0141] The aforementioned adaptive mechanism enables the observation noise covariance matrix to dynamically adjust with environmental changes, thereby improving fusion stability.

[0142] (iv) Calculation of output correction amount

[0143] After obtaining the lateral deviation and heading angle from the fused state estimation, the corrective control parameters are calculated:

[0144]

[0145] in: To correct the output of the control. , All are control gain coefficients

[0146] The control input is sent to the trolley control system to achieve automatic correction.

[0147] (v) Location prediction

[0148] Based on the results of the two-layer adaptive unscented Kalman filter, the position of the large vehicle can be predicted more accurately.

Claims

1. A method for correcting vehicle deviation based on a two-layer adaptive unscented Kalman filter, characterized in that: Includes the following steps: Step 1: Perform lane line segmentation on the video stream data captured by the camera, and calculate the lateral offset and yaw angle of the vehicle relative to the lane centerline; Step 2: Calculate the vehicle's lateral offset and heading angle relative to the reference trajectory based on the satellite positioning signal output by the GNSS receiver; Step 3: Use a two-layer adaptive unscented Kalman filter to fuse the visual measurement results and GNSS measurement results, and output a unified state estimate. Step 4: Calculate the correction direction and correction data based on the fused unified state variables and send them to the trolley control system.

2. The large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering according to claim 1, characterized in that: The method for lane line segmentation of the video stream data captured by the camera described in step one is as follows: First, preprocess the image, including IPM preprocessing, noise reduction, and contrast enhancement; The second step is to input the preprocessed image into the trained deep learning instance segmentation model to perform pixel-level segmentation of the lane line region in the image, and obtain the lane line binary segmentation map and probability distribution map. The third step is to extract the effective area of ​​the lane line and fit the lane line pixels to obtain the lane line boundary or lane center line. The fourth step is to calculate the lane centerline based on the extracted lane line geometry information, and then, in conjunction with the vehicle reference point position, calculate the vehicle's lateral offset and heading angle relative to the lane centerline.

3. The large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering according to claim 1, characterized in that: The method for calculating the vehicle's lateral offset and heading angle relative to the reference trajectory based on the satellite positioning signal output by the GNSS receiver in step two is as follows: The first step is to survey the large vehicle's work site after the satellite antenna is installed, obtain reference trajectory data of the standard driving lane of the large vehicle, and establish a reference straight line model. The second step is to obtain the position coordinates of the satellite antenna installed on the top of the vehicle in real time through a GNSS receiver, and calculate the current driving trajectory direction of the vehicle based on the positioning coordinates at continuous time intervals. The third step is to obtain the lateral offset by calculating the vertical distance from the real-time GNSS positioning point to the reference line, and to calculate the heading deflection angle by the angle between the current driving direction and the direction of the reference line.

4. The large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering according to claim 1, characterized in that: The method described in step three for fusing visual measurement results and GNSS measurement results using a two-layer adaptive unscented Kalman filter is as follows: Step 1: Establish the state transition equation: in: For the large vehicle correction system in the first The state vector at time t, For lateral deviation, This represents the rate of change of the lateral deviation. For heading angle, The rate of change of heading angle, and the state dimension. , It is a nonlinear state transition function. For process noise, , The process noise covariance matrix; Step 2: Establish the observation equation: in: For the observation vector, For nonlinear observation functions, , To observe the noise, To observe the noise covariance matrix, This indicates the actual measured lateral deviation; Indicates the actual measured heading angle; Step 3: Generate Sigma points: in: Estimate the state at the current moment. For covariance, For scaling parameters, The square root of the matrix. For the first Column vectors The scaling factor for the Sigma point distribution. It is a second-order adjustment parameter; Step 4: Calculate the new mean weights Covariance weights : in: These are the parameters used for Gaussian distribution optimization; Step 5: Propagation of Sigma points via state functions: Predicted state mean: Predicted state covariance: Step 6: Propagation of the Sigma point via the observation function: Predicted observed mean: Predicted observation covariance: in, To adjust the observation covariance for confidence level, Step 7: Calculate the cross-covariance of state observations: Step 8: Calculate the Kalman gain: Step 9, Status Update: Step 10, Covariance Update: Step 11: Update the observation noise covariance matrix: in: Forgetting factor, To predict the observed covariance, To calculate the covariance for a sliding window.

5. The large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering according to claim 4, characterized in that: Construct confidence-adjusted observation covariance The method is as follows: (1) Calculate visual confidence : in: 1) The confidence score of the segmentation model is calculated using the following formula: In the formula: The set of effective lane line pixels; Confidence level for each pixel; The confidence level of the segmented area is calculated using the following formula: In the formula: The area is divided by the lane lines; This is the theoretical reference area; The connectivity index is calculated using the following formula: In the formula: The number of connected components; (2) Calculate GNSS confidence level : in: 1) It is calculated using the following formula: In the formula: Indicates the number of currently visible satellites. Indicates the number of theoretical reference satellites; 2) The HDOP factor is calculated using the following formula: In the formula: It is a constant and HDOP represents the horizontal precision factor; 3) The RTK state factor is calculated using the following formula: (3) Construct confidence-adjusted observation covariance: Let the basic observation covariance be: The confidence-adjusted observation covariance is: In the formula, For visual confidence, GNSS confidence level.

6. The large vehicle deviation correction method based on dual-layer adaptive unscented Kalman filtering according to claim 4, characterized in that: Construct a sliding window to calculate covariance using the following formula. : (1) Define the innovation sequence: (2) Constructing a sliding window to statistically analyze covariance: In the formula: This represents the length of the sliding window.

Images