A fusion preview PID lateral control method and system for port and mine autonomous driving

Abstract

The present invention discloses a fusion preview PID lateral control method and system for autonomous driving in ports and mines, belonging to the field of autonomous driving technology. The method specifically comprises: obtaining reference trajectory points and vehicle posture data, calculating the closest point of the vehicle relative to the reference trajectory and its curvature; switching between straight-line and turning control modes through a dynamic smooth transition factor; calculating the preview distance based on the curvature, vehicle speed, and wheelbase, and obtaining the preview point posture; calculating the lateral and heading errors, performing PID control to obtain feedback output; combining the feedforward control output, and obtaining the final steering control command by weighted fusion; real-time monitoring of vehicle posture and trajectory changes, and adjusting the control. The method achieves smooth autonomous driving control of the vehicle under different road conditions, improving driving safety and comfort.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving technology, and specifically relates to a fusion preview PID lateral control method and system for autonomous driving in ports and mines. Background Art

[0002] For autonomous vehicles operating in ports and mines, lateral control is a key technology for ensuring stable and precise driving along the planned path. Traditional lateral control methods have numerous shortcomings. For example, simple control algorithms struggle to adapt to the complex and changing road environments and operating conditions of ports and mines, leading to significant deviations in vehicle operation and compromising transportation efficiency and safety.

[0003] For example, a Chinese patent with authorization announcement number CN109001976B discloses a dual-path collaborative extensional lateral control method for an autonomous vehicle, including: establishing a two-degree-of-freedom dynamic model, establishing a trajectory tracking preview error model, feature extraction and domain demarcation, and calculating a dual-path correlation function and control system output. This technical solution selects lateral position deviation and heading deviation as feature quantities of the extension controller, establishes two extension sets, and performs domain demarcation on the extension sets, dividing the two extension sets into three regions: the classical domain, the extensional domain, and the non-domain. Then, two correlation function values are calculated based on the real-time feature quantities of the vehicle-road system. Based on the correlation function values, each real-time feature state quantity is classified into different regions. Based on this, the two front wheel steering angle output values are calculated and output respectively. Finally, the coordinated output of the dual-path extension controller is achieved through the coordination weight coefficient.

[0004] For example, Chinese patent application number CN108732921B discloses a lateral extension preview switching control method for autonomous vehicles. The method comprises: an upper-layer controller and a lower-layer controller. In the upper-layer controller, the lateral position deviation between the vehicle and the road centerline and the curvature of the road ahead are selected as the eigenvalues of the extension set to divide the extension set, solve the correlation function, and divide the vehicle-road system state into the classical domain, the extension domain, and the non-domain. In the lower-layer controller, a PD feedback controller based on lateral position deviation and heading deviation is used in the classical domain, and a PD feedforward-feedback controller based on the curvature of the road ahead is used in the extension domain. In the non-domain, if the vehicle-road system is out of control, emergency braking is applied. Depending on the vehicle-road system state, switching control between the classical domain and the extension domain is achieved. This technical solution successfully applies extension control theory to the field of lateral control of autonomous vehicles, meeting the vehicle's lateral control accuracy requirements.

[0005] The above existing technologies all have the following problems: lack of specific implementation details, failure to fully consider the complexity and diversity of vehicle and road systems, and lack of specific verification and evaluation; emergency braking measures have limitations and low control accuracy. Summary of the Invention

[0006] In response to the shortcomings of the existing technology, the present invention proposes a fusion preview PID lateral control method and system for autonomous driving in ports and mines. The method obtains reference trajectory points and vehicle posture data, calculates the vehicle's closest point relative to the reference trajectory and its curvature; switches between straight-line and turning control modes using a dynamic smooth transition factor; calculates the preview distance based on the curvature, vehicle speed, and wheelbase to obtain the preview point posture; calculates the lateral and heading errors, performs PID control to obtain feedback output; combines the feedforward control output and obtains the final steering control command by weighted fusion; monitors vehicle posture and trajectory changes in real time, and adjusts the control. This method achieves smooth autonomous driving control of the vehicle under different road conditions, improving driving safety and comfort.

[0007] To achieve the above object, the present invention provides the following technical solutions:

[0008] A fusion preview PID lateral control method for autonomous driving in ports and mines, comprising:

[0009] Step S1: Obtain reference trajectory point data and the vehicle's own posture data, and use the Euclidean distance and interpolation method to calculate the closest trajectory point of the vehicle's own position relative to the reference trajectory, and record its index value;

[0010] Step S2: according to the reference trajectory point index value, obtain the index values of the two reference trajectory points adjacent to the reference trajectory point, and use the three-point circle fitting method to calculate the corresponding reference trajectory point curvature value according to the reference trajectory point and its two adjacent reference trajectory points.

