An emergency material transportation robot control system and method

Abstract

The present invention discloses a control system and method for an emergency material transport robot, relating to the field of trajectory tracking control for emergency material transport robots. To address the difficulties inherent in trajectory tracking control for emergency material transport robots, various methods are employed to optimize system performance. A recursive least squares parameter identification method is employed to identify the robot's parameters, improving model accuracy. A fuzzy control method is employed to accelerate the robot's trajectory tracking speed. A model predictive control method, which can effectively handle multivariable and constrained problems, is employed to design the controller. Although model predictive control offers strong robustness, its performance deteriorates when the system is subject to persistent disturbances. Therefore, an extended state observer is incorporated to estimate disturbances and improve trajectory tracking accuracy.

Description

Technical Field

[0001] The present invention relates to the field of trajectory tracking control of emergency material transport robots, and in particular to a control system and method for an emergency material transport robot. Background Art

[0002] Contactless delivery plays a crucial role in the transportation of emergency supplies. Trajectory tracking and control of emergency supply transport robots is a key technology for achieving precise contactless delivery. Emergency supply transport robots can assist or even replace human labor. When rescue and supply transport operations are limited in inaccessible environments such as fires, earthquakes, deep water, and potholes, the flexible and adaptable nature of these robots can effectively reduce the risks posed to disaster victims and provide essential support. In certain infectious disease medical rescue operations, it's crucial to minimize the flow of personnel and reduce the risk of cross-infection. Thanks to the increasing adoption of emergency supply transport robots, the accuracy and efficiency of supply transport are guaranteed while maximizing human safety and health. Therefore, emergency supply transport robots capable of contactless delivery play a crucial role, replacing manual labor with machines and enabling precise, rapid, and safe supply transport through the control of these robots.

[0003] Currently, sliding mode control strategies are commonly used for trajectory tracking control of emergency material transport robots. This strategy is not only robust and responsive, but also eliminates the need for online system identification. However, the system's chattering problem can affect control accuracy. When transporting loads, determining certain system parameters is difficult, and factors such as unknown mass and friction can reduce control accuracy. Actual robot systems contain a variety of constraints, and ignoring these constraints can lead to poor control performance or even system instability. Emergency material transport robots are susceptible to interference during transport, including not only internal disturbances such as parameter uncertainty but also external disturbances such as static friction and control signal noise. Therefore, improving the control system's anti-interference capability is crucial.

[0004] To address the difficulties in trajectory tracking control for an emergency material transport robot, different approaches are employed to optimize system performance. Recursive least squares parameter identification is used to identify the robot's parameters and improve model accuracy. Fuzzy control is employed to accelerate the robot's trajectory tracking. Finally, a model predictive control approach, which effectively handles multivariable and constrained problems, is employed to design the controller. Although model predictive control offers strong robustness, its performance deteriorates when the system is subject to persistent disturbances. Therefore, an extended state observer (ESO) is incorporated to estimate disturbances and improve trajectory tracking accuracy. This control method for the emergency material transport robot, based on fuzzy model predictive control and the ESO, ensures that the robot maintains stability and anti-interference capabilities while accurately transporting materials, enabling "contactless," efficient, and convenient material transportation. Summary of the Invention

[0005] The purpose of the present invention is to provide an emergency material transport robot control system and method to solve the problems raised in the above background technology.

[0006] To achieve the above objectives, the present invention provides the following technical solutions: a control system and method for an emergency material transport robot, characterized in that it includes the following steps:

[0007] S1: Define the state quantity X of the emergency material transport robot, including the horizontal coordinate position x, the vertical coordinate position y, and the angle θ between the forward direction and the horizontal coordinate. The control quantity u includes the speed v and the angular velocity ω. Given the desired state quantity X r (k) and the desired control quantity u r (k);

[0008] S2: Establish a kinematic model of the emergency material transport robot and use the recursive least squares method to identify the parameters in the model;

[0009] S3: The state error is obtained based on the actual state and the expected state: The control error is Establish a discrete error model for trajectory tracking;

[0010] S4: Analyze the impact of the error between the actual position and the expected position of the emergency material transport robot on the trajectory tracking speed;

[0011] S5: According to the influence of the position error of the emergency material transport robot on the trajectory tracking speed, fuzzy control is used to optimize the speed so that:

[0012] (1) When the lateral and longitudinal errors between the actual position of the emergency material transport robot and the reference trajectory are relatively large, in order to quickly reduce the tracking error, the robot's tracking goal should be emphasized and the value of the speed error should be appropriately increased;

[0013] (2) When the lateral error and longitudinal error between the actual position of the emergency material transport robot and the reference trajectory are relatively small, in order to maintain better stability, the value of the speed error is appropriately reduced;

[0014] S6: As the emergency material transport robot is inevitably subject to external interference during actual operation, an extended state observer is designed to accurately estimate the disturbance and compensate it into the control variable obtained in S7;

[0015] S7: Design a model predictive controller to implement trajectory tracking control of the emergency material transport robot:

[0016] (1) According to the goals to be achieved and the constraints existing in the emergency material transport robot, the objective function J(k) and the constraints are given;

[0017] (2) Given the prediction time domain N p and control time domain N c , transform the objective function into a quadratic programming form;

[0018] (3) Obtain all control inputs in the control time domain by solving the quadratic programming problem, and take the first element in the control sequence as the actual control input to act on the system;

[0019] (4) If the emergency material transport robot does not track the reference trajectory, return to S2;

[0020] Furthermore, an expected trajectory is set on the activity trajectory of the emergency material transport robot, so that the emergency material transport robot tracks the expected trajectory and determines the current state quantity and control quantity of the robot.

[0021] Furthermore, the model of the emergency material transport robot uses a recursive least squares method to identify parameters in the model, and the parameter identification process includes:

[0022] (1) Splitting: The emergency material transport robot system is a MIMO system. The MIMO system is split into multiple SISO systems using recursive least squares parameter identification.

[0023] (2) Goal: Make the state quantity of each measurement consistent with the estimated value of the parameter to be estimated Determined measurement estimate The sum of squares of the differences is minimal;

[0024] (3) Algorithm: The observation data obtained from N+1 experiments are used to obtain the recursive least squares algorithm as shown in formulas (1) to (3).

[0025]

[0026]

[0027]

[0028] Where: Definition Time estimate, K j(N+1) —Gain matrix, X j (N+1)—the observed value of the state quantity at time N+1, F j(N+1) —The set of measured values of state and control quantities at time N+1, —prediction of the value of this measurement based on the previous measurement, — prediction error;

[0029] (4) Stopping mechanism: When the parameter estimation of the recursive least squares method reaches a certain accuracy, the stopping criterion shown in formula (4) is used to automatically stop the recursive operation.

[0030]

[0031] Where: δ is a suitably small number.

[0032] Furthermore, the emergency material transport robot adjusts the tracking direction according to the angular velocity ω and eliminates the state error according to the linear velocity v; when the actual position of the emergency material transport robot lags behind the expected position, it needs to accelerate, and when the emergency material transport robot is ahead of the expected position, it needs to decelerate to compensate for the error.

[0033] Furthermore, x e and y e As the input of the fuzzy controller, v e and ω e A fuzzy controller is designed as the output of the fuzzy controller, and 7 fuzzy subsets of the input and output are selected. The fuzzy control subset language can be expressed as {NB (negative large), NM (negative medium), NS (negative small), ZE (zero), PS (positive small), PM (positive medium), PB (positive large)}.

[0034] Furthermore, the controller input variable adopts a Gaussian membership function as the membership function of the input variable, and the controller output variable adopts a triangle and a trapezoid as the membership function of the output variable; the membership functions obtained by the Gaussian membership function, the triangle and the trapezoid clarify the fuzzy value through the centroid method.

[0035] Furthermore, the total disturbance of the system is estimated using ESO, and the estimated value is used to perform feedforward compensation on the emergency material transport robot system to improve the stability and anti-interference ability of the system. The linear ESO is designed as shown in formula (5).

[0036]

[0037] Where: — estimated value of the system state ξ(k), -interference The estimated value of , β0, β1 — observer gain matrix;

[0038] Furthermore, the objective function can simultaneously reflect the system's requirements for the controller's accuracy in tracking the reference trajectory and the stability of the control input variation. The objective function can accurately constrain the output state error and the control increment and add a relaxation factor.

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

