Cleaning robot path tracking method based on low-calculation-amount model predictive control

Abstract

The invention discloses a cleaning robot path tracking method based on low-calculation-amount model predictive control. A robot Ackerman steering kinematics model is established to construct a model predictive control optimization problem with a tracking error penalty term as a core; in order to solve the problem of high solving complexity, an efficient solver with low calculation amount is used, an Riccati equation is used for recursively calculating original variable update in the ADMM, a steady-state matrix is pre-calculated offline, complex matrix operation is avoided, and meanwhile, according to the situation that the convergence speed of the ADMM algorithm is relatively low, a prediction-corrector step is introduced to accelerate convergence of the ADMM algorithm. According to the method, the calculated amount is remarkably reduced while the path tracking precision is ensured, and the method effectively adapts to a real-time scene with limited hardware resources.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of cleaning robot device control, in particular to a cleaning robot path tracking method based on low-computational-model predictive control. BACKGROUND

[0002] With the development of modern social economy and technology and the substantial improvement of people's living standards, people's consumption patterns have changed greatly, and the demand for pork has steadily increased. China, as the world's largest pig breeding and pork product consumption country, has continuously expanded its breeding scale. If the excrement produced during breeding is not disposed of in a timely manner, harmful gases will accumulate, posing a serious threat to the health of pigs and breeders. This has led to the need for timely and rapid cleaning of the manure in the pig house. Therefore, regular environmental cleaning and sanitation in the house, improving the efficiency of manure removal and resource utilization technology have become an urgent need in the industry.

[0003] However, with the deepening of urbanization and rural aging, a large number of rural labor forces have transferred, and the aging trend of farmers has become increasingly apparent, especially in special breeding environments such as pig farms, where manual manure removal operations face problems such as low efficiency, high labor intensity, and poor working conditions. Therefore, with the advancement of Industry 4.0 and intelligent manufacturing, replacing manual labor with robots is the trend of development. The path tracking control method of the robot is an important part of the robot, which is responsible for controlling and managing the robot's movements to achieve the desired tasks and functions, enabling the robot to efficiently and accurately complete various complex tasks and improve production efficiency. Model predictive control (MPC) as a control strategy has significant advantages in controlling mobile robots. It establishes a system model to predict the state of a future time period and solves the optimal control sequence online in real time, such as minimizing path tracking error and minimizing energy consumption, achieving dynamic trade-offs for multiple objectives, especially suitable for complex pig house scenarios, providing an effective and reliable control method for cleaning robots.

[0004] However, MPC has high computational requirements, and implementation on resource-constrained robot platforms is often impractical. Existing cleaning robots need to consider equipment costs in the actual agricultural context, often facing resource constraints, poor stability, and low control accuracy when facing complex environments and harsh conditions in pig houses. Therefore, a cleaning robot path tracking method based on low-computational-model predictive control is needed to solve the above problems. SUMMARY

[0005] In order to overcome the shortcomings of the existing technology, the present invention provides a cleaning robot path tracking method based on low-computational model predictive control to solve the problems raised in the above background technology that the existing cleaning robots have poor stability and low control accuracy when facing limited resources and the complex environment and harsh conditions in the pig house.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] A cleaning robot path tracking method based on low-computation model predictive control, the method comprising the following steps:

[0008] S1. Establish a kinematic state model of the cleaning robot. The cleaning robot uses a mobile chassis based on Ackerman steering. The chassis model is simplified to an Ackerman single-vehicle kinematic model. After linearization and discretization, a discrete state space expression is obtained with vehicle position and heading as states and vehicle control as input.

[0009] S2. Establish a model predictive control optimal control problem. Using the robot's predefined fixed path and velocity curve as a tracking reference, define a quadratic penalty term and a linear term for the tracking error between the state and control input, and form a cost function in the multi-step prediction time domain. The problem is further constrained to consider the robot's workspace limitations and the hardware limitations on its motion speed, establishing speed constraints.

