MPC-based AGV adaptive path tracking method

Abstract

The invention relates to an automatic guided vehicle (AGV) adaptive path tracking method based on MPC. The method comprises the following steps: S1, establishing a kinematic discretization error model based on the kinematic characteristics of the two-wheel differential AGV, processing a continuous kinematic equation by adopting an Euler discretization method, expressing a dynamic change relationship between a transverse deviation distance and an angle deviation in a state-space equation form, and generating a kinematic discrete state-space model; the method has the advantages that the continuous equation is processed by establishing the kinematics discretization error model and adopting the Euler discretization method, the deviation dynamic relation is expressed through the state space, the calculation process is simplified, the precision is kept, sensor data are fused, the wheel type odometer, IMU and laser data are integrated through the extended Kalman filtering algorithm, and the precision is improved. The real-time position and angle deviation are calculated, the positioning accuracy is improved, a model prediction controller objective function is designed, a weight matrix and boundary constraint are introduced according to constraint conditions, and the effect of obstacle avoidance constraint is combined.

Description

Technical Field

[0001] This application relates to the field of AGV motion control technology, specifically to an AGV adaptive path tracking method based on MPC. Background Technology

[0002] The field of AGV motion control technology focuses on the motion management and execution mechanisms of automated guided vehicles, involving path tracking, navigation systems, obstacle avoidance strategies, and dynamic environmental adaptability. The core objective is to ensure that AGVs operate efficiently and accurately on predetermined paths, while handling real-time disturbances such as obstacles or path deviations. Commonly used technologies include traditional control algorithms such as PID and modern optimization methods such as model predictive control to improve handling efficiency, safety, and robustness. Among them, an AGV adaptive path tracking method based on MPC refers to using model predictive control algorithms to achieve adaptive adjustments of AGVs during path tracking. By simplifying the kinematic model to reduce computing power requirements and dynamically optimizing control weights, it is used to achieve high-precision path tracking and autonomous obstacle avoidance in complex environments, generating smooth local paths to improve the operational efficiency and reliability of AGVs.

[0003] Current AGV motion control technologies rely on fixed-weight model predictive control, lacking a dynamic mechanism for weight matrix adjustment. This leads to lag in response to dynamic environments such as sudden obstacle appearances, hindering timely obstacle avoidance and increasing collision risk. The system constraint handling employs rigid boundary methods without relaxation mechanisms. When environmental constraints conflict, the optimization problem becomes unsolvable, forcing the AGV to stop or interrupt its path, reducing operational continuity. The trajectory generation process does not incorporate forward integral prediction of the real-time control sequence, relying solely on static path planning. This results in locally broken or uneven paths, increasing AGV mechanical wear and energy consumption. Furthermore, delayed pose update feedback in closed-loop control, insufficient sensor data fusion efficiency, and accumulated errors in pose deviation calculation all negatively impact tracking accuracy.

[0004] Application content

[0005] To address the shortcomings of existing technologies, this application provides an AGV adaptive path tracking method based on MPC. This method solves the problems of existing technologies that rely on model predictive control with fixed weights in AGV motion control. The lack of a dynamic mechanism for adjusting the weight matrix leads to delayed response in dynamic environments such as the appearance of sudden obstacles, making it impossible to prioritize obstacle avoidance in a timely manner and increasing the risk of collision. Furthermore, the system constraint processing adopts a rigid boundary method without introducing a relaxation mechanism. When environmental constraints conflict, the optimization problem may become unsolvable, forcing the AGV to stop or interrupt the path, thus reducing the continuity of operation.

[0006] To achieve the above objectives, this application provides the following technical solution: an AGV adaptive path tracking method based on MPC, comprising the following steps;

[0007] S1: Based on the motion characteristics of a two-wheel differential AGV, a kinematic discretization error model is established. The Euler discretization method is used to process the continuous kinematic equations, and the lateral deviation distance is expressed in state-space equation form. and angle deviation The dynamic relationship of change is used to generate a kinematic discrete state-space model;

[0008] S2: Based on the kinematic discrete state space model, the extended Kalman filter algorithm is used to fuse data from wheel odometer, IMU and laser odometer, and the pose transformation matrix is ​​used to calculate the real-time position deviation and angle deviation of the AGV relative to the current reference path to generate global pose and path deviation data.

[0009] S3: Based on the global pose and path deviation data, design the objective function and constraints of the model prediction controller. The objective function incorporates a state error weight matrix. and control weight matrix The constraints include system control variable boundaries and environmental obstacle avoidance constraints. The constraint conflicts are handled by the slack variable method to generate the MPC optimization problem framework.

[0010] S4: Based on the aforementioned MPC optimization problem framework, an adaptive weight adjustment strategy is adopted to dynamically update... When the lidar detects an obstacle, reduce Weight and increase Weighting prioritizes obstacle avoidance; increases weight when there are no obstacles. Weights are used to enhance tracking accuracy, and the optimal control sequence is solved in real time using a quadratic programming solver to generate an adaptive control quantity sequence.

[0011] S5: Based on the adaptive control sequence, a local reference trajectory is generated using the forward integration method. Taking the current AGV pose as the initial state, the control sequence is input into the kinematic discrete state-space model to predict the future... The set of AGV poses for each step is used to generate a local reference trajectory;

[0012] S6: Based on the local reference trajectory, design a trajectory tracking controller, use a linear quadratic regulator algorithm to calculate the tracking control quantity, verify the convergence of the tracking error through Lyapunov stability analysis, and generate real-time tracking control commands;

