Tracking control method of heavy-load AGV based on Koopman operator

Abstract

The invention relates to the technical field of AGV control, and provides a Koopman operator-based heavy-load AGV tracking control method, which comprises the steps of constructing a Koopman operator theory-based MPC model, a state constraint and a target function, obtaining position data, corner data and a road adhesion coefficient through a sensor of a heavy-load AGV at the beginning of each control period, and determining the position data, the corner data and the road adhesion coefficient of the heavy-load AGV according to the position data, the corner data and the road adhesion coefficient of the heavy-load AGV. Substituting into a state constraint and a target function, determining a control sequence through solving, and determining an expected corner increment according to the control sequence; and determining the output rotation angle output by the heavy-load AGV in the next control period through a PI D control system based on RBF adjustment according to the expected rotation angle increment. According to the invention, tracking control of the heavy-load AGV rotation angle is realized through the LPV-MPC model based on the Koopman operator theory, so that the rotation angle output by the heavy-load AGV adapts to various conditions such as different road adhesion degrees.

Description

Technical Field

[0001] The present invention relates to the technical field of AGV control, and in particular to a tracking control method of a heavy-load AGV based on a Koopman operator. Background Art

[0002] Autonomous driving has always been a research focus in the field of automotive engineering. In a non-end-to-end autonomous driving framework, the control layer task is path tracking, which enables the vehicle to follow the reference path and speed curve during interaction with the actuator. The research goal is to ensure vehicle stability while maximizing the accuracy and smoothness of path tracking.

[0003] The spectral decomposition of the Koopman operator decomposes the dynamic behavior of the system into a series of specific evolution modes. Each eigenfunction corresponds to a mode, and the eigenvalue represents the evolution rate of the mode. By analyzing the eigenfunction, we can deeply understand the behavior of the system in different modes. Currently, the Koopman operator has been applied to unknown dynamic processes to obtain linearizable data-driven models.

[0004] Specifically, the Koopman operator is used for path tracking lateral control. There are many types of existing vehicle path tracking control methods. The system modeling is usually studied under ideal dry roads. However, due to the characteristics of long-distance transportation of actual commercial vehicles, rainy and snowy weather is inevitable during actual driving. If the model established under dry roads is still used at this time, the control accuracy may decrease or even control failure. At the same time, the change of the road adhesion coefficient will cause the braking distance of the vehicle to change, and the fixed vehicle spacing algorithm is difficult to ensure vehicle safety. Therefore, it is necessary to establish a vehicle model that adapts to the change of the road adhesion coefficient and design a lateral and longitudinal coordinated controller that can ensure vehicle safety under different road adhesion coefficients.

[0005] The road adhesion coefficient is the ratio of adhesion to the normal pressure of the wheel (direction perpendicular to the road surface). It can be regarded as the static friction coefficient between the tire and the road surface. The larger the adhesion coefficient, the stronger the adhesion between the wheel and the road surface, making the car less likely to slip during driving. In the application of heavy-duty AGV (Automated Guided Vehicle), the road adhesion coefficient is a key parameter, which directly affects the driving stability, braking performance and handling positioning accuracy of the AGV.

[0006] Therefore, how to make the Koopman operator-based corner tracking control of heavy-load AGV adapt to the degree of road adhesion is a technical problem that needs to be solved. Summary of the invention

[0007] To this end, the present invention provides a tracking control method for a heavy-loaded AGV based on the Koopman operator, which realizes the tracking control of the turning angle of the heavy-loaded AGV through the LPV-MPC model based on the Koopman operator theory, so that the turning angle output by the heavy-loaded AGV can adapt to various situations such as different road adhesion degrees.

[0008] To achieve the above object, the present invention proposes a tracking control method for a heavy-duty AGV based on a Koopman operator, comprising:

[0009] The heavy-load AGV obtains the position data, turning angle data and road adhesion coefficient of the current control cycle through sensors;

[0010] Constructing a Koopman operator theoretical model, obtaining a first eigenfunction matrix and a second eigenfunction matrix of the Koopman operator theoretical model by interpolation calculation according to the road adhesion coefficient, and generating a model state quantity of the Koopman operator theoretical model according to the position data and the turning angle data;

[0011] Constructing an objective function for obtaining an increment of an angular velocity control variable and minimizing the model state quantity, wherein input variables of the objective function include the model state quantity, a model state expected deviation quantity, and a weight matrix, wherein the model state expected deviation quantity is calculated by the position data and the angular data through a local path planning model;