[0011] Step S3: Dynamically calculate different smooth transition factors based on the curvature value of the current reference trajectory point, and perform linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factors to smoothly switch the control mode;

[0012] Step S4: Calculate the preview distance based on the curvature value of the current reference trajectory point, the current vehicle speed, and the front and rear wheelbase of the vehicle, and obtain the corresponding preview point pose data at the reference trajectory point based on the preview distance;

[0013] Step S5: Calculate the lateral error and heading error in the vehicle coordinate system based on the preview point pose data and the current vehicle pose data, and perform PID calculation on the lateral error and heading error respectively to obtain corresponding control outputs. At the same time, the sum of the control output of the lateral error and the control output of the heading error is used as the feedback control output;

[0014] Step S6: Calculate the feedforward control output based on the vehicle's front and rear wheelbase and the curvature value of the current reference trajectory point, and perform weighted fusion on the calculated feedforward value and feedback value according to the weight ratio to obtain the final steering command control output value;

[0015] Step S7: Obtain the steering control command according to the final steering command control output value, and monitor the changes in the vehicle's posture data and reference trajectory points in real time.

[0016] Specifically, the specific steps of step S2 include:

[0017] S2.1: Based on the calculated reference trajectory point index value, the current trajectory point and its two previous and next trajectory points are extracted through the smoothing window value.

[0018] S2.2: Using the smoothing window value as the index value, obtain the current point (x2, y2) on the reference trajectory, and obtain the two points before and after the current point (x1, y1) and (x3, y3) based on the smoothing window value;

[0019] S2.3: Based on (x1, y1), (x2, y2), and (x3, y3), calculate the current curvature value based on the product of the lengths of the three sides (den) and the area of the triangle formed by the three sides (area). The curvature value calculation formula is as follows:

[0020]

[0021] Where rate is the calculated curvature value.

[0022] Specifically, the specific steps of step S3 include:

[0023] S3.1: Calculate the smooth transition factor based on the obtained reference trajectory point curvature value Where g represents the gain coefficient and exp(·) represents the exponential function;

[0024] S3.2: Based on the current vehicle state and the reference trajectory, extract the straight segment and the turning segment of the current vehicle's predetermined travel from the reference trajectory. The straight segment includes the starting point, end point, and direction vector of the straight segment. The turning segment includes the starting point, end point, turning radius, and curvature of the turning curve.

[0025] S3.3: Calculate the vehicle's speed difference e1 on the straight section and the speed difference e2 on the curve section based on the vehicle's current speed and the desired speed, and obtain acceleration or deceleration parameter values for the straight section and the curve section.

[0026] S3.4: Use the smooth transition factor α as a weight to perform linear interpolation on the parameter values under the straight line control strategy and the turning control strategy to obtain the smooth transition parameter value β = (1-α) × p str +α×p turn , where p str represents the parameter value under the linear control strategy, p turnRepresents the parameter value under the turning control strategy;

[0027] S3.5: The smooth transition parameter value β is used as a parameter of the preview PID control algorithm to participate in the calculation of the control quantity output.

[0028] Specifically, the specific steps of step S4 include:

[0029] S4.1: Obtain vehicle status and reference trajectory information and calculate preview distance: Among them, d final Indicates the calculated preview distance, L zhou Indicates the front and rear wheelbase of the vehicle, k speed Indicates the gain coefficient of the current vehicle speed, g rate represents the preview curvature gain coefficient, v ad Indicates the current speed of the vehicle;

[0030] S4.2: Starting from the nearest point on the reference trajectory relative to the current vehicle position, traverse the reference trajectory points, calculate the preview distance from the current point to the next point step by step, and accumulate them to obtain the accumulated distance d lei ;

[0031] If d lei ≥d final , then set the current point as the preview point;

[0032] If d lei <d final , then determine whether the reference trajectory points have been traversed; if the traversal is completed, select the last point as the preview point; if the traversal is not completed, return to S4.2 and re-calculate the accumulation;

[0033] S4.3: According to the position of the preview point on the reference trajectory, obtain the corresponding preview point pose data.

[0034] Specifically, the specific steps of step S5 include:

[0035] S5.1: Obtain the preview point pose data and the vehicle's current pose data, and calculate the lateral error and heading error in the vehicle coordinate system based on the relationship between the preview point and the vehicle's current position. The formula for the lateral error is: e y (t) = -sin(θ ad (t))×x err +cos(θ ad (t))×y err , the heading error formula is: e hang (t) = θ ref (t)-θ ad (t), where e y (t) represents the lateral error value, ehang (t) represents the heading error value, θ ad (t) represents the current heading angle of the vehicle, θ ref (t) represents the heading angle of the preview point, x err Indicates the difference between the x coordinate value of the preview point and the current vehicle point, y err represents the difference between the y-coordinate value of the preview point and the current vehicle point, t represents the current time, sin(·) represents the sine function, and cos(·) represents the cosine function;

