Speed correction method and system applied to automatic driving container truck in port

Abstract

The invention discloses a speed estimation and error calibration method and system applied to an automatic driving container truck in a port, and the method comprises the steps: fusing the information of a plurality of vehicle-mounted sensors based on a multi-sensor fusion vehicle speed correction method, and reducing the probability of inaccurate and invalid vehicle speed measurement caused by a low-speed dead zone, a fault and slipping of a wheel speed sensor; in addition, two error compensation modes for the wheel speed scale coefficient error comprise offline compensation and online compensation, and the precision of speed estimation in the whole scene is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent driving, and particularly relates to a speed correction method and system for an automated container truck (AIV) used in port autonomous driving. Background Art

[0002] Port container handling operations involve three main processes, namely, an AIV loading and unloading containers at a quay crane, transporting between the quay crane and the yard, and loading and unloading containers at the yard. Among them, the process of an AIV transporting containers is called horizontal transportation. Against the backdrop of the gradual transformation of port construction from "automation" to "unmanned operation", intelligent driving flatbed transfer vehicles (hereinafter referred to as AIVs) have begun to be used in port horizontal transportation tasks to reduce the labor and management costs brought by manual container trucks. The positioning system provides the position, attitude, and speed information of the vehicle itself for the entire AIV intelligent driving system, which is a basic function of the AIV. The stability of the positioning system determines the reliability of the intelligent driving system.

[0003] In autonomous driving, the control system usually obtains the chassis vehicle speed information as speed feedback. The chassis vehicle speed information is calculated through a vehicle kinematic model by measuring the wheel speed with a wheel speed sensor and combining tire radius, wheel rotation angle, and mechanical parameter information. Calculating the vehicle speed from the wheel speed sensor has the following problems: (1) The tire radius can be affected by factors such as vehicle load, vehicle movement, and natural changes in tire pressure, resulting in a scale factor deviation in calculating the vehicle speed from the wheel speed; (2) The wheel speed sensor has a dead zone characteristic under conditions such as vehicle inching, acceleration, and deceleration moments, resulting in a non-linear error in speed measurement; (3) There is a principle error in calculating the vehicle speed through the wheel speed when the tire slips; (4) There is a failure in calculating the vehicle speed due to a wheel speed sensor fault.

[0004] Considering the defects in the above method of calculating the vehicle linear speed through the wheel speed sensor and the system design requirements for safety redundancy during the port automated container handling operation, this patent discloses a speed correction method and system for an automated container truck used in port autonomous driving based on multi-sensor fusion.

[0005] The information disclosed in this background art section is only intended to enhance the overall understanding of the present invention and should not be regarded as an admission or any form of suggestion that this information constitutes prior art already known to those of ordinary skill in the art. Summary of the Invention

[0006] In view of the problems of errors and failures in the vehicle speed measurement of existing port automatic driving internal container trucks in the port loading and unloading container operation scenario, this patent discloses a method for online estimating the vehicle speed by fusing multi-sensor information (including IMU, wheel speed sensor, wheel angle sensor, drive motor speed sensor, high-precision encoder, GNSS-RTK), and compensating for the wheel speed scale factor error in an online or offline manner, so as to reduce the vehicle speed error and improve the vehicle speed stability, thereby improving the control accuracy and safety of the vehicle automatic driving system.

[0007] To achieve the above object, the present invention provides a method for calibrating the wheel speed scale factor error of a port automatic driving internal container truck, including the following steps: Real-time collect the tire pressure data, speed data and the corresponding estimated value of the wheel speed scale factor during the vehicle operation; Divide the collected data according to the preset tire pressure interval and speed interval to form multiple groups of wheel speed scale factor sequences; In each interval, use the RANSAC algorithm (Random Sample Consensus) to perform outlier rejection and linear fitting processing on the wheel speed scale factor sequence; (The RANSAC algorithm is an iterative algorithm for fitting a mathematical model from data containing "outliers").

[0008] Determine the sampling value of the wheel speed scale factor corresponding to the center point of each interval according to the fitting result; Construct a two-dimensional mapping table including all tire pressure - speed intervals, and retain the historical sampling values for the uncovered intervals; Store the mapping table in the cloud and associate it with the vehicle ID for subsequent operation calls.

[0009] Preferably, in the above technical solution, the linear fitting processing specifically includes: Perform least squares linear regression analysis on the wheel speed scale factor sequence in each tire pressure - speed interval, and calculate the slope and intercept parameters of the regression equation; Substitute the coordinates of the interval center point into the regression equation to obtain the sampling value of this interval.

