A PID-based line-following control algorithm for unmanned vehicles

Abstract

This invention discloses a PID-based unmanned vehicle tracking control algorithm, comprising: S1, inputting the coordinates of the unmanned vehicle's start point, end point, and waypoints in the form of an [n×2]-dimensional matrix, where n is the number of path points, and the coordinates are established in a Cartesian coordinate system with the start point as the origin; S2, fitting a path curve formed by alternating straight lines and Bézier curves using the input path coordinate vectors, numbering each curve according to the fitting order, and storing the path information in the form of a function coefficient matrix; S3, using vehicle sensors to acquire the vehicle's current state in real time; S4, calculating the vehicle's closest point on the curve, and calculating the coordinates of the aiming point and the tangent direction of the aiming point based on the forward aiming distance; S5, calculating the distance error and angle error, and using the PID control algorithm, with the vehicle's steering curvature as the signal output, to control the vehicle's movement. This invention reduces the use of storage space and greatly reduces the computational power requirement.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle technology, specifically to a PID-based unmanned vehicle tracking control algorithm. Background Technology

[0002] Currently, most existing autonomous vehicle tracking control algorithms store path curves as discrete points. During motion control planning, multiple discrete points are continuously retrieved to calculate the cost function, and then the optimal target point is selected for control. This algorithm consumes a lot of storage space in the controller and uses a lot of computing power when iteratively calculating the cost function for discrete points. Insufficient computing power will cause significant deviations in the real-time control of the entire vehicle and untimely response. Summary of the Invention

[0003] To address the aforementioned problems, this invention provides a PID-based unmanned vehicle tracking control algorithm.

[0004] The technical solutions to the above technical problems are as follows:

[0005] A PID-based autonomous vehicle tracking control algorithm includes the following steps:

[0006] S1, input the coordinates of the start point, end point and waypoints of the autonomous vehicle in the form of an [n×2] dimensional matrix, where n is the number of path points, and the coordinates are established in a Cartesian coordinate system with the start point as the origin;

[0007] S2 uses the input path coordinate vector to fit the path curve formed by alternating straight lines and Bézier curves, numbers each curve in the fitting order, and stores the path information in the form of a function coefficient matrix.

[0008] S3 uses vehicle sensors to obtain the vehicle's current status in real time, such as vehicle speed, heading angle, and vehicle coordinates relative to the starting point.

[0009] S4. Use the vehicle's current position coordinates to determine the path number the vehicle is currently traveling on, and calculate the closest point on the curve based on the current path and the vehicle's current position. Calculate the coordinates of the forward aiming point and the tangent direction of the aiming point based on the forward aiming distance.

[0010] S5 calculates the distance error and angle error based on the vehicle's current position coordinates, heading angle information, coordinates of the aiming point, and tangent direction of the aiming point. Using a PID control algorithm, it controls the vehicle's movement by outputting the vehicle's steering curvature as a signal.

[0011] Furthermore, in S2, the path curve fitting method formed by alternating straight lines and Bézier curves is as follows: the straight line is a line connecting two fixed endpoints, and the Bézier curve is a second-order Bézier curve fitted with three points.

[0012] Furthermore, in S3, the real-time acquisition of the vehicle's current status information is achieved by inertial navigation, and the vehicle speed information is output by the vehicle speed calculation module of the vehicle's VCU controller.

[0013] Furthermore, in S4, the closest point is specifically the coordinate of the point on the Bezier curve that has the smallest absolute distance to the vehicle's current position, and the forward aiming distance is a fixed arc length.

[0014] Furthermore, in S4, the aiming point must meet the following condition: taking the nearest point as the starting point, the arc length between the aiming point and the nearest point is equal to the forward aiming distance.

[0015] Furthermore, in S5, the input of the PID algorithm is the distance error and angle error calculated based on the vehicle's current position coordinates, heading angle information, coordinates of the aiming point, and tangent direction of the aiming point. The PID input is the sum of the distance error and angle error, and the PID output is the vehicle's steering curvature.

[0016] The beneficial effects of this invention are: the PID-based unmanned vehicle tracking control algorithm provided by this invention solves the problem of computing power loss caused by numerous discrete points, and realizes a series of functions of unmanned vehicle, such as path fitting, autonomous obstacle avoidance and tracking control, after knowing the target point information, thus achieving the purpose of autonomous obstacle avoidance.