[0010] S3. Design a quadratic programming solver based on the alternating direction multiplier method ADMM and the linear quadratic regulator LQR. By introducing slack variables and dual variables into the variables of the cost function in step S2, a new quadratic programming problem is constructed, and then the improved alternating direction multiplier method ADMM is used for distributed iterative solution. The quadratic programming problem has the same structure as the LQR problem, so the Riccati equation is used to update the original variables in the ADMM algorithm to recursively solve the optimal variables. At the same time, in view of the relatively slow convergence speed of the ADMM algorithm, a predictor-corrector step is introduced to accelerate the convergence of the ADMM algorithm. The solver reduces the computational complexity of large-scale prediction time domain problems by integrating the distributed characteristics of ADMM, the recursive calculation advantages of LQR and the prediction and correction steps;

[0011] S4. Design a low-computational calculation method to address the high computational cost of solving the original variable update in the ADMM iteration. Based on the fact that the original variable update problem described in step S3 has the same structure as the LQR problem, use the Riccati equation to pre-calculate the steady-state gain matrix and Hessian matrix of the infinite time range LQR problem offline, and reuse the pre-calculated results in the online calculation stage. Further, by analyzing the Riccati recursive structure, only the linear terms are updated online, avoiding real-time matrix decomposition operations and compressing the computational complexity to matrix-vector multiplication.

[0012] Furthermore, in step S1, the kinematic equation established based on the Ackerman bicycle model is as follows:

[0013]

[0014] in, are the lateral and longitudinal velocity components of the vehicle in the global coordinate system, is the rate of change of the heading angle; v is the longitudinal velocity of the vehicle, δ is the front wheel steering angle, ψ is the heading angle (the angle between the longitudinal axis of the vehicle body and the x-axis of the global coordinate system), and L is the vehicle wheelbase.

[0015] Furthermore, in step S1, the vehicle state is assumed to be x = [x yψ] T , where x and y represent the lateral and longitudinal positions of the vehicle in the global coordinate system, and the control input is u = [vδ] T , let the reference state be x r =[x r y r ψ r ] T , the reference input is u r =[v r δ r ] T ,Will exist Taylor expansion is performed at , ignoring the higher-order terms. The error state equation obtained by linearizing Equation (1-1) is as follows:

[0016]

[0017] in, Jacobian matrix and They are:

[0018]

[0019] Using the forward Euler method, assuming the sampling period is T, the above formula (1-3) is converted into a discrete form:

[0020]

[0021] The final discrete error state space expression is obtained as follows:

[0022]

[0023] where A d = TA+I, B d = TB.

[0024] Further, in the step S2, the reference trajectory is a predefined path and speed curve, including state quantity reference x r = [x r y r ψ r ] T and control quantity reference u r = [v r δ r ] T In the prediction horizon N, a weighted quadratic penalty term and a linear term of multi-step tracking error are designed to establish a cost function as follows:

[0025]

[0026] where N is the prediction interval, Q is the state error weight matrix, R is the control quantity error weight matrix, Q f is the terminal state error weight matrix, q and r are linear term vectors;

[0027] In combination with the physical limitations of the robot and the actual working scene requirements, the control quantity constraint is set as follows:

[0028]

[0029] where,

[0030] Based on the discrete error state space expression (1-5) of step S1, the kinematic constraint is set to force the state transition in the prediction horizon to satisfy the kinematic law;

[0031] The above cost function (1-6) and the constraints constructed by (1-5) and (1-7) are combined into the following constrained optimization problem:

[0032]

[0033] where, represents the state sequence from time k to k+N, represents the control input sequence from time k to k+N-1.

[0034] Further, in the step S3, the MPC optimization problem (1-8) defined in the step S2 is introduced with the relaxation variables and the dual variables λ 0:N , μ 0:N-1 , and the original problem is converted into the following form:

[0035]

[0036] where J is the cost function of the original problem, and is the indicator function of the set constraint, defined as shown in (1-10), λ and μ are the state relaxation variables, control relaxation variables, state dual variables and control dual variables respectively over the whole time horizon, and ρ is the coefficient of the augmented Lagrangian penalty term;

[0037]

[0038] where the set χ is defined as: the set σ is defined as:

[0039]

[0040] Further, in the step S3, the Alternating Direction Multiplier Method (ADMM) is used to solve the optimization problem by alternating the update of variables in the following three steps:

[0041] (3.1) Original variable update: fixing the relaxation variables and the dual variables λ 0:N , μ 0:N-1 , solve the original variables

[0042] (3.2) Relaxation variable update: fixing the original variables and the dual variables λ 0:N , μ 0:N-1 , solve the relaxation variables