[0012] The Koopman operator theoretical model is used as the state constraint of the objective function, and the objective function is solved to obtain the expected rotation angle increment;

[0013] The desired rotation angle increment is used to determine the output rotation angle of the heavy-duty AGV in the next control cycle through a PID control system based on RBF regulation.

[0014] Further, the first eigenfunction matrix of the current cycle is obtained by weighted summing and normalizing the first eigenfunction matrices of the multiple control cycles, wherein the weight of the weighted summing is determined by interpolating the road adhesion coefficient of the current control cycle;

[0015] The second eigenfunction matrix of the current cycle is obtained by weighted summing and normalizing the second eigenfunction matrices of multiple control cycles.

[0016] In the above scheme, the set interpolation fitting function makes the MPC model more accurate.

[0017] Furthermore, the model state expected deviation is calculated by the position data and the turning angle data through the local path planning model, specifically:

[0018] The local path planning model includes a kinematic model, a dynamic window method model and an error calculation formula;

[0019] The position data is calculated by a kinematic model of a two-wheel differential drive heavy-duty AGV to determine a predicted position and a predicted turning angle;

[0020] Determine the recommended moving point by using the predicted position and the predicted turning angle through a dynamic window method model;

[0021] The expected deviation of the model state is determined by the coordinates of the recommended moving point and the coordinates of the current moving point.

[0022] Furthermore, the expected deviation of the model state includes a path tracking lateral deviation and a path tracking heading deviation, the path tracking lateral deviation is calculated by the difference between the coordinates of the recommended moving point and the coordinates of the current moving point and the heading angle, and the path tracking heading deviation is determined by the heading angle of the current control cycle and the heading angle of the previous control cycle.

[0023] In the above scheme, the road curvature information is converted into a reference state variable and then moved to the objective function calculation. The steady-state error of the state quantity of the dynamic model can be reduced without feedforward compensation. At the same time, when constructing the state space, there is no need to add the longitudinal attribute of the vehicle to assist in the y coordinate conversion.

[0024] Furthermore, the state quantity weight matrix includes a speed state weight and a stability state weight;

[0025] The speed state weight is determined by performing gradient discrimination on the speed of the heavy-load AGV;

[0026] The stability state weight is determined according to the stability of the heavy-load AGV.

[0027] Furthermore, the stability is calculated by the path tracking lateral deviation, the path tracking heading deviation and their corresponding set maximum values.

[0028] Further, the RBF-adjusted PID control system includes a PID controller and an RBF neural network;

[0029] Obtaining an input error by calculation according to the desired rotation angle increment, the rotation angle data and the network output rotation angle error;

[0030] Inputting the input error into the PID controller to obtain the output angle;

[0031] The output rotation angle and the rotation angle data are input into the RBF neural network to obtain the network output rotation angle error.

[0032] Further, performing weighted summation of the expected turning angle increment and the turning angle data to determine the expected turning angle;

[0033] The input error is obtained by subtracting the expected rotation angle from the network output rotation angle error.

[0034] Furthermore, the RBF neural network is also used to calculate setting parameters of the PID controller through a gradient descent method based on the output angle and the angle data.

[0035] In the above scheme, the PID control system based on RBF regulation realizes that the parameters and input quantities of PID can be adjusted without coding or establishing a rule base based on experience.

[0036] Furthermore, the state constraints include the angular velocity control variable constraints and the model state quantity constraint matrix;

[0037] The stability is calculated by the path tracking lateral deviation, the path tracking heading deviation and their corresponding set maximum values;

[0038] The angular velocity control variable constraint is determined according to the maximum angular velocity of the heavy-load AGV and the sampling duration.

[0039] Furthermore, the model state quantity constraint matrix is ​​determined according to the rollover condition of the heavy-loaded AGV.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] 1. The tracking control of the heavy-duty AGV turning angle is realized through the LPV-MPC model based on Koopman operator theory, so that the turning angle output by the heavy-duty AGV can adapt to various situations such as different road adhesion levels.

[0042] 2. The set interpolation fitting function makes the MPC model more accurate.

[0043] 3. The road curvature information is converted into a reference state variable, and then moved to the objective function calculation. The steady-state error of the state quantity of the dynamic model can be reduced without feedforward compensation. At the same time, when constructing the state space, there is no need to add the longitudinal attribute of the vehicle to assist in the y coordinate conversion.