[0017] The beneficial effects of this invention are: the PID-based unmanned vehicle tracking control algorithm provided by this invention solves the problem of computing power loss caused by numerous discrete points, and realizes a series of functions of unmanned vehicle, such as path fitting, autonomous obstacle avoidance and tracking control, after knowing the target point information, thus achieving the purpose of autonomous obstacle avoidance. Attached Figure Description

[0018] Figure 1 This is a flowchart of the PID-based unmanned vehicle tracking control algorithm of the present invention;

[0019] Figure 2 This is a schematic diagram of path fitting for the PID-based unmanned vehicle tracking control algorithm of this invention.

[0020] Figure 3 This is a schematic diagram illustrating the selection of the vehicle reference point in the PID-based unmanned vehicle tracking control algorithm of this invention.

[0021] Figure 4 This is a Simulink modeling framework diagram of the PID-based unmanned vehicle tracking control algorithm of this invention;

[0022] Figure 5 This is a simulation error analysis diagram of the PID-based unmanned vehicle tracking control algorithm of this invention. Detailed Implementation

[0023] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0024] like Figures 1 to 5 As shown, a PID-based unmanned vehicle tracking control algorithm includes the following steps:

[0025] S1, input the coordinates of the start point, end point and waypoints of the autonomous vehicle in the form of an [n×2] dimensional matrix, where n is the number of path points, and the coordinates are established in a Cartesian coordinate system with the start point as the origin;

[0026] S2 uses the input path coordinate vector to fit the path curve formed by alternating straight lines and Bézier curves, numbers each curve in the fitting order, and stores the path information in the form of a function coefficient matrix.

[0027] S3 uses vehicle sensors to obtain the vehicle's current status in real time, such as vehicle speed, heading angle, and vehicle coordinates relative to the starting point.

[0028] S4. Use the vehicle's current position coordinates to determine the path number the vehicle is currently traveling on, and calculate the closest point on the curve based on the current path and the vehicle's current position. Calculate the coordinates of the forward aiming point and the tangent direction of the aiming point based on the forward aiming distance.

[0029] S5 calculates the distance error and angle error based on the vehicle's current position coordinates, heading angle information, coordinates of the aiming point, and tangent direction of the aiming point. Using a PID control algorithm, it controls the vehicle's movement by outputting the vehicle's steering curvature as a signal.

[0030] In S2, the path curve fitting method formed by alternating straight lines and Bézier curves is as follows: the straight line is a line connecting two fixed endpoints, and the Bézier curve is a second-order Bézier curve fitted with three points.

[0031] In S3, the real-time acquisition of the vehicle's current status information is achieved by inertial navigation, while the vehicle speed information is output by the vehicle speed calculation module of the vehicle's VCU controller.

[0032] In S4, the closest point is specifically the coordinates of the point on the Bezier curve that has the smallest absolute distance to the vehicle's current position, and the forward aiming distance is a fixed arc length.

[0033] In S4, the aiming point must meet the following condition: taking the nearest point as the starting point, the arc length between the aiming point and the nearest point is equal to the forward aiming distance.

[0034] In S5, the input of the PID algorithm is the vehicle's current position coordinates, heading angle information, coordinates of the aiming point and tangent direction of the aiming point, and the distance error and angle error are calculated. The PID input is the sum of the distance error and angle error, and the PID output is the vehicle's steering curvature.

[0035] Specifically, the algorithm is designed, verified, and simulated using Matlab as the development environment. The algorithm logic is based on... Figure 1 For example;

[0036] First, it is stipulated that the algorithm is applied in the Cartesian coordinate system, and the coordinates of the following points are represented by (x, y);

[0037] Given n path points (including the start and end points), use an n*2 two-dimensional matrix (denoted as PathM) to store the path point information (x, y), fit a path containing (2n-3) segments connected end to end by straight lines and Bézier curves, and then use a (2n-3)*8 two-dimensional matrix (denoted as PathFcnM) to store the path function information of each segment.