[0043] (3.3) Dual variable update: fixing the original variables and the relaxation variables solve the dual variables λ 0:N , μ 0:N-1 .

[0044] Further, in the step S3, the Alternating Direction Multiplier Method (ADMM) is used to solve the optimization problem by alternating the update of variables in the following three steps:

[0045]

[0046] where

[0047] The relaxation variable update is converted into linear projection in the case of quadratic programming, and the specific form is as follows:

[0048]

[0049] The dual variable update formula is as follows:

[0050]

[0051] The original variable update problem (1-11) has the same structure as LQR, and the Riccati equation is used to solve the original variable update;

[0052] The iteration termination condition of the ADMM algorithm is as follows:

[0053]

[0054] wherein, and is the original feasibility residual, and is the dual feasibility residual, when the original feasibility residual and the dual feasibility residual are both less than the respective given termination condition, the ADMM iteration ends, and the algorithm terminates.

[0055] Further, in the step S3, the form of the prediction correction step is as follows:

[0056]

[0057] wherein, when or that is, when the original feasibility residual is less than the last original feasibility residual or the dual feasibility residual is less than the last dual feasibility residual, the relaxation variable update and the dual variable update are changed to formula (1-15).

[0058] Further, in the step S4, the implementation steps of the low-computational-load optimization method are as follows:

[0059] (4.1) Offline pre-computing steady-state matrix:

[0060]

[0061] wherein, i∈[0, N-1], for the LQR problem of infinite time range, K N is solved by starting from P f (where N is infinite) and recursively from back to front, and K i and P i, and the steady-state gain matrix K is obtained ∞ and the Hessian matrix P ∞ , K ∞ With P ∞ Substituting back into the Riccati equation (1-16):

[0062]

[0063] Among them, i∈[0,N-1], C2=(A d -B d K i ) T ;

[0064] Pre-calculate and store the terms C1 and C2 in equation (1-17) that have the largest iterative update computational complexity.

[0065] (4.2) Optimization of online calculation phase:

[0066] Using the pre-calculated stored C1 and C2, let p N =q f Calculate p by reverse recursion 0:N-1 with d 0:N-1 , and the final original variable update conversion is as follows:

[0067]

[0068] Among them, A d is the system error state matrix, B d is the system error control input matrix, K ∞ is the steady-state gain matrix;

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

[0070] To meet the cleaning needs of complex farming environments like piggeries, the system integrates a low-computation Model Predictive Control (MPC) optimization algorithm to significantly reduce the algorithm's reliance on hardware resources, enabling efficient path tracking on resource-constrained equipment. While ensuring safe obstacle avoidance, it improves cleaning efficiency and reduces equipment costs, effectively resolving the conflict between rural labor shortages and the difficulty in popularizing high-cost automated equipment. It provides a highly stable, low-cost robotic solution for smart farming in an aging population. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 It is a block diagram for implementing the method of the present invention;

[0072] Figure 2 is a reference path diagram of an example of the present invention;

[0073] Figure 3 is a figure of verification results of an example method of the present application;

[0074] Figure 4 is a figure of iteration number of an example alternating direction multiplier method acceleration algorithm of the present application. DETAILED DESCRIPTION

[0075] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the present application.

[0076] Reference Figures 1-4 In this example, a kinematic model of the cleaning robot is modeled to track a Bernoulli double spiral function to verify the method proposed in the present application. Before the cleaning robot tracks the specified task, the pre-set Bernoulli double spiral function is as shown in Figure 2 First, in step S1, the state space expression of the cleaning robot is established, and a discretization process is performed. Then, in step S2, a multi-step prediction model predictive control problem is constructed, which contains a quadratic penalty term and a linear term of tracking error, and fuses the workspace and hardware speed constraint limits. Then, in step S3, a low-complexity solver is designed, which uses the distributed iterative characteristics of ADMM and the recursive structure of Riccati equation of LQR to reduce the computational complexity, and introduces a prediction correction step to accelerate the convergence of the ADMM algorithm. Finally, in step S4, an offline pre-computation of complex matrices and an online update of only the linear term mechanism are adopted to achieve efficient solution. The optimal control input of the cleaning robot is obtained by solving the constructed model predictive control problem using the designed solver, which is used to guide the motion of the cleaning robot chassis. The experimental results are shown in Figure 3 , which shows the tracking trajectory of the path tracking problem established based on the kinematic model of the cleaning robot. The iteration number figure of the accelerated ADMM algorithm is shown in Figure 4 , which shows the iteration convergence number of ADMM each time the optimization problem is solved as the time domain of model predictive control is rolled.