[0044] 4. The PID control system based on RBF regulation can adjust the parameters and input quantities of PID without coding or establishing a rule base based on experience. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a schematic diagram of the general flow of the tracking control method of the heavy-load AGV based on the Koopman operator according to an embodiment of the present invention;

[0046] Figure 2 It is a schematic flow chart of a tracking control method for a heavy-duty AGV based on a Koopman operator according to an embodiment of the present invention;

[0047] Figure 3 It is a schematic diagram of the framework of the tracking control method of the heavy-duty AGV based on the Koopman operator according to an embodiment of the present invention;

[0048] Figure 4 Schematic diagram of the navigation system framework of the tracking control method of the heavy-load AGV based on the Koopman operator according to an embodiment of the present invention. DETAILED DESCRIPTION

[0049] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0050] The preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principles of the present invention and are not intended to limit the protection scope of the present invention.

[0051] It should be noted that, in the description of the present invention, terms such as "up", "down", "left", "right", "inside" and "outside" indicating directions or positional relationships are based on the directions or positional relationships shown in the drawings. This is merely for the convenience of description and does not indicate or imply that the device or element must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it cannot be understood as a limitation on the present invention.

[0052] In addition, it should be noted that in the description of the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For those skilled in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0053] like Figures 1 to 4 As shown, the present invention provides a tracking control method for a heavy-loaded AGV based on the Koopman operator, which realizes the tracking control of the turning angle of the heavy-loaded AGV through the LPV-MPC model based on the Koopman operator theory, so that the turning angle output by the heavy-loaded AGV can adapt to various situations such as different road adhesion levels.

[0054] in, Figure 1It is a schematic diagram of the general flow of the tracking control method of the heavy-load AGV based on the Koopman operator according to an embodiment of the present invention; Figure 2 It is a schematic flow chart of a tracking control method for a heavy-duty AGV based on a Koopman operator according to an embodiment of the present invention; Figure 3 It is a schematic diagram of the framework of the tracking control method of the heavy-duty AGV based on the Koopman operator according to an embodiment of the present invention; Figure 4 Schematic diagram of the navigation system framework of the tracking control method of the heavy-load AGV based on the Koopman operator according to an embodiment of the present invention.

[0055] like Figures 1 to 4 As shown, this embodiment proposes a tracking control method for a heavy-load AGV based on a Koopman operator, including: the heavy-load AGV obtains position data, turning angle data and road adhesion coefficient of the current control cycle through a sensor;

[0056] Constructing a Koopman operator theoretical model, obtaining a first eigenfunction matrix and a second eigenfunction matrix of the Koopman operator theoretical model by interpolation calculation according to the road adhesion coefficient, and generating a model state quantity of the Koopman operator theoretical model according to the position data and the turning angle data;

[0057] Constructing an objective function for obtaining an increment of an angular velocity control variable and minimizing the model state quantity, wherein input variables of the objective function include the model state quantity, a model state expected deviation quantity, and a weight matrix, wherein the model state expected deviation quantity is calculated by the position data and the angular data through a local path planning model;

[0058] The Koopman operator theoretical model is used as the state constraint of the objective function, and the objective function is solved to obtain the expected rotation angle increment;

[0059] The desired rotation angle increment is used to determine the output rotation angle of the heavy-duty AGV in the next control cycle through a PID control system based on RBF regulation.

[0060] Specifically, the state constraint of the MPC model based on Koopman operator theory is constructed by the following formula:

[0061]

[0062] In the formula, μ k is the road adhesion coefficient of the kth control cycle, A(μ k ) and B(μ k ) are the first eigenfunction matrix and the second eigenfunction matrix of the kth control period, respectively, which are determined by interpolation calculation according to the road adhesion coefficient, x k and xk+1 are the model state quantities of the kth control cycle and the k+1th control cycle, respectively, u k The angular velocity control variable of the kth control cycle, C is the state extraction matrix, y k+1 is the observer of the k+1th control cycle, x lim is the linear model state constraint matrix, u lim is the control variable constraint for angular velocity;

[0063] The objective function of the MPC model is constructed by the following formula:

[0064]

[0065] Where J is the output of the objective function, x k 、u k are the model state quantity and the angular velocity control variable of the kth control cycle, respectively. N is the number of prediction time domain steps. Δu is the increment of the angular velocity control variable. r, k is the expected deviation of the model state in the kth control cycle, T represents the transpose calculation of the matrix, Q is the state quantity weight matrix, and R is the control quantity weight matrix;