[0040] 1. In view of the difficulties in trajectory tracking control of the emergency material transport robot, different methods are adopted to optimize the system performance; the recursive least squares parameter identification method is used to identify the parameters of the emergency material transport robot to improve the accuracy of the model; the fuzzy control method is used to accelerate the robot's trajectory tracking speed; and the model predictive control method that can effectively handle multi-variable and constrained problems is used to design the controller.

[0041] 2. Although model predictive control is highly robust, its performance deteriorates when the system is subject to continuous interference. Therefore, an extended state observer is added to estimate interference and improve trajectory tracking accuracy, enabling the robot to maintain its own stability and anti-interference capabilities while accurately transporting materials, achieving "contactless", efficient and convenient material transportation. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 This is a flow chart of trajectory tracking control for an emergency material transport robot based on fuzzy model predictive control and extended state observer of the present invention;

[0043] Figure 2 This is a schematic diagram of the trajectory tracking control principle of the emergency material transport robot of the present invention;

[0044] Figure 3 A diagram showing the expected and actual positions of the emergency material transport robot of the present invention;

[0045] Figure 4 This is a structural diagram of the fuzzy control system for the speed of the emergency material transport robot of the present invention;

[0046] Figure 5 This is a structural diagram of the model predictive control system based on the extended state observer of the present invention. DETAILED DESCRIPTION

[0047] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0048] See also Figures 1 to 5 In the embodiment of the present invention,

[0049] The present invention provides a control system and method for an emergency material transport robot. The method steps of trajectory tracking control are as follows: Figure 1 As shown:

[0050] S1: If Figure 2 As shown in the figure, the state quantity X of the emergency material transport robot includes the horizontal coordinate position x, the vertical coordinate position y and the angle θ between the forward direction and the horizontal coordinate. The control quantity u includes the speed v and the angular velocity ω. Given a reference trajectory and the desired state quantity X r (k) and the desired control quantity u r (k) enabling the emergency material transport robot to track a reference trajectory;

[0051] S2: Establish a kinematic model for the emergency material transport robot and use the recursive least squares method to identify the parameters in the model. The parameter identification process includes:

[0052] (1) Splitting: The emergency material transport robot system is a MIMO system. The MIMO system is split into multiple SISO systems using recursive least squares parameter identification.

[0053] (2) Goal: Make the state quantity of each measurement consistent with the estimated value of the parameter to be estimated Determined measurement estimate The sum of squares of the differences is minimal;

[0054] (3) Algorithm: The observation data obtained from N+1 experiments are used to obtain the recursive least squares algorithm as shown in formulas (1) to (3).

[0055]

[0056]

[0057]

[0058] Where: Definition —N time estimated value, K j(N+1) —Gain matrix, X j (N+1)—the observed value of the state quantity at time N+1, F j(N+1)—The set of measured values of state and control quantities at time N+1, —prediction of the value of this measurement based on the previous measurement, — prediction error;

[0059] (4) Stopping mechanism: When the parameter estimation of the recursive least squares method reaches a certain accuracy, the stopping criterion shown in formula (4) is used to automatically stop the recursive operation.

[0060]

[0061] Where: δ is a suitably small number;

[0062] S3: The state error is obtained based on the actual state and the expected state: The control error is Establish a discrete error model for trajectory tracking;

[0063] S4: The adjustment of the tracking direction of the emergency material transport robot mainly depends on the angular velocity ω, and the elimination of the state error mainly depends on the linear velocity v. The influence of the error between the actual position and the expected position of the emergency material transport robot on the trajectory tracking speed is analyzed; Figure 3 As shown in Figure (a), if the lateral error of the robot at II is greater than 0, it means that the robot lags behind the expected position in the horizontal coordinate direction and needs to be accelerated. If the longitudinal error is greater than 0, it means that the robot lags behind the expected position in the vertical coordinate direction and needs to be accelerated. If the lateral error of the robot at II is less than 0 in Figure (b), it means that the robot is ahead of the expected position in the horizontal coordinate direction and needs to be decelerated to compensate for the lateral error. If the longitudinal error is less than 0, it means that the robot is ahead of the expected position in the vertical coordinate direction, but the forward direction is now to the right, so acceleration is needed to compensate for the longitudinal error.

[0064] S5: If Figure 4 As shown in the figure, according to the influence of the position error of the emergency material transport robot on the trajectory tracking speed, fuzzy control is used to optimize the speed so that:

[0065] (1) When the lateral and longitudinal errors between the actual position of the emergency material transport robot and the reference trajectory are relatively large, in order to quickly reduce the tracking error, the robot's tracking goal should be emphasized and the value of the speed error should be appropriately increased;

[0066] (2) When the lateral error and longitudinal error between the actual position of the emergency material transport robot and the reference trajectory are relatively small, in order to maintain better stability, the value of the speed error is appropriately reduced;

[0067] Since the lateral error and longitudinal error between the actual trajectory of the robot and the reference trajectory are important parameters affecting the tracking accuracy and speed of the system, x e and y eAs the input of the fuzzy controller, v e and ω e As the output of the fuzzy controller, a fuzzy controller is designed; 7 fuzzy subsets are selected for input and output, and the fuzzy control subset language can be expressed as {NB (negative large), NM (negative medium), NS (negative small), ZE (zero), PS (positive small), PM (positive medium), PB (positive large)}; since the controller requires the input variable to be input in a relatively flat curve, the Gaussian membership function is selected as the membership function of the input variable, and the output variable should pay more attention to sensitivity, so that the system can quickly respond to the performance indicators of tracking and ensure safe operation, so the triangle and trapezoid are used as the membership function of the output variable. Establish a speed fuzzy control rule table, as shown in Table 1 and Table 2,

[0068] Table 1 Fuzzy control rules for linear velocity error

[0069]

[0070] Table 2 Fuzzy control rules for angular velocity error

[0071]

[0072] The control output obtained by the above fuzzy rule reasoning is still a fuzzy value. In order to be able to be applied to the actual robot system, the center of gravity method with simple structure and convenient calculation is used to clarify the fuzzy value.

[0073] Step 6: In view of the fact that the emergency material transport robot is inevitably subject to external interference during its actual operation, an extended state observer is designed to estimate the total disturbance received by the system, and the estimated value is feedforward compensated in the emergency material transport robot system to improve the stability and anti-interference ability of the system. The linear ESO is designed as shown in formula (5).

[0074]

[0075] Where: — estimated value of the system state ξ(k), -interference The estimated value of , β0, β1 — observer gain matrix;

[0076] Step 7: Figure 5 As shown in the figure, a model predictive controller is designed to realize the trajectory tracking control of the emergency material transport robot:

[0077] (1) Based on the goals and constraints of the emergency material transport robot, the objective function J(k) and the constraints are given. The objective function must reflect the system's requirements for the controller's accuracy in tracking the reference trajectory and the stability requirements for the control input variation. The objective function must accurately constrain the output state error and control increment and add a relaxation factor to ensure that there is a feasible solution at each sampling moment in the execution process.

[0078] (2) Given the prediction time domain N p and control time domain N c , transform the objective function into a quadratic programming form;

[0079] (3) Obtain all control inputs in the control time domain by solving the quadratic programming problem, and take the first element in the control sequence as the actual control input to act on the system;

[0080] (4) If the emergency material transport robot does not track the reference trajectory, return to step 2.

[0081] To address the difficulties in trajectory tracking control for an emergency material transport robot, different approaches were employed to optimize system performance. Recursive least squares parameter identification was used to identify the robot's parameters, improving model accuracy. Fuzzy control was employed to accelerate the robot's trajectory tracking. Finally, a model predictive control approach, which effectively handles multivariable and constrained problems, was employed to design the controller. Although model predictive control offers strong robustness, its performance deteriorates in the presence of persistent disturbances. Therefore, an extended state observer was added to estimate disturbances and improve trajectory tracking accuracy.

[0082] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims rather than the foregoing description. It is intended that all variations within the meaning and range of equivalents of the claims be embraced herein, and any reference signs in the claims should not be construed as limiting the claims to which they relate.