[0013] S7: Based on the real-time tracking control command, execute AGV drive control, adjust wheel speed through motor encoder feedback closed loop, and feed back actual posture data to S2 for the next cycle deviation calculation to achieve closed loop control and generate AGV motion state.

[0014] Preferably, the S1-based generative kinematic discrete state-space model includes the following steps:

[0015] S101: Based on the motion characteristics of a two-wheel differential AGV, the continuous kinematics equation modeling method is used to define the position and heading angle. The differential relationship is obtained, and the basic equations are derived through the Newton-Euler formula to generate a continuous kinematic model;

[0016] S102: Based on the aforementioned continuous kinematics model, the Euler discretization method is used to discretize the period. The state variables are discretized over time to generate a discretized kinematic model;

[0017] S103: Based on the discretized kinematic model, the lateral deviation distance is calculated using the error state definition method. and angle deviation The mathematical expression is used to generate the error state equation;

[0018] S104: Based on the aforementioned error state equation, a state-space reconstruction method is used to construct a system... Discrete state-space equations for state vectors Generate a discrete state-space model of kinematics.

[0019] Preferably, the generation of global pose and path deviation data based on S2 includes the following steps:

[0020] S201: Employs a multi-sensor synchronous acquisition method, using a wheeled odometer to acquire displacement data, an IMU to acquire attitude data, and a laser odometer to acquire environmental point cloud data, generating a raw sensor dataset.

[0021] S202: Based on the original sensor dataset, the extended Kalman filter algorithm is used to perform data fusion, estimate the global pose of the AGV, and generate the fused global pose;

[0022] S203: Based on the fused global pose and predefined reference path, the position deviation is solved using the geometric deviation calculation method. and angle deviation Generate global pose and path deviation data.

[0023] Preferably, the S3-based generative MPC optimization problem framework includes the following steps:

[0024] S301: Based on the global pose and path deviation data, a quadratic objective function design method is used to define the state error weight matrix. and control weight matrix Construct the objective function Generate the objective function framework;

[0025] S302: Based on the physical limitations of the AGV, the boundary constraint method is used to set the speed. and front wheel cornering The upper and lower bound constraints are used to generate system boundary constraints;

[0026] S303: Based on obstacle detection results, a geometric obstacle avoidance constraint method is used, with the addition of distance constraints for circular obstacles. Generate environmental obstacle avoidance constraints;

[0027] S304: Based on the aforementioned system boundary constraints and environmental obstacle avoidance constraints, slack variables are added using the slack variable introduction method. and penalty items To obtain the objective function, avoid unsolvable cases and generate a relaxed processing framework;

[0028] S305: Based on the objective function framework, system boundary constraints, environmental obstacle avoidance constraints, and relaxation processing framework, the optimization problem integration method is adopted to form a complete optimal control problem and generate the MPC optimization problem framework.

[0029] Preferably, generating the adaptive control input sequence based on S4 includes the following steps:

[0030] S401: Employs LiDAR obstacle detection to output environmental complexity scores in real time and generate environmental status indicators;

[0031] S402: Based on the aforementioned environmental state indicators and the MPC optimization problem framework, a dynamic weight adjustment strategy is adopted: when the score is high, the weight is reduced. And increase Prioritize obstacle avoidance; increase when score is low. and reduce To enhance tracking, the optimal control sequence is calculated using a quadratic programming solver. Generate an adaptive control input sequence.

[0032] Preferably, the generation of a local reference trajectory based on S5 includes the following steps:

[0033] S501: Based on the current AGV pose, the state initialization method is used to set the predicted initial state. Generate the initial predicted state;

[0034] S502: Based on the initial predicted state and the adaptive control input sequence, the forward integral iterative method is used to progressively calculate the future... The pose of each step is used to generate a pose prediction sequence;

[0035] S503: Based on the pose prediction sequence, the discrete trajectory generation method is used to output the path point set and generate the discrete trajectory point set;

[0036] S504: Based on the discrete trajectory point set, a smooth and continuous path is constructed using cubic spline interpolation to generate a smooth and continuous trajectory;

[0037] S505: Based on the smooth and continuous trajectory and environmental obstacle avoidance constraints, the trajectory feasibility verification method is used to check whether the obstacle avoidance requirements are met and generate a feasible trajectory.

[0038] S506: Based on the feasible trajectory, the reference trajectory output method is used to generate the final local path and generate a local reference trajectory.

[0039] Preferably, generating real-time tracking control commands based on S6 includes the following steps:

[0040] S601: Based on the local reference trajectory and the current pose deviation, a tracking controller is designed using a linear quadratic regulator algorithm, and the error convergence is ensured by Lyapunov stability analysis to generate real-time tracking control commands.

[0041] Preferably, generating the AGV motion state based on S7 includes the following steps:

[0042] S701: Based on the real-time tracking control command, the motor drive control method is adopted to output the wheel speed command to the AGV actuator and generate the wheel speed control signal;

[0043] S702: Based on the wheel speed control signal, the encoder feedback acquisition method is used to obtain the actual wheel speed and displacement data and generate actual motion feedback;

[0044] S703: Based on the actual motion feedback, a pose update algorithm is used to calculate the new global pose of the AGV and generate updated pose data;

[0045] S704: Based on the updated pose data, it is fed back to S203 for path deviation calculation in the next control cycle, generating the AGV motion state.

[0046] In summary, this application includes at least one of the following beneficial technical effects:

[0047] By establishing a kinematic discretization error model and employing the Euler discretization method to handle continuous equations, the dynamic relationship of deviations is expressed through state space, simplifying the calculation process while maintaining accuracy. Sensor data is fused using an extended Kalman filter algorithm to integrate data from wheel odometers, IMUs, and lasers to calculate real-time position and angle deviations, improving positioning accuracy. A model predictive controller objective function and constraints are designed, incorporating weight matrices and boundary constraints. Combined with obstacle avoidance constraints, a relaxation variable method is used to handle conflicts, enhancing system robustness. The weight matrix is ​​dynamically updated based on environmental detection results, adjusting state errors and control variable weights to prioritize obstacle avoidance or enhance tracking. An optimal sequence is generated through a quadratic programming solver, achieving self-control. To achieve adaptive response, a local reference trajectory is generated, and forward integration is used to predict future poses, ensuring path smoothness. A trajectory tracking controller is designed, and a linear quadratic regulator algorithm is used to verify stability and ensure error convergence. The drive control is executed by adjusting wheel speed and updating pose through closed-loop feedback, forming a continuous optimization loop. The overall processing logic simplifies the model to closed-loop control, reduces computational requirements through discretization, optimizes environmental adaptability through adaptive weights, enhances constraint feasibility through relaxation variables, improves trajectory continuity through forward integration, ensures control reliability through stability analysis, and maintains accuracy persistence through closed-loop feedback. This enables high-precision tracking, efficient obstacle avoidance, and low-latency response in complex environments, improving operational efficiency and system reliability. Attached Figure Description

[0048] Figure 1 This is a schematic diagram of the main steps of this application;

[0049] Figure 2 This is a detailed schematic diagram of S1 in this application;

[0050] Figure 3 This is a detailed schematic diagram of S2 in this application;

[0051] Figure 4 This is a detailed schematic diagram of S3 in this application;

[0052] Figure 5 This is a detailed schematic diagram of S4 in this application;

[0053] Figure 6 This is a detailed schematic diagram of S5 in this application;

[0054] Figure 7 This is a detailed schematic diagram of S6 in this application;

[0055] Figure 8 This is a detailed schematic diagram of S7 in this application. Detailed Implementation

[0056] The following is in conjunction with the appendix Figures 1-8 This application will be described in further detail.

[0057] See Figure 1 An AGV adaptive path tracking method based on MPC includes the following steps;

[0058] S1: Based on the motion characteristics of a two-wheel differential AGV, a kinematic discretization error model is established. The Euler discretization method is used to process the continuous kinematic equations, and the lateral deviation distance is expressed in state-space equation form. and angle deviation The dynamic relationship of change is used to generate a kinematic discrete state-space model;

[0059] S2: Based on the kinematic discrete state space model, the extended Kalman filter algorithm is used to fuse data from wheel odometer, IMU and laser odometer, and the pose transformation matrix is ​​used to calculate the real-time position deviation and angle deviation of the AGV relative to the current reference path to generate global pose and path deviation data.

[0060] S3: Based on the global pose and path deviation data, design the objective function and constraints of the model prediction controller. The objective function incorporates a state error weight matrix. and control weight matrix The constraints include system control variable boundaries and environmental obstacle avoidance constraints. The constraint conflicts are handled by the slack variable method to generate the MPC optimization problem framework.

[0061] S4: Based on the aforementioned MPC optimization problem framework, an adaptive weight adjustment strategy is adopted to dynamically update... When the lidar detects an obstacle, reduce Weight and increase Weighting prioritizes obstacle avoidance; increases weight when there are no obstacles. Weights are used to enhance tracking accuracy, and the optimal control sequence is solved in real time using a quadratic programming solver to generate an adaptive control quantity sequence.

[0062] S5: Based on the adaptive control sequence, a local reference trajectory is generated using the forward integration method. Taking the current AGV pose as the initial state, the control sequence is input into the kinematic discrete state-space model to predict the future... The set of AGV poses for each step is used to generate a local reference trajectory;

[0063] S6: Based on the local reference trajectory, design a trajectory tracking controller, use a linear quadratic regulator algorithm to calculate the tracking control quantity, verify the convergence of the tracking error through Lyapunov stability analysis, and generate real-time tracking control commands;

[0064] S7: Based on the real-time tracking control command, execute AGV drive control, adjust wheel speed through motor encoder feedback closed loop, and feed back actual posture data to S2 for the next cycle deviation calculation to achieve closed loop control and generate AGV motion state.

[0065] By establishing a kinematic discretization error model and employing the Euler discretization method to handle continuous equations, the dynamic relationship of deviations is expressed through state space, simplifying the calculation process while maintaining accuracy. Sensor data is fused using an extended Kalman filter algorithm to integrate data from wheel odometers, IMUs, and lasers to calculate real-time position and angle deviations, improving positioning accuracy. A model predictive controller objective function and constraints are designed, incorporating weight matrices and boundary constraints. Combined with obstacle avoidance constraints, a relaxation variable method is used to handle conflicts, enhancing system robustness. The weight matrix is ​​dynamically updated based on environmental detection results, adjusting state errors and control variable weights to prioritize obstacle avoidance or enhance tracking. An optimal sequence is generated through a quadratic programming solver, achieving self-control. To achieve adaptive response, a local reference trajectory is generated, and forward integration is used to predict future poses, ensuring path smoothness. A trajectory tracking controller is designed, and a linear quadratic regulator algorithm is used to verify stability and ensure error convergence. The drive control is executed by adjusting wheel speed and updating pose through closed-loop feedback, forming a continuous optimization loop. The overall processing logic simplifies the model to closed-loop control, reduces computational requirements through discretization, optimizes environmental adaptability through adaptive weights, enhances constraint feasibility through relaxation variables, improves trajectory continuity through forward integration, ensures control reliability through stability analysis, and maintains accuracy persistence through closed-loop feedback. This enables high-precision tracking, efficient obstacle avoidance, and low-latency response in complex environments, improving operational efficiency and system reliability.