[0036] S5.2: For e y (t) and e hang (t) Perform PID control calculation to obtain the control output u of the lateral error y (t) and the control output u of the heading error hang (t), the PID control calculation formula is:

[0037]

[0038] Among them, δ p , δ j and δ c represents the proportional coefficient, Δt represents the time interval, e y (t-1) represents the lateral error at time t-1, e hang (t-1) represents the heading error at time t-1, e y (i) represents the lateral error at time i, e hang (i) represents the lateral error at time i, and n represents the number of time sampling points;

[0039] S5.3: Add the control output of the lateral error and the control output of the heading error to obtain the final feedback control output u(t) = u y (t)+u hang (t).

[0040] Specifically, the feedforward control output out in step S6 qian And the final steering command control output out final The calculation formula is:

[0041]

[0042] Among them, w qian Represents the feedforward weight coefficient, w fan represents the feedback weight coefficient, and atan(·) represents the inverse tangent function.

[0043] A fusion preview PID lateral control system for autonomous driving in ports and mines, comprising: a closest point matching module, a curvature calculation module, a smooth switching module, a preview module, a control module, a fusion module, and a monitoring module;

[0044] The closest point matching module is used to obtain reference trajectory point data and the vehicle's own posture data, and calculate the closest point of the vehicle relative to the reference trajectory;

[0045] The curvature calculation module is used to extract the current trajectory point and its two preceding and following points through a smoothing window value according to the index value of the nearest point, and calculate the curvature value of the corresponding reference trajectory point;

[0046] The smooth switching module is used to dynamically calculate the smooth transition factor according to the curvature value of the current reference trajectory point, and smoothly switch the straight line control and turning control strategies;

[0047] The preview module is used to calculate the preview distance according to the curvature value of the current reference trajectory point, the vehicle speed and the front and rear wheelbase of the vehicle, and obtain the corresponding preview point posture data;

[0048] The control module is used to calculate the lateral error and the heading error according to the preview point posture data and the current posture data of the vehicle, and perform PID calculation to obtain the control output;

[0049] The fusion module is used to calculate the feedforward control output according to the vehicle dynamics model and the control algorithm, and perform weighted fusion of the feedforward value and the feedback value to obtain the final steering command control output value;

[0050] The monitoring module is used to control the output value according to the final steering command, obtain the steering control command, and monitor the changes in the vehicle's posture data and reference trajectory points in real time.

[0051] Specifically, the smooth switching module includes: a smooth transition factor unit and a control strategy switching unit;

[0052] The smooth transition factor unit is used to calculate the smooth transition factor according to the curvature value;

[0053] The control strategy switching unit is used to perform linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factor.

[0054] Specifically, the preview module includes: a preview distance unit and a preview point acquisition unit;

[0055] The preview distance unit is used to calculate the preview distance according to the curvature value, speed and wheelbase;

[0056] The preview point acquisition unit is used to acquire corresponding preview point posture data on the reference trajectory according to the preview distance.

[0057] Specifically, the control module includes: an error calculation unit and a PID control unit;

[0058] The error calculation unit is used to calculate the lateral error and the heading error;

[0059] The PID control unit is used to perform PID calculations on the lateral error and the heading error respectively to obtain corresponding control outputs.

[0060] Compared with the prior art, the present invention has the following beneficial effects:

[0061] 1. The present invention proposes a fusion preview PID lateral control system for automatic driving in ports and mines, and optimizes and improves the architecture, operation steps and processes. The system has the advantages of simple process, low investment and operation costs, and low production work costs.

[0062] 2. The present invention proposes a fusion preview PID lateral control method for autonomous driving in ports and mines, which realizes efficient control under low computational complexity, reduces the performance requirements for hardware, and has higher real-time performance and engineering implementation convenience. Compared with the pure tracking algorithm or PID control algorithm with fixed parameters, the present application can optimize the control output in real time according to the path characteristics, especially in the scenario of sudden change in path curvature, effectively avoiding excessive or insufficient response, and ensuring the dynamic adaptability of the vehicle; at the same time, the vehicle steers more smoothly during the switching process between sharp turns and straight lines, reduces steering wheel vibration and frequent adjustments, and improves control stability and driving comfort. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 This is a schematic diagram of a fusion preview PID lateral control method for autonomous driving in ports and mines according to the present invention;

[0064] Figure 2 This is a principle flow chart of a fusion preview PID lateral control method for autonomous driving in ports and mines according to the present invention;

[0065] Figure 3 This is a flow chart of calculating the preview point according to the preview distance and the nearest point index value in a fusion preview PID lateral control method for automatic driving of a port mine according to the present invention;

