AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation

Abstract

The present application belongs to the technical field of AGV tracking control, and particularly relates to a kind of AGV trajectory tracking control method based on two-way wheel speed micro-excitation, steps include: obtaining the running state of AGV, reference path curvature and basic control information, determine the micro-excitation test interval;Establish the nominal discrete response model of the heading angle increment and yaw rate increment caused by wheel speed micro-excitation, generate positive and negative direction wheel speed micro-excitation sequence of zero terminal;Extract the incremental yaw response caused by wheel speed micro-excitation;Build positive direction response interval and negative direction response interval;Establish the direction related prediction model and recursively obtain the predicted reachable range of each prediction point;Construct robust model predictive control objective function, solve the optimal differential control amount, and combine the longitudinal velocity command to generate left and right drive wheel target linear velocity, output to AGV to execute trajectory tracking.The present application can significantly improve the trajectory tracking accuracy of complex curved road conditions without interfering with normal operation.

Description

Technical Field

[0001] This invention belongs to the field of AGV tracking and control technology, specifically relating to an AGV trajectory tracking and control method based on bidirectional wheel speed micro-excitation. Background Technology

[0002] Automated Guided Vehicles (AGVs) are widely used in workshop material distribution, process transfer, and fixed-point material feeding. To ensure the stable operation of AGVs along a preset path, the trajectory tracking control system typically calculates the target speeds of the left and right drive wheels based on the AGV's current position, heading angle, yaw rate, and reference path information to reduce lateral and heading tracking errors. Existing AGV trajectory tracking methods mainly include pure tracking control, feedback control based on lateral and heading errors, kinematic control, and model predictive control (MPC). Among these, model predictive control can use the AGV's motion model to predict the operating state at multiple sampling points in the future, and comprehensively considers trajectory tracking errors, control magnitude, and drive wheel speed constraints. Therefore, it is more suitable for trajectory tracking control under complex path conditions such as continuous curves and multiple steering changes, and is gradually becoming the mainstream technology in this field.

[0003] However, existing model predictive control methods rely on predictive models whose steering response parameters are mostly derived from fixed nominal values ​​or offline experimental identification, assuming that AGVs have the same or approximately the same steering response when turning in the positive and negative directions. In reality, the steering response of an AGV is affected by various factors such as loading status, the state of the left and right drive wheels, wheel speed closed-loop characteristics, and wheel-ground contact status. On the one hand, the yaw response produced by the same wheel speed difference often differs significantly between positive and negative turns; on the other hand, this response difference also changes dynamically with load variations and operating conditions, causing the fixed parameters obtained from offline identification to gradually lose their applicability in actual operation. Furthermore, some robust control methods only use a uniform range of uncertain parameters to describe the steering response, without distinguishing between steering directions. This results in positive and negative directional responses being mixed in the same uncertainty interval, leading to either an overly conservative prediction range or an inability to cover actual response changes in a specific steering direction.

[0004] To obtain accurate steering response characteristics online, existing online identification methods typically require continuous or unidirectional excitation. However, both of these methods have significant limitations. Continuous excitation causes residual deviations in the AGV's heading angle and yaw rate, interfering with normal trajectory tracking. Unidirectional excitation, on the other hand, only acquires response information for a single steering direction, failing to characterize the inconsistent responses in the positive and negative directions. In summary, the core deficiency of existing technologies lies in their inability to acquire steering responses in both positive and negative directions without significantly interfering with the AGV's normal operation, and their inability to effectively integrate the obtained directional response differences into the trajectory prediction and control variable solution process. How to achieve online identification of bidirectional steering responses without affecting normal operation, and how to select matching response parameters for robust predictive control based on the steering direction of the predicted path, are urgent problems to be solved to improve the tracking accuracy of AGVs on complex paths. Summary of the Invention

[0005] In view of the shortcomings of the prior art, the purpose of this invention is to provide an AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation, which can significantly improve the trajectory tracking accuracy in complex curved road conditions without interfering with normal operation.

[0006] To achieve the above objectives, this invention provides an AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation, comprising the following steps: S1. Obtain the AGV's operating status, reference path curvature, and basic control information. The operating status includes longitudinal speed, yaw rate, lateral tracking error, and heading angle error. The basic control information includes longitudinal speed commands and basic differential control quantities. When at least one of the positive and negative directions has not completed the wheel speed micro-excitation test, or when a change in the AGV's loading status is detected, determine the micro-excitation test interval based on the current operating status and the safe road section ahead. S2. Establish a nominal discrete response model for the heading angle increment and yaw rate increment caused by wheel speed micro-excitation, generate a positive direction wheel speed micro-excitation sequence where both the terminal heading angle increment and the terminal yaw rate increment are zero, and take their opposite to obtain the negative direction wheel speed micro-excitation sequence. S3. Within the micro-excitation test range, the wheel speed micro-excitation sequence in the corresponding direction is superimposed on the basic differential control quantity and applied to the AGV. The incremental yaw response caused by the wheel speed micro-excitation is extracted by using a reference model without micro-excitation that only receives the basic differential control quantity. S4. Based on the incremental yaw response in the positive and negative directions, obtain the positive steering response coefficient and the negative steering response coefficient and their dispersion, and construct the positive response interval and the negative response interval accordingly. S5. Determine the corresponding turning direction based on the reference path curvature of each prediction point in the prediction time domain, select the corresponding direction response interval for each prediction point, establish a direction-related prediction model, and recursively obtain the predicted reachability range of each prediction point. S6. Construct a robust model predictive control objective function based on the predicted reachable range, solve for the optimal differential control quantity, and generate the target linear velocity of the left and right drive wheels in combination with the longitudinal speed command, and output it to the AGV to perform trajectory tracking; return to S5 at the next sampling time to perform rolling execution until trajectory tracking control is completed.

[0007] As a preferred embodiment of the present invention, in S1, the longitudinal velocity of the AGV at the k-th sampling time is obtained. yaw rate The lateral tracking error is determined based on the relationship between the AGV's current position and the reference path. Heading angle error and reference path curvature ; Obtain the basic target linear velocity of the left drive wheel output by the normal trajectory tracking controller at the kth sampling time. and the base target linear velocity of the right drive wheel and determine the longitudinal speed command. and basic differential control quantity : (1); (2); Regulation The corresponding preset yaw direction is positive, i.e., the positive direction. The corresponding preset yaw direction is negative, i.e., the negative direction.

[0008] As a preferred embodiment of the present invention, in S1, the wheel speed micro-excitation test is a test process in which a pre-constructed wheel speed micro-excitation sequence is superimposed on the basic differential control quantity and applied to the AGV on the basis of normal trajectory tracking control, so as to actively excite and observe the AGV steering response. When the wheel speed micro-excitation test has not been completed in at least one of the positive and negative directions, the response test request for the direction that has not been completed is triggered; when a change in the AGV loading status is detected, the response test requests for both the positive and negative directions are retried. After the test request is triggered, it continuously determines whether the current running status of the AGV and the continuous safe road segment ahead both satisfy equation (3): (3); In the formula, , , and These are the path curvature threshold, lateral error threshold, heading angle error threshold, and safe distance threshold, respectively. The minimum lateral distance between the outer contour of the AGV vehicle body and the boundary of the allowed driving area at the k-th sampling time; The remaining length of the road segment that continuously meets the path curvature threshold requirement and has a preset safe passage margin from the current position of the AGV along the reference path; M represents the sampling period; M represents the total number of sampling points in the wheel speed micro-excitation sequence. This represents the maximum number of recovery sampling points after the test ends. When equation (3) is satisfied, the continuous road segment starting from the kth sampling time and whose length can cover the wheel speed micro-excitation sequence of the direction to be executed once and its post-test recovery process is determined as the micro-excitation test interval; when equation (3) is not satisfied, the direction response test request is maintained and the subsequent running road segment is continued to be detected; after the wheel speed micro-excitation test of one direction is completed, if the wheel speed micro-excitation test of another direction has not been completed, the micro-excitation test interval corresponding to that direction is re-determined according to equation (3).

[0009] As a preferred embodiment of the present invention, in S2, at the j-th sampling point of the wheel speed micro-excitation sequence, the heading angle increment caused by the wheel speed micro-excitation is... and yaw rate increment Composition of micro-excitation incremental state The superscript T denotes transpose. A nominal discrete response model for the incremental state of the micro-excitation is established: (4); In the formula, This represents the micro-excitation increment state at the (j+1)th sampling point of the wheel speed micro-excitation sequence; The nominal state matrix represents the incremental states. The sampling period is The yaw rate maintenance coefficient; For the nominal input matrix, The nominal discrete steering response coefficient; The wheel speed micro-excitation is superimposed on the differential control quantity at the j-th sampling point of the wheel speed micro-excitation sequence. Define the positive direction wheel speed micro-excitation sequence ,in Let M represent the positive direction wheel speed micro-excitation at the j-th sampling point, where M is the total number of sampling points in the wheel speed micro-excitation sequence; and let M be the incremental state at the end of the positive direction wheel speed micro-excitation sequence, given that the initial value of the micro-excitation increment state is zero. for: (5); In the formula, express M-1-j powers; In the first M-2 sampling points, the positive direction main test segment and the smooth recovery segment are set sequentially, and the last two sampling points are used as the terminal compensation amount, that is, the positive direction wheel speed micro-excitation at sampling points j=M-2 and j=M-1. , Defined as two terminal compensation quantities; the wheel speed micro-excitation in the positive direction main test segment is set along the preset positive steering direction to generate the yaw rate increment that can characterize the positive steering response; the wheel speed micro-excitation in the smooth recovery segment changes continuously to reduce the heading angle increment and yaw rate increment formed when the positive direction main test segment propagates to the end of the positive direction wheel speed micro-excitation sequence. With the compensation values ​​of the last two terminals temporarily set to zero, the predicted incremental state q formed when the wheel speed micro-excitation of the first M-2 sampling points propagates to the end of the positive direction wheel speed micro-excitation sequence is denoted as: (6); In the formula, , These are the predicted heading angle increment and predicted yaw rate increment formed when the wheel speed micro-excitation propagates from the first M-2 sampling points to the end of the positive direction wheel speed micro-excitation sequence; According to the nominal discrete response model, the compensation quantities for the two terminals are determined sequentially as follows: (7); (8); Define the negative direction wheel speed micro-excitation sequence ,in This represents the negative direction wheel speed micro-excitation at the j-th sampling point; the negative direction wheel speed micro-excitation sequence is set as the opposite of the positive direction wheel speed micro-excitation sequence, i.e. .

[0010] As a preferred embodiment of the present invention, when and At that time, the two terminal compensation quantities in equations (7) and (8) can be determined sequentially, thereby ensuring that the positive direction wheel speed micro-excitation sequence satisfies the nominal discrete response model when calculated. That is, the predicted heading angle increment and the predicted yaw rate increment are both zero at the end of the positive direction wheel speed micro-excitation sequence; a finite length wheel speed micro-excitation sequence with the above characteristics is defined as a zero terminal wheel speed micro-excitation sequence. When the amplitude of the wheel speed micro-excitation at any sampling point in the constructed positive direction wheel speed micro-excitation sequence is... When the preset upper limit of the micro-excitation amplitude is exceeded, the entire positive direction wheel speed micro-excitation sequence is scaled according to the same scaling ratio so that each sampling point after scaling meets the micro-excitation amplitude limit. Since the nominal discrete response model is a linear model, the entire positive direction wheel speed micro-excitation sequence still meets the zero terminal condition after being scaled according to the same ratio. The negative direction wheel speed micro-excitation sequence adopts the same amplitude limit and scaling processing as the positive direction wheel speed micro-excitation sequence.

[0011] As a preferred embodiment of the present invention, the process of extracting the incremental yaw response caused by the wheel speed micro-excitation in S3 is as follows: set up The starting sampling time for the micro-excitation test of steering wheel speed is The normal trajectory tracking controller in the first The basic differential control quantity determined at each sampling time is: Then superimposed Differential control quantity after micro-excitation of steering wheel speed for: (9); In the formula, ; express or ,Right now The wheel speed micro-excitation at the j-th sampling point in the directional wheel speed micro-excitation sequence; P and N represent the positive and negative directions, respectively; Based on the differential control value, the target linear velocity of the left drive wheel is set to... Set the target linear velocity of the right drive wheel to ,in For the first Longitudinal velocity command at each sampling time; While applying wheel speed micro-excitation, establish a reference model without micro-excitation that is synchronously updated with the actual AGV but only receives basic differential speed control values ​​and not wheel speed micro-excitations: (10); (11); In the formula, , The reference model without micro-excitation is in The yaw rate and heading angle at the j-th sampling point were measured using a micro-excitation test of the steering wheel speed. , These represent the yaw rate and heading angle at the (j+1)th sampling point in the wheel speed micro-excitation sequence for the reference model without micro-excitation. exist At the start of the micro-excitation test of the steering wheel speed, the initial yaw rate and initial heading angle of the reference model without micro-excitation are set to the yaw rate and heading angle measured by the AGV at the start of the test, so that the actual AGV and the reference model without micro-excitation have the same initial state. exist At the j-th sampling point in the micro-excitation sequence of wheel speed, the difference between the actual yaw rate of the AGV and the yaw rate of the reference model without micro-excitation is recorded as the yaw rate increment. The difference between the actual heading angle of the AGV and the heading angle of the reference model without micro-excitation is denoted as the heading angle increment. The difference between the test differential control value and the basic differential control value is recorded as the test differential increment. ;in, This refers to the incremental yaw response caused by the wheel speed micro-excitation; based on the superposition relationship of differential control quantities, when no command limiting occurs... ; Apply positive and negative wheel speed micro-excitations sequentially according to the preset test sequence.

[0012] As a preferred embodiment of the present invention, in step S4, the process of obtaining the positive steering response coefficient, the negative steering response coefficient, and their dispersion is as follows: according to The incremental yaw response in the direction is obtained as follows: (12); In the formula, for The yaw rate increment at the (j+1)th sampling point is measured using a micro-excitation test of the steering wheel speed. for Discrete steering response coefficients in direction; The yaw response residuals are caused by unmodeled factors and measurement noise. from In the main test segment of the direction, select valid sampling points with complete adjacent sampling data, and form a set by numbering the corresponding sampling points. Determined according to the following formula Steering response coefficient : (13); In the formula, Let be the non-negative normalized sampling weight corresponding to the j-th valid sampling point; To prevent regularization parameters with a denominator of zero; The degree of dispersion of the directional response coefficients is determined based on the fitting residuals. : (14).

[0013] As a preferred embodiment of the present invention, in S4... Response range of direction for: (15); In the formula, and These are the lower and upper limits of the response range, respectively. and These are the physical lower limit and physical upper limit of the steering response coefficient, respectively; This is the interval width coefficient.

[0014] In a preferred embodiment of the present invention, in step S5, the reference path curvature of the i-th prediction point in the prediction time domain is obtained. k is the sampling index corresponding to the current sampling time, and k+i represents the sampling index corresponding to the i-th prediction point; the corresponding directional response interval is selected according to the direction of the curvature of the reference path: (16); In the formula, The directional response interval used for the i-th prediction point; , These are the positive and negative response intervals, respectively. Threshold for determining steering direction; , These are the lower and upper limits of the positive response range, respectively. , These are the lower and upper limits of the negative response range, respectively. The lateral tracking error at the k-th sampling time Heading angle error and yaw rate Composition of trajectory tracking state The longitudinal velocity of the i-th prediction point is denoted as... Establish a direction-related prediction model: (17); In the formula, The differential control quantity for the i-th prediction point is determined based on the known information at the current sampling time k. For the i-th prediction point, from Steering response coefficient with a value of [value missing] ; , These are the trajectory tracking states of the i-th and i+1-th predicted points, respectively, predicted based on the state information of the current sampling index k. For a given candidate differential control sequence, the current trajectory tracking state is taken as a set. The state range is predicted recursively according to the following formula: (18); In the formula, , , These correspond to the state matrix, input matrix, and path curvature term in equation (17), respectively; , These are the predictable reach ranges of the i-th and i+1-th prediction points, respectively, which are the sets of predicted states that can be generated for the same candidate differential control quantity within the directional response interval. for any predicted state in the set; co represents the minimal convex set formed by the predicted states in the set.

[0015] As a preferred embodiment of the present invention, in S6, a robust model predictive control objective function J is constructed by combining the predicted reachable range: (19); In the formula, To predict the differential control quantity sequence in the time domain; To predict the length of the time domain; This is the reference state for the i-th prediction point. and These are the dimensionless predicted state and dimensionless reference state obtained after dimensionless processing of the predicted state and reference state, respectively; Q is the dimensionless state error weight matrix. , These are the weights of the differential control quantity and its change; for The dimensionless differential control quantity obtained after dimensionless processing; Let be the change in differential control quantity at the i-th prediction point relative to the differential control quantity at the previous prediction point. for The dimensionless differential control quantity change obtained after dimensionless processing; Equation (19) is solved using an iterative optimization method to obtain the optimal differential control quantity sequence, and the first differential control quantity is taken as the optimal differential control quantity at the current sampling time. ; Based on the longitudinal velocity command at the current sampling moment Set the target linear velocity of the left drive wheel to Set the target linear velocity of the right drive wheel to And output to the left and right wheel speed controllers; At the next sampling moment, return to S5 and continue execution until trajectory tracking control is completed; when the AGV loading status is detected to change again, re-execute S1 to S4 to update the positive and negative response intervals.

[0016] The beneficial effects of this invention are: This invention is applicable to trajectory tracking control scenarios for AGVs under conditions of continuous curves, alternating positive and negative turns, and changes in loading status. Compared with trajectory tracking methods that use fixed nominal steering response parameters, this invention obtains the positive and negative steering response coefficients of the AGV separately and constructs mutually independent positive and negative response intervals, which can more accurately describe the actual response differences of the AGV under different steering directions.

[0017] This invention employs a zero-terminal wheel speed micro-excitation sequence for directional response testing. When the wheel speed micro-excitation sequence is calculated according to the nominal discrete response model, the terminal heading angle increment and terminal yaw rate increment at the end of the sequence are both zero. This ensures that no residual heading or yaw deviation occurs after the active test, thereby reducing the continuous impact of micro-excitation testing on the normal operating trajectory of the AGV while obtaining directional response information. Simultaneously, by establishing a micro-excitation-free reference model that only receives basic differential speed control quantities and extracting the incremental yaw response, it is possible to distinguish the response changes caused by wheel speed micro-excitation from the response generated by normal trajectory tracking control, thereby improving the pertinence and accuracy of steering response coefficient identification.

[0018] Based on the turning direction of the reference path curvature at each prediction point in the prediction time domain, this invention selects a corresponding positive or negative response interval for each prediction point, so that the direction-related prediction model adopts a response parameter range that matches the actual turning direction at each prediction point, avoiding prediction deviation or over-conservatism caused by unifying the differences between positive and negative response into a single parameter interval.

[0019] This invention recursively derives the predictable reachability range of each prediction point based on the directional response interval, and solves for the optimal differential control quantity in the robust model predictive control objective function according to the unfavorable response boundary within the predictable reachability range. This allows the control result to balance directional response uncertainty, trajectory tracking accuracy, and drive wheel control constraints. Through this method, the lateral tracking error and local error peaks of the AGV can be reduced in the peak region of curves, the steering direction switching region, and under conditions where there are significant differences in positive and negative directional responses.

[0020] This invention can re-execute bidirectional wheel speed micro-excitation tests and update the positive and negative response ranges when the loading state changes, without requiring long-term use of fixed offline identification parameters. This improves the adaptability of AGV trajectory tracking control to changes in vehicle operating conditions. Simulation results show that the method reduces the overall lateral tracking error and the tracking error on turning sections in both directions without significantly increasing the fluctuation of differential control, thus balancing trajectory tracking accuracy, control stability, and operational reliability. Attached Figure Description

[0021] Figure 1 This is a flowchart illustrating the principle of this invention; Figure 2 These are AGV trajectory tracking results diagrams using different trajectory tracking control methods; Figure 3 This is a graph showing the lateral tracking error curves for different trajectory tracking control methods. Detailed Implementation

[0022] The embodiments of the present invention will be further described below with reference to the accompanying drawings: Example 1: As Figure 1 As shown, an AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation includes the following steps: S1. Obtain the AGV's operating status, reference path curvature, and basic control information. The operating status includes longitudinal speed, yaw rate, lateral tracking error, and heading angle error. The basic control information includes longitudinal speed commands and basic differential control quantities. When at least one of the positive and negative directions has not completed the wheel speed micro-excitation test, or when a change in the AGV's loading status is detected, determine the micro-excitation test interval based on the current operating status and the safe road section ahead. S2. Establish a nominal discrete response model for the heading angle increment and yaw rate increment caused by wheel speed micro-excitation, generate a positive direction wheel speed micro-excitation sequence where both the terminal heading angle increment and the terminal yaw rate increment are zero (zero terminal), and take their opposite to obtain the negative direction wheel speed micro-excitation sequence. S3. Within the micro-excitation test range, the wheel speed micro-excitation sequence in the corresponding direction is superimposed on the basic differential control quantity and applied to the AGV. The incremental yaw response caused by the wheel speed micro-excitation is extracted by using a reference model without micro-excitation that only receives the basic differential control quantity. S4. Based on the incremental yaw response in the positive and negative directions, obtain the positive steering response coefficient and the negative steering response coefficient and their dispersion, and construct the positive response interval and the negative response interval accordingly. S5. Determine the corresponding turning direction based on the reference path curvature of each prediction point in the prediction time domain, select the corresponding direction response interval for each prediction point, establish a direction-related prediction model, and recursively obtain the predicted reachability range of each prediction point. S6. Construct a robust model predictive control objective function based on the predicted reachable range, solve for the optimal differential control quantity, and generate the target linear velocity of the left and right drive wheels in combination with the longitudinal speed command, and output it to the AGV to perform trajectory tracking; return to S5 at the next sampling time to perform rolling execution until trajectory tracking control is completed.

[0023] In this embodiment, the method applies zero-end wheel speed micro-excitations in both positive and negative directions within a road segment that meets safety testing conditions. The incremental yaw response caused by the micro-excitations is extracted, and the steering response coefficients in the positive and negative directions are identified and corresponding directional response intervals are constructed. Subsequently, the corresponding response interval is selected according to the steering direction of the predicted path, a direction-related prediction model and a predicted reachable range are established, a robust model predictive control objective function is constructed, the differential control quantity is solved and converted into the target speeds of the left and right drive wheels, thereby realizing AGV trajectory tracking control.

[0024] In S1, the longitudinal velocity of the AGV at the k-th sampling time is obtained. yaw rate The lateral tracking error is determined based on the relationship between the AGV's current position and the reference path. Heading angle error and reference path curvature Specifically: The system searches for the path point closest to the AGV's current position on the reference path as the matching point, and obtains the reference position, reference heading angle, and reference path curvature at the matching point. The reference path curvature is determined by the curvature of the arc defined by the matching point and its adjacent path points before and after it. Its sign is defined according to the turning direction and is consistent with the preset yaw direction. That is, a positive reference path curvature corresponds to a positive turning direction, and a negative one corresponds to a negative turning direction. The heading angle error is the difference between the AGV's current heading angle and the reference heading angle at the matching point, and the difference is normalized. The lateral tracking error is the projection of the position deviation vector between the AGV's current position and the reference position at the matching point onto the normal direction of the reference path at the matching point. It is defined that the left side along the travel direction of the reference path is positive and the right side is negative, and its absolute value is the lateral distance of the AGV deviating from the reference path.

[0025] Obtain the basic target linear velocity of the left drive wheel output by the normal trajectory tracking controller at the kth sampling time. and the base target linear velocity of the right drive wheel and determine the longitudinal speed command. and basic differential control quantity : (1); (2); Regulation The corresponding preset yaw direction is positive, i.e., the positive direction. The corresponding preset yaw direction is negative, i.e., the negative direction. and The units are all m / s.

[0026] The wheel speed micro-excitation test is a test process in which a pre-constructed wheel speed micro-excitation sequence is superimposed on the basic differential control quantity and applied to the AGV on the basis of normal trajectory tracking control, so as to actively excite and observe the steering response of the AGV. When the wheel speed micro-excitation test has not been completed in at least one of the positive and negative directions, the response test request for the direction that has not been completed is triggered; when a change in the AGV loading status is detected, the response test requests for the positive and negative directions are retried to reacquire the steering response information in both directions. After the test request is triggered, it continuously determines whether the current running status of the AGV and the continuous safe road segment ahead both satisfy equation (3): (3); In the formula, , , and These are the path curvature threshold, lateral error threshold, heading angle error threshold, and safety distance threshold, with the preferred values ​​being [values ​​to be filled in]. , , , ; The minimum lateral distance between the outer contour of the AGV vehicle body and the boundary of the allowed driving area at the k-th sampling time is calculated by transforming the outer contour of the vehicle body to the field coordinate system according to the current pose, obtaining the real-time position of each side (or vertex) of the outer contour, and calculating the shortest distance from each vertex (or side) of the outer contour to the boundary of the allowed driving area; the minimum value among all distances is the minimum lateral distance between the outer contour of the vehicle body and the boundary of the allowed driving area at the sampling time. The remaining length of the road segment that continuously meets the path curvature threshold requirement and has a preset safe passage margin while moving forward along the reference path from the current position of the AGV is defined as the length of the remaining road segment that continuously meets the absolute value of the reference path curvature requirement without exceeding a certain threshold while moving forward along the reference path from the current position of the AGV. Furthermore, the minimum distance between the expected outer contour of the AGV body, determined according to the AGV body's external dimensions, and the boundary of the permitted driving area shall not be less than the length of a continuous road segment of ms. The sampling period is 0.05s; M is the total number of sampling points in the wheel speed micro-excitation sequence, preferably 24. This represents the maximum number of recovery sampling points after the test ends. When equation (3) is satisfied, the continuous road segment starting from the kth sampling time and whose length can cover the wheel speed micro-excitation sequence of the direction to be executed once and its post-test recovery process is determined as the micro-excitation test interval; when equation (3) is not satisfied, the direction response test request is maintained and the subsequent running road segment is continued to be detected; after the wheel speed micro-excitation test of one direction is completed, if the wheel speed micro-excitation test of another direction has not been completed, the micro-excitation test interval corresponding to that direction is re-determined according to equation (3).

[0027] In S2, at the j-th sampling point of the wheel speed micro-excitation sequence, the heading angle increment caused by the wheel speed micro-excitation is... and yaw rate increment Composition of micro-excitation incremental state The superscript T denotes transpose. A nominal discrete response model for the incremental state of the micro-excitation is established: (4); In the formula, This represents the micro-excitation increment state at the (j+1)th sampling point of the wheel speed micro-excitation sequence; The nominal state matrix represents the incremental states. The sampling period is The dimensionless yaw state maintenance coefficient is preferably taken as 0.92; For the nominal input matrix, The nominal discrete steering response coefficient is expressed in meters (m). -1 The value is determined based on the center distance between the left and right wheels. When the center distance between the left and right wheels is 0.60m, 0.133m can be used. -1 ; The wheel speed micro-excitation is superimposed on the differential control quantity at the j-th sampling point of the wheel speed micro-excitation sequence, with units of m / s; Define the positive direction wheel speed micro-excitation sequence ,in Let M represent the positive direction wheel speed micro-excitation at the j-th sampling point, where M is the total number of sampling points in the wheel speed micro-excitation sequence; and let M be the incremental state at the end of the positive direction wheel speed micro-excitation sequence, given that the initial value of the micro-excitation increment state is zero. for: (5); In the formula, express M-1-j powers; In the first M-2 sampling points, the positive direction main test segment and the smooth recovery segment are set sequentially, and the last two sampling points are used as the terminal compensation amount, that is, the positive direction wheel speed micro-excitation at sampling points j=M-2 and j=M-1. , Defined as two terminal compensation quantities; the wheel speed micro-excitation in the positive direction main test segment is set along the preset positive steering direction to generate the yaw rate increment that can characterize the positive steering response; the wheel speed micro-excitation in the smooth recovery segment changes continuously to reduce the heading angle increment and yaw rate increment formed when the positive direction main test segment propagates to the end of the positive direction wheel speed micro-excitation sequence. With the compensation values ​​of the last two terminals temporarily set to zero, the predicted incremental state q formed when the wheel speed micro-excitation of the first M-2 sampling points propagates to the end of the positive direction wheel speed micro-excitation sequence is denoted as: (6); In the formula, , These are the predicted heading angle increment and predicted yaw rate increment formed when the wheel speed micro-excitation propagates from the first M-2 sampling points to the end of the positive direction wheel speed micro-excitation sequence, respectively, with units of rad and rad / s; According to the nominal discrete response model, the compensation quantities for the two terminals are determined sequentially as follows: (7); (8); Define the negative direction wheel speed micro-excitation sequence ,in This represents the negative direction wheel speed micro-excitation at the j-th sampling point; the negative direction wheel speed micro-excitation sequence is set as the opposite of the positive direction wheel speed micro-excitation sequence, i.e. .

[0028] when and At that time, the two terminal compensation quantities in equations (7) and (8) can be determined sequentially, thereby ensuring that the positive direction wheel speed micro-excitation sequence satisfies the nominal discrete response model when calculated. That is, the predicted heading angle increment and the predicted yaw rate increment are both zero at the end of the positive direction wheel speed micro-excitation sequence; a finite length wheel speed micro-excitation sequence with the above characteristics is defined as a zero terminal wheel speed micro-excitation sequence. When the amplitude of the wheel speed micro-excitation at any sampling point in the constructed positive direction wheel speed micro-excitation sequence is... When the preset upper limit of the micro-excitation amplitude is exceeded (preferably 0.04 m / s), the entire positive direction wheel speed micro-excitation sequence is scaled by the same scaling ratio so that each sampling point after scaling meets the micro-excitation amplitude limit. Since the nominal discrete response model is a linear model, the entire positive direction wheel speed micro-excitation sequence still meets the zero-terminal condition after being scaled by the same ratio. The negative direction wheel speed micro-excitation sequence adopts the same amplitude limit and scaling processing as the positive direction wheel speed micro-excitation sequence. The upper limit of the micro-excitation amplitude is determined based on the remaining adjustment range of the basic differential control quantity, so that after the wheel speed micro-excitation is superimposed on the basic differential control quantity, it does not exceed the target linear speed limit of the left and right drive wheels, the amplitude limit of the differential control quantity, and the variation limit of the differential control quantity.

[0029] The target linear velocity limit for the left and right drive wheels refers to the fact that the target linear velocities allocated to the left and right wheels after superimposed micro-excitation (the right wheel is the sum of half of the longitudinal speed command and the differential control quantity, and the left wheel is the difference of half of the two) cannot exceed the speed range allowed by the wheel speed actuator. That is, the absolute value of the target linear velocity of the left and right wheels must not exceed the maximum allowable linear velocity of the drive wheels. The differential control quantity amplitude limit refers to the fact that the absolute value of the superimposed differential control quantity (the sum of the basic differential control quantity and the wheel speed micro-excitation) must not exceed the preset upper limit of the differential control quantity (such as 0.30m / s), to prevent the AGV from turning violently or slipping due to excessive speed difference between the left and right wheels. The differential control quantity change limit refers to the fact that the absolute value of the difference between the differential control quantities at two adjacent sampling times must not exceed the preset upper limit of the differential control quantity change (such as 0.04m / s), that is, to limit the abrupt change amplitude of the differential command, to ensure that the wheel speed command is smooth and the drive system can track and execute it. Therefore, at each sampling point, based on the target linear velocity limit of the left and right drive wheels, the amplitude limit of the differential control quantity, and the variation limit of the differential control quantity, the maximum absolute value of the wheel speed micro-excitation allowed to be superimposed under the current basic differential control quantity is determined, and it is used as the upper limit of the wheel speed micro-excitation amplitude at that sampling point, so that the superimposed wheel speed micro-excitation simultaneously satisfies the above three constraints.

[0030] In S3, the process of extracting the incremental yaw response caused by the wheel speed micro-excitation is as follows: set up The starting sampling time for the micro-excitation test of steering wheel speed is The normal trajectory tracking controller in the first The basic differential control quantity determined at each sampling time is: Then superimposed Differential control quantity after micro-excitation of steering wheel speed for: (9); In the formula, ; express or ,Right now The wheel speed micro-excitation at the j-th sampling point in the directional wheel speed micro-excitation sequence; P and N represent the positive and negative directions, respectively; Based on the differential control value, the target linear velocity of the left drive wheel is set to... Set the target linear velocity of the right drive wheel to ,in For the first The longitudinal speed command at each sampling moment; the wheel speed micro-excitation changes the difference between the target linear velocities of the left and right wheels, but does not change the average value of the two.

[0031] While applying wheel speed micro-excitation, establish a reference model without micro-excitation that is synchronously updated with the actual AGV but only receives basic differential speed control values ​​and not wheel speed micro-excitations: (10); (11); In the formula, , The reference model without micro-excitation is in The yaw rate and heading angle at the j-th sampling point were measured using a micro-excitation test of the steering wheel speed. , These represent the yaw rate and heading angle at the (j+1)th sampling point in the wheel speed micro-excitation sequence for the reference model without micro-excitation. exist At the start of the micro-excitation test of the steering wheel speed, the initial yaw rate and initial heading angle of the reference model without micro-excitation are set to the yaw rate and heading angle measured by the AGV at the start of the test, so that the actual AGV and the reference model without micro-excitation have the same initial state. exist At the j-th sampling point in the micro-excitation sequence of wheel speed, the difference between the actual yaw rate of the AGV and the yaw rate of the reference model without micro-excitation is recorded as the yaw rate increment. The difference between the actual heading angle of the AGV and the heading angle of the reference model without micro-excitation is denoted as the heading angle increment. The difference between the test differential control value and the basic differential control value is recorded as the test differential increment. ;in, This refers to the incremental yaw response caused by the wheel speed micro-excitation; based on the superposition relationship of differential control quantities, when no command limiting occurs... The above difference reduces the impact of the normal trajectory tracking process and the basic differential control on the test data, so that the extracted incremental state mainly reflects the yaw response generated by the wheel speed micro-excitation.

[0032] The positive and negative wheel speed micro-excitations are applied sequentially according to a preset test order. For example, the positive wheel speed micro-excitation is applied first; after the positive wheel speed micro-excitation test ends and the yaw rate increment and heading angle increment recover to within the corresponding preset thresholds, the negative wheel speed micro-excitation is applied.

[0033] The corresponding preset thresholds are the yaw rate increment threshold and the heading angle increment threshold. These are used to determine whether the incremental response caused by the micro-excitation test of wheel speed in the current direction has sufficiently decayed. That is, whether the deviation between the actual yaw rate and the yaw rate of the reference model without micro-excitation, and the deviation between the actual heading angle and the heading angle of the reference model without micro-excitation, have both fallen back to the corresponding preset threshold range. This ensures that the residual response of the previous direction test will not be carried over into the subsequent direction test, affecting the accuracy of the steering response coefficient identification. The yaw rate increment threshold and the heading angle increment threshold should be significantly smaller than the allowable yaw rate and heading angle error threshold during normal operation, respectively. For example, the yaw rate increment threshold can be 0.02 rad / s, and the heading angle increment threshold can be 0.01 rad. If the response still does not recover to the corresponding preset threshold range after reaching the maximum number of recovery sampling points after the test, the test data is deemed invalid, and the micro-excitation test interval is redefined before the wheel speed micro-excitation test in that direction is executed again.

[0034] In S4, the process of obtaining the positive steering response coefficient, the negative steering response coefficient, and their dispersion is as follows: according to The incremental yaw response in the direction is obtained as follows: (12); In the formula, for The yaw rate increment at the (j+1)th sampling point is measured using a micro-excitation test of the steering wheel speed. for Discrete steering response coefficients in direction, in m -1 ; The yaw response residuals are caused by unmodeled factors and measurement noise, in rad / s. from In the main test segment of the direction, select valid sampling points with complete adjacent sampling data, and form a set by numbering the corresponding sampling points. Determined according to the following formula Steering response coefficient : (13); In the formula, Let be the non-negative normalized sampling weight corresponding to the j-th valid sampling point, and The sum of the sampling weights within the sample is 1; To prevent regularization parameters from having a denominator of zero, the unit is (m / s). 2 ; Calculated The unit is m -1 .

[0035] The degree of dispersion of the directional response coefficients is determined based on the fitting residuals. : (14).

[0036] The steering response coefficients in the positive direction are obtained using equations (13) and (14). and its degree of dispersion And the steering response coefficient in the negative direction. and its degree of dispersion .

[0037] Based on the obtained steering response coefficient estimates and their dispersion, response intervals for the positive and negative directions are constructed respectively. Before constructing the response intervals, the steering response coefficient estimates are limited to a range determined by their physical lower and upper limits. Response range of direction for: (15); In the formula, and These are the lower and upper limits of the response range, respectively. and These are the physical lower limit and physical upper limit of the steering response coefficient, respectively, with preferred values ​​of 0.07m. -1 0.20m -1 ; is a dimensionless interval width coefficient, preferably 0.35. The directional response interval represents the possible range of values ​​for the response coefficient between the differential control quantity and the actual yaw rate increment in a specified steering direction, used to characterize the changes in directional response caused by loading conditions, wheel speed closed-loop conditions, and wheel-ground interaction conditions. Let respectively and This corresponds to the positive and negative response intervals.

[0038] When the estimated steering response coefficient obtained by equation (13) exceeds the preset physical upper and lower limits, it is restricted to the value obtained by equation (13). and Within the range of physical values, the directional response interval is constructed using the constrained estimated values ​​to avoid forming an empty set.

[0039] In S5, the reference path curvature of the i-th prediction point in the prediction time domain is obtained. k is the sampling index corresponding to the current sampling time, and k+i represents the sampling index corresponding to the i-th prediction point; the corresponding directional response interval is selected according to the direction of the curvature of the reference path: (16); In the formula, The directional response interval used for the i-th prediction point; , These are the positive and negative response intervals, respectively. The preferred threshold for determining the steering direction is 0.025m. -1 When the absolute value of the curvature of the reference path is not greater than When using this method, the smallest interval that can simultaneously cover both the positive and negative response intervals is adopted. , These are the lower and upper limits of the positive response range, respectively. , These are the lower and upper limits of the negative response range, respectively. The lateral tracking error at the k-th sampling time Heading angle error and yaw rate Composition of trajectory tracking state The longitudinal velocity of the i-th prediction point is denoted as... Establish a direction-related prediction model: (17); In the formula, The differential control quantity for the i-th prediction point is determined based on the known information at the current sampling time k. For the i-th prediction point, from Steering response coefficient with a value of [value missing] ; , These are the trajectory tracking states of the i-th and i+1-th predicted points, respectively, predicted based on the state information of the current sampling index k. Equation (12) is obtained by subtracting the yaw rate of the actual AGV from the yaw rate of the reference model without micro-excitation. Equation (12) represents the recursive relationship between the yaw rate increment and the differential speed increment. Its structure is consistent with the recursive relationship of the yaw rate component in the direction-dependent prediction model. Therefore, the identified positive and negative steering response coefficients can be used as the input gain corresponding to the differential control quantity in the direction-dependent prediction model, that is, as the input matrix in Equation (17). The basis for its value.

[0040] For a given candidate differential control sequence, the current trajectory tracking state is taken as a set. The state range is predicted recursively according to the following formula: (18); In the formula, , , These correspond to the state matrix, input matrix, and path curvature term in equation (17), respectively; , These are the predictable reach ranges of the i-th and i+1-th prediction points, respectively, which are the sets of predicted states that can be generated for the same candidate differential control quantity within the directional response interval. for any predicted state in the set; co represents the minimal convex set formed by the predicted states in the set.

[0041] In S6, the reference state of the i-th prediction point corresponds to zero lateral tracking error, zero heading angle error, and yaw rate. A robust model is constructed based on the predicted reachability range to predict the control objective function J: (19); In the formula, To predict the differential control quantity sequence in the time domain; To predict the time domain length (preferably 16); This is the reference state for the i-th prediction point. and These are the dimensionless predicted state and dimensionless reference state obtained after dimensionless processing of the predicted state and reference state, respectively; Q is the dimensionless state error weight matrix. , These are the dimensionless weights of the differential control quantity and its change; for The dimensionless differential control quantity obtained after dimensionless processing; Let be the change in differential control quantity at the i-th prediction point relative to the differential control quantity at the previous prediction point. , , Let be the differential control quantity corresponding to the (i-1)th prediction point, with the symbol . This means that the corresponding predicted quantity is determined based on the known information at the current sampling time k. When i=0, the differential control quantity actually output at the previous sampling time is used as the comparison benchmark. for The dimensionless differential control quantity change obtained after dimensionless processing; Representing the weighted square norm, Q is in diagonal matrix form, with diagonal elements... , and These are the weights corresponding to the lateral tracking error, heading angle error, and yaw rate deviation, respectively. and Determined based on the trade-off between trajectory tracking accuracy, differential control amplitude, and differential control smoothness, to improve... or Used to enhance the suppression of corresponding tracking errors and improve Used to limit the amplitude of differential control quantity, improve Used to suppress changes in the differential control quantity between adjacent sampling times.

[0042] The dimensionless processing adopts a per-unit method, in which each state component of the predicted state and the reference state is divided by the corresponding allowable value, and the differential control quantity and the differential control quantity change quantity are divided by the corresponding allowable maximum value. For example, the upper limit of the differential control quantity is set to 0.30 m / s, and the upper limit of the differential control quantity change quantity between adjacent sampling times is set to 0.04 m / s.

[0043] Equation (19) is solved using an iterative optimization method to obtain the optimal differential control quantity sequence, and the first differential control quantity is taken as the optimal differential control quantity at the current sampling time. ; For example, when solving equation (19), the upper and lower boundaries of the directional response intervals of each prediction point are taken as candidate boundary values ​​of the response coefficients. The prediction state is recursively calculated according to the directional correlation prediction model, and the corresponding objective function value is calculated. The maximum objective function value under each boundary condition is taken as the robust optimization evaluation value. The differential control quantity sequence in the prediction time domain is taken as the decision variable and solved by numerical iterative optimization method. The interior point method is preferred. When the absolute value of the difference between the objective function values ​​of two adjacent iterations is not greater than the preset convergence threshold, the iteration is stopped, and the optimal differential control quantity sequence is obtained.

[0044] Based on the longitudinal velocity command at the current sampling moment Set the target linear velocity of the left drive wheel to Set the target linear velocity of the right drive wheel to And output to the left and right wheel speed controllers; At the next sampling moment, return to S5 and continue execution until trajectory tracking control is completed; when the AGV loading status is detected to change again, re-execute S1 to S4 to update the positive and negative response intervals.

[0045] To visually demonstrate the control effectiveness of different trajectory tracking methods in complex reference path scenarios, a set of AGV trajectory tracking examples containing multiple continuous curves and curvature variation ranges were selected for comparative visualization. The results are as follows: Figure 2 and Figure 3 As shown. Figure 2The comparison results between the reference path and the actual trajectory of the AGV are given. The black solid line represents the reference path, the blue dashed line represents the traditional pure tracking control method, the orange dashed line represents the lateral-heading dual-error PID control method, the purple dashed line represents the Kanayama kinematic control method, and the green solid line represents the control method of this embodiment. The horizontal axis (x-axis) is the longitudinal position of the AGV in the ground reference coordinate system, ranging from 0 to 50m, which is the main forward direction of the AGV. The vertical axis (y-axis) is the lateral position of the AGV in the ground reference coordinate system, ranging from -2 to 2m, reflecting the left and right curvature of the path. Figure 3 The curves showing the change of lateral tracking error over time for four different methods are presented.

[0046] Depend on Figure 2 As can be seen, under the selected complex curve reference path, all methods can basically complete the path tracking task. However, there are significant differences in the degree of trajectory deviation among different methods in the peak section and the section with rapid curvature change. Compared with traditional pure tracking control, lateral-heading dual-error PID control, and Kanayama kinematic control, the actual trajectory of the control method in this embodiment is closer to the reference path overall, especially in the areas with multiple steering switches and large curvature curves, showing smaller trajectory deviation. This indicates that the control method in this embodiment can more effectively suppress the path deviation caused by inconsistent directional response.

[0047] Depend on Figure 3 As can be seen, the lateral tracking error of the control method in this embodiment is relatively small throughout the tracking process, and it has a more significant suppression effect at the error peak position. Compared with the other three control methods, the control method in this embodiment has a lower error peak and faster error recovery in the main curve section, indicating that it can more effectively cope with the response differences of AGV under different steering directions and improve the trajectory tracking accuracy and operational stability under complex path conditions.

[0048] Furthermore, Figure 2 and Figure 3 Correspondingly, this embodiment shows that by introducing the difference between positive and negative directional responses and performing directional correlation robust processing in predictive control, the control method can not only reduce the deviation of the actual trajectory from the reference path, but also effectively reduce the peak value of lateral tracking error, thereby improving the trajectory tracking performance of AGV in complex curve scenarios.

[0049] To further avoid the random effects of a single reference path and a single set of vehicle parameters, 50 sets of AGV trajectory tracking scenarios were randomly generated for statistical verification. Each scenario included straight road sections, positive-direction turning sections, negative-direction turning sections, and turning direction switching sections. Within a preset range, the lateral amplitude of the reference path, turning change frequency, AGV longitudinal speed, initial lateral tracking error, initial heading angle error, positive-direction steering response coefficient, and negative-direction steering response coefficient were randomly set to simulate the trajectory tracking process under different path shapes, initial tracking states, loading states, and degrees of inconsistency between positive and negative directional responses.

[0050] Fifty trajectory tracking scenarios were simulated using traditional pure tracking control, lateral-heading dual-error PID control, Kanayama kinematic control, and the control method of this embodiment. Within each scenario, all control methods used the same reference path, initial state, positive steering response coefficient, negative steering response coefficient, longitudinal velocity, sampling period, and left and right drive wheel speed constraints, changing only the trajectory tracking control method to ensure consistent comparison conditions.

[0051] The root mean square value of the lateral tracking error, the maximum absolute error, the root mean square value of the lateral tracking error in the positive turning section, the root mean square value of the lateral tracking error in the negative turning section, and the average change of the differential control quantity were statistically analyzed for each control method under 50 working conditions. The average values ​​of each index under 50 working conditions were calculated. The statistical results are shown in Table 1.

[0052] Table 1. Comparison of average performance of different control methods under multiple random trajectory tracking conditions.

[0053] Table 1 shows the average values ​​for 50 sets of random AGV trajectory tracking scenarios. Each scenario includes alternating positive and negative turning sections. All control methods use the same reference path, initial state, vehicle response parameters, and control constraints within the same scenario. The average total RMS error represents the average root mean square value of the lateral tracking error throughout the entire trajectory tracking process for each scenario; the average maximum absolute error represents the average of the maximum absolute values ​​of the lateral tracking error for each scenario; the average RMS error for positive and negative turning sections represents the average root mean square value of the lateral tracking error during the AGV's turns in the preset positive and negative directions, respectively; the average change in differential control quantity represents the average absolute value of the change in differential control quantity at adjacent sampling times.

[0054] As shown in Table 1, under 50 sets of random AGV trajectory tracking conditions, the average total RMS error of the control method in this embodiment is 0.04082m and the average maximum absolute error is 0.19515m, both of which are lower than the other three control methods.

[0055] Compared to traditional pure tracking control, the average total RMS error of the control method in this embodiment is reduced from 0.09853m to 0.04082m, a reduction of approximately 58.57%; the average maximum absolute error is reduced from 0.41698m to 0.19515m, a reduction of approximately 53.20%.

[0056] Compared to lateral-heading dual-error PID control, the control method in this embodiment reduces the average total RMS error by approximately 65.50% and the average maximum absolute error by approximately 55.49%. Compared to Kanayama kinematic control, it reduces the average total RMS error by approximately 63.87% and the average maximum absolute error by approximately 55.06%. These results demonstrate that the control method in this embodiment can effectively reduce the overall lateral tracking error and local maximum deviation under different reference paths, vehicle response parameters, and initial tracking states.

[0057] Furthermore, the average RMS errors of the control method in this embodiment on positive-direction turning sections and negative-direction turning sections are 0.02048m and 0.05979m, respectively, both lower than the other three control methods. Compared to traditional pure tracking control, the average RMS error of the control method in this embodiment is reduced by approximately 29.81% on positive-direction turning sections and by approximately 60.16% on negative-direction turning sections.

[0058] The results show that selecting the corresponding positive or negative response interval based on the turning direction of the predicted path can reduce the impact of inconsistent AGV responses in different turning directions on trajectory tracking accuracy, and can significantly improve trajectory tracking performance on negative turning sections.

[0059] Meanwhile, the average change in differential control quantity of the control method in this embodiment is 0.00334 m / s, which is lower than that of traditional pure tracking control, lateral-heading dual-error PID control, and Kanayama kinematic control. This shows that the control method in this embodiment reduces trajectory tracking error without increasing differential control quantity fluctuation, and can balance trajectory tracking accuracy and control stability.

[0060] Example 2: An AGV trajectory tracking control device based on bidirectional wheel speed micro-excitation, comprising: One or more processors; Memory, used to store one or more computer programs; When one or more computer programs are executed by one or more processors, the one or more processors perform the method in Embodiment 1.

[0061] Example 3: A computer-readable storage medium having executable instructions stored thereon, which, when executed by a processor, cause the processor to perform the method in Example 1.

[0062] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can make equivalent substitutions or modifications based on the technical solution and concept of the present invention within the scope of the technology disclosed in the present invention, and such modifications should also be considered to fall within the scope of protection of the present invention.

Claims

1. A bidirectional wheel speed micro-actuated AGV trajectory tracking control method, characterized in that, Includes the following steps: S1. Obtain the AGV's operating status, reference path curvature, and basic control information. The operating status includes longitudinal speed, yaw rate, lateral tracking error, and heading angle error. The basic control information includes longitudinal speed commands and basic differential control quantities. When at least one of the positive and negative directions has not completed the wheel speed micro-excitation test, or when a change in the AGV's loading status is detected, determine the micro-excitation test interval based on the current operating status and the safe road section ahead. S2. Establish a nominal discrete response model for the heading angle increment and yaw rate increment caused by wheel speed micro-excitation, generate a positive direction wheel speed micro-excitation sequence where both the terminal heading angle increment and the terminal yaw rate increment are zero, and take their opposite to obtain the negative direction wheel speed micro-excitation sequence. S3. Within the micro-excitation test range, the wheel speed micro-excitation sequence in the corresponding direction is superimposed on the basic differential control quantity and applied to the AGV. The incremental yaw response caused by the wheel speed micro-excitation is extracted by using a reference model without micro-excitation that only receives the basic differential control quantity. S4. Based on the incremental yaw response in the positive and negative directions, obtain the positive steering response coefficient and the negative steering response coefficient and their dispersion, and construct the positive response interval and the negative response interval accordingly. S5. Determine the corresponding turning direction based on the reference path curvature of each prediction point in the prediction time domain, select the corresponding direction response interval for each prediction point, establish a direction-related prediction model, and recursively obtain the predicted reachability range of each prediction point. S6. Construct a robust model predictive control objective function based on the predicted reachable range, solve for the optimal differential control quantity, and generate the target linear velocity of the left and right drive wheels in combination with the longitudinal speed command, and output it to the AGV to perform trajectory tracking; return to S5 at the next sampling time to perform rolling execution until trajectory tracking control is completed.

2. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 1, characterized in that, In the S1, the longitudinal speed of the AGV at the kth sampling time is acquired , the yaw rate , the lateral tracking error is determined according to the relationship between the current position of the AGV and the reference path , the heading angle error , and the reference path curvature ; Obtain the basic target linear velocity of the left drive wheel output by the normal trajectory tracking controller at the kth sampling time. and the base target linear velocity of the right drive wheel and determine the longitudinal speed command. and basic differential control quantity : （1）； （2）； Regulation The corresponding preset yaw direction is positive, i.e., the positive direction. The corresponding preset yaw direction is negative, i.e., the negative direction.

3. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 2, characterized in that, In S1, the wheel speed micro-excitation test is a test process in which a pre-constructed wheel speed micro-excitation sequence is superimposed on the basic differential control quantity and applied to the AGV on the basis of normal trajectory tracking control, so as to actively excite and observe the AGV steering response. When the wheel speed micro-excitation test has not been completed in at least one of the positive and negative directions, the response test request for the direction that has not been completed is triggered; when a change in the AGV loading status is detected, the response test requests for both the positive and negative directions are retried. After the test request is triggered, it continuously determines whether the current running status of the AGV and the continuous safe road segment ahead both satisfy equation (3): （3）； In the formula, , , and These are the path curvature threshold, lateral error threshold, heading angle error threshold, and safe distance threshold, respectively. The minimum lateral distance between the outer contour of the AGV vehicle body and the boundary of the allowed driving area at the k-th sampling time; The remaining length of the road segment that continuously meets the path curvature threshold requirement and has a preset safe passage margin from the current position of the AGV along the reference path; M represents the sampling period; M represents the total number of sampling points in the wheel speed micro-excitation sequence. This represents the maximum number of recovery sampling points after the test ends. When equation (3) is satisfied, the continuous road segment that starts from the kth sampling time and whose length can cover the wheel speed micro-excitation sequence and its post-test recovery process in the direction to be executed once is determined as the micro-excitation test interval; When equation (3) is not satisfied, maintain the directional response test request and continue to detect subsequent running road segments; after completing the wheel speed micro-excitation test in one direction, if the wheel speed micro-excitation test in another direction has not been completed, then re-determine the micro-excitation test interval corresponding to that direction according to equation (3).

4. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 1, characterized in that, In S2, at the j-th sampling point of the wheel speed micro-excitation sequence, the heading angle increment caused by the wheel speed micro-excitation is... and yaw rate increment Composition of micro-excitation incremental state The superscript T denotes transpose. A nominal discrete response model for the incremental state of the micro-excitation is established: （4）； In the formula, This represents the micro-excitation increment state at the (j+1)th sampling point of the wheel speed micro-excitation sequence; The nominal state matrix represents the incremental states. The sampling period is The yaw rate maintenance coefficient; For the nominal input matrix, The nominal discrete steering response coefficient; The wheel speed micro-excitation is superimposed on the differential control quantity at the j-th sampling point of the wheel speed micro-excitation sequence. Define the positive direction wheel speed micro-excitation sequence ,in Let M represent the positive direction wheel speed micro-excitation at the j-th sampling point, where M is the total number of sampling points in the wheel speed micro-excitation sequence; and let M be the incremental state at the end of the positive direction wheel speed micro-excitation sequence, given that the initial value of the micro-excitation increment state is zero. for: （5）； In the formula, express M-1-j powers; In the first M-2 sampling points, the positive direction main test segment and the smooth recovery segment are set sequentially, and the last two sampling points are used as the terminal compensation amount, that is, the positive direction wheel speed micro-excitation at sampling points j=M-2 and j=M-1. , Defined as two terminal compensation quantities; the wheel speed micro-excitation in the positive direction main test segment is set along the preset positive steering direction to generate the yaw rate increment that can characterize the positive steering response; the wheel speed micro-excitation in the smooth recovery segment changes continuously to reduce the heading angle increment and yaw rate increment formed when the positive direction main test segment propagates to the end of the positive direction wheel speed micro-excitation sequence. With the compensation values ​​of the last two terminals temporarily set to zero, the predicted incremental state q formed when the wheel speed micro-excitation of the first M-2 sampling points propagates to the end of the positive direction wheel speed micro-excitation sequence is denoted as: （6）； In the formula, , These are the predicted heading angle increment and predicted yaw rate increment formed when the wheel speed micro-excitation propagates from the first M-2 sampling points to the end of the positive direction wheel speed micro-excitation sequence; According to the nominal discrete response model, the compensation quantities for the two terminals are determined sequentially as follows: （7）； （8）； Define the negative direction wheel speed micro-excitation sequence ,in This represents the negative direction wheel speed micro-excitation at the j-th sampling point; the negative direction wheel speed micro-excitation sequence is set as the opposite of the positive direction wheel speed micro-excitation sequence, i.e. .

5. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 4, characterized in that, when and At that time, the two terminal compensation quantities in equations (7) and (8) can be determined sequentially, thereby ensuring that the positive direction wheel speed micro-excitation sequence satisfies the nominal discrete response model when calculated. That is, the predicted heading angle increment and the predicted yaw rate increment are both zero at the end of the positive direction wheel speed micro-excitation sequence; a finite length wheel speed micro-excitation sequence with the above characteristics is defined as a zero terminal wheel speed micro-excitation sequence. When the amplitude of the wheel speed micro-excitation at any sampling point in the constructed positive direction wheel speed micro-excitation sequence is... When the preset upper limit of the micro-excitation amplitude is exceeded, the entire positive direction wheel speed micro-excitation sequence is scaled according to the same scaling ratio so that each sampling point after scaling meets the micro-excitation amplitude limit. Since the nominal discrete response model is a linear model, the entire positive direction wheel speed micro-excitation sequence still meets the zero terminal condition after being scaled according to the same ratio. The negative direction wheel speed micro-excitation sequence adopts the same amplitude limit and scaling processing as the positive direction wheel speed micro-excitation sequence.

6. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 4, characterized in that, In S3, the process of extracting the incremental yaw response caused by the wheel speed micro-excitation is as follows: set up The starting sampling time for the micro-excitation test of steering wheel speed is The normal trajectory tracking controller in the first The basic differential control quantity determined at each sampling time is: Then superimposed Differential control quantity after micro-excitation of steering wheel speed for: （9）； In the formula, ; express or ,Right now The wheel speed micro-excitation at the j-th sampling point in the directional wheel speed micro-excitation sequence; P and N represent the positive and negative directions, respectively; Based on the differential control value, the target linear velocity of the left drive wheel is set to... Set the target linear velocity of the right drive wheel to ,in For the first Longitudinal velocity command at each sampling time; While applying wheel speed micro-excitation, establish a reference model without micro-excitation that is synchronously updated with the actual AGV but only receives basic differential speed control values ​​and not wheel speed micro-excitations: （10）； （11）； In the formula, , The reference model without micro-excitation is in The yaw rate and heading angle at the j-th sampling point were measured using a micro-excitation test of the steering wheel speed. , These represent the yaw rate and heading angle at the (j+1)th sampling point in the wheel speed micro-excitation sequence for the reference model without micro-excitation. exist At the start of the micro-excitation test of the steering wheel speed, the initial yaw rate and initial heading angle of the reference model without micro-excitation are set to the yaw rate and heading angle measured by the AGV at the start of the test, so that the actual AGV and the reference model without micro-excitation have the same initial state. exist At the j-th sampling point in the micro-excitation sequence of wheel speed, the difference between the actual yaw rate of the AGV and the yaw rate of the reference model without micro-excitation is recorded as the yaw rate increment. The difference between the actual heading angle of the AGV and the heading angle of the reference model without micro-excitation is denoted as the heading angle increment. The difference between the test differential control value and the basic differential control value is recorded as the test differential increment. ;in, This refers to the incremental yaw response caused by the wheel speed micro-excitation; based on the superposition relationship of differential control quantities, when no command limiting occurs... ; Apply positive and negative wheel speed micro-excitations sequentially according to the preset test sequence.

7. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 6, characterized in that, In S4, the process of obtaining the positive steering response coefficient, the negative steering response coefficient, and their dispersion is as follows: according to The incremental yaw response in the direction is obtained as follows: （12）； In the formula, for The yaw rate increment at the (j+1)th sampling point is measured using a micro-excitation test of the steering wheel speed. for Discrete steering response coefficients in direction; The yaw response residuals are caused by unmodeled factors and measurement noise. from In the main test segment of the direction, select valid sampling points with complete adjacent sampling data, and form a set by numbering the corresponding sampling points. Determined according to the following formula Steering response coefficient : （13）； In the formula, Let be the non-negative normalized sampling weight corresponding to the j-th valid sampling point; To prevent regularization parameters with a denominator of zero; The degree of dispersion of the directional response coefficients is determined based on the fitting residuals. : （14）。 8. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 7, characterized in that, In S4, Response range of direction for: （15）； In the formula, and These are the lower and upper limits of the response range, respectively. and These are the physical lower limit and physical upper limit of the steering response coefficient, respectively; This is the interval width coefficient.

9. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 8, characterized in that, In step S5, the reference path curvature of the i-th prediction point in the prediction time domain is obtained. k is the sampling index corresponding to the current sampling time, and k+i represents the sampling index corresponding to the i-th prediction point; the corresponding directional response interval is selected according to the direction of the curvature of the reference path: （16）； In the formula, The directional response interval used for the i-th prediction point; , These are the positive and negative response intervals, respectively. Threshold for determining steering direction; , These are the lower and upper limits of the positive response range, respectively. , These are the lower and upper limits of the negative response range, respectively. The lateral tracking error at the k-th sampling time Heading angle error and yaw rate Composition of trajectory tracking state The longitudinal velocity of the i-th prediction point is denoted as... Establish a direction-related prediction model: （17）； In the formula, The differential control quantity for the i-th prediction point is determined based on the known information at the current sampling time k. For the i-th prediction point, from Steering response coefficient with a value of [value missing] ; , These are the trajectory tracking states of the i-th and i+1-th predicted points, respectively, predicted based on the state information of the current sampling index k. For a given candidate differential control sequence, the current trajectory tracking state is taken as a set. The state range is predicted recursively according to the following formula: （18）； In the formula, , , These correspond to the state matrix, input matrix, and path curvature term in equation (17), respectively; , These are the predictable reach ranges of the i-th and i+1-th prediction points, respectively, which are the sets of predicted states that can be generated for the same candidate differential control quantity within the directional response interval. for any predicted state in the set; co represents the minimal convex set formed by the predicted states in the set.

10. The AGV trajectory tracking control method based on bidirectional wheel speed micro-excitation according to claim 9, characterized in that, In S6, a robust model predictive control objective function J is constructed by combining the predicted reachability range: （19）； In the formula, To predict the differential control quantity sequence in the time domain; To predict the length of the time domain; This is the reference state for the i-th prediction point. and These are the dimensionless predicted state and dimensionless reference state obtained after dimensionless processing of the predicted state and reference state, respectively; Q is the dimensionless state error weight matrix. , These are the weights of the differential control quantity and its change; for The dimensionless differential control quantity obtained after dimensionless processing; Let be the change in differential control quantity at the i-th prediction point relative to the differential control quantity at the previous prediction point. for The dimensionless differential control quantity change obtained after dimensionless processing; Equation (19) is solved using an iterative optimization method to obtain the optimal differential control quantity sequence, and the first differential control quantity is taken as the optimal differential control quantity at the current sampling time. ; Based on the longitudinal velocity command at the current sampling moment Set the target linear velocity of the left drive wheel to Set the target linear velocity of the right drive wheel to And output to the left and right wheel speed controllers; At the next sampling moment, return to S5 and continue execution until trajectory tracking control is completed; when the AGV loading status is detected to change again, re-execute S1 to S4 to update the positive and negative response intervals.

Images