[0066] See Figure 2 The generative kinematic discrete state-space model based on S1 includes the following steps:

[0067] S101: Based on the motion characteristics of a two-wheel differential AGV, the continuous kinematics equation modeling method is used to define the position and heading angle. The differential relationship is obtained, and the basic equations are derived through the Newton-Euler formula to generate a continuous kinematic model;

[0068] Based on the motion characteristics of a two-wheeled differential speed AGV, a continuous kinematic equation modeling method is used to define the differential relationship between position and heading angle. The fundamental equations are derived using the Newton-Euler formula. For example, in a warehouse AGV handling scenario, the AGV travels along a straight path at a moderate speed. The rate of change of position is determined by the speed value, which is derived from the average value measured by sensors. The change in heading angle is based on feedback from the steering mechanism. The differential relationship is set as follows: the change in position in the x-direction is proportional to the cosine function of speed multiplied by the heading angle, and the change in position in the y-direction is proportional to the sine function of speed multiplied by the heading angle. The speed value is typically taken in the range of 0.5 meters per second, and the heading angle is approximately 15 degrees. The Newton-Euler formula should be applied... When using this method, considering the uniform mass distribution of the AGV and its center of mass located at the geometric center, the derivation of the basic equations involves the calculation of the moment of inertia. The moment of inertia is based on the design parameters of a standard AGV, such as a mass of about 10 kg and a wheelbase of 0.5 meters. In this example, it is assumed that the initial position of the AGV is 0.0 meters, the heading angle is 0 degrees, the speed is 0.5 meters per second, and the time interval is 0.1 seconds. The position change at the next moment is calculated. The position x increases by 0.05 meters multiplied by the cosine of 0 degrees, which is 0.05 meters. The position y increases by 0.05 meters multiplied by the sine of 0 degrees, which is 0 meters. The heading angle remains unchanged. This process is repeated to obtain a continuous trajectory. When the derivation of the basic equations is completed, the dynamic relationship between the position and the heading angle is output, generating a continuous kinematic model.

[0069] S102: Based on the aforementioned continuous kinematics model, the Euler discretization method is used to discretize the period. The state variables are discretized over time to generate a discretized kinematic model;

[0070] Based on a continuous kinematics model, the Euler discretization method is used to discretize the state variables over time using a discrete period. For example, in a factory AGV navigation scenario, the discrete period value is set according to the control frequency, which is around 10 Hz. The discrete period is 0.1 seconds. The state variables include position x, position y, and heading angle. During discretization, the time step is fixed at 0.1 seconds, and forward difference is used to approximate the differential. The discrete value of position x is equal to the previous moment's position x acceleration multiplied by the discrete period multiplied by the cosine of the heading angle. The velocity is taken as the average value of actual measurements, such as 0.6 meters per second. The heading angle is set to 20 degrees. In this example, the initial position x is 0.0 meters, the position y is 0.0 meters, and the heading angle is 0 degrees. After one discrete cycle, the new value of position x is 0.0 meters plus 0.6 multiplied by 0.1 multiplied by 0.06 cosine, which is 0.06 meters. The new value of position y is 0.0 meters plus 0.6 multiplied by 0.1 multiplied by 0.06 sine, which is 0 meters. The new value of the heading angle is calculated based on the steering input. For example, when the steering angle is 5 degrees, the rate of change is the velocity multiplied by the tangent function divided by the wheelbase. The wheelbase is 0.5 meters, so the heading angle increases by 0.06 radians. This process is repeated iteratively to generate a sequence and a discretized kinematic model.

[0071] S103: Based on the discretized kinematic model, the lateral deviation distance is calculated using the error state definition method. and angle deviation The mathematical expression is used to generate the error state equation;

[0072] Based on a discrete kinematics model, the mathematical expressions for lateral deviation distance and angular deviation are calculated using the error state definition method. For example, in the AGV path tracking scenario in a logistics center, the reference path is a straight line with y=0. The lateral deviation distance is defined as the current position y minus the reference y value, which is fixed at zero. The angular deviation is defined as the current heading angle minus the reference heading angle, which is zero degrees. The calculation process obtains the current pose data. The position y comes from the output of the previous sub-step, such as 0.1 meters, and the heading angle comes from the model output, such as 5 degrees. The lateral deviation distance is calculated as 0.1 meters minus zero, which is 0.1 meters. The angular deviation is calculated as 5 degrees minus zero, which is 5 degrees. The variables in the error state expression are directly related to the measured values. In the example, when the AGV deviates from the path, the position y is 0.2 meters, the heading angle is 10 degrees, the lateral deviation is 0.2 meters, and the angular deviation is 10 degrees. The mathematical expression output is a deviation value table, generating the error state equation.

[0073] S104: Based on the aforementioned error state equation, a state-space reconstruction method is used to construct a system... Discrete state-space equations for state vectors Generate a discrete state-space model of kinematics.