[0077] A cleaning robot path tracking method based on low-complexity model predictive control, the method comprising the following steps:

[0078] S1, a kinematic state model of a cleaning robot is established, the cleaning robot uses a mobile chassis based on Ackerman steering, the chassis model is simplified as an Ackerman bicycle kinematic model, after linearization and discretization, a discrete state space expression is obtained, taking the vehicle position and heading as the state, and the vehicle control as the input;

[0079] In step S1, the kinematic equation established based on the Ackerman bicycle model is as follows:

[0080]

[0081] in, are the lateral and longitudinal velocity components of the vehicle in the global coordinate system, is the rate of change of the heading angle; v is the longitudinal velocity of the vehicle, δ is the front wheel steering angle, ψ is the heading angle (the angle between the longitudinal axis of the vehicle body and the x-axis of the global coordinate system), and L is the vehicle wheelbase; in this example, L is set to 0.9m;

[0082] In step S1, the vehicle state is x = [x y ψ] T , where x and y represent the lateral and longitudinal positions of the vehicle in the global coordinate system, and the control input is u = [vδ] T , let the reference state be x r =[x r y r ψ r ] T , the reference input is u r =[v r δ r ] T ,Will exist Taylor expansion is performed at , ignoring the higher-order terms. The error state equation obtained by linearizing Equation (1-1) is as follows:

[0083]

[0084] in, Jacobian matrix and They are:

[0085]

[0086] In step S1, the forward Euler method is used, and the sampling period is set to T, and the above formula (1-3) is converted into a discrete form:

[0087]

[0088] Finally, the discrete error state space expression is obtained:

[0089]

[0090] Among them A d =TA+I,B d =TB; in this example, the reference path

[0091] i = 0, 1,..., 360, i = 0, 1,..., 360, ψ r equal to the reference

[0092] The slope of the path is examined, and there are 360 state quantities x r on the reference path r = 0.5 m / s, δ r equal to v r The product of the curvature on the reference path;

[0093] S2, establish a model predictive control optimal control problem, to track the reference of the predefined fixed path and speed curve of the robot, define the tracking error quadratic penalty term and linear term of the state quantity and control input, form the cost function in the multi-step prediction horizon, the problem is further set the constraint condition, considering the workspace limit of the robot, and its movement speed is subject to hardware limit, set the speed constraint condition;

[0094] In the step S2, the reference trajectory is a predefined path and speed curve, including the state quantity reference x r = [x r y r ψ r ] T And the control quantity reference u r = [v r δ r ] T In the prediction horizon N, the weighted quadratic penalty term and linear term of the multi-step tracking error are designed, and the cost function is established as follows:

[0095]

[0096] Wherein, N is the prediction interval, Q is the state error weight matrix, R is the control quantity error weight matrix, Q f is the terminal state error weight matrix, q and r are linear term vectors; in this example, The initial state x(k) = [27 12 π] T ;

[0097] In the step S2, the control quantity constraint is set in combination with the physical limit of the robot and the actual working scene demand:

[0098]

[0099] Wherein, In this example, v min = 0 m / s, δ min = -0.6981 rad, vmax = 2.7 m / s, δ max = 0.6981 rad;

[0100] In the step S2, based on the discrete error state space expression (1-5) of the step S1, kinematic constraints are set to force the state transition in the prediction horizon to satisfy the kinematic law;

[0101] The above cost function (1-6) is combined with the constraints constructed by (1-5), (1-7) as follows:

[0102]

[0103] wherein, represents a state sequence from time k to k+N, represents a control input sequence from time k to k+N-1;

