Fusion preview PID transverse control method and system for automatic driving of port mine

Abstract

The invention discloses a fusion preview PID transverse control method and system for port and mine automatic driving, and belongs to the technical field of automatic driving, and the method specifically comprises the steps: obtaining a reference trajectory point and vehicle pose data, and calculating a nearest point of a vehicle relative to a reference trajectory and the curvature of the nearest point; switching a straight line control mode and a turning control mode through a dynamic smooth transition factor; calculating a preview distance according to the curvature, the vehicle speed and the wheelbase, and obtaining the pose of the preview point; transverse and course errors are calculated, and PID control is carried out to obtain feedback output; combining feedforward control output, and obtaining a final steering control command according to weight fusion; and vehicle pose and track changes are monitored in real time, and adjustment and control are performed. According to the method, stable automatic driving control of the vehicle under different road conditions is realized, and the driving safety and comfort are improved.

Description

Technical Field

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

[0002] During the driving process of autonomous vehicles in ports and mines, lateral control is one of the key technologies to ensure that the vehicle is stable and accurately drives on the predetermined path. Traditional lateral control methods have many shortcomings. For example, some simple control algorithms are difficult to adapt to the complex and changeable port and mine road environment and working conditions, which can easily lead to large deviations in vehicle driving, affecting transportation efficiency and safety.

[0003] For example, a Chinese patent with the authorization announcement number CN109001976B discloses a dual-path collaborative extension lateral control method for an autonomous driving vehicle, including: establishing a two-degree-of-freedom dynamics model, establishing a trajectory tracking preview error model, feature extraction and domain boundary division, and calculating a dual-path correlation function and control system output. This technical solution selects lateral position deviation and heading deviation as the characteristic quantities of the extension controller, and establishes two extension sets, divides the domain boundaries of the extension sets, and divides the two extension sets into three areas: the classical domain, the extension domain, and the non-domain. Then, two correlation function values ​​are calculated through the real-time characteristic quantities of the vehicle-road system, and each real-time characteristic state quantity is classified into each area based on the correlation function value. Based on this, the output values ​​of the two front wheel steering angles are calculated and output respectively, and finally the coordinated output of the dual-path extension controller is realized through the coordination weight coefficient.

[0004] For example, a Chinese patent with the authorization announcement number CN108732921B discloses a lateral extension preview switching control method for an autonomous vehicle, including: consisting of an upper and lower controller, in the upper controller, the lateral position deviation between the vehicle and the center line of the road and the curvature value 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 controller, the classical domain adopts a PD feedback controller based on the lateral position deviation and the heading deviation, and the extension domain adopts a PD feedforward-feedback controller based on the curvature of the road ahead. In the non-domain, the vehicle-road system is in an out-of-control state, and emergency braking is adopted. According to the different states of the vehicle-road system, the switching control of the two control strategies in the classical domain and the extension domain is realized. This technical solution successfully applies the extension control theory to the field of lateral control of autonomous vehicles, meeting the requirements for vehicle lateral control accuracy.

[0005] The above existing technologies all have the following problems: lack of specific implementation details, failure to fully consider the complexity and diversity of vehicles 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 view of the deficiencies of the prior art, the present invention proposes a fusion preview PID lateral control method and system for port and mine automatic driving, which obtains reference trajectory points and vehicle posture data, calculates the closest point of the vehicle relative to the reference trajectory and its curvature; switches the straight line and turning control modes through a dynamic smooth transition factor; calculates the preview distance according to the curvature, vehicle speed and wheelbase, and obtains 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 weight fusion; monitors the vehicle posture and trajectory changes in real time, and adjusts the control. This method realizes smooth automatic driving control of the vehicle under different road conditions, and improves 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 port and mine automatic driving, 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 according to the reference trajectory point and its two adjacent reference trajectory points, use the three-point circle fitting method to calculate the corresponding reference trajectory point curvature value;

[0011] Step S3: dynamically calculating different smooth transition factors through the curvature value of the current reference trajectory point, and performing linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factor to smoothly switch the control mode;

[0012] Step S4: Calculate the preview distance according to 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 according to the preview distance;

[0013] Step S5: Calculate the lateral error and heading error in the vehicle coordinate system according to the preview point posture data and the current posture data of the vehicle, and perform PID calculation on the lateral error and the heading error respectively to obtain the corresponding control output. 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 according to the front and rear wheelbase of the vehicle 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 of the vehicle's posture data and reference trajectory points in real time.

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

[0017] S2.1: According to the calculated reference trajectory point index value, the current trajectory point and its two previous and next trajectory points are extracted by 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 (x1, y1) and (x3, y3) before and after the current point based on the smoothing window value;

[0019] S2.3: Based on (x1, y1), (x2, y2), and (x3, y3), the current curvature value is calculated according to 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, exp(·) represents the exponential function;

[0024] S3.2: according to the current vehicle state and the reference trajectory, extracting the straight segment part and the turning segment part of the current vehicle preset driving from the reference trajectory, wherein the straight segment part includes the starting point, the end point and the direction vector of the straight segment, and the turning segment part includes the starting point, the end point, the turning radius and the curvature information of the turning segment curve;