[0074] Based on the error state equation, a discrete state-space equation with lateral deviation distance and angular deviation as state vectors is constructed using the state-space reconstruction method. For example, in the control scenario of a warehouse AGV, the state vector contains two elements: lateral deviation distance and angular deviation. The discrete equation form is that the next state is equal to the matrix multiplied by the current state plus the matrix multiplied by the control input. The matrix coefficients are set according to the dynamic characteristics of the AGV. For example, the coefficient of matrix A is 0.9 and the coefficient of matrix B is 0.2 and the coefficient of matrix B is 0.3. The control input includes speed and steering angle. The speed is 0.5 meters per second and the steering angle is 5 degrees. When calculating, the lateral deviation of the current state is 0.1 meters and the angular deviation is 5 degrees. The lateral deviation of the next state is calculated as 0.9 multiplied by 0.1 plus 0.1 multiplied by 5 degrees, which is 0.14. The angular deviation is calculated as 0.1 multiplied by 0.1 plus 0.3 multiplied by 5 degrees, which is 1.51. The state-space reconstruction process is repeated iteratively to generate a kinematic discrete state-space model.

[0075] See Figure 3 The generation of global pose and path deviation data based on S2 includes the following steps:

[0076] S201: Employs a multi-sensor synchronous acquisition method, using a wheeled odometer to acquire displacement data, an IMU to acquire attitude data, and a laser odometer to acquire environmental point cloud data, generating a raw sensor dataset.

[0077] A multi-sensor synchronous acquisition method is adopted, using wheel odometry to acquire displacement data, IMU to acquire attitude data, and laser odometry to acquire environmental point cloud data. For example, in the operation scenario of an automated warehouse AGV, the wheel odometry sampling frequency is 10 Hz, and the displacement data is calculated as wheel speed multiplied by time. The wheel speed is 0.5 m / s, the time interval is 0.1 seconds, and the displacement increases by 0.05 meters. The IMU sampling frequency is 100 Hz. Attitude data, such as the heading angle, is obtained by integrating the gyroscope. The gyroscope outputs an angular velocity of 0.1 degrees per second, a time interval of 0.01 seconds, and a heading angle change of 0.01 degrees. The laser odometry point cloud data includes obstacle distances, which are calculated using laser flight time. For example, if the transmission and reception time difference is 1 microsecond, the distance is 0.3 meters. Data synchronization is achieved using timestamp alignment. In the example, when the AGV moves, the wheel odometry data shows a displacement of 0.1 meters, the IMU shows a heading angle of 10 degrees, and the laser odometry data shows an obstacle distance of 2 meters, generating the original sensor dataset.

[0078] S202: Based on the original sensor dataset, the extended Kalman filter algorithm is used to perform data fusion, estimate the global pose of the AGV, and generate the fused global pose;

[0079] Based on the original sensor dataset, an extended Kalman filter algorithm is used for data fusion to estimate the global pose of the AGV. For example, in an AGV positioning scenario in a factory environment, the filter model states include position x, position y, and heading angle. The process noise covariance is set to 0.01, the measurement noise covariance is set to 0.02, and the prediction step uses a motion model. The new value of position x equals the old value, acceleration multiplied by time multiplied by cosine, and heading angle is 0.5 m / s, time is 0.1 s, and heading angle is 5 degrees. The predicted position x is increased by 0.05 multiplied by cosine. Five degrees is approximately 0.05 meters. The update step fuses sensor measurements. The wheel data displacement of 0.1 meters corresponds to position x, the IMU heading angle is 10 degrees, and the laser data position y is 0.1 meters. During fusion, the Kalman gain is calculated. The gain value is based on the noise covariance, such as 0.5. The estimated position x is equal to the predicted value plus the gain multiplied by the measurement minus the prediction. The measured position x is 0.1 meters, the predicted position is 0.05 meters, the gain is 0.5, and the estimated position is 0.05 plus 0.5 multiplied by 0.05, which is 0.075 meters. The global pose is repeatedly acquired to generate the fused global pose.

[0080] S203: Based on the fused global pose and predefined reference path, the position deviation is solved using the geometric deviation calculation method. and angle deviation Generate global pose and path deviation data.

[0081] Based on the fusion of global pose and predefined reference path, the geometric deviation calculation method is used to solve the position deviation and angle deviation. For example, in the AGV tracking scenario on the assembly line, the reference path is a straight line with y=0 and the reference heading angle is 0 degrees. The position deviation is calculated as the current position y minus the reference y, where the reference y is 0. The angle deviation is calculated as the current heading angle minus the reference heading angle, where the reference heading angle is 0 degrees. The current pose data is obtained, with position y=0.1 meters and heading angle=5 degrees. The position deviation is 0.1 meters minus 0, which is 0.1 meters. The angle deviation is 5 degrees minus 0, which is 5 degrees. The deviation values ​​are output as a numerical table, generating global pose and path deviation data.

[0082] See Figure 4 The S3-based framework for generating MPC optimization problems includes the following steps:

[0083] S301: Based on the global pose and path deviation data, a quadratic objective function design method is used to define the state error weight matrix. and control weight matrix Construct the objective function Generate the objective function framework;

[0084] Based on global pose and path deviation data, a quadratic objective function design method is used to define the state error weight matrix and the control quantity weight matrix to construct the objective function. For example, in the AGV path optimization scenario, the weight matrix Q is for the state error, such as setting the lateral deviation weight to 0.5 and the angle deviation weight to 0.3. The matrix R is for the control quantity, with the speed weight set to 0.2 and the steering angle weight set to 0.1. The objective function is in the form of the weighted sum of the squares of the error plus the weighted sum of the squares of the control quantity. When calculating, the state error is 0.1 meters for lateral deviation and 5 degrees for angle deviation, and the control quantity is 0.5 meters per second for speed and 5 degrees for steering angle. The objective value is calculated as 0.5 x 0.1 squared + 0.3 x 5 squared + 0.2 x 0.5 squared + 0.1 x 5 squared, approximately 7.75, thus generating the objective function framework.