[0010] A multi-sensor fusion speed estimation system for a port automatic driving internal container truck, including: A sensor data acquisition module for obtaining real-time measurement data of an inertial measurement unit (IMU), a wheel speed sensor, a wheel rotation angle sensor, a driving motor speed sensor, and GNSS-RTK; (GNSS-RTK (Real-Time Kinematic) is a high-precision positioning technology based on the Global Navigation Satellite System (GNSS). By the collaborative work of a reference station and a rover station, it eliminates errors using carrier phase differential to achieve centimeter-level real-time positioning).

[0011] A kinematic calculation module that converts the wheel speed sensor data into a first linear velocity estimate based on the Ackerman model; A compensation decision module that selects an online / offline compensation mode according to the convergence state of an error state Kalman filter; A multi-source fusion module that achieves through an error state Kalman filter: a) Integrating the IMU acceleration to obtain a velocity prediction value; b) Using the compensated wheel speed linear velocity and the motor speed linear velocity as the first observation inputs; c) Converting the GNSS-RTK velocity and using it as the second observation input; d) Iteratively updating the vehicle velocity and the wheel speed scale factor estimate values.

[0012] Preferably, in the above technical solution, the compensation decision module specifically includes: When the error state Kalman filter has not converged, performing piecewise linear interpolation compensation based on a cloud two-dimensional mapping table; When the error state Kalman filter has converged, directly using the online estimate value of the current scale factor for compensation.

[0013] Preferably, in the above technical solution, the observation update process of the multi-source fusion module includes: Assigning a first weight coefficient to the compensated linear velocity of the wheel speed sensor; Assigning a second weight coefficient to the compensated linear velocity of the driving motor speed; Assigning a dynamically adjusted third weight coefficient to the GNSS-RTK converted velocity, and this coefficient is positively correlated with the GNSS signal quality index.

[0014] Preferably, in the above technical solution, it further includes an anomaly handling module, which: Monitors the confidence index of each sensor data in real time; When the wheel speed sensor fails, setting the covariance matrix element of the corresponding observation channel to a preset maximum value; When the RTK drops out of the fixed solution state, or when the GNSS speed accuracy factor is greater than the effective threshold, set the weight coefficient of the GNSS-RTK conversion speed described in claim 5 to zero, and set the convergence state of the error state Kalman filter to non-convergent.

[0015] A vehicle speed error compensation device, integrating the error calibration method and the speed estimation system described above, includes: An embedded processing unit that performs real-time data fusion and compensation calculations; A vehicle-cloud communication module that realizes the upload / download function of calibration data; A parameter storage unit that locally caches the calibration mapping tables of the most recent N operations.

[0016] Preferably, in the above technical solution, the parameter storage unit is configured with: An adaptive update mechanism that automatically downloads and overwrites the local old version when the cloud mapping table version is updated; A version rollback function that automatically restores the previous valid version when it is detected that the new version mapping table causes an increase in speed error.

[0017] An autonomous driving vehicle control method, adapted to the speed estimation system described above, inputs the finally fused speed estimation value into a model predictive controller; constructs a cost function based on the speed estimation error to optimize the output of the longitudinal control command; and monitors the speed estimation variance index in real time, triggering functional safety degradation actions such as speed reduction or braking when it exceeds the safety threshold.

[0018] A computer-readable storage medium stores program instructions that, when executed by a processor, implement all the functional steps of the error calibration method and the speed estimation system described above.

[0019] Compared with the prior art, the present invention has the following beneficial effects: 1. It provides a vehicle speed correction method and system based on multi-sensor fusion, which fuses information from multiple vehicle-mounted sensors, reducing the probability of inaccurate and ineffective vehicle speed measurement caused by the low-speed dead zone, faults, and slipping of wheel speed sensors; 2. It provides two error compensation methods for the wheel speed scale factor error, including offline compensation and online compensation, improving the accuracy of speed estimation in all scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is a schematic diagram of the system flow; Figure 2 It is a flowchart of the online speed estimation module. DETAILED DESCRIPTION OF THE INVENTION

[0021] The specific embodiments of the present invention are described in detail below, but it should be understood that the protection scope of the present invention is not limited by the specific embodiments.

[0022] Unless explicitly stated otherwise, throughout the specification and claims, the term “comprise” or variations such as “include” or “comprising”, etc., will be understood to include the stated elements or components but not to exclude other elements or components.

[0023] This solution provides a vehicle speed correction and error calibration system for port loading and unloading operations, including two modules: online speed correction and offline error calibration. The online speed correction module estimates the vehicle speed and wheel speed scale coefficient error in real time online by integrating multiple sensor information (such as IMU, wheel speed sensor, wheel angle sensor, drive motor speed sensor, high-precision encoder, GNSS-RTK, etc.); the offline error calibration module generates a mapping table of different tire pressures, speeds and wheel speed scale coefficients based on the online estimated wheel speed scale coefficient, which is used as the initial value and default value when the system is powered on.