[0066] At the beginning of each control cycle, the position data, turning angle data and road adhesion coefficient of the heavy-load AGV are obtained through the sensor of the heavy-load AGV, the position data is substituted into the model state quantity and used to calculate the expected deviation of the model state through the local path planning model, the turning angle data is substituted into the turning angle speed control variable, the road adhesion coefficient is used to calculate the first eigenfunction matrix and the second eigenfunction matrix, the control sequence is determined by solving the state constraints and the objective function, and the turning angle output by the heavy-load AGV is determined according to the control sequence.

[0067] It can be understood that in the state constraints, the first road adhesion coefficient matrix and the second road adhesion coefficient matrix are determined quantities, and the model state quantity is the input quantity, which is also a determined quantity. Therefore, the state constraints and objective functions are actually the process of solving the angular velocity control variables, and generating a control sequence with the solved variables to achieve smooth angular output and smooth tracking control of the heavy-loaded AGV path turning.

[0068] It can be understood that in the objective function, the evaluation characteristic matrix of the heavy-loaded AGV operating state can be determined through the model state quantity, the model state expected deviation quantity and the state quantity weight matrix, and the heavy-loaded AGV turning evaluation characteristic matrix can be determined through the angular velocity control variable and the control quantity weight matrix, so that the angular velocity increment and the angular velocity are smaller, more stable and smoother while considering the road adhesion coefficient matrix in the state constraint.

[0069] At the beginning of each cycle, the vehicle observations are used to update the model objective function and state constraints, and the OSQP solver is used to solve the problem. That is, when the state constraints are met, the data collected by different paths and different sensors are substituted into the state constraints to iteratively calculate their outputs, and then the elements in the control sequence are updated, and then the appropriate expected turning angle increment is selected for output.

[0070] The linear model of the Koopman operator theory can be expressed as: K = L (x k ,u k ,μ k ), where K is the operator that depends on the road adhesion coefficient μ k , model state x k , angular velocity control variable u k Although the three are related, it is impossible to obtain an accurate functional relationship. Therefore, the general MPC model is obtained by interpolating and fitting a set of linear models near a series of working points to obtain an overall MPC model, which can be understood as the polyhedral modeling method of the MPC model.

[0071] It should be noted that the road adhesion coefficient is the ratio of adhesion to wheel normal pressure. The MPC model is specifically an LPV-MPC linear parametric model predictive control model. The state extraction matrix C and the control amount weight matrix R are set as fixed matrices. The time-varying parameter of the Koopman operator is selected as the sum of the road adhesion coefficient and the angle increment.

[0072] Further, the first eigenfunction matrix of the current cycle is obtained by weighted summing and normalizing the first eigenfunction matrices of multiple control cycles;

[0073] The second eigenfunction matrix of the current cycle is obtained by weighted summing and normalizing the second eigenfunction matrices of multiple control cycles.

[0074] Furthermore, the weight of the weighted sum is determined by performing an interpolation calculation on the road adhesion coefficient of the current control cycle.

[0075] Specifically, the first road adhesion coefficient matrix and the second road adhesion coefficient matrix are both determined by interpolation calculation according to the road adhesion coefficient, including:

[0076] The first road adhesion coefficient matrix and the second road adhesion coefficient matrix are interpolated and calculated by the following formula:

[0077]

[0078] In the formula, A(μ k ) and B(μ k) are the first eigenfunction matrix and the second eigenfunction matrix respectively, Co represents the co-occurrence matrix of the matrix, A(μ1)...A(μ n ) are the first eigenfunction matrices from the first control period to the nth control period, B(μ1)...B(μ n ) are the second eigenfunction matrices from the first control period to the nth control period, n is the current nth control period, α i is the weight coefficient of the ith control cycle, which is calculated and determined according to the road adhesion coefficient.

[0079] Furthermore, the road adhesion coefficient is calculated by the following formula to determine the weight coefficient:

[0080]

[0081] In the formula, α i is the weight coefficient of the i-th point, μ(t) is the interpolated road adhesion coefficient, μ i-1 , μ i and μ i+1 They are, in sequence, the road adhesion coefficient at the i-1th point, the road adhesion coefficient at the i-th point, and the road adhesion coefficient at the i-th point.

[0082] In the above scheme, the set interpolation fitting function makes the MPC model more accurate.