[0066] Figure 4 This is an architecture diagram of a fusion preview PID lateral control system for autonomous driving in ports and mines according to the present invention. DETAILED DESCRIPTION

[0067] Example 1

[0068] See also Figure 1-Figure 3The present invention provides an embodiment of a fusion preview PID lateral control method for port and mine automatic driving, comprising the following steps:

[0069] Step S1: Obtain reference trajectory point data and the vehicle's own posture data, and use the Euclidean distance and interpolation method to calculate the closest trajectory point of the vehicle's own position relative to the reference trajectory, and record its index value;

[0070] Furthermore, the specific steps of step S1 include:

[0071] (1) Initialize the communication interface with the planning layer, send a connection request and wait for confirmation to ensure a stable communication connection with the planning layer;

[0072] (2) Create a data buffer or queue to store the reference trajectory point data received from the planning layer, and write a loop or event listener to continuously wait and receive the reference trajectory point data sent by the planning layer. When new reference trajectory point data is received, it is added to the data buffer or queue;

[0073] (3) Initialize the communication interface with the vehicle positioning and attitude measurement system. Similarly, write a loop or event listener to continuously wait for and receive the vehicle's own posture data, including position, speed, and heading. When new posture data is received, store it in an appropriate variable or data structure.

[0074] (4) Initialize a variable to store the index value of the nearest trajectory point, and at the same time, initialize a variable to store the minimum distance;

[0075] (5) traverse each point in the reference trajectory point data buffer or queue, and use the Euclidean distance formula to calculate the distance between the vehicle's current position and each point. If the calculated distance is less than the currently stored minimum distance, update the minimum distance value and record the index value of the point. The Euclidean distance formula is a prior art content in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0076] (6) After the traversal is completed, the index value of the nearest trajectory point is stored in an appropriate variable or data structure for subsequent use.

[0077] Step S2: according to the reference trajectory point index value, obtain the index values of the two reference trajectory points adjacent to the reference trajectory point, and use the three-point circle fitting method to calculate the corresponding reference trajectory point curvature value according to the reference trajectory point and its two adjacent reference trajectory points.

[0078] Step S3: Dynamically calculate different smooth transition factors based on the curvature value of the current reference trajectory point, and perform linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factors to smoothly switch the control mode;

[0079] Step S4: Calculate the preview distance based on the curvature value of the current reference trajectory point, the current vehicle speed, and the front and rear wheelbase of the vehicle, and obtain the corresponding preview point pose data at the reference trajectory point based on the preview distance;

[0080] Step S5: Calculate the lateral error and heading error in the vehicle coordinate system based on the preview point pose data and the current vehicle pose data, and perform PID calculation on the lateral error and heading error respectively to obtain corresponding control outputs. At the same time, the sum of the control output of the lateral error and the control output of the heading error is used as the feedback control output;

[0081] Step S6: Based on the vehicle's front and rear wheelbases and the curvature value of the current reference trajectory point, a feedforward control output is calculated using a pre-established vehicle dynamics model and control algorithm. The calculated feedforward value and feedback value are weighted and fused according to a weight ratio to obtain the final steering command control output value.

[0082] Step S7: Obtain the steering control command according to the final steering command control output value, and monitor the changes in the vehicle's posture data and reference trajectory points in real time.

[0083] Furthermore, the specific steps of step S7 include:

[0084] (1) Based on the weighted fusion of feedforward control and feedback control in step S6, the final steering command control output value out is obtained final ;

[0085] (2) The final steering command control output value out final Converted into steering control commands executable by the vehicle, such as steering wheel angle and steering motor current;

[0086] 1) The vehicle's steering system type needs to be identified, as different types of steering systems receive and execute steering control commands differently.

[0087] 2) Steering command control output value out final represents the desired vehicle steering angle or steering torque. This value needs to be parsed and converted into a format or signal recognizable by the vehicle steering system. The parsing process is a prior art in this field and does not constitute the inventive solution of this application, so it will not be described in detail here.

[0088] 3) Based on the steering system type, the parsed steering command control output value is mapped to the steering system's actuator. For example, in an electric steering system, this value may be converted into a current or voltage signal for the motor; in a hydraulic steering system, it may be converted into a hydraulic valve opening or pressure signal.

[0089] 4) The mapped steering control command is sent to the steering system's actuator, which performs corresponding actions based on the received signal, thereby driving the vehicle's steering system to achieve steering.

[0090] (3) Use GPS to obtain the vehicle's current location in real time, such as latitude and longitude coordinates, speed, acceleration, heading angle and other posture data;

[0091] (4) Based on predefined reference trajectories, such as parking paths and driving routes, the deviations between the vehicle and the reference trajectory points, such as lateral deviation, longitudinal deviation, and heading deviation, are calculated in real time;