[0104] S3, design a quadratic programming solver based on the alternating direction multiplier method (ADMM) and the linear quadratic regulator (LQR), introduce a relaxation variable and a dual variable to the variables of the cost function in step S2 to construct a new quadratic programming problem, and then use the improved alternating direction multiplier method (ADMM) to solve it iteratively. The quadratic programming problem has the same structure as the LQR problem, so the Riccati equation is used to update the original variable in the ADMM algorithm to solve the optimal variable recursively. In addition, in order to speed up the convergence of the ADMM algorithm, a predictor-corrector step is introduced to accelerate the convergence of the ADMM algorithm. The solver reduces the computational complexity of large-scale prediction horizon problems by combining the distributed characteristics of ADMM, the recursive calculation advantage of LQR, and the predictor-corrector step;

[0105] In the step S3, the MPC optimization problem (1-8) defined in step S2 is introduced into the relaxation variable and the dual variable λ 0:N , μ 0:N-1 , and the original problem is converted to the following form:

[0106]

[0107] wherein J is the cost function of the original problem, and are the indicator functions of the set constraints, which are defined as shown in (1-10), λ and μ are the state relaxation variable, the control relaxation variable, the state dual variable and the control relaxation variable in the entire time domain, respectively, and ρ is the augmented Lagrange penalty term coefficient;

[0108]

[0109] where the set χ is defined as: The set σ is defined as:

[0110]

[0111] In the step S3, the alternating direction multiplier method ADMM solves the optimization problem by updating the variables alternately through the following three steps:

[0112] (3.1) Original variable update: fixing the relaxation variable and the dual variable λ 0:N , μ 0:N-1 , solve the original variable

[0113] (3.2) Relaxation variable update: fixing the original variable and the dual variable λ 0:N , μ 0:N-1 , solve the relaxation variable

[0114] (3.3) Dual variable update: fixing the original variable and the relaxation variable Solve the dual variable λ 0:N , μ 0:N-1 ;

[0115] In the present application, in the original variable update, the optimization problem (1-9) becomes the following optimization problem:

[0116]

[0117] where

[0118] In the present application, in the relaxation variable update, in the case of quadratic programming, it is converted into linear projection, and the specific form is as follows:

[0119]

[0120] In the present application, the dual variable update formula is as follows:

[0121]

[0122] In the step S3, the defined original variable update problem (1-11) has the same structure as LQR, and the original variable is updated and solved by Riccati equation;

[0123] In the present application, the iteration termination condition of the ADMM algorithm is as follows:

[0124]

[0125] wherein, and are primal feasibility residuals, and are dual feasibility residuals, the ADMM iteration ends and the algorithm terminates when both the primal feasibility residuals and the dual feasibility residuals are simultaneously less than their respective given termination conditions;

[0126] In the step S3, the prediction correction step has the following specific form:

[0127]

[0128] wherein, when or that is, when the primal feasibility residual is less than the previous primal feasibility residual or the dual feasibility residual is less than the previous dual feasibility residual, the relaxation variable update and the dual variable update are changed to formula (1-15); in the present example, φ0=1;

[0129] S4, a low-computational-cost calculation method is designed, which is based on the fact that the primal variable update problem in the ADMM iteration has the same structure as the LQR problem, and uses the Riccati equation to pre-calculate the steady-state gain matrix and the Hessian matrix of the LQR problem in an infinite time range offline, and reuse the pre-calculation results in the online calculation stage. The step further analyzes the Riccati recursive structure, only updates the linear term online, completely avoids real-time matrix decomposition operations, and compresses the calculation amount to matrix and vector multiplication;

[0130] In the step S4, the implementation steps of the low-computational-cost optimization method are as follows:

[0131] (4.1) Pre-calculate the steady-state matrix offline:

[0132]

[0133] wherein, i∈[0, N-1], for the LQR problem in an infinite time range, K N is solved from the back to the front starting from P f (where N is infinite) in the above algebraic Riccati equation (1-16), K i and P i are obtained, K ∞ is the steady-state gain matrix and P ∞ is the Hessian matrix, K ∞ and P ∞ are substituted back into the Riccati equation (1-16):

[0134]

[0135] wherein, i∈[0, N-1], C2= (A d -B d K i ) T ;

[0136] The item C1 and C2 with the largest iterative update calculation amount in the formula (1-17) above are pre-calculated and stored;

[0137] (4.2) Online calculation stage optimization:

[0138] Using the pre-calculated and stored C1 and C2, let p N =q f Through the way of backward recursion, p 0:N-1 and d 0:N-1 are calculated, and finally the original variable update is converted as follows:

[0139]

[0140] wherein, A d is a system error state matrix, B d is a system error control input matrix, and K ∞ is a steady-state gain matrix.

[0141] It is apparent to those skilled in the art that the present application is not limited to the details of the foregoing exemplary embodiments, but can be implemented in other concrete forms without departing from the spirit or essential characteristics of the present application. Thus, the embodiments should be considered in all respects as illustrative and not restrictive, the scope of the present application being defined by the appended claims rather than the foregoing description, and it is intended that all changes and modifications which come within the meaning and range of equivalency of the claims are to be embraced therein, any reference signs in the claims not being regarded as limiting the claims concerned.

[0142] The above description is only the preferred embodiment of the present application, but the protection scope of the present application is not limited to this, and any skilled person in the art can make equivalent substitution or change according to the technical solution and the inventive concept of the present application within the technical range disclosed by the present application, which should be covered in the protection scope of the present application.

Claims

1. A cleaning robot path tracking method based on low computational model predictive control, characterized in that: The method comprises the following steps: S1. Establish a kinematic state model of the cleaning robot. The cleaning robot uses a mobile chassis based on Ackerman steering. The chassis model is simplified to an Ackerman single-vehicle kinematic model. After linearization and discretization, a discrete state space expression is obtained with vehicle position and heading as states and vehicle control as input. S2. Establish a model predictive control optimal control problem. Using the robot's predefined fixed path and velocity curve as a tracking reference, define a quadratic penalty term and a linear term for the tracking error between the state and control input, and form a cost function in the multi-step prediction time domain. The problem is further constrained to consider the robot's workspace limitations and the hardware limitations on its motion speed, establishing speed constraints. S3. Design a quadratic programming solver based on the alternating direction multiplier method ADMM and the linear quadratic regulator LQR. By introducing slack variables and dual variables into the variables of the cost function in step S2, a new quadratic programming problem is constructed, and then the improved alternating direction multiplier method ADMM is used for distributed iterative solution. The quadratic programming problem has the same structure as the LQR problem, so the Riccati equation is used to update the original variables in the ADMM algorithm to recursively solve the optimal variables. At the same time, in view of the relatively slow convergence speed of the ADMM algorithm, a prediction and correction step is introduced to accelerate the convergence of the ADMM algorithm. The solver integrates the distributed characteristics of ADMM, the recursive calculation advantages of LQR, and the prediction and correction step; S4. Design a low-computational calculation method to address the high computational cost of solving the original variable update in the ADMM iteration. Based on the fact that the original variable update problem described in step S3 has the same structure as the LQR problem, use the Riccati equation to pre-calculate the steady-state gain matrix and Hessian matrix of the infinite time range LQR problem offline, and reuse the pre-calculated results in the online calculation stage. Further, by analyzing the Riccati recursive structure, only the linear terms are updated online, avoiding real-time matrix decomposition operations, and compressing the computational complexity to matrix and vector multiplication.

2. The cleaning robot path tracking method based on low computational model predictive control according to claim 1 is characterized in that: In step S1, the kinematic equation established based on the Ackerman bicycle model is as follows: in, are the lateral and longitudinal velocity components of the vehicle in the global coordinate system, is the rate of change of the heading angle; v is the longitudinal velocity of the vehicle, δ is the front wheel steering angle, ψ is the heading angle, that is, the angle between the longitudinal axis of the vehicle body and the x-axis of the global coordinate system, and L is the vehicle wheelbase.

3. The cleaning robot path tracking method based on low computational model predictive control according to claim 2, characterized in that: In step S1, the vehicle state is x = [xy ψ] T , where x and y represent the lateral and longitudinal positions of the vehicle in the global coordinate system, and the control input is u = [v δ] T , let the reference state be x r =[x r y r ψ r ] T , the reference input is u r =[v r δ r ] T ,Will exist Taylor expansion is performed at , ignoring the higher-order terms. The error state equation obtained by linearizing Equation (1-1) is as follows: in, Jacobian matrix and They are: Using the forward Euler method, assuming the sampling period is T, the above formula (1-3) is converted into a discrete form: Finally, the discrete error state space expression is obtained: Part A d = TA+I, B d =TB.