[0083] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A control method for an emergency material transport robot, characterized in that: The following steps are involved: S1: Define the state of the emergency material transport robot Including the horizontal axis position , vertical coordinate position The angle between the forward direction and the horizontal coordinate , control quantity Including speed and angular velocity , given the expected state and the expected control quantity ; S2: Establish a kinematic model of the emergency material transport robot and use the recursive least squares method to identify the parameters in the model; S3: The state error is obtained based on the actual state and the expected state: , the control error is , establish a discrete error model for trajectory tracking; S4: Analyze the impact of the error between the actual position and the expected position of the emergency material transport robot on the trajectory tracking speed; S5: According to the influence of the position error of the emergency material transport robot on the trajectory tracking speed, fuzzy control is used to optimize the speed so that: (1) When the lateral and longitudinal errors between the actual position of the emergency material transport robot and the reference trajectory are relatively large, in order to quickly reduce the tracking error, the robot's tracking goal should be emphasized and the value of the speed error should be appropriately increased; (2) When the lateral error and longitudinal error between the actual position of the emergency material transport robot and the reference trajectory are relatively small, in order to maintain better stability, the value of the speed error is appropriately reduced; S6: As the emergency material transport robot is inevitably subject to external interference during actual operation, an extended state observer is designed to accurately estimate the disturbance and compensate it into the control variable obtained in S7; S7: Design a model predictive controller to implement trajectory tracking control of the emergency material transport robot: (1) According to the goals and constraints to be achieved by the emergency material transport robot, the objective function is given and constraints; (2) Given the prediction time domain and control time domain , transform the objective function into a quadratic programming form; (3) Obtain all control inputs in the control time domain by solving the quadratic programming problem, and take the first element in the control sequence as the actual control input to act on the system; (4) If the emergency material transport robot does not track the reference trajectory, return to S2.

2. The control method of an emergency material transport robot according to claim 1, characterized in that: An expected trajectory is set on the activity trajectory of the emergency material transport robot, so that the emergency material transport robot tracks the expected trajectory and determines the current state quantity and control quantity of the robot.

3. The control method of an emergency material transport robot according to claim 1, characterized in that: The model of the emergency material transport robot uses the recursive least squares method to identify the parameters in the model. The parameter identification process includes: (1) Splitting: The emergency material transport robot system is a MIMO system. The MIMO system is split into multiple SISO systems for recursive least squares parameter identification. (2) Goal: Make the state quantity of each measurement consistent with the estimated value of the parameter to be estimated Determined measurement estimate The sum of squares of the differences is minimal; (3) Algorithm: The observation data obtained from N+1 experiments are used to obtain the recursive least squares algorithm as shown in formulas (1) to (3). （1） （2） （3） Where: Definition , —N time estimated value, — gain matrix, —Observation value of the state quantity at time N+1, —The set of measured values of state and control quantities at time N+1, —prediction of the value of this measurement based on the previous measurement, — prediction error; (4) Stopping mechanism: When the parameter estimation of the recursive least squares method reaches a certain accuracy, the stopping criterion shown in formula (4) is used to automatically stop the recursive operation. （4） Where: is a suitably small number.

4. The method for controlling an emergency material transport robot according to claim 1, wherein: The emergency material transport robot is configured to Adjust the tracking direction according to the line speed Eliminate state errors; when the actual position of the emergency material transport robot lags behind the expected position, it needs to accelerate; when the emergency material transport robot is ahead of the expected position, it needs to decelerate to compensate for the error.

5. The control method of an emergency material transport robot according to claim 1, characterized in that: Will and As the input of the fuzzy controller, and A fuzzy controller is designed as the output of the fuzzy controller, and 7 fuzzy subsets of the input and output of the fuzzy controller are selected. The language of the fuzzy subsets can be expressed as {NB (negative large), NM (negative medium), NS (negative small), ZE (zero), PS (positive small), PM (positive medium), PB (positive large)}.

6. The method for controlling an emergency material transport robot according to claim 5, wherein: The controller input variable adopts a Gaussian membership function as the input variable membership function, and the controller output variable adopts a triangle and a trapezoid as the output variable membership function; the membership functions obtained by the Gaussian membership function, the triangle and the trapezoid clarify the fuzzy value through the centroid method.

7. The method for controlling an emergency material transport robot according to claim 1, wherein: The total disturbance of the system is estimated using ESO, and the estimated value is used to perform feedforward compensation on the emergency material transport robot system to improve the stability and anti-interference ability of the system. The linear ESO is designed as shown in formula (5). （5） Where: —System Status The estimated value of -interference The estimated value of 、 —Observer gain matrix.

8. The method for controlling an emergency material transport robot according to claim 1, wherein: The objective function can simultaneously reflect the system's requirements for the controller's accuracy in tracking the reference trajectory and the stability of the control input variation. The objective function can accurately constrain the output state error and the control increment and add a relaxation factor.

Images