[0085] S302: Based on the physical limitations of the AGV, the boundary constraint method is used to set the speed. and front wheel cornering The upper and lower bound constraints are used to generate system boundary constraints;

[0086] Based on the physical limitations of the AGV, the boundary constraint method is used to set the upper and lower limits of speed and front wheel angle. For example, in the safe operation scenario of warehouse AGV, the upper limit of speed is set to 1 meter per second and the lower limit is set to 0.1 meters per second. The upper limit of front wheel angle is 30 degrees and the lower limit is -30 degrees. The constraint settings are based on the motor performance, such as the maximum output torque of the motor limiting the speed and the mechanical limit of the steering mechanism limiting the angle. In the example, the control quantity speed of 0.5 meters per second is valid within 0 to 1, and the angle of 10 degrees is valid within -30 to 30. The constraint output is a system of inequalities, which generates the system boundary constraints.

[0087] S303: Based on obstacle detection results, a geometric obstacle avoidance constraint method is used, with the addition of distance constraints for circular obstacles. Generate environmental obstacle avoidance constraints;

[0088] Based on obstacle detection results, a geometric obstacle avoidance constraint method is used to add distance constraints for circular obstacles. For example, in a dynamic AGV obstacle avoidance scenario, the obstacle position is derived from the sensor, such as the LiDAR detecting an obstacle center with a distance of 1 meter x and 1 meter y, and a radius of 0.2 meters. The AGV radius is 0.3 meters, and the safety distance is set at 0.5 meters. The constraint calculation is that the square of the distance from the AGV center to the obstacle center is greater than or equal to the square of the radius. The AGV position is 0.5 meters x and 0.5 meters y. The square of the distance is 0.5 minus 1 square plus 0.5 minus 1 square, which equals 0.5. The radius is 0.5 squared, which is 0.25. The constraint 0.5 is greater than 0.25 is satisfied; otherwise, an adjustment is triggered to generate environmental obstacle avoidance constraints.

[0089] S304: Based on the aforementioned system boundary constraints and environmental obstacle avoidance constraints, slack variables are added using the slack variable introduction method. and penalty items To obtain the objective function, avoid unsolvable cases and generate a relaxed processing framework;

[0090] Based on system boundary constraints and environmental obstacle avoidance constraints, a relaxation variable introduction method is used to add relaxation variables and penalty terms to the objective function to avoid unsolvable situations. For example, in a constraint conflict scenario, the relaxation variable is initially set to 0.1, the penalty weight is set to 10, and a penalty term is added to the objective function, such as 10 times the square of the relaxation variable. When there is a constraint conflict, such as insufficient distance, the relaxation variable is increased by 0.2, and the penalty term is increased by 10 times 0.04, i.e., 0.4. The increase in the objective value prompts the optimizer to adjust. The adjustment of the relaxation variable is based on the degree of conflict. When the conflict is high, the increment is large, generating a relaxation processing framework.

[0091] S305: Based on the objective function framework, system boundary constraints, environmental obstacle avoidance constraints, and relaxation processing framework, the optimization problem integration method is adopted to form a complete optimal control problem and generate the MPC optimization problem framework.

[0092] Based on the objective function framework, system boundary constraints, environment obstacle avoidance constraints, and relaxation processing framework, an optimization problem integration method is used to form a complete optimal control problem. For example, the objective function weight constraint relaxation term is integrated to output the optimization problem description and generate the MPC optimization problem framework.

[0093] See Figure 5 The generation of adaptive control input sequences based on S4 includes the following steps:

[0094] S401: Employs LiDAR obstacle detection to output environmental complexity scores in real time and generate environmental status indicators;

[0095] The system uses lidar obstacle detection to output environmental complexity scores in real time. For example, obstacle density is calculated as the number of obstacles per unit area. If the area is 10 square meters and there are 5 obstacles, the density is 0.5. The score range is 0 to 1, and a density of 0.5 corresponds to a score of 0.5, thus generating environmental status indicators.

[0096] S402: Based on the aforementioned environmental state indicators and the MPC optimization problem framework, a dynamic weight adjustment strategy is adopted: when the score is high, the weight is reduced. And increase Prioritize obstacle avoidance; increase when score is low. and reduce To enhance tracking, the optimal control sequence is calculated using a quadratic programming solver. Generate an adaptive control input sequence.

[0097] Based on environmental state indicators and the MPC optimization problem framework, a dynamic weight adjustment strategy is adopted. When the score is high, the state error weight is reduced and the control quantity weight is increased to prioritize obstacle avoidance. When the score is low, the state error weight is increased and the control quantity weight is reduced to strengthen tracking. The optimal control sequence is calculated through a quadratic programming solver. For example, if the score is 0.7, which is higher than the threshold of 0.6, the state error weight is reduced from 0.5 to 0.3, and the control quantity weight is increased from 0.2 to 0.4. The control sequence is solved, such as a speed of 0.6 m / s and a steering angle of 10 degrees, generating an adaptive control quantity sequence.

[0098] See Figure 6 The generation of local reference trajectories based on S5 includes the following steps:

[0099] S501: Based on the current AGV pose, the state initialization method is used to set the predicted initial state. Generate the initial predicted state;

[0100] Based on the current AGV pose, the state initialization method is used to set the predicted initial state, such as position x 0.0 meters, y 0.0 meters, heading angle 0 degrees, and the initial state is set with lateral deviation of zero and angular deviation of zero to generate the initial predicted state.

[0101] S502: Based on the initial predicted state and the adaptive control input sequence, the forward integral iterative method is used to progressively calculate the future... The pose of each step is used to generate a pose prediction sequence;