[0024] The workflow of the offline error calibration module is as follows: First, it obtains the wheel speed coefficient state convergence of the error state Kalman filter in the online speed correction module. Once the state converges, the current tire pressure, speed and wheel speed coefficient estimates will be uploaded to the cloud at a certain frequency. Subsequently, the cloud will process these uploaded data and generate an offline wheel speed coefficient table, which provides a two-dimensional mapping relationship from tire pressure and speed to wheel speed coefficient. The main steps of the offline processing submodule include: 1. After an autonomous driving mission is completed, the cloud will obtain all the data during the operation and sort the data according to different tire pressures and speed ranges to obtain multiple sets of wheel speed coefficient sequences.

[0025] 2. For each tire pressure and speed interval, the RANSAC algorithm is used to remove abnormal points in the wheel speed scale coefficient sequence, and linear fitting is performed to obtain a linear representation of tire pressure and speed to wheel speed scale coefficient. Then, the wheel speed scale coefficient at the center point of the piecewise linear fitting is saved as the sampling value in the interval. Based on the wheel speed scale coefficient sampling values ​​of each interval, a two-dimensional mapping table of wheel speed scale coefficients corresponding to all tire pressure and speed distribution intervals during this operation is generated.

[0026] 3. For the uncovered tire pressure and speed ranges, the original wheel speed scale coefficient sampling values ​​are maintained unchanged, and the sampling values ​​of all ranges are combined to obtain a complete offline wheel speed scale coefficient table, which is saved in the cloud according to the vehicle ID. Before the next autonomous driving mission begins, the corresponding offline wheel speed scale coefficient table is downloaded and loaded from the cloud according to the vehicle ID.

[0027] 4. The working process of the online speed estimation module is as follows: First, load the offline wheel speed scale factor table from the offline error calibration module; then, perform fault diagnosis on the in-vehicle sensor data and eliminate abnormal data; next, determine whether the wheel speed scale factor state in the error state Kalman filter converges, and select the offline or online method to compensate for the wheel speed scale factor error according to its convergence; finally, perform multi-sensor fusion through the error state Kalman filter to obtain the best estimated values of the vehicle speed and the wheel speed scale factor.

[0028] The specific steps of the wheel speed scale factor error compensation module are as follows: Obtain the wheel speed and wheel rotation angle information from the wheel speed sensor and the wheel rotation angle sensor, combine mechanical parameters such as the tire radius and the vehicle wheelbase, and calculate the vehicle linear speed based on the Ackerman vehicle kinematic model.

[0029] Determine the compensation method according to the convergence of the wheel speed scale factor state in the error state Kalman filter. If the state does not converge, the offline method is used to compensate for the wheel speed scale factor error; if the state has converged, the online method is used for compensation.

[0030] In the offline method, according to the offline wheel speed scale factor table, use the piecewise linear interpolation method to obtain the wheel speed scale factor value corresponding to the current tire pressure and speed, and use this wheel speed scale factor to compensate the calculated vehicle linear speed.

[0031] In the online method, directly use the wheel speed scale factor obtained from the error state Kalman filter to compensate the calculated vehicle linear speed.

[0032] Calculate the vehicle linear speed corresponding to the driving motor speed through the tire radius and perform compensation according to the same steps as above.

[0033] The main steps of the multi-sensor fusion sub-module are as follows: Use the acceleration measured by the IMU after removing the gravity influence as the control input item of the error state Kalman filter, and integrate the acceleration to obtain the speed prediction value.

[0034] Use the vehicle linear speed calculated and compensated by the scale factor using the measured value of the wheel speed sensor and the driving motor speed as the observation value of the error state Kalman filter to correct the speed prediction value.

[0035] Similarly, use the vehicle linear speed calculated and compensated by the scale factor from the driving motor speed as the observation value of the error state Kalman filter to further correct the speed prediction value.

[0036] Convert the speed measured by GNSS-RTK to the vehicle coordinate system as the observation value of the error state Kalman filter, and at the same time perform the final correction on the speed prediction value and the wheel speed scale factor.

[0037] Repeat the iteration according to the above steps to obtain real-time estimated values of the vehicle speed and the wheel speed scale factor.

[0038] This specific embodiment details how to operate the proposed system and technical solution in practical applications to achieve more accurate vehicle speed estimation and error compensation, thereby enhancing the safety and efficiency of automated driving of container trucks in the port.

[0039] Abbreviation Full English Name Explanation GNSS Global Navigation Satellite System Global Navigation Satellite System RTK Real-Time Kinematic Real-Time Kinematic Differential IMU Inertial Measurement Unit Inertial Measurement Unit MEMS Microelectro Mechanical Systems Microelectro Mechanical Systems RANSAC Random Sample Consensus Random Sample Consensus