[0092] (5) Based on the real-time monitored vehicle posture data and changes in the reference trajectory points, the steering control command is dynamically adjusted to reduce the deviation so that the vehicle can travel stably along the reference trajectory.

[0093] The specific steps of step S2 include:

[0094] S2.1: Based on the calculated reference trajectory point index value, the current trajectory point and its two previous and next trajectory points are extracted through the smoothing window value.

[0095] S2.2: Using the smoothing window value as the index value, obtain the current point (x2, y2) on the reference trajectory, and obtain the two points before and after the current point (x1, y1) and (x3, y3) based on the smoothing window value;

[0096] S2.3: Based on (x1, y1), (x2, y2), and (x3, y3), calculate the current curvature value based on the product of the lengths of the three sides (den) and the area of the triangle formed by the three sides (area). The curvature value calculation formula is as follows:

[0097]

[0098] Where rate is the calculated curvature value.

[0099] The specific steps of step S3 include:

[0100] S3.1: Calculate the smooth transition factor based on the obtained reference trajectory point curvature value Where g represents the gain coefficient and exp(·) represents the exponential function;

[0101] S3.2: Based on the current vehicle state and the reference trajectory, extract the straight segment and the turning segment of the current vehicle's predetermined travel from the reference trajectory. The straight segment includes the starting point, end point, and direction vector of the straight segment. The turning segment includes the starting point, end point, turning radius, and curvature of the turning curve.

[0102] Furthermore, the specific steps of extracting the straight segment and the turning segment of the current vehicle preset driving from the reference trajectory include:

[0103] (1) Obtain the vehicle's reference trajectory and smooth it to eliminate noise and jitter, thereby improving the accuracy and reliability of the trajectory;

[0104] (2) Obtain the current vehicle status information, including position, speed, acceleration, and heading angle, through the on-board sensors, and determine whether the vehicle is currently in a straight-line driving state or a turning state based on the vehicle status information. This can be achieved by analyzing parameters such as the vehicle's acceleration, speed change, and heading angle change;

[0105] (3) In order to distinguish between straight segments and turning segments, acceleration thresholds, speed change thresholds, and heading angle change thresholds are set. These thresholds can be set based on factors such as vehicle type, road conditions, and driving habits;

[0106] (4) Starting from the starting point of the reference trajectory, traverse the trajectory points one by one. If the acceleration, velocity change, heading angle change and other parameters of the current trajectory point and its adjacent trajectory points are all less than the set threshold, the trajectory segment is considered to be a straight line segment;

[0107] If any of the acceleration, velocity change, heading angle change, and other parameters of the current trajectory point and its adjacent trajectory points is greater than the set threshold, the trajectory is considered to be a turning segment;

[0108] (5) Mark the trajectories judged to be straight segments and turning segments and extract them separately;

[0109] (6) Further optimization of the extracted straight and turning segments, such as removing noise points and smoothing trajectories;

[0110] (7) Generate corresponding vehicle control instructions based on the extracted straight segments and turning segments, such as steering angle, acceleration and deceleration, etc.

[0111] S3.3: Calculate the vehicle's speed difference e1 on the straight section and the speed difference e2 on the curve section based on the vehicle's current speed and the desired speed, and obtain acceleration or deceleration parameter values for the straight section and the curve section.

[0112] S3.4: Use the smooth transition factor α as a weight to perform linear interpolation on the parameter values under the straight line control strategy and the turning control strategy to obtain the smooth transition parameter value β = (1-α) × p str +α×p turn , where p str represents the parameter value under the linear control strategy, p turn Represents the parameter value under the turning control strategy;

[0113] S3.5: Use the smooth transition parameter value β as a parameter of the preview PID control algorithm to participate in the calculation of the subsequent control quantity output.

[0114] Specifically, the specific steps of step S4 include:

[0115] S4.1: Obtain vehicle status and reference trajectory information and calculate preview distance: Among them, d final Indicates the calculated preview distance, L zhou Indicates the front and rear wheelbase of the vehicle, k speed Indicates the gain coefficient of the current vehicle speed, g rate represents the preview curvature gain coefficient, v ad Indicates the current speed of the vehicle;

[0116] S4.2: Starting from the nearest point on the reference trajectory corresponding to the current vehicle position, traverse the reference trajectory points, calculate the preview distance from the current point to the next point step by step, and accumulate them to obtain the accumulated distance d lei ;

[0117] If d lei ≥d final , then set the current point as the preview point;

[0118] If d lei <d final , then determine whether the reference trajectory points have been traversed; if the traversal is completed, select the last point as the preview point; if the traversal is not completed, return to S4.2 and re-calculate the accumulation;

[0119] S4.3: According to the position of the preview point on the reference trajectory, obtain the corresponding preview point pose data.

[0120] Specifically, the specific steps of step S5 include:

[0121] S5.1: Obtain the preview point pose data and the vehicle's current pose data, and calculate the lateral error and heading error in the vehicle coordinate system based on the relationship between the preview point and the vehicle's current position. The formula for the lateral error is: e y (t) = -sin(θ ad (t))×x err +cos(θ ad (t))×y err , the heading error formula is: e hang (t) = θ ref (t)-θ ad (t), where e y (t) represents the lateral error value, e hang (t) represents the heading error value, θad (t) represents the current heading angle of the vehicle, θ ref (t) represents the heading angle of the preview point, x err Indicates the difference between the x coordinate value of the preview point and the current vehicle point, y err represents the difference between the y-coordinate value of the preview point and the current vehicle point, t represents the current time, sin(·) represents the sine function, and cos(·) represents the cosine function;

[0122] S5.2: For e y (t) and e hang (t) Perform PID control calculation to obtain the control output u of the lateral error y (t) and the control output u of the heading error hang (t), the PID control calculation formula is:

[0123]

[0124] Among them, δ p , δ j and δ c represents the proportional coefficient, Δt represents the time interval, e y (t-1) represents the lateral error at time t-1, e hang (t-1) represents the heading error at time t-1, e y (i) represents the lateral error at time i, e hang (i) represents the lateral error at time i, and n represents the number of time sampling points;

[0125] S5.3: Add the control output of the lateral error and the control output of the heading error to obtain the final feedback control output u(t) = u y (t)+u hang (t).

[0126] Specifically, the feedforward control output out in step S6 qian And the final steering command control output out final The calculation formula is:

[0127]

[0128] Among them, w qian Represents the feedforward weight coefficient, w fan represents the feedback weight coefficient, and atan(·) represents the inverse tangent function.

[0129] Example 2

[0130] See also Figure 4 Another embodiment provided by the present invention is a fusion preview PID lateral control system for autonomous driving in a port mine, comprising:

[0131] Closest point matching module, curvature calculation module, smooth switching module, preview module, control module, fusion module, monitoring module;

[0132] The closest point matching module is used to obtain the reference trajectory point data and the vehicle's own posture data, and calculate the closest point of the vehicle relative to the reference trajectory;

[0133] The curvature calculation module is used to extract the current trajectory point and its two preceding and following points through a smooth window value according to the index value of the nearest point, and calculate the curvature value of the corresponding reference trajectory point;

[0134] Smooth switching module, used to dynamically calculate the smooth transition factor according to the curvature value of the current reference trajectory point, and smoothly switch between the straight line control strategy and the turning control strategy;

[0135] The preview module is used to calculate the preview distance and obtain the corresponding preview point pose data based on the curvature value of the current reference trajectory point, vehicle speed, and vehicle front and rear wheelbase;

[0136] The control module is used to calculate the lateral error and heading error based on the preview point posture data and the vehicle's current posture data, and perform PID calculation to obtain the control output;

[0137] The fusion module is used to calculate the feedforward control output based on the vehicle dynamics model and control algorithm, and perform weighted fusion of the feedforward value and the feedback value to obtain the final steering command control output value;

[0138] The monitoring module is used to control the output value according to the final steering command, obtain the steering control command, and monitor the changes in the vehicle's posture data and reference trajectory points in real time.

[0139] The curvature calculation module includes: point selection unit and three-point circle fitting unit;

[0140] A point selection unit is used to select the nearest reference trajectory point and its adjacent points according to the index value;

[0141] The three-point circle fitting unit is used to calculate the curvature value of the selected points using the three-point circle fitting method.

[0142] The smooth switching module includes: a smooth transition factor unit and a control strategy switching unit;

[0143] Smooth transition factor unit, used to calculate the smooth transition factor according to the curvature value;

[0144] The control strategy switching unit is used to perform linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factor to achieve smooth switching.

[0145] The preview module includes: a preview distance unit and a preview point acquisition unit;

[0146] Preview distance unit, used to calculate preview distance based on curvature value, speed and wheelbase;

[0147] The preview point acquisition unit is used to obtain the corresponding preview point posture data on the reference trajectory according to the preview distance.

[0148] The control module includes: an error calculation unit and a PID control unit;

[0149] An error calculation unit, used to calculate lateral error and heading error;

[0150] The PID control unit is used to perform PID calculations on the lateral error and the heading error respectively to obtain corresponding control outputs.

[0151] The fusion module includes: a feedforward control calculation unit and a weighted fusion unit;

[0152] A feedforward control calculation unit is used to calculate the feedforward control output based on the front and rear wheelbase of the vehicle and the curvature value of the current reference trajectory point using a pre-established vehicle dynamics model and control algorithm;

[0153] The weighted fusion unit is used to perform weighted fusion of the feedforward value and the feedback value according to the weight ratio to obtain the final steering command control output value.

[0154] The monitoring module includes: steering control command unit and real-time monitoring unit;

[0155] A steering control command unit, configured to control an output value according to a final steering command and output a steering control command;

[0156] The real-time monitoring unit is used to monitor the vehicle's posture data and changes in reference trajectory points in real time to ensure the stability and accuracy of the system.

[0157] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific embodiments. The above-mentioned specific embodiments are merely illustrative and not restrictive. Under the guidance of the present invention, ordinary technicians in this field can also change, modify, replace and modify the above-mentioned embodiments without departing from the purpose and scope of protection of the present invention. These are all protected by the present invention.

Claims

1. A fusion preview PID lateral control method for automatic driving in ports and mines, characterized in that: include: Step S1: Obtain reference trajectory point data and the vehicle's own posture data, and use the Euclidean distance and interpolation method to calculate the closest trajectory point of the vehicle's own position relative to the reference trajectory, and record its index value; Step S2: according to the reference trajectory point index value, obtain the index values of the two reference trajectory points adjacent to the reference trajectory point, and use the three-point circle fitting method to calculate the corresponding reference trajectory point curvature value according to the reference trajectory point and its two adjacent reference trajectory points. Step S3: Dynamically calculate different smooth transition factors based on the curvature value of the current reference trajectory point, and perform linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factors to smoothly switch the control mode; Step S4: Calculate the preview distance based on the curvature value of the current reference trajectory point, the current vehicle speed, and the front and rear wheelbase of the vehicle, and obtain the corresponding preview point pose data at the reference trajectory point based on the preview distance; Step S5: Calculate the lateral error and heading error in the vehicle coordinate system based on the preview point pose data and the current vehicle pose data, and perform PID calculation on the lateral error and heading error respectively to obtain corresponding control outputs. At the same time, the sum of the control output of the lateral error and the control output of the heading error is used as the feedback control output; Step S6: Calculate the feedforward control output based on the vehicle's front and rear wheelbase and the curvature value of the current reference trajectory point, and perform weighted fusion on the calculated feedforward value and feedback value according to the weight ratio to obtain the final steering command control output value; Step S7: Obtain the steering control command according to the final steering command control output value, and monitor the changes in the vehicle's posture data and reference trajectory points in real time.

2. The fusion preview PID lateral control method for automatic driving of a port mine as claimed in claim 1 is characterized in that: The specific steps of step S2 include: S2.1: Based on the calculated reference trajectory point index value, the current trajectory point and its two previous and next trajectory points are extracted through the smoothing window value. S2.2: Using the smoothing window value as the index value, obtain the current point (x2, y2) on the reference trajectory, and obtain the two points before and after the current point (x1, y1) and (x3, y3) based on the smoothing window value; S2.3: Based on (x1, y1), (x2, y2), and (x3, y3), calculate the current curvature value based on the product of the lengths of the three sides (den) and the area of the triangle formed by the three sides (area). The curvature value calculation formula is as follows: Where rate is the calculated curvature value.

3. The fusion preview PID lateral control method for automatic driving of a port mine as claimed in claim 2 is characterized in that: The specific steps of step S3 include: S3.1: Calculate the smooth transition factor based on the obtained reference trajectory point curvature value Where g represents the gain coefficient and exp(·) represents the exponential function; S3.2: Based on the current vehicle state and the reference trajectory, extract the straight segment and the turning segment of the current vehicle's predetermined travel from the reference trajectory. The straight segment includes the starting point, end point, and direction vector of the straight segment. The turning segment includes the starting point, end point, turning radius, and curvature of the turning curve. S3.3: Calculate the vehicle's speed difference e1 on the straight section and the speed difference e2 on the curve section based on the vehicle's current speed and the desired speed, and obtain acceleration or deceleration parameter values for the straight section and the curve section. S3.4: Use the smooth transition factor α as a weight to perform linear interpolation on the parameter values under the straight line control strategy and the turning control strategy to obtain the smooth transition parameter value β = (1-α) × p str +α×p turn , where p str represents the parameter value under the linear control strategy, p turn Represents the parameter value under the turning control strategy; S3.5: The smooth transition parameter value β is used as a parameter of the preview PID control algorithm to participate in the calculation of the control quantity output.

4. The fusion preview PID lateral control method for automatic driving of a port mine as claimed in claim 3 is characterized in that: The specific steps of step S4 include: S4.1: Obtain vehicle status and reference trajectory information and calculate preview distance: Among them, d final Indicates the calculated preview distance, L zhou Indicates the front and rear wheelbase of the vehicle, k speed Indicates the gain coefficient of the current vehicle speed, g rate represents the preview curvature gain coefficient, v ad Indicates the current speed of the vehicle; S4.2: Starting from the nearest point on the reference trajectory relative to the current vehicle position, traverse the reference trajectory points, calculate the preview distance from the current point to the next point step by step, and accumulate them to obtain the accumulated distance d lei ; If d lei ≥d final , then set the current point as the preview point; If d lei <d final , then determine whether the reference trajectory points have been traversed; if the traversal is completed, select the last point as the preview point; if the traversal is not completed, return to S4.2 and re-calculate the accumulation; S4.3: According to the position of the preview point on the reference trajectory, obtain the corresponding preview point pose data.