[0025] S3.3: Calculate the speed difference e1 of the vehicle in the straight section and the speed difference e2 of the vehicle in the turning section according to the current speed of the vehicle and the expected speed, and obtain the acceleration or deceleration parameter values ​​in the straight section and the turning 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 a smooth transition parameter value β = (1-α) × p str +α×p turn , where p str represents the parameter value under the linear control strategy, p turnIndicates the parameter value under the turning control strategy;

[0027] S3.5: Use the smooth transition parameter value β 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 segment by segment, 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 , it is determined whether the reference trajectory points have been traversed; if the traversal is completed, the last point is selected as the preview point; if the traversal is not completed, it returns to S4.2 and recalculates the accumulation;

[0033] S4.3: According to the position of the preview point on the reference trajectory, obtain the corresponding preview point posture 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: 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 formula for PID control calculation is:

[0037]

[0038] Among them, δ p , δ j and δ c represents the proportionality 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 port and mine automatic driving, 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 smooth window value according to the index value of the nearest point, and calculate the corresponding reference trajectory point curvature value;

[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 to 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 of 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 position and 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 port and mine automatic driving, 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 cost.

[0062] 2. The present invention proposes a fusion preview PID lateral control method for port and mine automatic driving, which realizes efficient control under low computational complexity, reduces the performance requirements for hardware, and has higher real-time performance and engineering 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 effectively avoid excessive or insufficient response in the scenario of sudden change in path curvature, and ensure 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 oscillation 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 port and mine automatic driving according to the present invention;

[0064] Figure 2 This is a principle flow chart of a fusion preview PID lateral control method for port and mine automatic driving according to the present invention;

[0065] Figure 3 It 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 port and mine automatic driving of 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 3, an embodiment provided by the present invention: 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-mounted 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 value of the minimum distance and record the index value of the point. The Euclidean distance formula is a prior art in the art and is not an inventive solution of the present 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 according to the reference trajectory point and its two adjacent reference trajectory points, use the three-point circle fitting method to calculate the corresponding reference trajectory point curvature value;

[0078] Step S3: dynamically calculating different smooth transition factors through the curvature value of the current reference trajectory point, and performing linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factor to smoothly switch the control mode;

[0079] Step S4: Calculate the preview distance according to 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 according to the preview distance;

[0080] Step S5: Calculate the lateral error and heading error in the vehicle coordinate system according to the preview point posture data and the current posture data of the vehicle, and perform PID calculation on the lateral error and the heading error respectively to obtain the corresponding control output. 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: According to the front and rear wheelbases of the vehicle and the curvature value of the current reference trajectory point, the feedforward control output is calculated through the pre-established vehicle dynamics model and control algorithm, and the calculated feedforward value and feedback value are weightedly fused according to the 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 of 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 Convert into steering control commands executable by the vehicle, such as steering wheel angle and steering motor current;

[0086] 1) It is necessary to clarify the type of vehicle's steering system, because different types of steering systems will differ in how they receive and execute steering control commands;

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

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

[0089] 4) The mapped steering control command is sent to the actuator of the steering system, and the actuator performs corresponding actions according to the received signal, thereby driving the steering system of the vehicle to achieve steering.

[0090] (3) Use GPS to obtain the vehicle's current location in real time, such as longitude and latitude 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: According to the calculated reference trajectory point index value, the current trajectory point and its two previous and next trajectory points are extracted by 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 (x1, y1) and (x3, y3) before and after the current point based on the smoothing window value;

[0096] S2.3: Based on (x1, y1), (x2, y2), and (x3, y3), the current curvature value is calculated according to 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, exp(·) represents the exponential function;

[0101] S3.2: according to the current vehicle state and the reference trajectory, extracting the straight segment part and the turning segment part of the current vehicle preset driving from the reference trajectory, wherein the straight segment part includes the starting point, the end point and the direction vector of the straight segment, and the turning segment part includes the starting point, the end point, the turning radius and the curvature information of the turning segment 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 reference trajectory of the vehicle and smooth the reference trajectory to eliminate noise and jitter and improve the accuracy and reliability of the trajectory;

[0104] (2) Obtain the current vehicle status information, including position, speed, acceleration, and heading angle, through the vehicle's onboard 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 the vehicle's acceleration, speed change, heading angle change and other parameters;

[0105] (3) In order to distinguish between straight segments and turning segments, an acceleration threshold, a speed change threshold, and a heading angle change threshold are set, wherein these thresholds can be set according to 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 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 segment 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, such as steering angle, acceleration and deceleration, based on the extracted straight segments and turning segments.

[0111] S3.3: Calculate the speed difference e1 of the vehicle in the straight section and the speed difference e2 of the vehicle in the turning section according to the current speed of the vehicle and the expected speed, and obtain the acceleration or deceleration parameter values ​​in the straight section and the turning 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 a smooth transition parameter value β = (1-α) × p str +α×p turn , where p str represents the parameter value under the linear control strategy, p turn Indicates 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 segment by segment, 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 , it is determined whether the reference trajectory points have been traversed; if the traversal is completed, the last point is selected as the preview point; if the traversal is not completed, it returns to S4.2 and recalculates the accumulation;