[0040] The foregoing description of specific exemplary embodiments of the present invention has been presented for purposes of illustration and example. These descriptions are not intended to limit the invention to the precise forms disclosed, and it is apparent that, according to the above teachings, many modifications and variations are possible. The purpose of selecting and describing the exemplary embodiments is to explain the specific principles of the invention and its practical applications, so that those skilled in the art can implement and utilize the various different exemplary embodiments of the invention, as well as various different selections and modifications. The scope of the invention is intended to be defined by the claims and their equivalents.

Claims

1. A method for calibrating the wheel speed coefficient error of container trucks in port automatic driving, characterized in that: The following steps are involved: Real-time collection of tire pressure data, speed data and corresponding wheel speed coefficient estimation during vehicle operation; The collected data is divided into intervals according to the preset tire pressure intervals and speed intervals to form multiple groups of wheel speed scale coefficient sequences; The RANSAC algorithm is used to remove outliers and perform linear fitting on the wheel speed coefficient sequence in each interval; Determine the wheel speed scale coefficient sampling value corresponding to the center point of each interval according to the fitting result; Construct a two-dimensional mapping table containing all tire pressure-speed intervals, and retain historical sampling values ​​for uncovered intervals; The mapping table is stored in the cloud and associated with the vehicle ID for subsequent operations.

2. The error calibration method according to claim 1, characterized in that: The linear fitting process specifically includes: Perform least squares linear regression analysis on the wheel speed coefficient sequence in each tire pressure-speed interval, and calculate the slope and intercept parameters of the regression equation; Substitute the coordinates of the center point of the interval into the regression equation to obtain the sampling value of the interval.

3. A multi-sensor fusion speed estimation system for container trucks in port autonomous driving, characterized in that: include: Sensor data acquisition module, used to obtain real-time measurement data of inertial measurement unit IMU, wheel speed sensor, wheel angle sensor, drive motor speed sensor and GNSS-RTK; A kinematics calculation module converts wheel speed sensor data into a first linear speed estimation value based on an Ackerman model; The compensation decision module selects the online / offline compensation mode according to the convergence state of the error state Kalman filter; Multi-source fusion module, implemented through error state Kalman filter: a) Integrate the IMU acceleration to obtain the velocity prediction value; b) using the compensated wheel speed linear velocity and motor speed linear velocity as the first observation input; c) Convert the GNSS-RTK velocity into the second observation input; d) Iteratively update the estimated values ​​of vehicle speed and wheel speed coefficients.

4. The speed estimation system according to claim 3, characterized in that: The compensation decision module specifically includes: When the error state Kalman filter does not converge, piecewise linear interpolation compensation is performed based on the two-dimensional mapping table in the cloud; When the error state Kalman filter converges, the online estimate of the current scale factor is directly used for compensation.

5. The speed estimation system according to claim 3, characterized in that: The observation update process of the multi-source fusion module includes: Assigning a first weight coefficient to the linear speed after the wheel speed sensor is compensated; Assigning a second weight coefficient to the linear velocity after the drive motor speed is compensated; A dynamically adjusted third weight coefficient is assigned to the GNSS-RTK conversion speed, and the third weight coefficient is positively correlated with the GNSS signal quality index.

6. The speed estimation system according to claim 3, characterized in that: Also included is an exception handling module, which: Real-time monitoring of each sensor data confidence index; When the wheel speed sensor fails, the covariance matrix element of the corresponding observation channel is set to a preset maximum value; When the RTK falls out of the fixed solution state, or the GNSS speed precision factor is poor, the weight coefficient of the GNSS-RTK conversion speed described in claim 5 is set to 0, and the convergence state of the error state Kalman filter is set to non-convergence.

7. A vehicle speed error compensation device, characterized in that: A speed estimation system integrating the error calibration method of claim 1 and any one of claims 3 to 6, comprising: Embedded processing unit to perform real-time data fusion and compensation calculations; Vehicle-to-cloud communication module, which enables the upload / download function of calibration data; The parameter storage unit locally caches the calibration mapping tables of the most recent N operations.

8. The compensation device according to claim 7, characterized in that: The parameter storage unit is configured with: Adaptive update mechanism: when the cloud mapping table version is updated, it will automatically download and overwrite the local old version; The version rollback function automatically restores the last valid version when it detects that the new version mapping table causes an increase in speed error.

9. A method for controlling an autonomous driving vehicle, adapted to the speed estimation system of any one of claims 3 to 6, characterized in that: The final fused speed estimate is input into the model predictive controller; Construct a cost function based on the speed estimation error and optimize the output of longitudinal control instructions; Monitor the speed estimation variance indicator in real time, and trigger functional safety degradation actions such as speed reduction or braking when it exceeds the safety threshold.

10. A computer-readable storage medium storing program instructions, characterized in that: When the instructions are executed by the processor, all functional steps of the error calibration method of any one of claims 1-2 and the speed estimation system of any one of claims 3-6 are implemented.

Images