[0102] Based on the initial predicted state and the adaptive control sequence, the pose of the future steps is calculated step by step using the forward integral iterative method. For example, the prediction steps are five steps, the control sequence velocity is 0.5 meters per second, the turning angle is 5 degrees, the time step is 0.1 seconds, the pose calculation position x is increased by 0.05 meters, the heading angle is increased by 0.1 degrees, and the pose prediction sequence is generated.

[0103] S503: Based on the pose prediction sequence, the discrete trajectory generation method is used to output the path point set and generate the discrete trajectory point set;

[0104] Based on the pose prediction sequence, a discrete trajectory generation method is used to output a set of path points. For example, if the sequence contains five points, each point has a position and a heading angle of xy, the output point set is used to generate a discrete trajectory point set.

[0105] S504: Based on the discrete trajectory point set, a smooth and continuous path is constructed using cubic spline interpolation to generate a smooth and continuous trajectory;

[0106] Based on discrete trajectory point sets, a smooth and continuous path is constructed using cubic spline interpolation. For example, with a point set spacing of 0.1 meters, a curved path is generated through interpolation, thus producing a smooth and continuous trajectory.

[0107] S505: Based on the smooth and continuous trajectory and environmental obstacle avoidance constraints, the trajectory feasibility verification method is used to check whether the obstacle avoidance requirements are met and generate a feasible trajectory.

[0108] Based on smooth and continuous trajectories and environmental obstacle avoidance constraints, a trajectory feasibility verification method is used to check whether the obstacle avoidance requirements are met. For example, if the distance between the trajectory point and the obstacle is greater than the safe distance of 0.5 meters, it is valid and a feasible trajectory is generated.

[0109] S506: Based on the feasible trajectory, the reference trajectory output method is used to generate the final local path and generate a local reference trajectory.

[0110] Based on the feasible trajectory, the final local path is generated using the reference trajectory output method, such as outputting the path coordinate sequence to generate a local reference trajectory.

[0111] See Figure 7 The generation of real-time tracking control commands based on S6 includes the following steps:

[0112] S601: Based on the local reference trajectory and the current pose deviation, a tracking controller is designed using a linear quadratic regulator algorithm, and the error convergence is ensured by Lyapunov stability analysis to generate real-time tracking control commands.

[0113] Based on the local reference trajectory and the current pose deviation, a tracking controller is designed using a linear quadratic regulator algorithm, and the Lyapunov stability analysis method is used to ensure error convergence. For example, if the controller gain is set to 0.5, the lateral error deviation of 0.1 meters converges to zero, generating real-time tracking control commands.

[0114] See Figure 8 The generation of AGV motion state based on S7 includes the following steps:

[0115] S701: Based on the real-time tracking control command, the motor drive control method is adopted to output the wheel speed command to the AGV actuator and generate the wheel speed control signal;

[0116] Based on real-time tracking control commands, the wheel speed command is output to the AGV actuator using the motor drive control method. For example, the command speed is 0.5 meters per second, and the differential speed of the left and right wheels is generated to produce a wheel speed control signal.

[0117] S702: Based on the wheel speed control signal, the encoder feedback acquisition method is used to obtain the actual wheel speed and displacement data and generate actual motion feedback;

[0118] Based on the wheel speed control signal, the actual wheel speed and displacement data are obtained by the encoder feedback acquisition method. For example, when the encoder pulse count is one hundred, the displacement is 0.1 meters, and the actual motion feedback is generated.

[0119] S703: Based on the actual motion feedback, a pose update algorithm is used to calculate the new global pose of the AGV and generate updated pose data;

[0120] Based on actual motion feedback, a pose update algorithm is used to calculate the new global pose of the AGV. For example, if the displacement is 0.1 meters and the heading angle changes by 1 degree, the pose update position x is increased by 0.1 meters to generate updated pose data.

[0121] S704: Based on the updated pose data, it is fed back to S203 for path deviation calculation in the next control cycle, generating the AGV motion state.

[0122] The updated pose data is fed back to S203 for path deviation calculation in the next control cycle, generating the AGV motion state.

[0123] The above are all preferred embodiments of this application, and are not intended to limit the scope of protection of this application. Therefore, all equivalent changes made in accordance with the structure, shape and principle of this application should be covered within the scope of protection of this application.

Claims

1. An AGV adaptive path tracking method based on MPC, characterized in that, Includes the following steps; S1: Based on the motion characteristics of a two-wheel differential AGV, a kinematic discretization error model is established. The Euler discretization method is used to process the continuous kinematic equations, and the lateral deviation distance is expressed in the form of a state-space equation. and angle deviation The dynamic relationship of change is used to generate a kinematic discrete state-space model; S2: Based on the kinematic discrete state space model, the extended Kalman filter algorithm is used to fuse data from wheel odometer, IMU and laser odometer, and the pose transformation matrix is ​​used to calculate the real-time position deviation and angle deviation of the AGV relative to the current reference path to generate global pose and path deviation data. S3: Based on the global pose and path deviation data, design the objective function and constraints of the model prediction controller. The objective function incorporates a state error weight matrix. and control weight matrix The constraints include system control variable boundaries and environmental obstacle avoidance constraints. The constraint conflicts are handled by the slack variable method to generate the MPC optimization problem framework. S4: Based on the aforementioned MPC optimization problem framework, an adaptive weight adjustment strategy is adopted to dynamically update... When the lidar detects an obstacle, reduce Weight and increase Weighting prioritizes obstacle avoidance; increases weight when there are no obstacles. Weights enhance tracking accuracy, and a quadratic programming solver is used to solve for the optimal control sequence in real time, generating an adaptive control quantity sequence. S5: Based on the adaptive control sequence, a local reference trajectory is generated using the forward integration method. Taking the current AGV pose as the initial state, the control sequence is input into the kinematic discrete state-space model to predict the future. The set of AGV poses for each step is used to generate a local reference trajectory; S6: Based on the local reference trajectory, design a trajectory tracking controller, use a linear quadratic regulator algorithm to calculate the tracking control quantity, verify the convergence of the tracking error through Lyapunov stability analysis, and generate real-time tracking control commands; S7: Based on the real-time tracking control command, execute AGV drive control, adjust wheel speed through closed-loop feedback of motor encoder, and feed back actual posture data to S2 for deviation calculation in the next cycle to achieve closed-loop control and generate AGV motion state.

2. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; The S1-based generative kinematic discrete state-space model includes the following steps: S101: Based on the motion characteristics of a two-wheel differential AGV, the continuous kinematics equation modeling method is used to define the position and heading angle. The differential relationship is obtained, and the basic equations are derived through the Newton-Euler formula to generate a continuous kinematic model; S102: Based on the aforementioned continuous kinematics model, the Euler discretization method is used to discretize the period. The state variables are discretized over time to generate a discretized kinematic model; S103: Based on the discretized kinematic model, the lateral deviation distance is calculated using the error state definition method. and angle deviation The mathematical expression is used to generate the error state equation; S104: Based on the aforementioned error state equation, a state-space reconstruction method is used to construct a system... Discrete state-space equations for state vectors Generate a discrete state-space model of kinematics.

3. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; The generation of global pose and path deviation data based on S2 includes the following steps: S201: Employs a multi-sensor synchronous acquisition method, using a wheeled odometer to acquire displacement data, an IMU to acquire attitude data, and a laser odometer to acquire environmental point cloud data, generating a raw sensor dataset. S202: Based on the original sensor dataset, the extended Kalman filter algorithm is used to perform data fusion, estimate the global pose of the AGV, and generate the fused global pose; S203: Based on the fused global pose and predefined reference path, the position deviation is solved using the geometric deviation calculation method. and angle deviation Generate global pose and path deviation data.

4. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; The S3-based generative MPC optimization problem framework includes the following steps: S301: Based on the global pose and path deviation data, a quadratic objective function design method is used to define the state error weight matrix. and control weight matrix Construct the objective function Generate the objective function framework; S302: Based on the physical limitations of the AGV, the boundary constraint method is used to set the speed. and front wheel cornering The upper and lower bound constraints are used to generate system boundary constraints; S303: Based on obstacle detection results, a geometric obstacle avoidance constraint method is used, with the addition of distance constraints for circular obstacles. Generate environmental obstacle avoidance constraints; S304: Based on the aforementioned system boundary constraints and environmental obstacle avoidance constraints, slack variables are added using the slack variable introduction method. and penalty items To obtain the objective function, avoid unsolvable cases and generate a relaxed processing framework; S305: Based on the objective function framework, system boundary constraints, environmental obstacle avoidance constraints, and relaxation processing framework, the optimization problem integration method is adopted to form a complete optimal control problem and generate the MPC optimization problem framework.

5. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; The generation of adaptive control input sequences based on S4 includes the following steps: S401: Employs LiDAR obstacle detection to output environmental complexity scores in real time and generate environmental status indicators; S402: Based on the aforementioned environmental state indicators and the MPC optimization problem framework, a dynamic weight adjustment strategy is adopted: when the score is high, the weight is reduced. And increase Prioritize obstacle avoidance; increase when score is low. and reduce To enhance tracking, the optimal control sequence is calculated using a quadratic programming solver. Generate an adaptive control input sequence.

6. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; The generation of local reference trajectories based on S5 includes the following steps: S501: Based on the current AGV pose, the state initialization method is used to set the predicted initial state. Generate the initial predicted state; S502: Based on the initial predicted state and the adaptive control input sequence, the forward integral iterative method is used to progressively calculate the future... The pose of each step is used to generate a pose prediction sequence; S503: Based on the pose prediction sequence, the discrete trajectory generation method is used to output the path point set and generate the discrete trajectory point set; S504: Based on the discrete trajectory point set, a smooth and continuous path is constructed using cubic spline interpolation to generate a smooth and continuous trajectory; S505: Based on the smooth and continuous trajectory and environmental obstacle avoidance constraints, the trajectory feasibility verification method is used to check whether the obstacle avoidance requirements are met and generate a feasible trajectory. S506: Based on the feasible trajectory, the reference trajectory output method is used to generate the final local path and generate a local reference trajectory.

7. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; Generating real-time tracking control commands based on S6 includes the following steps: S601: Based on the local reference trajectory and the current pose deviation, a tracking controller is designed using a linear quadratic regulator algorithm, and the error convergence is ensured by Lyapunov stability analysis to generate real-time tracking control commands.

8. The AGV adaptive path tracking method based on MPC according to claim 1, characterized in that; Generating AGV motion state based on S7 includes the following steps: S701: Based on the real-time tracking control command, the motor drive control method is adopted to output the wheel speed command to the AGV actuator and generate the wheel speed control signal; S702: Based on the wheel speed control signal, the encoder feedback acquisition method is used to obtain the actual wheel speed and displacement data and generate actual motion feedback; S703: Based on the actual motion feedback, a pose update algorithm is used to calculate the new global pose of the AGV and generate updated pose data; S704: Based on the updated pose data, it is fed back to S203 for path deviation calculation in the next control cycle, generating the AGV motion state.

Images