4. The cleaning robot path tracking method based on low computational model predictive control according to claim 3 is characterized in that: In step S2, the reference trajectory is set as a predefined path and speed curve, including the state reference x r =[x r y r ψ r ] T and control quantity reference u r =[v r δ r ] T , within the prediction time domain N, the weighted quadratic penalty term and linear term of the multi-step tracking error are designed, and the cost function is established as follows: Among them, N is the prediction interval, Q is the state error weight matrix, R is the control error weight matrix, Q f is the terminal state error weight matrix, q and r are linear term vectors; Combining the physical limitations of the robot with the actual working scenario requirements, set the control quantity constraints: in, Based on the discrete error state space expression (1-5) of step S1, kinematic constraints are set to force the state transition in the prediction time domain to satisfy the kinematic law; Combining the above cost function (1-6) with the constraints constructed by (1-5) and (1-7) into the following constrained optimization problem: in, represents the state sequence from time k to k+N, Represents the control input sequence from time k to k+N-1.

5. The cleaning robot path tracking method based on low computational model predictive control according to claim 4, characterized in that: In step S3, slack variables are introduced into the MPC optimization problem (1-8) defined in step S2. and the dual variable λ 0:N 、μ 0:N-1 , transforming the original problem into the following form: Among them, J is the cost function of the original problem, and is the indicator function of the set constraint, which is defined as follows (1-10). λ and μ are the state slack variable, control slack variable, state dual variable and control slack variable in the entire time domain, respectively; ρ is the coefficient of the augmented Lagrangian penalty term; The set χ is defined as: The set σ is defined as:

6. The cleaning robot path tracking method based on low computational model predictive control according to claim 5, characterized in that: In step S3, the alternating direction multiplier method ADMM solves the optimization problem by alternatingly updating variables in the following three steps: (3.1) Original variable update: fixed slack variables and the dual variable λ 0:N , μ 0:N-1 , solving for the original variable (3.2) Slack variable update: fix the original variables and the dual variable λ 0:N , μ 0:N-1 , solve for the slack variables (3.3) Dual variable update: fix the original variable and slack variables Solving for the dual variable λ 0:N , μ 0:N-1 .

7. The cleaning robot path tracking method based on low computational model predictive control according to claim 6, characterized in that: In the original variable update, the optimization problem (1-9) becomes the following optimization problem: in In the update of slack variables, in the case of quadratic programming, it is converted into linear projection, which is as follows: The dual variable update formula is as follows: The original variable update problem (1-11) defined in formula (1-11) has the same structure as LQR, and the original variable update is solved using the Riccati equation; The iteration termination conditions of the ADMM algorithm are as follows: Among them, l and is the original feasibility residual, and is the dual feasibility residual. When both the original feasibility residual and the dual feasibility residual are less than their respective given termination conditions, the ADMM iteration ends and the algorithm terminates.

8. The cleaning robot path tracking method based on low computational model predictive control according to claim 7, characterized in that: In step S3, the prediction and correction steps are as follows: Among them, when l i+1 <l i , or That is, when the original feasibility residual is smaller than the previous original feasibility residual or the dual feasibility residual is smaller than the previous dual feasibility residual, the slack variable update and the dual variable update are changed to formula (1-15).

9. The cleaning robot path tracking method based on low computational model predictive control according to claim 8, characterized in that: In step S4, the low computational effort optimization method is implemented as follows: (4.1) Offline pre-calculation of steady-state matrix: Among them, i∈[0, N-1], for the LQR problem with infinite time horizon, by N =Q f (where N is infinite) and work backwards to solve the above algebraic Riccati equation (1-16) for K. i With P i , and the steady-state gain matrix K is obtained ∞ and the Hessian matrix P ∞ , K ∞ With P ∞ Substituting back into the Riccati equation (1-16): Among them, i∈[0,N-1], C2=(A d -B d K i ) T ; Pre-calculate and store the terms C1 and C2 in equation (1-17) that have the largest iterative update computational complexity. (4.2) Optimization of online calculation phase: Using the pre-calculated stored C1 and C2, let p N =q f Calculate p by reverse recursion 0:N-1 with d 0:N-1 , and the final original variable update conversion is as follows: Among them, A d is the system error state matrix, B d is the system error control input matrix, K ∞ is the steady-state gain matrix.

Images