[0038] For the line Ay + Bx + C = 0, [FcnType,A,B,C,x1,x2,y1,y2], where A, B, and C represent the coefficients of the general equation for a line segment, and x1,x2,y1,y2 represent the x and y coordinates of the start and end points of the line segment, the general equation for a second-order Bézier curve is:

[0039] B(t)=(1-t) 2 P1+2t(1-t)P2+t 2 P3

[0040] In the formula, t is the parameter of Bézier curve, P1, P2, and P3 are the coordinates of three points, and B(t) is the coordinate of the Bézier curve with respect to t.

[0041] There is [FcnType,x1,y1,x2,y2,x3,y3,B t ], where x1, y1, x2, y2, x3, y3 represent the x and y coordinates of points P1, P2, and P3, respectively. P1, P2, and P3 refer to the three coordinate points selected for Bézier fitting. t The temporary fill values ​​are used to fill all 8 columns of the matrix;

[0042] In this algorithm, the angle range is defined as (-180°, 180°], corresponding to radians (-pi, pi]. This means that in a Cartesian coordinate system, angles in the first and second quadrants are positive, and those in the third and fourth quadrants are negative. The vehicle position is denoted as: VehPos = (x... Veh ,y Veh ).

[0043] R path segments can be fitted from n path points, where R = (2n-3). Let Nr be the path index. The information to be filled into the path function matrix (denoted as PathFcnM) is related to Nr. The information in the Nrth row of PathFcnM corresponds to the information of the Nrth path. For a straight line, the coordinates of the start and end points are needed. For a Bézier curve, the coordinates of three points are needed to fit this type of curve. The selection method is as follows:

[0044] ①When Nr=1,

[0045] FunType = 0,

[0046] Node1 = (x1, y1) = PathM(N) r ),

[0047]

[0048] ②When Nr>1, rem(Nr,2)=0,

[0049] FunType = 1,

[0050]

[0051] Node2=(x2,y2)=PathM(Nr / 2+1),

[0052]

[0053] ③When Nr>1, rem(Nr,2)≠0,

[0054] FunType = 0,

[0055]

[0056]

[0057] ④rem(Nr,2)≠0, hour,

[0058] rem(Nr, 2) calculates the remainder of Nr with respect to 2. This indicates that the vehicle has reached the last path corresponding to the destination, and special handling is required. The output is:

[0059] FunType = 0,

[0060]

[0061]

[0062] In the above, Node1, Node2, and Node3 refer to the selected point coordinates, Nr represents the path number, PathM(Nr) refers to the information in the Nr-th row of the path point matrix PathM, PathM(Nr) is the coordinate of a (x,y) point, FunType refers to the fitted path type, and PathFcnM refers to the matrix storing the fitted path information. Depending on the FunType, the stored information in each row follows the rules described above.

[0063] After obtaining the coordinates of the starting point and the ending point, Node1 and Node2, we can obtain the general expression of the straight line, Ay + Bx + C = 0.

[0064] For a Bézier curve, once the coordinates of three points Node1, Node2, and Node3 are known, they can be filled into the PathFcnM matrix according to Nr.

[0065] In path fitting, the fitting results are specifically as follows: Figure 2 As shown, the input for this step is the PathM path point information matrix, and the output is the PathFcnM path function matrix.

[0066] The method for calculating the nearest distance point MinDstNode is as follows:

[0067] (1) Straight-line path: First, calculate the perpendicular point IntersectNode(x) from the vehicle to the straight line Ay+Bx+C=0. I y I The foot of the perpendicular (x) is calculated. I y I Then compare it with the start and end points Node1(x1,y1) and Node2(x2,y2) of the referenced straight path. If it satisfies min(x1,x2)≤x I ≤max(x1,x2)and min(y1,y2)≤y I ≤max(y1,y2) means that the foot of the perpendicular lies on the reference straight line path, and MinDstNode=(x I ,y I Otherwise, calculate and compare the distances between the vehicle's position and the start and end points of the straight line. The specific calculation formula is as follows:

[0068]

[0069] In the formula, (x Veh ,y Veh ) represents the vehicle's position, Node1(x1,y1) and Node2(x2,y2) represent the coordinates of the start and end points of the straight line, and Dst represents the vehicle's position. i Indicates the distance between the vehicle's position and the start and end points of the straight line;

[0070] If Dst1 ≤ Dst2, MinDstNode = (x1, y1), otherwise MinDstNode = (x2, y2);

[0071] MinDstNode represents the coordinates of the nearest point.

[0072] (2) Bézier curve type path: The formula for a second-order Bézier curve is as follows:

[0073] B(t)=(1-t) 2 Node1+2t(1-t)Node2+t 2 Node3, t∈[0,1]

[0074] The formulas for calculating x and y are as follows:

[0075] x(t)=(1-t) 2 x1+2t(1-t)x2+t 2 x3,t∈[0,1]

[0076] y(t)=(1-t) 2 y1+2t(1-t)y2+t 2 y3,t∈[0,1]

[0077] In the formula, t represents the curve parameter of Bézier, B(t) is the coordinate of the fitted Bézier curve with respect to t, and x(t) and y(t) represent the abscissa and ordinate of the Bézier curve with respect to parameter t.

[0078] Record the vehicle's position (x) Veh ,y Veh The distance to the Bézier curve is calculated using the following formula:

[0079]

[0080] In the formula, (x Veh ,y Veh ) represents the vehicle's position, and Dst(t) represents the distance from the current vehicle coordinates to the coordinates corresponding to the value t on the Bézier curve;

[0081] Find the minimum value of Dst(t) on t∈[0,1] corresponding to t. min Then MinDstNode=(x(t) min ),y(t min MinDstNode represents the coordinates of the nearest point.

[0082] In the calculation of the nearest point, the input for this step is PathFcnM, and the output is the coordinates of the nearest point, MinDstNode.

[0083] The method for calculating the reference path number Nr is as follows:

[0084] The reference path number Nr is initially set to 1. After the vehicle starts moving, it is updated by comparing the minimum distance MinDst1 to the current reference path and the minimum distance MinDst2 to the next reference path. MinDst_i is the distance between the vehicle's position and the nearest point MinDstNode on the path, calculated according to the method in Section 3. The update occurs when the vehicle is closer to the next reference path (i.e., MinDst2 ≤ MinDst1) or when the distance to the next reference path is less than a certain calibration value Dst. Obj That is, MinDst2≤Dst Obj We update the reference path number, Nr = Nr + 1 (note that Nr cannot exceed the maximum value 2n - 3).

[0085] The coordinates of the two closest points, MinDstNode1 and MinDstNode2, are calculated in real time. Only one reference point is used to calculate the forward aiming point FAimNode. When Nr is not updated, MinDstNode = MinDstNode1 is output; otherwise, MinDstNode = MinDstNode2.

[0086] In the calculation of the reference path number, the input for this step is the current vehicle coordinates VehPos = (x Veh ,y Veh The function matrix PathFcnM is used to output the reference path number Nr.

[0087] The forward aiming point FAimNode is calculated as follows:

[0088] The forward aiming point FAimNode is calculated using line integrals. For a continuous function y = f(x), the arc length formula is:

[0089]

[0090] For parametric functions x(t) and y(t), the formula for calculating the arc length S is:

[0091]

[0092] Given MinDstNode and the target arc length ArcLength, the calculation will differ depending on the reference path.

[0093] For the line Ay+Bx+C=0, first calculate the distance from the nearest point MinDstNode to the end of the current reference path, DstEnd=Dst(MinDstNode,Node2). If DstEnd≥ArcLength, it means the forward preview point is on the current reference path; otherwise, it means the forward preview point is on the next path. In this case, the remaining arc length RemainArcLength=ArcLength-DstEnd needs to be used to calculate the next reference path. The same applies to Bézier curves.

[0094] For the line Ay + Bx + C = 0, calculate the arc length using the following formula.

[0095]

[0096]

[0097] In the formula, x MDN y MDN Let MinDstNode be the coordinate of the nearest point, and ArcLength be the aiming arc length. The coordinates x of the forward aiming point can be obtained by solving the integral according to the formula. FAN or y FAN Then, using the general formula for a straight line, Ay + Bx + C = 0, we can calculate another coordinate value. On the straight line, the tangent direction of the forward aiming point is represented as:

[0098]

[0099] Where FAimAg is the tangent function value of the forward aiming point.

[0100] After calculating FAimAg, since the range of the inverse trigonometric function after calculation is (-0.5pi, 0.5pi], we should correct it according to the orientation of the line so that the range is (-pi, pi).

[0101] For Bézier curves

[0102] x(t)=(1-t) 2 x1+2t(1-t)x2+t 2 x3,t∈[0,1]

[0103] y(t)=(1-t) 2 y1+2t(1-t)y2+t 2 y3,t∈[0,1]

[0104] Where x(t) and y(t) represent the horizontal and vertical coordinate parameter functions of the Bézier curve with respect to t, respectively.

[0105]

[0106] ArcLength is the calculated value of the arc length of the Bézier curve with respect to the parameter t.

[0107] x′(t)=(2x1-4x2+2x3)t+(2x2-2x1)

[0108] y′(t)=(2y1-4y2+2y3)t+(2y2-2y1)

[0109] For ease of calculation, it is denoted as:

[0110] A1 = 2x1 - 4x2 + 2x3, B1 = 2x2 - 2x1

[0111] A2 = 2y1 - 4y2 + 2y3, B2 = 2y2 - 2y1

[0112] have

[0113]

[0114] To simplify the formula, it is denoted as:

[0115]

[0116] b = 2A1B1 + 2A2B2

[0117]

[0118] When a≠0

[0119]

[0120] When a = 0

[0121]

[0122] t0 and t1 represent the parameter values ​​corresponding to the starting and ending points of the Bézier curve arc length calculation, and S is the arc length distance between the two points.

[0123] For Bézier curves, the arc length from the nearest point to the endpoint of the reference point should also be calculated first. However, unlike the calculation for straight lines, when calculating the nearest point of a Bézier curve, a corresponding t value t is obtained. min ,

[0124] a≠0

[0125]

[0126] When a = 0,

[0127]

[0128] DstEnd represents the arc length distance from the nearest point to the endpoint of the Bézier curve segment.

[0129] If DstEnd ≥ ArcLength, it means the forward preview point is on the current reference path; otherwise, it means the forward preview point is on the next path. In this case, the remaining arc length RemainArcLength = ArcLength - DstEnd needs to be used to calculate the next reference path.

[0130] If the aiming point is on the current reference path, then the formula is:

[0131]

[0132] For the above formula, substituting the forward aiming distance value into ArcLength, t min Substituting the parameter value t of the nearest point in the Bézier curve, the parameter value t of the forward aiming point is calculated. FAN .

[0133] After solving this equation, we can obtain t. FAN Substituting these values ​​into the parameter functions x(t) and y(t), we can obtain the coordinates of the forward aiming point on the Bézier curve. Simultaneously, we can determine the coordinates based on t. FAN Calculate the tangent direction at that point as follows:

[0134]

[0135] In the formula, k(t) represents the slope of the tangent line at the point t. FAN Substituting into the formula for k(t), we can obtain the tangent function value k of the forward aiming point on the Bézier curve. Calculating its inverse trigonometric function yields the angle value.

[0136] FAimAg=arctan(k)

[0137] In the formula, FAimAg is the angle of the tangent of the forward aiming point on the Bézier curve.

[0138] Based on the type of the reference path and the size of the pre-aiming arc length, after the above algorithm, a unique forward pre-aiming point coordinate FAimNode and the tangent direction FAimAg of that point can be obtained. If the point found by the pre-aiming arc length exceeds the end point of the total path, the end point coordinates are output.

[0139] In this implementation scheme, there are two input values ​​for PID control: one is the angle difference between the vehicle's heading angle VehYaw and the tangent direction FAimAg of the forward aiming point, and the other is the lateral distance between the vehicle and the path.

[0140] Err1=FAimAg-VehYaw

[0141] When calculating lateral distance, the direction of the road relative to the vehicle body should be taken into account. Define the ray with the vehicle body coordinate as the endpoint and the ray containing the vehicle's heading angle as one side of the angle l1. The ray connecting the vehicle body coordinate and the nearest point is the other side l2. If the angle formed by rotating counterclockwise around the endpoint l1 to l2 is equal to 0, the lateral distance from the vehicle body position to the nearest point is defined as 0, and the direction label is 0. If it is less than or equal to 180°, the lateral distance from the vehicle body position to the nearest point is defined as positive, and the direction label is 1. Otherwise, it is negative, and the direction label is -1.

[0142]

[0143] The direction indicator is represented by Sign.

[0144] The discrete PID algorithm is as follows:

[0145]

[0146] In the formula, k is the sampling number, e(k) is the sampled value, Kp is the proportional coefficient of the PID controller, Ki is the integral coefficient, and Kd is the derivative coefficient.

[0147] Let the output of the PID control be the steering curvature, and use the error values ​​Err1 and Err2 as the inputs to the PID, as follows:

[0148] κ1 = PID(Err1), where Kp is 4, Ki is 0.5, and Kd is 0;

[0149] κ2 = PID(Err2), where Kp is 10, Ki is 0.5, and Kd is 0.

[0150] κ=κ1+κ2

[0151] In the formula, κ represents the turning curvature of the output.

[0152] In PID control, the inputs for this step are the coordinates of the forward aiming point FAimNode and the tangent direction FAimAg of that point, and the current vehicle coordinates VehPos = (x Veh ,y Veh The vehicle's current heading angle Yaw is output as the vehicle's steering curvature κ.

[0153] In this implementation scheme, the Simulink modeling framework of the algorithm and the simulation error analysis are as follows: Figure 4 and 5 As shown.

[0154] Finally, it should be noted that the above-described embodiments are merely preferred embodiments of the present invention used to illustrate the technical solutions of the present invention, and are not intended to limit the invention or restrict its scope of protection. Although the invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to depart from the scope of protection of the claims.

Claims

1. A PID-based unmanned vehicle tracking control algorithm, characterized in that, Includes the following steps: S1, input the coordinates of the start point, end point and waypoints of the autonomous vehicle in the form of an [n×2] dimensional matrix, where n is the number of path points, and the coordinates are established in a Cartesian coordinate system with the start point as the origin; S2 uses the input path coordinate vector to fit the path curve formed by alternating straight lines and Bézier curves, numbers each curve in the fitting order, and stores the path information in the form of a function coefficient matrix. S3, use vehicle sensors to obtain the current status of the vehicle in real time, including vehicle speed, heading angle, and vehicle coordinate position relative to the starting point; S4. Use the vehicle's current position coordinates to determine the path number the vehicle is currently traveling on, and calculate the closest point on the curve based on the current path and the vehicle's current position. Calculate the coordinates of the forward aiming point and the tangent direction of the aiming point based on the forward aiming distance. S5 calculates the distance error and angle error based on the vehicle's current position coordinates, heading angle information, coordinates of the aiming point, and tangent direction of the aiming point. Using a PID control algorithm, it controls the vehicle's movement by outputting the vehicle's steering curvature as a signal.

2. The PID-based unmanned vehicle tracking control algorithm according to claim 1, characterized in that, In S2, the path curve fitting method formed by alternating straight lines and Bézier curves is as follows: the straight line is a line connecting two fixed endpoints, and the Bézier curve is a second-order Bézier curve fitted with three points.

3. The PID-based unmanned vehicle tracking control algorithm according to claim 1, characterized in that, In S3, the real-time acquisition of the vehicle's current status information is achieved by inertial navigation, while the vehicle speed information is output by the vehicle speed calculation module of the vehicle's VCU controller.

4. The PID-based unmanned vehicle tracking control algorithm according to claim 1, characterized in that, In S4, the closest point is specifically the coordinates of the point on the Bezier curve that has the smallest absolute distance to the vehicle's current position, and the forward aiming distance is a fixed arc length.

5. The PID-based unmanned vehicle tracking control algorithm according to claim 1, characterized in that, In S4, the aiming point must meet the following condition: taking the nearest point as the starting point, the arc length between the aiming point and the nearest point is equal to the forward aiming distance.

6. The PID-based unmanned vehicle tracking control algorithm according to claim 1, characterized in that, In S5, the input to the PID control algorithm is the sum of the distance error and the angle error, and the output of the PID control algorithm is the vehicle's steering curvature.

Images