[0083] The purpose of interpolation is to obtain smooth reference matching point information. Even when different reference points are matched in adjacent sampling periods, the expected state change amplitude given by the matching point is the same as that in any other sampling period.

[0084] Furthermore, the control sequence is specifically:

[0085] U * =[Δu((k),Δu((k+1)…Δu((k+N-1)]

[0086] Where U * Represents the control sequence, Δu(k), Δu(k+1)...Δu(k+N-1) are the increments of the angular velocity control variable from the kth control period to the k+N-1th control period respectively.

[0087] In the above scheme, the set interpolation fitting function makes the MPC model more accurate and the output smoother.

[0088] Furthermore, the model state expected deviation is calculated by the position data and the turning angle data through the local path planning model, specifically:

[0089] The local path planning model includes a kinematic model, a dynamic window method model and an error calculation formula;

[0090] The position data is calculated by a kinematic model of a two-wheel differential drive heavy-duty AGV to determine a predicted position and a predicted turning angle;

[0091] Determine the recommended moving point by using the predicted position and the predicted turning angle through a dynamic window method model;

[0092] The expected deviation of the model state is determined by the coordinates of the recommended moving point and the coordinates of the current moving point.

[0093] Furthermore, the expected deviation of the model state includes a path tracking lateral deviation and a path tracking heading deviation, the path tracking lateral deviation is calculated by the difference between the coordinates of the recommended moving point and the coordinates of the current moving point and the heading angle, and the path tracking heading deviation is determined by the heading angle of the current control cycle and the heading angle of the previous control cycle.

[0094] The use of the position data for calculating the expected model state through a local path planning model is specifically as follows: the expected state deviation includes a path tracking lateral deviation and a path tracking heading deviation, and the local path planning model includes a kinematic model, a dynamic window method model, and an error calculation formula; the position data is calculated through a kinematic model of a two-wheel differential drive heavy-duty AGV to determine a predicted position and a predicted turning angle; the predicted position and the predicted turning angle are determined through a dynamic window method model to determine a recommended moving point; the recommended moving point is determined through the error calculation formula to determine the path tracking lateral deviation and the path tracking heading deviation, and the error calculation formula is:

[0095]

[0096] In the formula, e y is the path tracking lateral deviation, e θ is the path tracking heading deviation, x t+1 and t+1 is the coordinate of the recommended moving point, x t and t is the coordinate of the current moving point, θ t is the heading angle of the heavy-load AGV at the current moving point, θ t+ 1 It is the heading angle of the heavy-load AGV at the recommended moving point. The heading angle is preferably the tire turning angle.

[0097] In the above scheme, the road curvature information is converted into a reference state variable and then moved to the objective function calculation. The steady-state error of the state quantity of the dynamic model can be reduced without feedforward compensation. At the same time, when constructing the state space, there is no need to add the longitudinal attribute of the vehicle to assist in the y coordinate conversion.

[0098] Furthermore, the kinematic model is specifically:

[0099]

[0100] Where x(t+1), y(t+1) and θ(t+1) are the abscissa of the predicted position, the ordinate of the abscissa of the predicted position and the predicted turning angle, respectively; x(t), y(t) and θ(t) are the abscissa of the current moving point, the ordinate of the current moving point and the tire turning angle of the heavy-loaded AGV at the current moving point, respectively; ω(t) is the angular velocity of the heavy-loaded AGV at the current moving point; and Δt is the prediction duration.

[0101] In a possible implementation, the dynamic window method model uses a conventional evaluation function, which is:

[0102] G(v,w)=ε(αhead(v,w)+βdist(v,w)+γvel(v,w))

[0103] Among them, the orientation weight α, obstacle weight β and speed weight γ are all set values, ε is a normalization coefficient, the best path in the path set is selected through the evaluation function, and the best path is matched with the speed sample to obtain the recommended moving point.

[0104] Specifically, the ROS (Robot Operating System) navigation system framework of the heavy-duty AGV, the core part of this navigation framework is the middle move_base function package; the static map is the node on the cloud platform; sensor coordinate conversion, odometer information, sensor information, chassis control are the nodes related to the heavy-duty AGV robot platform. The move_base function package is the core node in ROS navigation. The entire heavy-duty AGV navigation system is organized around move_base. It organizes the code in the form of a plug-in, and can replace the improved parts. These include global and local path planners, global and local cost maps, and recovery strategies. In this embodiment, the global path planner adopts the AStar algorithm, and the local path planner adopts the DWA dynamic window method that integrates global information.

[0105] Furthermore, the state quantity weight matrix includes a speed state weight and a stability state weight; the speed state weight is determined by performing gradient discrimination on the speed of the heavy-load AGV; and the stability state weight is determined according to the stability of the heavy-load AGV.

[0106] Furthermore, the stability is calculated by the path tracking lateral deviation, the path tracking heading deviation and their corresponding set maximum values. The calculation formula of the stability is as follows:

[0107]

[0108] Where A is the stability, e y is the path tracking lateral deviation, e ymax is the set maximum value of the path tracking lateral deviation, e θ is the path tracking heading deviation, e θmax Sets the maximum value for the path tracking heading deviation.

[0109] Specifically, the speed of the heavy-loaded AGV is determined according to the position data of the heavy-loaded AGV at multiple times, or the heavy-loaded AGV is determined by the collected value of the encoder of the power output shaft of the heavy-loaded AGV. The speed gradient of the heavy-loaded AGV includes levels one to five, and the corresponding speed ranges are less than 10 meters / minute, greater than or equal to 10 meters / minute and less than 15 meters / minute, greater than or equal to 15 meters / minute and less than 21 meters / minute, greater than or equal to 21 meters / minute and less than 28 meters / minute, and greater than or equal to 28 meters / minute. The corresponding stability ranges are less than 0.1, greater than or equal to less than 0.2, greater than or equal to 0.2 and less than 0.4, greater than or equal to 0.4 and less than 0.5, and greater than or equal to 0.5. The corresponding speed state weights are 0.1, 0.3, 0.5, 0.7 and 0.8.

[0110] Furthermore, the angular velocity control variable constraint is determined according to the maximum angular velocity of the heavy-load AGV and the sampling duration.

[0111] Specifically, the angular velocity control variable constraint is a weighted sum of the maximum angular velocity and the sampling duration, and the weights thereof are preferably 0.8 and 0.3, respectively.

[0112] Furthermore, the model state quantity constraint matrix is ​​determined according to the rollover condition of the heavy-loaded AGV.

[0113] Specifically, the value of each element in the model state constraint matrix is ​​equal, which is determined by multiplying the rollover index by 0.7, where the rollover index is the difference between the vertical force of the inner wheel and the vertical force of the outer wheel of the heavy-loaded AGV divided by the sum thereof.

[0114] In another possible implementation, the improved evaluation function in this embodiment is specifically:

[0115]

[0116] In the formula, ε is the normalization coefficient, is the orientation weight, α is the setting coefficient, R is the obstacle radius of the largest obstacle in the detection window, head(v,w) is the orientation function, β is the obstacle weight, dist(v,w) is the obstacle function, γ is the speed weight, vel(v,w) is the speed function, and (v,w) is the speed group.

[0117] Preferably, in the formula, the coefficient α is set to 1. When the detection window of the sensor of the heavy-load AGV is small, the coefficient α can be set to 0.2 to ensure that the proportion of the orientation weight is not too large.

[0118] It can be understood that the main function of the heading scoring function head(v,w) is to continuously guide the heavy-loaded AGV to move towards the target point during the movement of the heavy-loaded AGV. Its value can be calculated by the following formula:

[0119] head(v,w)=180-θ

[0120] Among them, θ represents the angle difference between the end of the path simulated by the kinematic model and the target point. Because the change of the heavy-duty AGV speed group directly affects the heavy-duty AGV posture state, the orientation score function also changes with the change of the speed group, and decreases with the increase of the heavy-duty AGV speed value.

[0121] The obstacle scoring function dist(v,w) represents the distance from the path simulated by the kinematic model to the nearest obstacle. The smaller the distance value, the more likely the heavy-load AGV is to collide with an obstacle; conversely, the larger the value, the farther away from the obstacle the path simulated by the current speed group is, and the safer the heavy-load AGV is to move.

[0122] The speed scoring function vel(v,w) represents the moving speed of the heavy-load AGV.

[0123] Specifically, the calculation formula of the obstacle weight is:

[0124]

[0125] Where, β is the obstacle weight, β min represents the minimum value of the obstacle distance scoring function weight when ensuring the minimum safety distance, β max represents the maximum value of the obstacle distance scoring function weight when ensuring the maximum safety distance, M represents the number of obstacles in the detection window, and ψ represents the obstacle priority.

[0126] Specifically, the calculation formula of the speed weight is:

[0127]

[0128] In the formula, γ is the speed weight. The dynamic window method is used to set the speed weight range [γ] by taking the maximum safety margin and the fastest speed as indicators in the environment and obtaining the values ​​of reaching the target point respectively. min ,γ max ], and the maximum value of the speed weight range γ is obtained max and the minimum value γ min , M represents the number of obstacles in the detection window, ρ represents the speed priority, D min Represents the minimum distance from the heavy-load AGV to the nearest obstacle, R w Represents the radius of the detection window. Therefore, it can be avoided that the speed weight is too large or too small.

[0129] It should be noted that the heavy-loaded AGV described in this embodiment is preferably a four-wheel drive steering wheel AGV omnidirectional transporter, and global path planning and local path planning are performed through laser radar scanning. The local path planning adopts the local window method model, and its purpose is to avoid obstacles in real time in a dynamic environment. During the driving process of the heavy-loaded AGV, the local path is continuously updated through the real-time data detected by the sensor, so as to locate the heavy-loaded AGV and perceive the surrounding environment of the heavy-loaded AGV, so as to find the local path to the target point and send it to the heavy-loaded AGV. There are many local path planning algorithms for heavy-loaded AGVs, and the advantages of the DWA dynamic window algorithm are mainly reflected in its low computational complexity, real-time obstacle avoidance and applicability to the two-wheel differential motion of full-phase heavy-loaded AGVs. Therefore, this embodiment uses an improved local path planning model to achieve gradient speed control of local movement of heavy-loaded AGVs.

[0130] Specifically, the detection window is a detection window of a laser radar sensor, and the obstacle density information can be obtained through scanning of the laser radar.

[0131] Further, see Figure 3 and 4 , Figure 3 The map module of the heavy-duty AGV shown in the figure collects target points and obstacle positions through laser sensors, and integrates them with the static map to generate a visual raster map. The map module uses the A-star algorithm to generate global path planning. The A-star algorithm can perform convenient and effective path planning from the starting node to the target node. However, it is limited by the classical algorithm, with a small search domain, and many redundant nodes and turning points, which makes the tracking efficiency of the heavy-duty AGV low in actual operation.

[0132] Furthermore, the PID control system based on RBF regulation includes a PID controller and a RBF neural network; an input error is obtained by calculation according to the expected angle increment, the angle data and the network output angle error; the input error is input into the PID controller to obtain the output angle; the output angle and the angle data are input into the RBF neural network to obtain the network output angle error.

[0133] Furthermore, the RBF (Radial Basis Function) neural network is also used to calculate the setting parameters of the PID controller through the gradient descent method according to the output angle and the angle data.

[0134] It should be noted that the RBF neural network preferably includes a first radial basis function neural network and a second radial basis function neural network, the first radial basis function neural network is used to generate a network output angle error by fitting the output angle and the angle data, so as to achieve the prediction of the angle error. The second radial basis function neural network is used to generate a proportional gain, an integral gain and a differential gain of a PID controller by fitting the output angle and the angle data through a gradient descent method, so that the setting parameters of the PID controller can be adaptively adjusted according to the deviation between the sensor acquisition data and the output data executed by the heavy-duty AGV, without coding or establishing a rule base based on experience, so as to achieve a better path tracking effect of the heavy-duty AGV.

[0135] The gradient descent method is an optimization algorithm for training RBF (Radial Basis Function) neural networks.

[0136] Furthermore, the expected angle increment and the angle data are weightedly summed to determine the expected angle; and the expected angle is subtracted from the network output angle error to obtain the input error.

[0137] The weights of the weighted sum of the expected rotation angle increment and the rotation angle data are preferably 0.4 and 0.6, respectively, and can be adjusted according to the sensitivity of the rotation angle data sensor. For example, if the sensitivity of the rotation angle data sensor as a voltage sensor reaches 100mV / °, the weights are 0.6 and 0.4, respectively, and if the sensitivity reaches 10mV / °, the weights are 0.4 and 0.6, respectively. Therefore, the expected rotation angle is corrected according to the sensor's ability to respond to the rotation angle change, making the input of the PID control system more accurate.

[0138] In the above scheme, the PID control system based on RBF regulation realizes that the parameters and input quantities of PID can be adjusted without coding or establishing a rule base based on experience.

[0139] It can be understood that the tracking control of the heavy-duty AGV turning angle is realized through the LPV-MPC model based on the Koopman operator theory, so that the turning angle output by the heavy-duty AGV can adapt to various situations such as different road adhesion levels. The set interpolation fitting function makes the MPC model more accurate. The road curvature information is converted into a reference state variable, and then moved to the objective function calculation. The steady-state error of the state quantity of the dynamic model can be reduced without feedforward compensation. At the same time, when constructing the state space, there is no need to add the vehicle longitudinal attribute quantity to help the y coordinate conversion.

[0140] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

[0141] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A tracking control method for heavy-duty AGV based on Koopman operator, characterized in that: include: The heavy-load AGV obtains the position data, turning angle data and road adhesion coefficient of the current control cycle through sensors; Constructing a Koopman operator theoretical model, obtaining a first eigenfunction matrix and a second eigenfunction matrix of the Koopman operator theoretical model by interpolation calculation according to the road adhesion coefficient, and generating a model state quantity of the Koopman operator theoretical model according to the position data and the turning angle data; Constructing an objective function for obtaining an increment of an angular velocity control variable and minimizing the model state quantity, wherein input variables of the objective function include the model state quantity, a model state expected deviation quantity, and a weight matrix, wherein the model state expected deviation quantity is calculated by the position data and the angular data through a local path planning model; The Koopman operator theoretical model is used as the state constraint of the objective function, and the objective function is solved to obtain the expected rotation angle increment; The desired rotation angle increment is used to determine the output rotation angle of the heavy-duty AGV in the next control cycle through a PID control system based on RBF regulation.

2. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 1 is characterized in that: The first eigenfunction matrix of the current cycle is obtained by weighted summing and normalizing the first eigenfunction matrices of multiple control cycles, wherein the weight of the weighted summing is determined by interpolating the road adhesion coefficient of the current control cycle; The second eigenfunction matrix of the current cycle is obtained by weighted summing and normalizing the second eigenfunction matrices of multiple control cycles.

3. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 1 is characterized in that: The model state expected deviation is calculated by the position data and the angle data through the local path planning model, specifically: The local path planning model includes a kinematic model, a dynamic window method model and an error calculation formula; The position data is calculated by a kinematic model of a two-wheel differential drive heavy-duty AGV to determine a predicted position and a predicted turning angle; Determine the recommended moving point by using the predicted position and the predicted turning angle through a dynamic window method model; The expected deviation of the model state is determined by the coordinates of the recommended moving point and the coordinates of the current moving point.

4. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 3 is characterized in that: The expected deviation of the model state includes a path tracking lateral deviation and a path tracking heading deviation. The path tracking lateral deviation is calculated by the difference between the coordinates of the recommended moving point and the coordinates of the current moving point and the heading angle. The path tracking heading deviation is determined by the heading angle of the current control cycle and the heading angle of the previous control cycle.

5. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 3 is characterized in that: The state quantity weight matrix includes a speed state weight and a stability state weight; The speed state weight is determined by performing gradient discrimination on the speed of the heavy-load AGV; The stability state weight is determined according to the stability of the heavy-load AGV.

6. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 5 is characterized in that: The stability is calculated by the path tracking lateral deviation, the path tracking heading deviation and their corresponding set maximum values.

7. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 1 is characterized in that: The RBF-adjusted PID control system includes a PID controller and an RBF neural network; Obtaining an input error by calculation according to the desired rotation angle increment, the rotation angle data and the network output rotation angle error; Inputting the input error into the PID controller to obtain the output angle; The output rotation angle and the rotation angle data are input into the RBF neural network to obtain the network output rotation angle error.

8. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 7 is characterized in that: Determine the expected turning angle by performing weighted summation on the expected turning angle increment and the turning angle data; The input error is obtained by subtracting the expected rotation angle from the network output rotation angle error.

9. The tracking control method of heavy-duty AGV based on Koopman operator according to claim 7, characterized in that: The RBF neural network is also used to calculate the setting parameters of the PID controller through the gradient descent method according to the output angle and the angle data.

10. The tracking control method for heavy-duty AGV based on Koopman operator according to any one of claims 1 to 9, characterized in that: The state constraints include angular velocity control variable constraints and model state quantity constraint matrices; The angular velocity control variable constraint is determined according to the maximum angular velocity of the heavy-load AGV and the sampling duration; The model state constraint matrix is ​​determined according to the overload AGV rollover condition.

Images