CN117369269A - AGV motion control method based on pure tracking model - Google Patents
AGV motion control method based on pure tracking model Download PDFInfo
- Publication number
- CN117369269A CN117369269A CN202311444607.4A CN202311444607A CN117369269A CN 117369269 A CN117369269 A CN 117369269A CN 202311444607 A CN202311444607 A CN 202311444607A CN 117369269 A CN117369269 A CN 117369269A
- Authority
- CN
- China
- Prior art keywords
- speed
- track
- follows
- agv
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000001133 acceleration Effects 0.000 claims description 47
- 230000007704 transition Effects 0.000 claims description 34
- 230000036461 convulsion Effects 0.000 claims description 23
- 238000004364 calculation method Methods 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 14
- 230000002457 bidirectional effect Effects 0.000 claims description 10
- 230000006870 function Effects 0.000 claims description 9
- 238000013139 quantization Methods 0.000 claims description 6
- 230000011218 segmentation Effects 0.000 claims description 6
- 241001061225 Arcos Species 0.000 claims description 3
- 230000000295 complement effect Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to the technical field of AGV path planning and control, and discloses an AGV motion control method based on a pure tracking model. The AGV motion control method based on the pure tracking model is characterized in that a speed look-ahead planning method is adopted for longitudinal control of path tracking control, self-adaptive speed adjustment is realized by analyzing the length of a subsequent path and speed constraint in advance, a fuzzy controller is adopted for transverse control of the path tracking control, a proper pre-aiming distance is self-adaptively determined according to the speed and transverse deviation of an AGV vehicle, and when the speed is higher, a longer pre-aiming distance is adopted; and when the speed is slower, a shorter pre-aiming distance is adopted, and finally, the course angle of the vehicle is adjusted according to the pre-aiming distance, so that the accuracy of path tracking is improved.
Description
Technical Field
The invention relates to the technical field of AGV path planning and control, in particular to an AGV motion control method based on a pure tracking model.
Background
In a motion control system of a vehicle, expected tracks are analyzed mainly by combining sensor data such as laser radar, GPS, MU and the like, and an optimal control amount of the vehicle is calculated. Then, the motion control system controls actuators such as an accelerator, a brake, a steering wheel and the like of the vehicle so as to track the vehicle. In the trajectory tracking control of an AGV vehicle, methods generally employed include PID, LQR, MPC, and control based on a pure tracking model.
PID control is a linear feedback mode that processes errors through three parts, namely proportional (P), integral (I) and derivative (D), to bring the system output close to the desired value. LQR control is an optimal control strategy that controls a linear system by solving a linear quadratic cost function. MPC control is a model-based predictive control method that predicts future system behavior based on current state and system model in each control cycle, and then optimizes a cost function to determine optimal control inputs. The control based on the pure tracking model is a method for tracking the track based on the look-ahead information, and the expected track tracking is realized by setting the pretightening distance and calculating the control input according to the current position and pretightening point information.
The conventional pure tracking model has some problems in path tracking, mainly because the fixed pretightening distance cannot adapt to the conditions of different speeds and transverse deviations, so that the accuracy of path tracking is not high. Furthermore, since it only focuses on the current state, regardless of the future situation, a reaction delay may occur when a sudden change is encountered, resulting in a failure of the vehicle to adjust its own behavior in time.
Disclosure of Invention
(one) solving the technical problems
Aiming at the defects of the prior art, the invention provides an AGV motion control method based on a pure tracking model, which has the advantages of improving the accuracy and stability of path tracking and the like and solves the technical problems.
(II) technical scheme
In order to achieve the above purpose, the present invention provides the following technical solutions: an AGV motion control method based on a pure tracking model comprises the following steps:
calculating a plurality of path points through linear interpolation and an arc transition joint model through an algorithm formula, and generating a smooth expected motion track;
step two, performing speed prospective planning on the motion trail, and setting the prospective number N p And pre-analyzing the subsequent path length and speed constraint values versus maximum speed v at the path corners max Calculating;
thirdly, adjusting the pre-aiming distance according to different speeds of the vehicle by setting a fuzzy controller for the passing speed of the AGV, the speed after prospective planning and the transverse deviation between the current speed and the expected track;
and step four, calculating the front wheel steering angle of the AGV according to the calculated pre-aiming distance.
As a preferable technical scheme of the invention, the arc transition joint model is used for the current point P i-1 Inflection point P i End point P i+1 And the line segment in which it is locatedAnd->Length S of (2) i And S is i+1 Determining, calculating radius and transition distance of transition arc, and P i-1 、P i And P i+1 The three points are not collinear.
As a preferable technical scheme of the invention, the line segmentAnd->The calculation formula of the included angle alpha is as follows:
wherein,representing line segment->Vector of->Representation->Is |P i-1 P i The I is vector +.>Is |P i P i +1| is vector +>Is used for predicting the transition arc radius R for the inverse trigonometric function, and the calculation formula of arcos () is as follows:
wherein delta max The calculation formula of the predicted transition distance L at this time is as follows:
wherein,the tangent value of half the expressed included angle alpha limits the transition distance due to the length of the line segment, and the actual transition distance l' is calculated as follows:
wherein min represents the pairAnd->The minimum value of (2) is taken out, r' represents the actual transition arc radius,/and r->Representing a cotangent value for half of the included angle alpha;
the radius r of the transition arc and the transition distance l are obtained in a comprehensive way as follows:
wherein min (R', R) represents the minimum value for both calculation modes.
As a preferable technical scheme of the invention, the speed look-ahead planning is performed according to the look-ahead segment number N each time p A new track is gradually added to carry out speed planning and parameter adjustment, and the specific process is as follows:
s1, N before acquisition p Length of segment track and maximum speed v max ；
S2, adjusting the starting point and the end point speed of each track by using a bidirectional scanning technology according to constraint conditions and maximum speed limitation;
s3, calculating time and maximum speed of each section of the S-shaped speed curve required by the current track section;
s4, adjusting step number parameters of the S-shaped speed curve to meet constraint conditions;
s5, combining an S-shaped addition and subtraction control algorithm to obtain a speed curve of the front track.
As a preferable technical scheme of the invention, the speed look-ahead planning adopts a smooth splicing strategy of linear arcs to input path points to regenerate a multi-section smooth look-ahead track, and constraint conditions in the step S2 bidirectional scanning comprise maximum speed, maximum acceleration, maximum scram, line segment length, starting point and end point speed.
As a preferred embodiment of the present invention, the maximum velocity v max The calculation formula under the constraint condition is as follows:
wherein epsilon is the maximum contour error allowed in the operation process, theta is the complementary angle value of epsilon, alpha AB Represents the angle between the vector AB and the positive direction of the x-axis, beta BC The included angle between the vector BC and the positive direction of the x-axis is AB is the vector of the current motion path of the vehicle, BC is the vector of the motion path after the vehicle turns, B point is an inflection point, a X-max Maximum acceleration allowed in the x-axis direction, a Y-max Is the maximum acceleration allowed in the y-axis direction, cos beta BC Expressed as beta BC Cosine value of cos alpha AB Denoted by alpha AB Cosine value of sin alpha AB Denoted by alpha AB Sine value of sin beta BC Expressed as beta BC The sine value of (i) indicates the absolute value of the internal data, and T indicates the period.
As a preferable mode of the present invention, the bidirectional scanning in the step S2 includes a reverse scanning and a forward scanning, the reverse scanning is performed from the last track to the first track, and the end speed of each track is set to b v and setting the final track end point speed b v N+1 =0, where N represents the number of paths after segmentation, b v i+1 for the ith track end point speed determined by the (i+1) th track through the reverse scan, setting tangential acceleration A t The specific process of the reverse scan is as follows:
1) Let i=n and b v N+1 =0, after which step 2) is performed;
2) Executing 3) if i=1, otherwise executing 2);
from the end of the path to the beginning, the end speed of each track is adjusted by applying the constraint that the S-shaped jerk curve is bounded, and then the tangential S-shaped jerk bounded cubic equation can be obtained as follows:
wherein J is t For tangential jerk, s i Solving the i-1 th track end point speed under the bounded constraint of the tangential S-shaped jerk curve by using the running time calculated by taking the i-th track start time as the zero pointIf it isIt means that a tangential jerk bounded constraint is fulfilled if +.>Does not satisfy the acceleration condition, at this time +.>And b v i+1 the satisfied relation of (2) is as follows:
the i-1 th segment end point speed determined by the backward scanning under the constraintSolving, and then executing 2b;
2b. Final end point of the i-1 th segment of the reverse scanSpeed of speed b v i Finally, it is determined by the following formula:
wherein v is max V (i) for maximum speed of the whole track max Maximum speed for each segment of track after segmentation, followed by 2c;
subtracting one from the value of i, and re-executing the step 2), wherein the step 3) is not executed until the judging condition is met;
3) All of b v i (i=1, 2, …, N) is stored into the buffer.
As a preferred embodiment of the present invention, the forward scan is used to determine the initial velocity of the ith track obtained under tangential acceleration and acceleration constraintsThe specific flow of forward scanning is as follows:
(1) setting i=1 and the start speed v of the first track 1 =0, after which step (2) is performed;
(2) executing step (3) if i=n, otherwise executing step (2);
2d、the solution can be performed by the following relation, which is as follows:
solving the initial velocity of the ith track under tangential acceleration and acceleration constraint by the above methodIf it isIt means that a tangential jerk bounded constraint is met if +.> Then the solution is continued by the following relation, and after the solution is completed, 2e is executed, where the specific relation is as follows:
2e, i+1th segment path start point velocity v i+1 Determined by the following formula:
wherein the method comprises the steps ofRepresenting the initial velocity obtained by the (i+1) th track under tangential acceleration and acceleration constraint;
2f, adding one to the value of i, re-executing the step (2), and executing the step (3) until the judging condition is met
(3) Will all v i+1 (i=1, 2, …, N) is stored in the buffer, and then the speed profile of the AGV vehicle is generated from the data obtained from the two scans.
As a preferable technical scheme of the invention, the fuzzy controller in the third step sets the speed V and the transverse distance P as input quantities, sets the pretightening distance L as output quantity, wherein the argument of the speed V is [0,2 ]]M/S, discrete domains { -5, -4, -3, -2, -1,0,1,2,3,4,5}, quantization factor set to 2, dividing the velocity domain into 5 fuzzy subsets, fuzzy language value { very slow, moderate, fast } = { VS, S, M, F, VF }, lateral distance P domain of [0,2 }]m, discrete domains are { -5, -4, -3, -2, -1,0,1,2,3,4,5}, quantization factor is set to 1, dividing the lateral distance P domainsFor 5 fuzzy subsets, the fuzzy linguistic value is { small, medium, large } = { VS P ,S P ,M p Dividing the argument of the pretightening distance L into 5 fuzzy subsets, wherein the fuzzy language value is { very near, moderate, far } = { VN, N, M } L ,F L ,VF L }。
As a preferable technical scheme of the invention, the calculation formula of the front wheel rotation angle is as follows:
wherein L is the pretightening distance, delta is the front wheel rotation angle, arctan () is the arctangent trigonometric function, and alpha is the included angle between the vehicle body and Ld.
Compared with the prior art, the invention provides an AGV motion control method based on a pure tracking model, which has the following beneficial effects:
1. according to the invention, the problem of poor path tracking effect caused by fixed pre-aiming distance is solved by dynamically adjusting the pre-aiming distance by using the fuzzy controller, the speed and the acceleration are adaptively adjusted based on the speed look-ahead planning control, smooth and continuous speed and acceleration are ensured, and the jerk is smooth and bounded, so that the track tracking task under the constraint of maximum speed and maximum acceleration is realized.
2. The invention can ensure smooth and continuous speed and acceleration, flexible jerk and smooth bounded acceleration through the path tracking method, realize the track tracking task under the constraint of given maximum speed and maximum acceleration, and reduce the impact in the motion process.
Drawings
FIG. 1 is a schematic view of a circular arc transition joint model according to the present invention;
FIG. 2 is a schematic flow chart of the motion control algorithm of the present invention;
FIG. 3 is a schematic view of the maximum speed of a corner of the present invention;
FIG. 4 is a schematic view of a control surface according to the present invention;
FIG. 5 is a schematic illustration of a planned trajectory of the present invention;
FIG. 6 is a schematic diagram of tracking error according to the present invention;
FIG. 7 is a schematic diagram of the position, velocity and acceleration profiles of the present invention;
FIG. 8 is a pure trace path diagram of the present invention;
FIG. 9 is a schematic diagram of the input and output of the fuzzy controller of the present invention;
FIG. 10 is a flow chart of the present invention.
Detailed Description
Embodiments of the present invention are described in further detail below with reference to the accompanying drawings and examples. The following examples are illustrative of the invention but are not intended to limit the scope of the invention.
In the description of the present invention, unless otherwise indicated, the meaning of "a plurality" is two or more; the terms "upper," "lower," "left," "right," "inner," "outer," "front," "rear," "head," "tail," and the like are used as an orientation or positional relationship based on that shown in the drawings, merely to facilitate description of the invention and to simplify the description, and do not indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore should not be construed as limiting the invention. Furthermore, the terms "first," "second," "third," and the like are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
In the description of the present invention, it should be noted that, unless explicitly specified and limited otherwise, the terms "connected," "connected," and "connected" are to be construed broadly, and may be either fixedly connected, detachably connected, or integrally connected, for example; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.
An AGV motion control method based on a pure tracking model comprises the following steps:
step one, according to fig. 1, calculating a plurality of path points through linear interpolation and an arc transition linking model through an algorithm formula, and generating a smooth expected motion track.
Arc transition linking model for current point P i-1 Inflection point P i End point P i+1 And the line segment in which it is locatedAndlength S of (2) i And S is i+1 Determining, calculating radius and transition distance of transition arc, and P i-1 、P i And P i+1 Three points are not collinear, P in the figure a And P e Respectively a starting point and an ending point of the circular arc, a line segment +.>And->The calculation formula of the included angle alpha is as follows:
wherein,representing line segment->Vector of->Representation->Is |P i-1 P i The I is vector +.>Is |P i P i +1| is vector P i P i+1 Is used for predicting the transition arc radius R for the inverse trigonometric function, and the calculation formula of arcos () is as follows:
wherein delta max The calculation formula of the predicted transition distance L at this time is as follows:
wherein,the tangent value of half the expressed included angle alpha limits the transition distance due to the length of the line segment, and the actual transition distance l' is calculated as follows:
wherein min represents the pairAnd->The minimum value of (2) is taken out, r' represents the actual transition arc radius,/and r->Representing a cotangent value for half of the included angle alpha;
the radius r of the transition arc and the transition distance l are obtained in a comprehensive way as follows:
wherein min (R', R) represents the minimum value for both calculation modes
Step two, performing speed prospective planning on the motion trail, and setting the prospective number N p And pre-analyzing the subsequent path length and speed constraint values versus maximum speed v at the path corners max Calculating;
wherein the speed look-ahead programming is based on the number of look-ahead segments N each time p A new track is gradually added to carry out speed planning and parameter adjustment, and the specific process is as follows:
s1, N before acquisition p Length of segment track and maximum speed v max ；
S2, adjusting the starting point and the end point speed of each track by using a bidirectional scanning technology according to constraint conditions and maximum speed limitation;
s3, calculating time and maximum speed of each section of the S-shaped speed curve required by the current track section;
s4, adjusting step number parameters of the S-shaped speed curve to meet constraint conditions;
s5, combining an S-shaped addition and subtraction control algorithm to obtain a speed curve of the front track.
Further, the main flow of the speed look-ahead algorithm uses a smooth splicing strategy of linear arcs for input path points to regenerate a plurality of sections of smooth look-ahead tracks, then calculates the maximum speed of the path line segment under the constraint of corners, adjusts the speed of the splicing points by adopting a bidirectional scanning strategy, meets constraint conditions of the maximum speed, the maximum acceleration, the maximum scram, the length of the line segment, the starting point, the end point speed and the like, and ensures the accessibility of the two ends of the path line segment; finally, a speed curve is generated by combining the bidirectional scanning with an S-shaped acceleration and deceleration control algorithm, so that smooth speed track generation is realized, and a specific flow chart of the algorithm is shown in fig. 2.
Further, referring to FIG. 3, maximum velocity v max In the constraint ofThe calculation formula under the condition is as follows:
wherein epsilon is the maximum contour error allowed in the operation process, theta is the complementary angle value of epsilon, alpha AB Represents the angle between the vector AB and the positive direction of the x-axis, beta BC The included angle between the vector BC and the positive direction of the x-axis is AB is the vector of the current motion path of the vehicle, BC is the vector of the motion path after the vehicle turns, B point is an inflection point, a X-max Maximum acceleration allowed in the x-axis direction, a Y-max Is the maximum acceleration allowed in the y-axis direction, cos beta BC Expressed as beta BC Cosine value of cos alpha AB Denoted by alpha AB Cosine value of sin alpha AB Denoted by alpha AB Sine value of sin beta BC Expressed as beta BC The sine value of (i) indicates the absolute value of the internal data, and T indicates the period.
Further, the bidirectional scanning comprises reverse scanning and forward scanning, and the specific process of the reverse scanning is as follows:
1) Let i=n and b v N+1 =0, after which step 2) is performed;
2) Executing 3) if i=1, otherwise executing 2);
from the end of the path to the beginning, the end speed of each track is adjusted by applying the constraint that the S-shaped jerk curve is bounded, and then the tangential S-shaped jerk bounded cubic equation can be obtained as follows:
wherein J is t For tangential jerk, s i Solving the i-1 th track end point speed under the bounded constraint of the tangential S-shaped jerk curve by using the running time calculated by taking the i-th track start time as the zero pointIf it isIt means that a tangential jerk bounded constraint is fulfilled if +.>Does not satisfy the acceleration condition, at this time +.>And b v i+1 the satisfied relation of (2) is as follows:
the i-1 th segment end point speed determined by the backward scanning under the constraintSolving, and then executing 2b;
2b. Final end speed of the i-1 th segment of the reverse scan b v i Finally, it is determined by the following formula:
wherein v is max V (i) for maximum speed of the whole track max Maximum speed for each segment of track after segmentation, followed by 2c;
subtracting one from the value of i, and re-executing the step 2), wherein the step 3) is not executed until the judging condition is met;
3) All of b v i (i=1, 2, …, N) is stored into the buffer.
The backward scan is performed from the last track to the first track, the end speed of each track is set to b v and setting the final track end point speed b v N+1 =0, where N represents the number of paths after segmentation, b v i+1 for the ith track end point speed determined by the (i+1) th track by reverse scan, A t Is tangential acceleration.
Further, forward scanning is used to determine the initial velocity obtained for the ith trajectory under tangential acceleration and acceleration constraintsThe specific process is as follows:
(1) setting i=1 and the start speed v of the first track 1 =0, after which step (2) is performed;
(2) executing step (3) if i=n, otherwise executing step (2);
2d、the solution can be performed by the following relation, which is as follows:
solving the initial velocity of the ith track under tangential acceleration and acceleration constraint by the above methodIf it isIt means that a tangential jerk bounded constraint is met if +.> Then the solution is continued by the following relation, and after the solution is completed, 2e is executed, where the specific relation is as follows:
2e, i+1th segment path start point velocity v i+1 Determined by the following formula:
wherein the method comprises the steps ofRepresenting the initial velocity obtained by the (i+1) th track under tangential acceleration and acceleration constraint;
2f, adding one to the value of i, re-executing the step (2), and executing the step (3) until the judging condition is met
(3) Will all v i+1 (i=1, 2, …, N) is stored in the buffer, and then a speed profile of the AGV vehicle is generated from the data obtained from the two scans.
Thirdly, adjusting the pre-aiming distance according to different speeds of the vehicle by setting a fuzzy controller for the passing speed of the AGV, the speed after prospective planning and the transverse deviation between the current speed and the expected track;
setting a speed V and a transverse distance P as input quantities through a fuzzy controller, and setting a pretightening distance L as output quantity, wherein the argument of the speed V is [0,2 ]]M/S, discrete domains are { -5, -4, -3, -2, -1,0,1,2,3,4,5}, quantization factor is set to 2, speed domains are divided into 5 fuzzy subsets, fuzzy language values { very slow, moderate, fast } = [ VS, S, M, F, VF }, and the domains of lateral distance P are [0,2 ]]m, the discrete argument is { -5, -4, -3, -2, -1,0,1,2,3,4,5}, the quantization factor is set to 1, the lateral distance P argument is divided into 5 fuzzy subsets, the fuzzy language value is { small, medium, large } = { VS P ,S P ,M p Dividing the argument of the pretightening distance L into 5 fuzzy subsets, wherein the fuzzy language value is { very near, moderate, far } = { VN, N, M } L ,F L ,CF L Specific AGV vehicle control surfaces as shown in FIG. 4, fuzzy controllerPlease refer to fig. 9 below:
step four, calculating the front wheel turning angle of the AGV according to the pre-aiming distance calculated by the parameters and the figure 6, wherein the calculation formula of the front wheel turning angle is as follows:
wherein L is the pre-aiming distance, delta is the front wheel rotation angle, arctan () is the arctangent trigonometric function, alpha is the included angle between the car body and Ld, and the rotation angle of the movement turning of the trolley can be calculated through the formula.
As shown in fig. 5 and 6, fig. 6 (a) is a fuzzy dynamic pretightening distance proposed by the present invention, and fig. 6 (b) is a fixed pretightening distance, and it can be found that when the dynamic pretightening distance method is adopted, the maximum error is only 8cm during turning, and the path tracking is more stable. This result shows that the control method presented herein is significantly better than the effect achieved with the conventional pretarget control method.
Referring to fig. 7, it can be found that the velocity, the acceleration and the jerk are bounded, and the velocity curve is smoothly transited, so that the adaptive adjustment of the velocity and the acceleration can be realized under the path constraint, and the bounded constraint is performed on the acceleration and the jerk, so as to reduce the impact in the motion process.
In fig. 8, a visual mark is added every 10 seconds, and the start and end visual marks of the arc segment path are found to be denser, which indicates that the vehicle is subjected to the deceleration process, so as to ensure stability and safety in turning.
The method utilizes the fuzzy controller to dynamically adjust the pretightening distance, and solves the problem of poor path tracking effect caused by fixed pretightening distance. The speed and the acceleration are adaptively adjusted based on the speed look-ahead planning control, smooth and continuous speed and acceleration are ensured, and jerk is smooth and bounded, so that the track tracking task under the constraint of maximum speed and maximum acceleration is realized.
Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made therein without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.
Claims (10)
1. The AGV motion control method based on the pure tracking model is characterized by comprising the following steps of: the method comprises the following steps:
calculating a plurality of path points through linear interpolation and an arc transition joint model through an algorithm formula, and generating a smooth expected motion track;
step two, performing speed prospective planning on the motion trail, and setting the prospective number N p And pre-analyzing the subsequent path length and speed constraint values versus maximum speed v at the path corners max Calculating;
thirdly, adjusting the pre-aiming distance according to different speeds of the vehicle by setting a fuzzy controller for the passing speed of the AGV, the speed after prospective planning and the transverse deviation between the current speed and the expected track;
and step four, calculating the front wheel steering angle of the AGV according to the calculated pre-aiming distance.
2. The method for controlling the motion of an AGV based on a pure tracking model according to claim 1 wherein: the arc transition connection model is opposite to the current point P i-1 Inflection point P i End point P i+1 And the line segment in which it is locatedAnd->Length S of (2) i And S is i+1 Determining, calculating radius and transition distance of transition arc, and P i-1 、P i And P i+1 The three points are not collinear.
3. A kind of according to claim 2The AGV motion control method based on the pure tracking model is characterized by comprising the following steps of: the line segmentAnd->The calculation formula of the included angle alpha is as follows:
wherein,representing line segment->Vector of->Representation->Is |P i-1 P i I is vectorIs |P i P i+1 The I is vector +.>Is used for predicting the transition arc radius R for the inverse trigonometric function, and the calculation formula of arcos () is as follows:
wherein delta max To maximum transportThe row track error value, sin () is a sine function, and the calculation formula of the predicted transition distance L at this time is as follows:
wherein,the tangent value of half the expressed included angle alpha limits the transition distance due to the length of the line segment, and the actual transition distance l' is calculated as follows:
wherein min represents the pairAnd->The minimum value of (2) is taken out, r' represents the actual transition arc radius,/and r->Representing a cotangent value for half of the included angle alpha;
the radius r of the transition arc and the transition distance l are obtained in a comprehensive way as follows:
wherein min (R', R) represents the minimum value for both calculation modes.
4. The method for controlling the motion of an AGV based on a pure tracking model according to claim 1 wherein: the speed look-ahead planning is based on the number of look-ahead segments N each time p A new track is gradually added to carry out speed planning and parameter adjustment, and the specific process is as follows:
s1, N before acquisition p The length of the segment track and the maximum speed vmax;
s2, adjusting the starting point and the end point speed of each track by using a bidirectional scanning technology according to constraint conditions and maximum speed limitation;
s3, calculating time and maximum speed of each section of the S-shaped speed curve required by the current track section;
s4, adjusting step number parameters of the S-shaped speed curve to meet constraint conditions;
s5, combining an S-shaped addition and subtraction control algorithm to obtain a speed curve of the front track.
5. The method for controlling the motion of an AGV based on a pure tracking model according to claim 4 wherein: and the speed look-ahead planning regenerates a multistage smooth look-ahead track by adopting a smooth splicing strategy of linear arcs on input path points, wherein constraint conditions in the step S2 of bidirectional scanning comprise maximum speed, maximum acceleration, maximum scram, line segment length, starting point and end point speed.
6. The method for controlling the motion of an AGV based on a pure tracking model according to claim 5 wherein: the maximum speed v max The calculation formula under the constraint condition is as follows:
wherein epsilon is the maximum contour error allowed in the operation process, theta is the complementary angle value of epsilon, alpha AB Represents the angle between the vector AB and the positive direction of the x-axis, beta BC To the direction ofThe included angle between the quantity BC and the positive direction of the x-axis is AB, which is the vector of the current motion path of the vehicle, BC is the vector of the motion path of the vehicle after the corner, B point is the inflection point, a X-max Maximum acceleration allowed in the x-axis direction, a Y-max For the maximum acceleration allowed in the y-axis direction, min represents the minimum value of the internal data, cos β BC Expressed as beta BC Cosine value of cos alpha AB Denoted by alpha AB Cosine value of sin alpha AB Denoted by alpha AB Sine value of sin beta BC Expressed as beta BC The sine value of (i) indicates the absolute value of the internal data, and T indicates the period.
7. The method of controlling the motion of an AGV based on a pure tracking model of claim 6, wherein: the bidirectional scanning in the step S2 comprises reverse scanning and forward scanning, wherein the reverse scanning is performed from the last track to the first track, and the end speed of the track of each track is set to be b v and setting the final track end point speed b v N+1 =0, where N represents the number of tracks after segmentation, b v i+1 for the ith track end point speed determined by the (i+1) th track through the reverse scan, setting tangential acceleration A t The specific process of the reverse scan is as follows:
1) Let i=n and b v N+1 =0, after which step 2) is performed;
2) Executing 3) if i=1, otherwise executing 2);
from the end of the path to the beginning, the end speed of each track is adjusted by applying the constraint that the S-shaped jerk curve is bounded, and then the tangential S-shaped jerk bounded cubic equation can be obtained as follows:
wherein J is t For tangential jerk, s i Run-time for starting calculation for ith track start time as zero pointSolving the i-1 th track end point speed under the bounded constraint of the tangential S-shaped jerk curve by the methodIf it isIt means that a tangential jerk bounded constraint is fulfilled if +.>Does not satisfy the acceleration condition, at this time +.>And b v i+1 the satisfied relation of (2) is as follows:
the i-1 th segment end point speed determined by the backward scanning under the constraintSolving, and then executing 2b;
2b. Final end speed of the i-1 th segment of the reverse scan b v i Finally, it is determined by the following formula:
wherein v is max V (i) for maximum speed of the whole track max Maximum speed for each segment of track after segmentation, followed by 2c;
subtracting one from the value of i, and re-executing the step 2), wherein the step 3) is not executed until the judging condition is met;
3) All of b v i (i＝1,2, …, N) is stored into the buffer.
8. The method for controlling the motion of an AGV based on a pure tracking model according to claim 7 wherein: the forward scan is used to determine the initial velocity of the ith track obtained under tangential acceleration and acceleration constraintsThe specific flow of forward scanning is as follows:
(1) setting i=1 and the start speed v of the first track 1 =0, after which step (2) is performed;
(2) executing step (3) if i=n, otherwise executing step (2);
2d、the solution can be performed by the following relation, which is as follows:
solving the initial velocity of the ith track under tangential acceleration and acceleration constraint by the above methodIf it isIt means that a tangential jerk bounded constraint is met if +.> Then the solution is continued by the following relation, and after the solution is completed, 2e is executed, where the specific relation is as follows:
2e, i+1th segment path start point velocity v i+1 Determined by the following formula:
wherein the method comprises the steps ofRepresenting the initial velocity obtained by the (i+1) th track under tangential acceleration and acceleration constraint;
2f, adding one to the value of i, re-executing the step (2), and executing the step (3) until the judging condition is met
(3) Will all v i+1 (i=1, 2, …, N) to the buffer;
and then generating a speed curve of the AGV according to the data obtained by the two scans.
9. The method for controlling the motion of an AGV based on a pure tracking model according to claim 1 wherein: setting the speed V and the transverse distance P as input values and the pretightening distance L as output values by the fuzzy controller in the step three, wherein the argument of the speed V is [0,2 ]]M/S, discrete domains { -5, -4, -3, -2, -1,0,1,2,3,4,5}, quantization factor set to 2, dividing the velocity domain into 5 fuzzy subsets, fuzzy language value { very slow, moderate, fast } = { VS, S, M, F, VF }, lateral distance P domain of [0,2 }]m, the discrete argument is { -5, -4, -3, -2, -1,0,1,2,3,4,5}, the quantization factor is set to 1, the lateral distance P argument is divided into 5 fuzzy subsets, the fuzzy language value is { small, medium, large } = { VS P ,S P ,M p Dividing the argument of the pretightening distance L into 5 fuzzy subsets, wherein the fuzzy language value is { very near, moderate, far } = { VN, N },M L ,F L ,VF L }。
10. The method of controlling the motion of an AGV based on a pure tracking model according to claim 9, wherein: the calculation formula of the front wheel rotation angle is as follows:
wherein L is the pretightening distance, delta is the front wheel rotation angle, arctan () is the arctangent trigonometric function, and alpha is the included angle between the vehicle body and Ld.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311444607.4A CN117369269A (en) | 2023-11-02 | 2023-11-02 | AGV motion control method based on pure tracking model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311444607.4A CN117369269A (en) | 2023-11-02 | 2023-11-02 | AGV motion control method based on pure tracking model |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117369269A true CN117369269A (en) | 2024-01-09 |
Family
ID=89400227
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311444607.4A Pending CN117369269A (en) | 2023-11-02 | 2023-11-02 | AGV motion control method based on pure tracking model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117369269A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117555291A (en) * | 2024-01-11 | 2024-02-13 | 佛山德玛特智能装备科技有限公司 | Interpolation method, interpolation device, interpolator and medium |
-
2023
- 2023-11-02 CN CN202311444607.4A patent/CN117369269A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117555291A (en) * | 2024-01-11 | 2024-02-13 | 佛山德玛特智能装备科技有限公司 | Interpolation method, interpolation device, interpolator and medium |
| CN117555291B (en) * | 2024-01-11 | 2024-03-22 | 佛山德玛特智能装备科技有限公司 | Interpolation method, interpolation device, interpolator and medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112519882B (en) | Vehicle reference track tracking method and system | |
| CN117369269A (en) | AGV motion control method based on pure tracking model | |
| CN106372758B (en) | Path following method and device of auxiliary parking system | |
| US8751089B2 (en) | Trajectory tracking control system and method for mobile unit | |
| CN112622903A (en) | Longitudinal and transverse control method for autonomous vehicle in vehicle following driving environment | |
| CN110588633A (en) | Path tracking and stability control method for intelligent automobile under limit working condition | |
| CN111123701B (en) | Automatic driving path tracking anti-interference control method based on pipeline prediction model | |
| CN112622895B (en) | Prediction control method applied to trajectory control of automatic driving | |
| JP2007097378A (en) | Automatic train operating device | |
| CN112947469A (en) | Automobile track-changing track planning and dynamic track tracking control method | |
| CN113183957A (en) | Vehicle control method, device and equipment and automatic driving vehicle | |
| JP2002215239A (en) | Method and device for controlling travel of vehicle system | |
| Kim et al. | Robust Nonlinear Torque Control Using Steering Wheel Torque Model for Electric Power Steering System | |
| JP6509301B1 (en) | Driving support device and driving support method | |
| CN116834754A (en) | Transverse and longitudinal cooperative control method for self-adaptive speed regulation of automatic driving vehicle | |
| CN113815602B (en) | Vehicle transverse self-adaptive control method | |
| CN115675459A (en) | Intelligent vehicle and control method, device and storage medium for path tracking of intelligent vehicle | |
| CN114940163A (en) | Transverse motion control method of rear wheel steering vehicle, rear wheel steering vehicle and electronic system | |
| Xiao et al. | Smooth trajectory tracking using longitudinal distance constraint for a 4WS4WD unmanned ground vehicle | |
| CN113608529A (en) | Wheel type mobile platform motion track preview tracking control system and method | |
| CN113311834B (en) | Automatic driving track tracking method, system and storage medium | |
| CN114637296B (en) | Tracking control system and control method for PRT vehicle | |
| Al-Saedi | A Modified Block Backstepping Controller for Autonomous Vehicle Lane Changing. | |
| CN116909154B (en) | Robot track optimization method for feedforward compensation through feature table lookup | |
| CN116853293A (en) | Discontinuous curvature track tracking control method based on self-adaptive pre-aiming path |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |