Model prediction-based agv trajectory tracking control algorithm and system

Abstract

The application relates to the field of industrial control technology, in particular to an AGV trajectory tracking control algorithm and system based on model prediction, which comprises the following steps: collecting laser radar positioning data, binocular vision positioning data, inertial measurement data, wheel speed encoder feedback data, steering angle sensor feedback data and obstacle historical trajectory, determining fused motion state and state credibility, combining the obstacle historical trajectory with a preset reference trajectory to construct a time-varying feasible trajectory domain, determining control weight and constraint boundary, solving a control sequence and controlling a steering mechanism and a walking motor, and updating vehicle kinematics model parameters, control weight or constraint margin according to the difference between the predicted motion state and the actual motion state. The application cooperates the state credibility, the time-varying feasible trajectory domain and the execution feedback, so that the control sequence corresponds to the perception quality, the obstacle occupation and the vehicle response, and the trajectory tracking deviation and the control amount fluctuation accumulation are inhibited.

Description

Technical Field

[0001] This invention relates to the field of industrial control technology, and in particular to a model prediction-based AGV trajectory tracking control algorithm and system. Background Technology

[0002] The field of industrial control technology mainly revolves around the acquisition, transmission, calculation and execution of mobile device operating status, environmental location data and control commands. Typically, encoders, inertial measurement units, lidar or vision devices acquire pose, speed and obstacle information, which is then processed by the vehicle controller and sent to the driver to send steering and speed commands. Task and status data are also exchanged with scheduling equipment through industrial networks.

[0003] Traditional AGV trajectory tracking control refers to the control process for AGVs to run along a preset trajectory. It typically reads the vehicle's current pose, speed, reference trajectory, and kinematic constraints, establishes a vehicle state prediction model, predicts the subsequent state in the discrete time domain, constructs an evaluation relationship based on position deviation, heading deviation, and changes in control quantity, obtains the control sequence within a finite prediction interval, transmits the current control command to the steering mechanism and travel motor, and repeats the calculation in the next sampling period based on the updated sensor data.

[0004] Traditional control processes establish state prediction relationships based on vehicle position and speed and preset reference trajectories, and obtain control sequences within a limited prediction interval according to position deviation, heading deviation, and control quantity changes. When sensor positioning results are missing, time out, or deviate from each other, the reliability of the fused state is difficult to reflect in the control weights and constraint boundaries. The temporal occupancy relationship between the future position of obstacles and the reference trajectory lacks a unified expression. After the vehicle kinematic prediction results deviate from the actual response of the steering mechanism and the travel motor, subsequent control still uses the original model parameters, resulting in the accumulation of trajectory tracking deviation and control quantity fluctuations. Summary of the Invention

[0005] The purpose of this invention is to provide an AGV trajectory tracking control algorithm and system based on model prediction, which incorporates the state reliability of multi-source positioning data, the time-varying feasible trajectory domain formed by dynamic obstacle prediction, the adjustment of control weights and constraint boundaries, and the execution feedback correction into the same control process, so as to handle trajectory tracking control problems under the conditions of missing or timed sensor data, changes in dynamic obstacle occupancy, and deviation between the vehicle kinematic prediction state and the actual motion state.

[0006] To achieve the above objectives, this invention provides an AGV trajectory tracking control algorithm based on model prediction, comprising the following steps: Acquire synchronized lidar positioning data, binocular vision positioning data, inertial measurement data, wheel speed encoder feedback data, steering angle sensor feedback data, preset reference trajectory, and historical obstacle trajectory.

[0007] Based on the differences between the positioning results formed by LiDAR positioning data, binocular vision positioning data, and inertial measurement data and the vehicle kinematics prediction results, the missing or timeout states of the three types of data, and the differences between the three types of positioning results, the fused motion state and state reliability are determined. The LiDAR positioning data, binocular vision positioning data, inertial measurement data, and wheel speed encoder feedback data are aligned according to the sampling time. Based on the fused motion state at the previous sampling time, the inertial measurement data at the current sampling time, and the wheel speed encoder feedback data, a vehicle kinematics prediction result is generated through the vehicle kinematics model. The observation residuals between the LiDAR positioning results, binocular vision positioning results, and inertial measurement positioning results and the vehicle kinematics prediction results are calculated separately, and the positioning result differences among the three types of positioning results are also calculated. Using extended Kalman filtering, the vehicle kinematics prediction results are corrected based on the observation residuals corresponding to positioning data that have not experienced missing or timeouts, resulting in the fused motion state at the current sampling time. Based on each observation residual, the missing or timeout states of each positioning data, and the positioning result differences, a state reliability corresponding to the fused motion state at the current sampling time is generated.

[0008] An obstacle position sequence is generated based on the obstacle's historical trajectory. A time-varying feasible trajectory domain is constructed by combining a preset reference trajectory, preset vehicle motion constraints, and preset obstacle occupancy range. The obstacle positions, directions of motion, and speeds corresponding to consecutive sampling moments in the obstacle's historical trajectory are arranged according to sampling time. The temporal variation features of the obstacle's historical trajectory are extracted using a Long Short-Term Memory (LSTM) network, and the obstacle positions corresponding to each prediction moment in the model's prediction time domain are generated. An obstacle occupancy range is generated based on the obstacle positions and contours corresponding to each prediction moment, and spatially expanded according to the AGV contour. The spatially expanded obstacle occupancy range is compared with the preset reference trajectory moment by moment. Within the vehicle motion constraint range, the position range, heading range, and speed range that do not overlap with the spatially expanded obstacle occupancy range are retained, and arranged according to the prediction moment to form the time-varying feasible trajectory domain.

[0009] Control weights and constraint boundaries are determined based on state reliability and the distance from the obstacle occupancy boundary to the preset reference trajectory. At each prediction time within the model's prediction time domain, the distance between the obstacle occupancy boundary and the corresponding reference trajectory point is calculated, and the minimum of these distances is determined as the distance from the obstacle to the preset reference trajectory. Preset position deviation weights, heading deviation weights, speed deviation weights, and control variable change weights are used as basic control weights. The control variable change weights and vehicle speed boundaries are adjusted based on state reliability. Similarly, the position deviation weights, speed deviation weights, and obstacle occupancy boundary are adjusted based on the distance from the obstacle to the preset reference trajectory. The adjusted basic control weights are then determined as the control weights, and the adjusted vehicle speed boundaries and obstacle occupancy boundary are determined as the constraint boundaries.

[0010] Furthermore, the state confidence level is compared with a confidence threshold. When the state confidence level is lower than the confidence threshold, the control variable change weight is increased and the vehicle speed boundary is decreased; when the state confidence level is not lower than the confidence threshold, the preset values ​​corresponding to the control variable change weight and the vehicle speed boundary are maintained. The distance from the obstacle to the preset reference trajectory is compared with a safe distance threshold. When the distance from the obstacle to the preset reference trajectory is less than the safe distance threshold, the obstacle occupancy range boundary is expanded and the speed deviation weight is increased; when the distance from the obstacle to the preset reference trajectory is not less than the safe distance threshold, the preset values ​​corresponding to the obstacle occupancy range boundary and the speed deviation weight are maintained. When both comparison results trigger adjustments, the adjusted vehicle speed boundary and obstacle occupancy range boundary with higher restriction levels are adopted respectively.

[0011] The control sequence is solved within the time-varying feasible trajectory domain. The fused motion state is used as the initial motion state in the model's prediction time domain. The corresponding reference position, reference heading, and reference velocity are determined from the time-varying feasible trajectory domain according to the prediction time. Based on the vehicle kinematics model and candidate control variables, the predicted motion state is generated time-by-time. The position deviation, heading deviation, and velocity deviation between the predicted motion state and the reference position, reference heading, and reference velocity are calculated, along with the control variable changes between adjacent candidate control variables. The position deviation, heading deviation, velocity deviation, and control variable changes are weighted according to control weights. Within the constraint boundaries and vehicle motion constraints, the candidate control variable combination with the smallest weighted result is selected to form the control sequence. The control variable corresponding to the current sampling time in the control sequence is sent to the steering mechanism and the travel motor.

[0012] The actual motion state is determined based on feedback data from the wheel speed encoder and steering angle sensor, and feedback updates are performed. The actual driving speed is determined based on the wheel speed encoder feedback data from the current and previous sampling times, and the actual steering angle and its change are determined based on the steering angle sensor feedback data from the current and previous sampling times. The actual motion state is then formed based on the actual driving speed, actual steering angle, and steering angle change. The predicted motion state corresponding to the current sampling time is extracted from the model prediction time domain. The position difference, heading difference, speed difference, and steering response difference between the predicted and actual motion states are calculated to form prediction error data. According to the error type corresponding to the prediction error data, the position difference, heading difference, and speed difference are used to update the vehicle kinematic model parameters, and the speed difference and steering response difference are used to adjust the control weights or constraint margins. The updated results are then used to solve the control sequence for the next sampling period.

[0013] Furthermore, the position difference, heading difference, and speed difference corresponding to multiple consecutive sampling periods are arranged according to the sampling time, and the direction of change and average difference of each type of difference within a preset time window are determined. When the same type of difference maintains the same direction of change continuously within the preset time window and the average difference exceeds the corresponding model correction threshold, a parameter correction amount is generated based on the difference between the average difference and the model correction threshold, and the speed response parameter or steering response parameter in the vehicle kinematics model is updated using the parameter correction amount. When the average difference in two consecutive preset time windows does not exceed the corresponding model correction threshold, the updating of the corresponding vehicle kinematics model parameters is stopped, and the updated vehicle kinematics model parameters are used to generate the vehicle kinematics prediction result for the next sampling period.

[0014] Furthermore, the speed difference and steering response difference are compared with their corresponding feedback correction thresholds. When the speed difference exceeds the corresponding feedback correction threshold, the speed deviation weight is increased and the constraint margin between the vehicle speed boundary and the current actual driving speed is reduced; when the speed difference does not exceed the corresponding feedback correction threshold, the speed deviation weight and corresponding constraint margin are maintained. When the steering response difference exceeds the corresponding feedback correction threshold, the control quantity change weight is increased and the constraint margin between the steering angle boundary and the current actual steering angle is reduced; when the steering response difference does not exceed the corresponding feedback correction threshold, the control quantity change weight and corresponding constraint margin are maintained. When both the speed difference and the steering response difference exceed their corresponding feedback correction thresholds, the two adjustment results are used together to solve the control sequence for the next sampling period.

[0015] The present invention also provides a model prediction-based AGV trajectory tracking control system for executing the aforementioned model prediction-based AGV trajectory tracking control algorithm. The system includes an onboard controller, a lidar unit, a binocular camera, an inertial measurement unit, a wheel speed encoder, a steering angle sensor, a steering mechanism, and a drive motor.

[0016] The onboard controller determines the fused motion state and state reliability based on the differences between the positioning results output by the LiDAR, binocular camera, and inertial measurement unit (IMU) and the AGV kinematic prediction results, the presence of missing or timed positioning results, and the differences between the three types of positioning results. The onboard controller forms historical obstacle trajectories based on obstacle positions output by the LiDAR and binocular camera, constructs a time-varying feasible trajectory domain by combining a preset reference trajectory, AGV motion constraints, and obstacle occupancy range. It determines control weights and constraint boundaries based on state reliability and the distance from the obstacle occupancy range boundary to the preset reference trajectory, solves the control sequence within the time-varying feasible trajectory domain, and sends the current control quantity in the control sequence to the steering mechanism and travel motor. The onboard controller determines the actual motion state based on feedback from the wheel speed encoder and steering angle sensor, and updates the vehicle kinematic model parameters used to form the AGV kinematic prediction results, the control weights determined based on state reliability and the distance from the obstacle to the preset reference trajectory, or the constraint margin determined based on the distance between the constraint boundary and the actual motion state, based on the difference between the predicted and actual motion states.

[0017] Compared with the prior art, the advantages and positive effects of the present invention are as follows: In this invention, by collecting lidar positioning data, binocular vision positioning data, and inertial measurement data, and based on the difference between the positioning results and kinematic prediction results, data missing timeout states, and differences between positioning results, the fused motion state and state reliability are determined, allowing state quality to participate in the determination of control weights and constraint boundaries. An obstacle position sequence is generated based on the obstacle's historical trajectory, and a time-varying feasible trajectory domain is constructed by combining a preset reference trajectory, motion constraints, and obstacle occupancy range, ensuring that the control sequence corresponds to the temporal occupancy state. The actual motion state is determined based on feedback data from the wheel speed encoder and steering angle sensor, and the difference between the predicted and actual motion states is used to update the vehicle kinematic model parameters, control weights, or constraint margins, suppressing the accumulation of model bias. Attached Figure Description

[0018] Figure 1 This is a flowchart of the AGV trajectory tracking control based on model prediction according to the present invention; Figure 2 This is a data flow diagram for multi-source localization fusion and state reliability determination in this invention; Figure 3 This is a schematic diagram illustrating the construction of the obstacle position sequence and time-varying feasible trajectory domain in this invention. Figure 4 This is a state transition diagram for the control weight constraint adjustment and feedback update of the present invention; Figure 5 This is a schematic diagram of the hardware and software collaborative structure of the AGV trajectory tracking control system of the present invention. Detailed Implementation

[0019] The following embodiments are based on the protection scope text, the technical solutions described in the application documents, and the original disclosure materials, and are used to illustrate the feasible implementation of the model prediction-based AGV trajectory tracking control algorithm and system. Hereafter, AGV refers to AGV, and LiDAR positioning data, binocular vision positioning data, inertial measurement data, wheel speed encoder feedback data, and steering angle sensor feedback data refer to the data output by the corresponding sensors within the same operating cycle and aligned by the sampling time; the fused motion state includes the AGV's position, heading, and speed states; state reliability is used to characterize the degree of consistency between the multi-source positioning results and the vehicle kinematics prediction results; the time-varying feasible trajectory domain is used to characterize the range of positions, headings, and speeds that the AGV is allowed to reach at each prediction time within the model prediction time domain. The original disclosure materials did not assign numerical labels to physical components, therefore, this embodiment does not assign component labels separately. The step numbers in the subsequent figures are only used to identify the process and do not constitute a limitation on quantity, parameters, order, or protection scope.

[0020] Please see Figures 1 to 4 This embodiment provides a model prediction-based AGV trajectory tracking control algorithm, applied to the control process of an AGV running along a preset reference trajectory. The control algorithm takes synchronized multi-source positioning data, vehicle execution feedback data, the preset reference trajectory, and historical obstacle trajectories as inputs. Within each sampling period, it sequentially completes the following steps: determining the fused motion state and state reliability; generating the obstacle position sequence; constructing the time-varying feasible trajectory domain; determining the control weights and constraint boundaries; solving the control sequence; and updating the actual motion state feedback. The control quantity for this sampling period is then transmitted to the steering mechanism and the travel motor.

[0021] S1 acquires and synchronizes the data required for the control process.

[0022] S101: The LiDAR continuously outputs LiDAR positioning data, the binocular camera continuously outputs binocular visual positioning data, the inertial measurement unit continuously outputs inertial measurement data, the wheel speed encoder outputs wheel speed encoder feedback data, and the steering angle sensor outputs steering angle sensor feedback data. The preset reference trajectory consists of a sequence of trajectory points corresponding to the AGV's current running task, and the obstacle historical trajectory consists of the obstacle's position, direction of movement, and speed at continuous sampling times.

[0023] S102: Using the sampling time of the current sampling period as the alignment reference, the sampling times of the LiDAR positioning data, binocular vision positioning data, inertial measurement data, and wheel speed encoder feedback data are read respectively. When the sampling time of a type of positioning data corresponds to the current sampling period, this type of positioning data is included in the data set of the current period; when a type of positioning data does not generate an output within the current sampling period, or its corresponding sampling time cannot be aligned with the current sampling period, this type of positioning data is marked as missing or timed out. The steering angle sensor feedback data enters the actual motion state determination process according to the same sampling time.

[0024] S103: In the first sampling period, an initial fused motion state is formed using the currently available lidar positioning results, binocular vision positioning results, and inertial measurement results, with wheel speed encoder feedback data used as the basis for the initial velocity state. In subsequent sampling periods, the fused motion state formed at the previous sampling time is used as the initial state for vehicle kinematics prediction. If the obstacle historical trajectory does not meet the continuous historical condition, the currently available historical sequence is formed using the already obtained obstacle position, obstacle movement direction, and obstacle movement speed; after the continuous historical condition is met, the obstacle historical trajectory is continuously updated according to the sampling time.

[0025] S2, determine the fusion motion state and state reliability.

[0026] S201: Based on the fused motion state from the previous sampling time, the inertial measurement data from the current sampling time, and the wheel speed encoder feedback data, the vehicle kinematics prediction result for the current sampling time is generated through the vehicle kinematics model. The vehicle kinematics prediction result includes at least the predicted position, predicted heading, and predicted speed, and serves as a unified comparison benchmark for LiDAR positioning results, binocular vision positioning results, and inertial measurement positioning results. If there is no fused motion state from the previous sampling time in the first sampling period, the initial fused motion state formed in S103 is used as the input to the vehicle kinematics model.

[0027] S202, the lidar positioning results, binocular vision positioning results, and inertial measurement positioning results are compared with the vehicle kinematics prediction results, respectively. The position and heading differences between the lidar positioning results and the vehicle kinematics prediction results form the lidar observation residual; the position and heading differences between the binocular vision positioning results and the vehicle kinematics prediction results form the binocular vision observation residual; and the heading and velocity change differences between the inertial measurement positioning results and the vehicle kinematics prediction results form the inertial measurement observation residual. Simultaneously, corresponding results from the three types of positioning results that can characterize the same position, heading, or velocity state are compared to form the positioning result difference among the three types of positioning results.

[0028] S203: The observation residuals corresponding to the positioning data that are not missing or timed out are input into the extended Kalman filter process. The extended Kalman filter process first uses the vehicle kinematics prediction results as the prior state, and then corrects the predicted position, predicted heading, and predicted velocity according to the observation residuals between various positioning results and the prior state, forming the fused motion state at the current sampling time. Positioning data that is missing or timed out does not participate in the state correction of this sampling period, avoiding the entry of positioning results without corresponding sampling times into the fusion process of the current period.

[0029] S204: Based on the observation residuals for each type, the missing or timed-out status of each type of positioning data, and the difference in positioning results among the three types of positioning results, the state reliability at the current sampling time is formed. When the observation residuals increase, the consistency between the corresponding positioning result and the vehicle kinematics prediction result decreases; when the difference in positioning results increases, the consistency between multi-source positioning results decreases; when positioning data is missing or timed out, the corresponding positioning result no longer provides a basis for state correction in the current period. The state reliability is jointly determined by the above states and, together with the fused motion state at the current sampling time, is passed to the subsequent control weight and constraint boundary determination process. The state reliability is formed once in this sampling period; subsequent steps only call this result and do not generate it repeatedly.

[0030] S3 generates an obstacle position sequence based on the obstacle's historical trajectory and constructs a time-varying feasible trajectory domain.

[0031] S301, arranges the obstacle position, obstacle movement direction and obstacle movement speed corresponding to the continuous sampling time in the obstacle's historical trajectory according to the sampling time, so that the movement state of the same obstacle at different sampling times maintains the time sequence.

[0032] S302, the historical obstacle trajectories arranged according to sampling time are input into the trained Long Short-Term Memory (LSTM) network model. The historical obstacle trajectories are first time-aligned according to the sampling period predicted and controlled by the model, and missing trajectory points are interpolated for compensation. Abnormal trajectory jumps are removed or smoothed. Then, using the current AGV pose or a preset reference trajectory coordinate system as a reference, the obstacle's position in the global coordinate system is converted into longitudinal distance, lateral distance, relative velocity, relative motion direction, and relative acceleration relative to the AGV's travel direction, forming an obstacle motion feature sequence. The LTM network model takes the obstacle motion feature sequence from multiple consecutive sampling times as input, and learns the trend of obstacle position change, velocity change, and motion direction in the AGV's travel scenario. The model extracts temporal-related features from the historical motion states of obstacles and combines them with the current positional relationship between the obstacle and the AGV and the preset reference trajectory to recursively predict the obstacle position at each prediction time within the model's prediction time domain. During the prediction process, motion continuity constraints are applied to the predicted changes in obstacle velocity, direction, and position to ensure that the obstacle positions at adjacent prediction times meet the preset maximum speed, maximum acceleration, and maximum turning change range, avoiding abrupt changes in predicted position due to trajectory noise. Finally, the model generates the predicted obstacle position for each prediction time within the model's prediction time domain and constructs an obstacle position sequence in chronological order. The prediction reliability of the obstacle position sequence is determined based on the completeness of the obstacle's historical trajectory, the continuity of the predicted position, and the prediction residual.

[0033] S303: Generate obstacle occupancy ranges based on obstacle positions and outlines at each prediction time. The obstacle occupancy range represents the area occupied by obstacles in space at the corresponding prediction time. Spatially expand the obstacle occupancy ranges according to the AGV outlines, ensuring that the expanded ranges simultaneously reflect the spatial relationship between the obstacle outlines and the AGV outlines. This spatial expansion process is performed separately at each prediction time; the obstacle occupancy range at one prediction time is not directly used to replace the obstacle occupancy ranges at other prediction times.

[0034] S304: The obstacle-occupied area after spatial expansion at each prediction time is compared with the corresponding reference trajectory point in the preset reference trajectory. Position, heading, and velocity ranges that overlap with the obstacle-occupied area after spatial expansion are not included in the feasible range for that prediction time; position, heading, and velocity ranges that do not overlap and meet the vehicle motion constraints are retained as feasible ranges for that prediction time. The feasible ranges for each prediction time within the model prediction time domain are arranged in chronological order to form a time-varying feasible trajectory domain, which is then passed to the control sequence solution process.

[0035] S4, determine the control weights and constraint boundaries.

[0036] S401, according to the prediction time within the model prediction time domain, determine the distance between the boundary of the spatially expanded obstacle occupancy area and the corresponding reference trajectory point in the preset reference trajectory. If multiple obstacle occupancy area boundaries exist at the same prediction time, compare the corresponding distances one by one, and take the smallest distance as the distance from the obstacle to the preset reference trajectory at that prediction time. The distances at each prediction time within the model prediction time domain are used to characterize the proximity of the obstacle occupancy area to the reference trajectory.

[0037] S402 uses preset position deviation weights, heading deviation weights, speed deviation weights, and control variable change weights as basic control weights. The position deviation weight corresponds to the deviation between the predicted position and the reference position; the heading deviation weight corresponds to the deviation between the predicted heading and the reference heading; the speed deviation weight corresponds to the deviation between the predicted speed and the reference speed; and the control variable change weight corresponds to the change between adjacent candidate control variables. These basic control weights serve as the starting point for adjusting the control weights in this sampling period.

[0038] S403, compare the state confidence level with the confidence level threshold. When the state confidence level is lower than the confidence level threshold, increase the control variable change weight and decrease the vehicle speed boundary, so that the control sequence solution process imposes stronger constraints on the changes between adjacent control variables and limits the vehicle speed range in the low confidence state; when the state confidence level is not lower than the confidence level threshold, maintain the preset values ​​corresponding to the control variable change weight and the vehicle speed boundary. The confidence level threshold is determined by the sensor state determination conditions used in the AGV control process and is not extended to a new numerical limit in this embodiment.

[0039] S404 compares the distance from the obstacle to the preset reference trajectory with a safe distance threshold. When the distance is less than the safe distance threshold, the obstacle's occupancy boundary is expanded and the speed deviation weight is increased, so that the control sequence solution process uses a tightened spatial range and speed deviation constraint when the obstacle approaches the reference trajectory; when the distance is not less than the safe distance threshold, the preset values ​​corresponding to the obstacle's occupancy boundary and speed deviation weight are maintained. The safe distance threshold is determined by the allowable distance conditions between the preset reference trajectory, the AGV profile, and the obstacle's occupancy range.

[0040] S405: When both the state confidence comparison result and the obstacle distance comparison result trigger adjustment, compare the restriction levels of the vehicle speed boundary and the obstacle occupancy range boundary before and after adjustment, and adopt the vehicle speed boundary and obstacle occupancy range boundary with the higher restriction level. The adjusted position deviation weight, heading deviation weight, speed deviation weight, and control variable change weight are determined as control weights, and the adjusted vehicle speed boundary and obstacle occupancy range boundary are determined as constraint boundaries. The control weights and constraint boundaries are then passed to the control sequence solution process.

[0041] S5 solves the control sequence in the time-varying feasible trajectory domain and executes the current control quantity.

[0042] S501, the fused motion state at the current sampling time is used as the initial motion state in the model's prediction time domain. The corresponding position range, heading range, and velocity range are read from the time-varying feasible trajectory domain according to the prediction time. Reference position, reference heading, and reference velocity corresponding to each prediction time are determined from the preset reference trajectory. The reference position, reference heading, and reference velocity are all located within the time-varying feasible trajectory domain of the corresponding prediction time.

[0043] S502 generates the predicted motion state time-by-time based on the vehicle kinematics model and candidate control variables. The candidate control variables include the steering control variable corresponding to the steering mechanism and the driving control variable corresponding to the travel motor. The predicted motion state at the current prediction time is jointly determined by the predicted motion state at the previous prediction time and the current candidate control variables, and sequentially forms the predicted position, predicted heading, and predicted speed in the model prediction time domain.

[0044] S503, compare the predicted position with the reference position, the predicted heading with the reference heading, and the predicted speed with the reference speed at each prediction time to form position deviation, heading deviation, and speed deviation; compare the candidate control variables at adjacent prediction times to form control variable changes. Weight the position deviation, heading deviation, speed deviation, and control variable changes according to control weights, and then summarize them in the order of prediction times to form the weighted results corresponding to the candidate control variable combinations.

[0045] S504, Constraint verification is performed on candidate control variable combinations. The predicted motion state generated by the candidate control variable combination should be within the time-varying feasible trajectory domain at the corresponding prediction time, the vehicle speed should be within the vehicle speed boundary, the predicted position should not enter the boundary of the obstacle occupancy area, and the changes between candidate control variables and adjacent candidate control variables should conform to the vehicle motion constraints. Candidate control variable combinations that do not meet any of the constraints are not included in the weighted result comparison.

[0046] S505: Among the candidate control quantity combinations that satisfy the constraint boundary and vehicle motion constraints, the weighted results corresponding to each candidate control quantity combination are compared, and the candidate control quantity combination with the smallest weighted result is selected to form a control sequence. The control sequence is arranged according to the prediction time in the model prediction time domain. The on-board controller only sends the steering control quantity corresponding to the current sampling time in the control sequence to the steering mechanism, and sends the driving control quantity corresponding to the current sampling time to the driving motor. The control quantities of the remaining prediction times are retained for prediction in the current sampling period and resolving in the next sampling period.

[0047] S6 determines the actual motion state based on the execution feedback and updates the control basis for subsequent sampling cycles.

[0048] S601 reads the wheel speed encoder feedback data from the current sampling time and the previous sampling time, and determines the current actual driving speed based on the wheel speed feedback from the two sampling times; it also reads the steering angle sensor feedback data from the current sampling time and the previous sampling time, and determines the current actual steering angle and the amount of steering angle change based on the steering angle feedback from the two sampling times. The actual driving speed, actual steering angle, and steering angle change are then correlated with the fused motion state at the current sampling time to form the actual motion state at the current sampling time.

[0049] S602: Extract the predicted motion state corresponding to the current sampling time from the model prediction time domain, and compare the predicted motion state with the actual motion state. The difference between the predicted position and the actual position forms the position difference; the difference between the predicted heading and the actual heading forms the heading difference; the difference between the predicted speed and the actual speed forms the speed difference; and the difference between the predicted steering response and the actual steering angle and the change in steering angle forms the steering response difference. The position difference, heading difference, speed difference, and steering response difference together form the prediction error data.

[0050] S603: Arrange the position difference, heading difference, and speed difference corresponding to multiple consecutive sampling periods according to the sampling time, and determine the direction of change and average difference of each type of difference within a preset time window. When the same type of difference maintains the same direction of change continuously within the preset time window and the average difference exceeds the corresponding model correction threshold, generate a parameter correction amount based on the difference between the average difference and the model correction threshold, and use the parameter correction amount to update the speed response parameters or steering response parameters in the vehicle kinematics model. The updated vehicle kinematics model parameters are used to generate vehicle kinematics prediction results in S201 of the next sampling period.

[0051] S604: When the average difference between position, heading, or speed within two consecutive preset time windows does not exceed the corresponding model correction threshold, the update of the corresponding vehicle kinematics model parameters is stopped, the existing speed response or steering response parameters are retained, and the retained vehicle kinematics model parameters are used in the next sampling period. The model correction threshold is used to distinguish between persistent prediction deviations and differences that do not meet the parameter update conditions; its value is determined by the vehicle control conditions supported by the original disclosure.

[0052] In step S605, the speed difference is compared with the corresponding feedback correction threshold. When the speed difference exceeds the corresponding feedback correction threshold, the speed deviation weight is increased, and the constraint margin between the vehicle speed boundary and the current actual driving speed is reduced. When the speed difference does not exceed the corresponding feedback correction threshold, the speed deviation weight and the corresponding constraint margin are maintained. The adjusted speed deviation weight and constraint margin are used as the basis for determining the control weight and constraint boundary in S4 and S5 of the next sampling period.

[0053] In step S606, the steering response difference is compared with the corresponding feedback correction threshold. When the steering response difference exceeds the corresponding feedback correction threshold, the control variable change weight is increased, and the constraint margin between the steering angle boundary and the current actual steering angle is reduced. When the steering response difference does not exceed the corresponding feedback correction threshold, the control variable change weight and the corresponding constraint margin are maintained. When both the speed difference and the steering response difference exceed the corresponding feedback correction threshold, both adjustment results are jointly passed to the control sequence solution process of the next sampling cycle. Therefore, the next sampling cycle simultaneously receives the updated vehicle kinematic model parameters, control weights, and constraint margins, and re-executes steps S1 to S6.

[0054] Please see Figure 1 and Figure 5 This embodiment provides a model-predictive AGV trajectory tracking control system for executing the aforementioned model-predictive AGV trajectory tracking control algorithm. The system includes an onboard controller, a lidar sensor, a binocular camera, an inertial measurement unit, a wheel speed encoder, a steering angle sensor, a steering mechanism, and a drive motor. These components form a closed loop based on the relationship between sensor input, onboard processing, control output, and execution feedback.

[0055] The LiDAR system outputs LiDAR positioning data and obstacle positions to the vehicle controller. The binocular camera system outputs binocular visual positioning data and obstacle positions to the vehicle controller. The inertial measurement unit (IMU) outputs inertial measurement data to the vehicle controller. The wheel speed encoder outputs wheel speed encoder feedback data to the vehicle controller. The steering angle sensor outputs steering angle sensor feedback data to the vehicle controller. The vehicle controller receives a preset reference trajectory and forms a historical obstacle trajectory based on the obstacle positions obtained by the LiDAR and binocular camera system at continuous sampling times.

[0056] The onboard controller determines the fused motion state and state reliability based on the differences between the positioning results output by the LiDAR, binocular camera, and inertial measurement unit and the AGV kinematic prediction results, the presence of missing or timed-out positioning results, and the differences between the three types of positioning results. The onboard controller generates an obstacle position sequence based on the obstacle's historical trajectory, and constructs a time-varying feasible trajectory domain by combining a preset reference trajectory, AGV motion constraints, and obstacle occupancy range. It then determines the control weights and constraint boundaries based on the state reliability and the distance from the obstacle occupancy range boundary to the preset reference trajectory.

[0057] The onboard controller solves the control sequence within the time-varying feasible trajectory domain, sending the current steering control quantity to the steering mechanism and the current driving control quantity to the drive motor. After the steering mechanism executes the current steering control quantity, the steering angle sensor returns the actual steering angle and the feedback data corresponding to the steering angle change to the onboard controller; after the drive motor executes the current driving control quantity, the wheel speed encoder returns the feedback data corresponding to the actual wheel speed to the onboard controller. Based on this, the onboard controller determines the actual motion state and updates the vehicle kinematic model parameters, control weights, or constraint margins used in subsequent sampling periods based on the position difference, heading difference, speed difference, and steering response difference between the predicted and actual motion states.

[0058] During the system's operation as industrial control software, sensing input, state fusion, obstacle prediction, construction of time-varying feasible trajectory domain, control sequence solving, actuator actions, and execution feedback are repeated according to the sampling period. The state reliability is generated only from the observation residual of the current sampling period, the missing or timed-out state of positioning data, and the difference in positioning results; subsequent control processes call the same state reliability. The obstacle position sequence is generated only from the extension of the obstacle's historical trajectory within the model prediction time domain. The actual motion state is formed only from the feedback data of the wheel speed encoder, the feedback data of the steering angle sensor, and the current fused motion state, thus maintaining consistency in the source, processing location, and subsequent use of each data object.

[0059] The above-described embodiments, including their steps, data field states, processing order, judgment conditions, parameter sources, system collaboration, and execution feedback relationships, are used to illustrate the possible implementations of the present invention and should not be limited to the specific embodiments listed. Without departing from the technical solutions described in this invention and the scope of the original disclosure, any equivalent substitutions, modifications, combinations, order adjustments, module replacements, equivalent transformations of field names, equivalent acceptance of the execution subject, or equivalent changes in the carrier form that can be conceived by those skilled in the art should fall within the scope of protection of this patent; however, they should not be extended to unclaimed topics, nor should they alter the substantive correspondence of the technical objects through changes in reference numerals. Equivalent changes must not replace, delete, or weaken the method features, physical components, control relationships, data relationships, temporal relationships, parameter relationships, or physical action relationships necessary to produce the technical effect.

Claims

1. An AGV trajectory tracking control algorithm based on model prediction, characterized in that, Includes the following steps: Acquire synchronized lidar positioning data, binocular vision positioning data, inertial measurement data, wheel speed encoder feedback data, steering angle sensor feedback data, preset reference trajectory, and historical obstacle trajectory; Based on the error between the fused positioning result formed by the three types of data—LiDAR positioning data, binocular vision positioning data, and inertial measurement data—and the vehicle state prediction result obtained based on the vehicle kinematics model, the missing or timed-out states of the three types of data, and the deviation between the three types of positioning results, the fused motion state and the state reliability are determined. An obstacle location sequence is generated based on the obstacle's historical trajectory, and a time-varying feasible trajectory domain is constructed by combining the preset reference trajectory, preset vehicle motion constraints, and preset obstacle occupancy range. The control weights and constraint boundaries are determined based on the state confidence level and the distance from the boundary of the obstacle occupancy area to the preset reference trajectory, and the control sequence is solved in the time-varying feasible trajectory domain. The control quantities in the control sequence are sent to the steering mechanism and the travel motor. The actual motion state is determined based on the feedback data from the wheel speed encoder and the feedback data from the steering angle sensor. The predicted motion state is obtained and the difference between the predicted motion state and the actual motion state is calculated. The vehicle kinematic model parameters used to generate the predicted motion state are updated, as well as the control weights determined based on the state confidence level and the distance from the obstacle to the preset reference trajectory, or the constraint margin determined based on the distance between the constraint boundary and the actual motion state.

2. The AGV trajectory tracking control algorithm based on model prediction according to claim 1, characterized in that, The process of determining the fused motion state and the state confidence level includes: The lidar positioning data, binocular vision positioning data, inertial measurement data, and wheel speed encoder feedback data are aligned according to the sampling time. Based on the fused motion state at the previous sampling time, the inertial measurement data at the current sampling time, and the wheel speed encoder feedback data, a vehicle kinematics prediction result is generated through a vehicle kinematics model. The observation residuals between the lidar positioning results, binocular vision positioning results, and inertial measurement positioning results and the vehicle kinematics prediction results are calculated respectively, and the positioning result differences between the three types of positioning results are also calculated. By using extended Kalman filtering, the vehicle kinematic prediction results are corrected based on the observation residuals corresponding to the positioning data that have not been missing or timed out, to obtain the fused motion state at the current sampling time. Based on each observation residual, the missing or timed out state of each positioning data, and the difference in the positioning results, a state confidence level corresponding to the fused motion state at the current sampling time is generated.

3. The AGV trajectory tracking control algorithm based on model prediction according to claim 1, characterized in that, The process of generating the obstacle position sequence and constructing the time-varying feasible trajectory domain based on the obstacle's historical trajectory includes: The obstacle position, obstacle direction of movement, and obstacle speed corresponding to consecutive sampling times in the obstacle historical trajectory are arranged according to the sampling time. The temporal change features of the obstacle historical trajectory are extracted through a long short-term memory network, and the obstacle position corresponding to each prediction time in the model prediction time domain is generated. The obstacle occupancy range is generated based on the obstacle position and obstacle outline corresponding to each prediction time, and the obstacle occupancy range is spatially expanded based on the AGV outline; The obstacle-occupied area after spatial expansion is compared with the preset reference trajectory time by time. Within the vehicle motion constraint range, the position range, heading range and speed range that do not overlap with the obstacle-occupied area after spatial expansion are retained and arranged according to the predicted time to form a time-varying feasible trajectory domain.

4. The AGV trajectory tracking control algorithm based on model prediction according to claim 1, characterized in that, The process of determining the control weights and the constraint boundaries includes: According to the predicted time in the model prediction time domain, calculate the distance between the boundary of the obstacle's occupied area and the corresponding reference trajectory point in the preset reference trajectory, and determine the minimum value of each distance as the distance from the obstacle to the preset reference trajectory; Using preset position deviation weights, heading deviation weights, speed deviation weights, and control quantity change weights as basic control weights, the control quantity change weights and vehicle speed boundaries are adjusted according to the state reliability. Based on the distance from the obstacle to the preset reference trajectory, the position deviation weight, the speed deviation weight, and the obstacle occupancy boundary are adjusted, and the adjusted basic control weight is determined as the control weight, and the adjusted vehicle speed boundary and obstacle occupancy boundary are determined as the constraint boundary.

5. The AGV trajectory tracking control algorithm based on model prediction according to claim 4, characterized in that, The process of adjusting the basic control weights and the constraint boundaries includes: The state confidence level is compared with a confidence threshold. When the state confidence level is lower than the confidence threshold, the control quantity change weight is increased and the vehicle speed boundary is reduced. When the state confidence level is not lower than the confidence threshold, the preset values ​​corresponding to the control quantity change weight and the vehicle speed boundary are maintained. The distance from the obstacle to the preset reference trajectory is compared with a safe distance threshold. When the distance from the obstacle to the preset reference trajectory is less than the safe distance threshold, the boundary of the obstacle's occupied area is expanded and the speed deviation weight is increased. When the distance from the obstacle to the preset reference trajectory is not less than the safe distance threshold, the preset values ​​corresponding to the boundary of the obstacle's occupied area and the speed deviation weight are maintained. When both comparison results trigger adjustments, the adjusted vehicle speed boundary and obstacle occupancy boundary, which have a higher degree of restriction, are adopted respectively.

6. The AGV trajectory tracking control algorithm based on model prediction according to claim 1, characterized in that, The process of solving the control sequence in the time-varying feasible trajectory domain includes: The fused motion state is used as the initial motion state in the model prediction time domain, and the corresponding reference position, reference heading, and reference velocity are determined from the time-varying feasible trajectory domain according to the prediction time. Based on the vehicle kinematics model and candidate control variables, predictive motion states are generated time-by-time. The position deviation, heading deviation, and speed deviation between the predicted motion states and the reference position, reference heading, and reference speed are calculated respectively. The control variable changes between adjacent candidate control variables are also calculated. The position deviation, heading deviation, speed deviation, and control quantity change are weighted according to the control weights. Within the constraint boundaries and vehicle motion constraints, the candidate control quantity combination with the smallest weighted result is selected to form a control sequence. The control quantity corresponding to the current sampling time in the control sequence is sent to the steering mechanism and the travel motor.

7. The AGV trajectory tracking control algorithm based on model prediction according to claim 1, characterized in that, The process of determining the actual motion state and updating the feedback based on the wheel speed encoder feedback data and the steering angle sensor feedback data includes: The actual driving speed is determined based on the wheel speed encoder feedback data at the current sampling time and the previous sampling time. The actual steering angle and the amount of change in steering angle are determined based on the steering angle sensor feedback data at the current sampling time and the previous sampling time. The actual motion state is formed based on the actual driving speed, the actual steering angle and the amount of change in steering angle. Extract the predicted motion state corresponding to the current sampling time from the model prediction time domain, and calculate the position difference, heading difference, velocity difference and steering response difference between the predicted motion state and the actual motion state to form prediction error data; According to the error type corresponding to the predicted error data, the position difference, heading difference, and speed difference are used to update the vehicle kinematic model parameters, the speed difference and steering response difference are used to adjust the control weights or constraint margins, and the update results are used to solve the control sequence for the next sampling period.

8. The AGV trajectory tracking control algorithm based on model prediction according to claim 7, characterized in that, The process of updating the vehicle kinematic model parameters includes: Arrange the position difference, heading difference, and velocity difference corresponding to multiple consecutive sampling periods according to the sampling time, and determine the direction of change and average difference of each type of difference within a preset time window; When the same type of difference continuously maintains the same direction of change within the preset time window and the average difference exceeds the corresponding model correction threshold, a parameter correction amount is generated based on the difference between the average difference and the model correction threshold, and the speed response parameter or steering response parameter in the vehicle kinematics model is updated using the parameter correction amount. When the average difference within two consecutive preset time windows does not exceed the corresponding model correction threshold, the corresponding vehicle kinematics model parameters are stopped from being updated, and the updated vehicle kinematics model parameters are used to generate the vehicle kinematics prediction results for the next sampling period.

9. The AGV trajectory tracking control algorithm based on model prediction according to claim 7, characterized in that, The process of adjusting the control weights and the constraint margins includes: The speed difference and steering response difference are compared with their respective feedback correction thresholds; When the speed difference exceeds the corresponding feedback correction threshold, the speed deviation weight is increased and the constraint margin between the vehicle speed boundary and the current actual driving speed is reduced; when the speed difference does not exceed the corresponding feedback correction threshold, the speed deviation weight and the corresponding constraint margin are maintained. When the steering response difference exceeds the corresponding feedback correction threshold, the control change weight is increased and the constraint margin between the steering angle boundary and the current actual steering angle is reduced. When the steering response difference does not exceed the corresponding feedback correction threshold, the control change weight and the corresponding constraint margin are maintained. When both the speed difference and the steering response difference exceed the corresponding feedback correction threshold, the two adjustment results are used together to solve the control sequence for the next sampling period.

10. A model-predictive AGV trajectory tracking control system, characterized in that, The AGV trajectory tracking control algorithm based on model prediction according to any one of claims 1-9, the system includes an on-board controller, a lidar, a binocular camera, an inertial measurement unit, a wheel speed encoder, a steering angle sensor, a steering mechanism, and a travel motor; The vehicle controller determines the fused motion state and state reliability based on the difference between the positioning results output by the lidar, the binocular camera, and the inertial measurement unit and the AGV kinematic prediction results, the missing or timed positioning results, and the differences between the three types of positioning results. The vehicle controller forms a historical trajectory of obstacles based on the obstacle positions output by the lidar and the binocular camera device, constructs a time-varying feasible trajectory domain by combining a preset reference trajectory, AGV motion constraints, and obstacle occupancy range, determines control weights and constraint boundaries based on the state reliability and the distance from the obstacle occupancy range boundary to the preset reference trajectory, solves the control sequence within the time-varying feasible trajectory domain, and sends the current control quantity in the control sequence to the steering mechanism and the walking motor. The vehicle controller determines the actual motion state based on feedback from the wheel speed encoder and the steering angle sensor, obtains the predicted motion state and calculates the difference between the predicted motion state and the actual motion state, updates the vehicle kinematic model parameters used to form the AGV kinematic prediction results, the control weights determined based on the state confidence level and the distance from the obstacle to the preset reference trajectory, or the constraint margin determined based on the distance between the constraint boundary and the actual motion state.

Images