5. The fusion preview PID lateral control method for automatic driving of a port mine as claimed in claim 4 is characterized in that: The specific steps of step S5 include: S5.1: Obtain the preview point pose data and the vehicle's current pose data, and calculate the lateral error and heading error in the vehicle coordinate system based on the relationship between the preview point and the vehicle's current position. The formula for the lateral error is: e y (t) = -sin(θ ad (t))×x err +cos(θ ad (t))×y err , the heading error formula is: e hang (t) = θ ref (t)-θ ad (t), where e y (t) represents the lateral error value, e hang (t) represents the heading error value, θ ad (t) represents the current heading angle of the vehicle, θ ref (t) represents the heading angle of the preview point, x err Indicates the difference between the x coordinate value of the preview point and the current vehicle point, y err represents the difference between the y-coordinate value of the preview point and the current vehicle point, t represents the current time, sin(·) represents the sine function, and cos(·) represents the cosine function; S5.2: For e y (t) and e hang (t) Perform PID control calculation to obtain the control output u of the lateral error y (t) and the control output u of the heading error hang (t), the PID control calculation formula is: Among them, δ p , δ j and δ c represents the proportional coefficient, Δt represents the time interval, e y (t-1) represents the lateral error at time t-1, e hang (t-1) represents the heading error at time t-1, e y (i) represents the lateral error at time i, e hang (i) represents the lateral error at time i, and n represents the number of time sampling points; S5.3: Add the control output of the lateral error and the control output of the heading error to obtain the final feedback control output u(t) = u y (t)+u hang (t).

6. The fusion preview PID lateral control method for automatic driving of a port mine as claimed in claim 5 is characterized in that: In step S6, the feedforward control output out qian And the final steering command control output out final The calculation formula is: Among them, w qian Represents the feedforward weight coefficient, w fan represents the feedback weight coefficient, and atan(·) represents the inverse tangent function.

7. A fusion preview PID lateral control system for autonomous driving of a port mine, which is used to implement a fusion preview PID lateral control method for autonomous driving of a port mine according to any one of claims 1 to 6, characterized in that: include: Closest point matching module, curvature calculation module, smooth switching module, preview module, control module, fusion module, monitoring module; The closest point matching module is used to obtain reference trajectory point data and the vehicle's own posture data, and calculate the closest point of the vehicle relative to the reference trajectory; The curvature calculation module is used to extract the current trajectory point and its two preceding and following points through a smoothing window value according to the index value of the nearest point, and calculate the curvature value of the corresponding reference trajectory point; The smooth switching module is used to dynamically calculate the smooth transition factor according to the curvature value of the current reference trajectory point, and smoothly switch the straight line control strategy and the turning control strategy; The preview module is used to calculate the preview distance according to the curvature value of the current reference trajectory point, the vehicle speed and the front and rear wheelbase of the vehicle, and obtain the corresponding preview point posture data; The control module is used to calculate the lateral error and the heading error according to the preview point posture data and the current posture data of the vehicle, and perform PID calculation to obtain the control output; The fusion module is used to calculate the feedforward control output according to the vehicle dynamics model and the control algorithm, and perform weighted fusion of the feedforward value and the feedback value to obtain the final steering command control output value; The monitoring module is used to control the output value according to the final steering command, obtain the steering control command, and monitor the changes in the vehicle's posture data and reference trajectory points in real time.

8. The fusion preview PID lateral control system for automatic driving of a port mine as claimed in claim 7, characterized in that: The smooth switching module includes: a smooth transition factor unit and a control strategy switching unit; The smooth transition factor unit is used to calculate the smooth transition factor according to the curvature value; The control strategy switching unit is used to perform linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factor.

9. The fusion preview PID lateral control system for automatic driving of a port mine as claimed in claim 8, characterized in that: The preview module includes: a preview distance unit and a preview point acquisition unit; The preview distance unit is used to calculate the preview distance according to the curvature value, speed and wheelbase; The preview point acquisition unit is used to acquire corresponding preview point posture data on the reference trajectory according to the preview distance.

10. The fusion preview PID lateral control system for automatic driving of a port mine as claimed in claim 9, characterized in that: The control module includes: an error calculation unit and a PID control unit; The error calculation unit is used to calculate the lateral error and the heading error; The PID control unit is used to perform PID calculations on the lateral error and the heading error respectively to obtain corresponding control outputs.

Images