[0119] S4.3: According to the position of the preview point on the reference trajectory, obtain the corresponding preview point posture 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: 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 formula for PID control calculation is:

[0123]

[0124] Among them, δ p , δ j and δ c represents the proportionality 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 port and mine automatic driving, 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 previous and next points through the smooth window value according to the index value of the nearest point, and calculate the curvature value of the corresponding reference trajectory point;

[0134] A 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;

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

[0136] The control module is used to calculate the lateral error and 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;

[0137] A fusion module is used to calculate the feedforward control output according to the vehicle dynamics model and the control algorithm, and to 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 of the vehicle's posture data and reference trajectory points in real time.

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

[0140] A point selection unit, 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] A smooth transition factor unit is used to calculate a smooth transition factor according to a 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 according to curvature value, speed and wheelbase;

[0147] The preview point acquisition unit is used to acquire the corresponding preview point position 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, used to calculate the feedforward control output through a pre-established vehicle dynamics model and control algorithm according to the front and rear wheelbases of the vehicle and the curvature value of the current reference trajectory point;

[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: a steering control command unit and a real-time monitoring unit;

[0155] A steering control command unit, used to control the output value according to the final steering command and output the 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 implementation modes, which are merely illustrative rather than restrictive. Under the guidance of the present invention, ordinary technicians in the field may also change, modify, replace and modify the above-mentioned embodiments without departing from the purpose and scope of protection of the present invention, and all of these are within the protection of the present invention.

Claims

1. A fusion preview PID lateral control method for port and mine automatic driving, 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 according to the reference trajectory point and its two adjacent reference trajectory points, use the three-point circle fitting method to calculate the corresponding reference trajectory point curvature value; Step S3: dynamically calculating different smooth transition factors through the curvature value of the current reference trajectory point, and performing linear interpolation on the straight line control strategy and the turning control strategy according to the smooth transition factor to smoothly switch the control mode; Step S4: Calculate the preview distance according to 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 according to the preview distance; Step S5: Calculate the lateral error and heading error in the vehicle coordinate system according to the preview point posture data and the current posture data of the vehicle, and perform PID calculation on the lateral error and the heading error respectively to obtain the corresponding control output. 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 according to the front and rear wheelbase of the vehicle 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 of the vehicle's posture data and reference trajectory points in real time.

2. A fusion preview PID lateral control method for port and mine automatic driving as claimed in claim 1, characterized in that: The specific steps of step S2 include: S2.1: According to the calculated reference trajectory point index value, the current trajectory point and its two previous and next trajectory points are extracted by 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 (x1, y1) and (x3, y3) before and after the current point based on the smoothing window value; S2.3: Based on (x1, y1), (x2, y2), and (x3, y3), the current curvature value is calculated according to 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. A fusion preview PID lateral control method for port and mine automatic driving as claimed in claim 2, 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, exp(·) represents the exponential function; S3.2: according to the current vehicle state and the reference trajectory, extracting the straight segment part and the turning segment part of the current vehicle preset driving from the reference trajectory, wherein the straight segment part includes the starting point, the end point and the direction vector of the straight segment, and the turning segment part includes the starting point, the end point, the turning radius and the curvature information of the turning segment curve; S3.3: Calculate the speed difference e1 of the vehicle in the straight section and the speed difference e2 of the vehicle in the turning section according to the current speed of the vehicle and the expected speed, and obtain the acceleration or deceleration parameter values ​​in the straight section and the turning 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 a smooth transition parameter value β = (1-α) × p str +α×p turn , where p str represents the parameter value under the linear control strategy, p turn Indicates the parameter value under the turning control strategy; S3.5: Use the smooth transition parameter value β as a parameter of the preview PID control algorithm to participate in the calculation of the control quantity output.

4. A fusion preview PID lateral control method for port and mine automatic driving as claimed in claim 3, 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 segment by segment, 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 , it is determined whether the reference trajectory points have been traversed; if the traversal is completed, the last point is selected as the preview point; if the traversal is not completed, it returns to S4.2 and recalculates the accumulation; S4.3: According to the position of the preview point on the reference trajectory, obtain the corresponding preview point posture data.

5. The fusion preview PID lateral control method for port and mine automatic driving 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: 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 formula for PID control calculation is: Among them, δ p , δ j and δ c represents the proportionality 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. A fusion preview PID lateral control method for port and mine automatic driving as claimed in claim 5, 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 port and mine automatic driving, which is used to implement a fusion preview PID lateral control method for port and mine automatic driving as claimed in 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 smooth window value according to the index value of the nearest point, and calculate the corresponding reference trajectory point curvature value; 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 to 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 of the vehicle's posture data and reference trajectory points in real time.

8. A fusion preview PID lateral control system for port and mine automatic driving 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. A fusion preview PID lateral control system for port and mine automatic driving 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 position and posture data on the reference trajectory according to the preview distance.

10. A fusion preview PID lateral control system for port and mine automatic driving 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