Disinfection robot control method and system considering fuzzy obstacle avoidance path

Abstract

The present invention discloses a control method for a disinfection robot taking into account a fuzzy obstacle avoidance path, comprising constructing an error parameter model corresponding to the kinematic model of the disinfection robot; obtaining an optimal operation path set of the disinfection robot according to a fuzzy rule selection model of the obstacle avoidance path; constructing and expanding a dynamic error parameter model of the disinfection robot according to the error parameter model and the optimal operation path set; constructing a high-order sliding mode controller corresponding to the dynamic error parameter model; obtaining the control gain and operation model constraints of the disinfection robot during operation according to the high-order sliding mode controller; and performing motion control operations on the disinfection robot according to the high-order sliding mode controller, the control gain, and the operation model constraints. The present invention mainly solves the problem of how to select a suitable path and maintain stable operation during the obstacle avoidance process; the present invention realizes real-time selection of the obstacle avoidance path, real-time derivation of the obstacle avoidance error, and precise tracking of the path during the operation of the disinfection robot.

Description

Technical Field

[0001] The present invention relates to the field of robot path planning and motion control technology, and specifically to a disinfection robot control method and system taking into account fuzzy obstacle avoidance paths. Background Art

[0002] Microorganisms harmful to the human body are easily accumulated in environments such as hospitals, infectious disease centers, and hotels. The environment must be disinfected according to regulatory guidelines. The use of disinfection robots can better achieve disinfection operations. During the disinfection operation, the disinfection robot inevitably needs to dynamically circumvent passing pedestrians, carts, and placed objects in order to bypass obstacles and continue operations.

[0003] However, many robot control methods in the prior art often adopt a waiting approach when encountering obstacles. When the obstacles exist for a long time, it is easy for the robot to remain in place for a long time.

[0004] Many existing robot control methods employ real-time determination of obstacle avoidance requirements. For example, Chinese Invention Patent Publication No. CN112327620A, titled "Robust Control Method and System for Mobile Robot Considering Obstacle Avoidance," discloses a technical solution for determining in real time whether obstacle avoidance is necessary to determine the real-time trajectory of the mobile robot. However, such control methods place extremely stringent requirements on the determination period. Improperly setting the determination period can result in excessive and significant jitter during the robot system's operation.

[0005] As a highly integrated system, the disinfection robot faces urgent challenges in choosing the right path and maintaining stable operation while circumventing obstacles. Summary of the Invention

[0006] The purpose of the present invention is to provide a disinfection robot control method and system that takes into account fuzzy obstacle avoidance paths, which can perform dynamic fuzzy obstacle avoidance and posture control on the disinfection robot.

[0007] To achieve the above object, the present invention provides the following technical solution: a disinfection robot control method considering a fuzzy obstacle avoidance path, comprising the following steps:

[0008] S1. Construct an error parameter model corresponding to the kinematic model of the disinfection robot;

[0009] S2. Selecting a model based on the fuzzy rules of the obstacle avoidance path to obtain an optimal operation path set for the disinfection robot;

[0010] S3. Constructing and expanding a dynamic error parameter model of the disinfection robot according to the error parameter model and the optimal operation path set;

[0011] S4, constructing a high-order sliding mode controller corresponding to the dynamic error parameter model;

[0012] S5. Obtaining control gains and operation model constraints of the disinfection robot during operation according to the high-order sliding mode controller;

[0013] S6. Perform motion control operations on the disinfection robot according to the high-order sliding mode controller, the control gain, and the operation model constraints.

[0014] In the above technical solution, in step S1, the kinematic model of the disinfection robot is specifically:

[0015]

[0016] Where: is the derivative of the x-axis coordinate of the current position of the disinfection robot, is the derivative of the y-axis coordinate of the current position of the disinfection robot, is the deflection angle of the current disinfection robot during operation, l is the center point position offset of the disinfection robot, v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot, g is the intermediate variable, and the matrix form of g is:

[0017]

[0018] In the above technical solution, in step S1, the specific method for constructing the error parameter model is:

[0019] S1.1. Based on the kinematic model of the disinfection robot, the input equation of the disinfection robot is solved as follows:

[0020]

[0021] S1.2. Based on the input equation of the disinfection robot, the error parameter model is designed as follows:

[0022]

[0023] Where: l(x) is the time-varying parameter of the tracking error, the point coordinates (x, y) are the actual coordinates of the disinfection robot at present, and the point coordinates (x r ,y r ) is the current reference coordinate of the disinfection robot, e x is the error of the disinfection robot in the x-axis direction, e y is the error of the disinfection robot in the y-axis direction;

[0024] e x and ey The definition of is: x =x r -x,e y =y r -y;

[0025] S1.3. Based on the error parameter model, the system error equation of the disinfection robot is obtained as follows:

[0026]

[0027] Where: e=[e x ,e y ] T , is the derivative of the error of the disinfection robot, f(e) is the set of parameter errors of the disinfection robot, specifically f(e) = [l(x)e x ,l(x)e y ] T , is an intermediate variable.

[0028] In the above technical solution, in step S2, the fuzzy rule selection model of the obstacle avoidance path is specifically:

[0029]

[0030] Where:

[0031] P(x) is the function set of the fuzzy rule selection model of the obstacle avoidance path;

[0032] O f is the set of barrier-free zones, is the membership function within the barrier-free interval, Specifically:

[0033]

[0034] Among them, P={p1,p n}, representing the obstacle-free path set of the disinfection robot;

[0035] O O is the set of intervals containing obstacles, is the membership function within the obstacle interval, Specifically:

[0036]

[0037] Among them, P={p1,p n}, representing the set of obstacle paths of the disinfection robot.

[0038] In the above technical solution, in step S2, the specific method for obtaining the optimal running path set is:

[0039] S2.1. Expand the function set P(x) of the fuzzy rule selection model for the obstacle avoidance path into a matrix function, expressed as follows:

[0040]

[0041] Where: O fi＝1,2；j＝1,...,n and O Oi＝1,2；j＝1,...,n A specific path value selected for the path of the disinfection robot;

[0042] S2.2. The matrix function of the function set P(x) of the fuzzy rule selection model of the obstacle avoidance path and the membership function in the obstacle-free interval And the membership function in the interval containing obstacles The optimal running path set is obtained as follows:

[0043]

[0044] In the above technical solution, in step S3, the dynamic error parameter model is specifically:

[0045]

[0046] Where: e x is the error of the disinfection robot in the x-axis direction, for e x The derivative of y is the error of the disinfection robot in the y-axis direction, for e y The derivative of ω is the angular error of the disinfection robot during operation, for e ω The derivative of

[0047] u1 is the system input of the disinfection robot, which is defined as u1 = -v + v r cose3;

[0048] u2 is also the system input of the disinfection robot, which is defined as u2 = ω r -ω.

[0049] In the above technical solution, in step S4, the high-order sliding mode controller corresponding to the dynamic error parameter model is a fractional-order high-order sliding mode controller, specifically:

[0050]

[0051] Where: is the derivative of the system input of the disinfection robot, μ i1 and μ i2 is the control gain of the disinfection robot, α i1 and α i2 is the fractional order of the high-order sliding mode controller, δ i＝1,2 and υ i is an intermediate variable, and δ1=e1+k1e2, δ2=e3-k2arctan(e2), sgn() is a sign function.

[0052] In the above technical solution, in step S5, the operation model constraints of the disinfection robot during operation are specifically:

[0053]

[0054]

[0055] Where: v r min is the minimum reference value of the input linear velocity of the disinfection robot, ω max is the maximum angular velocity of the disinfection robot, P is a symmetric positive definite matrix and satisfies:

[0056]

[0057] Among them are, M i＝1,2,3 are all symmetric positive definite matrices, and Q and R are matrices of appropriate dimensions.

[0058] In the above technical solution, in step S5, the control gain of the disinfection robot during operation is specifically:

[0059]

[0060] μ i =[μ i1 ,μ i2 ] T =P -1 Q;

[0061] Where: μ i is the set of constraints of the running model.

[0062] A disinfection robot control system considering a fuzzy obstacle avoidance path includes:

[0063] Kinematic model of the disinfection robot;

[0064] an error parameter model corresponding to a kinematic model of the disinfection robot;

[0065] A fuzzy rule selection model is used to obtain an optimal operation path set of the disinfection robot;

[0066] a dynamic error parameter model, which is constructed and extended by the error parameter model and the optimal operation path set;

[0067] a high-order sliding mode controller, corresponding to the dynamic error parameter model, for performing motion control operations on the disinfection robot;

[0068] control gain, used to perform motion control operations on the disinfection robot; and

[0069] The operating model constraints are used to perform motion control operations on the disinfection robot.

[0070] Compared with the prior art, the beneficial effects of the present invention are: the disinfection robot control method and system that considers fuzzy obstacle avoidance paths, its dynamic error parameter model includes an error parameter model corresponding to the kinematic model of the disinfection robot, which can describe the error of the disinfection robot during movement, and also includes the optimal operation path set of the disinfection robot, which can perform dynamic fuzzy obstacle avoidance on the disinfection robot. The high-order sliding mode controller based on this dynamic error parameter model can perform dynamic fuzzy obstacle avoidance and posture control on the disinfection robot; the present invention realizes real-time selection of obstacle avoidance paths, real-time derivation of obstacle avoidance errors and precise tracking of paths during the operation of the disinfection robot, and obtains a diversified and highly integrated dynamic error parameter model, which improves the control accuracy and robustness of the disinfection robot in the obstacle avoidance and error tracking process. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 This is a flowchart of the steps of embodiment 1 of the present invention.

[0072] Figure 2 This is a structural diagram of embodiment 2 of the present invention.

[0073] Figure 3 This is a flowchart of the steps of embodiment 3 of the present invention. DETAILED DESCRIPTION

[0074] 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.

[0075] Example 1:

[0076] See also Figure 1This embodiment provides a disinfection robot control method considering a fuzzy obstacle avoidance path, which is applied to a disinfection robot that uses a high-order sliding mode controller for motion control.

[0077] The control method of the disinfection robot considering the fuzzy obstacle avoidance path includes the following steps:

[0078] S1. Construct an error parameter model corresponding to the kinematic model of the disinfection robot;

[0079] S2. Selecting a model based on the fuzzy rules of the obstacle avoidance path to obtain an optimal operation path set for the disinfection robot;

[0080] S3. Constructing and expanding a dynamic error parameter model of the disinfection robot according to the error parameter model and the optimal operation path set;

[0081] S4, constructing a high-order sliding mode controller corresponding to the dynamic error parameter model;

[0082] S5. Obtaining control gains and operation model constraints of the disinfection robot during operation according to the high-order sliding mode controller;

[0083] S6. Perform motion control operations on the disinfection robot according to the high-order sliding mode controller, the control gain, and the operation model constraints.

[0084] Specifically, in step S1, the kinematic model of the disinfection robot is:

[0085]

[0086] Where: is the derivative of the x-axis coordinate of the current position of the disinfection robot, is the derivative of the y-axis coordinate of the current position of the disinfection robot, is the deflection angle of the current disinfection robot during operation, l is the center point position offset of the disinfection robot, v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot, g is the intermediate variable, and the matrix form of g is:

[0087]

[0088] After constructing a coordinate system in the place where the disinfection robot is required to disinfect, the x-axis coordinate and y-axis coordinate of the current position of the disinfection robot can be obtained through the position recording device of the disinfection robot; the deflection angle of the disinfection robot The position offset l from the center point can be obtained through the posture sensor of the disinfection robot; the linear velocity v and angular velocity ω of the disinfection robot can be obtained through the speed sensor of the disinfection robot.

[0089] After constructing the kinematic model, the x-axis coordinate and its derivative, y-axis coordinate and its derivative of the current position of the disinfection robot, and the deflection angle of the disinfection robot can be used. The center point position offset l, linear velocity v and angular velocity ω are used to describe the entire motion process of the disinfection robot, providing a mathematical basis for constructing the error parameter model of the disinfection robot.

[0090] Specifically, in step S1, the specific method for constructing the error parameter model is:

[0091] S1.1. Based on the kinematic model of the disinfection robot, the input equation of the disinfection robot is solved as follows:

[0092]

[0093] In this step, after solving the kinematic model of the disinfection robot, the linear velocity v and angular velocity ω of the disinfection robot, the x-axis coordinates and y-axis coordinates of the current position of the disinfection robot, and the deflection angle of the disinfection robot can be obtained. The relationship between the linear velocity v and the center point position offset l can be described by the readings of the position recording device and posture sensor of the disinfection robot.

[0094] S1.2. Based on the input equation of the disinfection robot, the error parameter model is designed as follows:

[0095]

[0096] Where: l(x) is the time-varying parameter of the tracking error, the point coordinates (x, y) are the actual coordinates of the disinfection robot at present, and the point coordinates (x r ,y r ) is the current reference coordinate of the disinfection robot, e x is the error of the disinfection robot in the x-axis direction, e y is the error of the disinfection robot in the y-axis direction;

[0097] e x and e y The definition of is: x =x r -x,e y =y r -y;

[0098] S1.3. Based on the error parameter model, the system error equation of the disinfection robot is obtained as follows:

[0099]

[0100] Where: e=[ex ,e y ] T , is the derivative of the error of the disinfection robot, f(e) is the set of parameter errors of the disinfection robot, specifically f(e)=[l(x)e x ,l(x)e y ] T , is an intermediate variable.

[0101] The error parameter model is obtained by combining the readings of the position recording device and the posture sensor of the disinfection robot with the reference coordinates (x r ,y r ), the error e of the disinfection robot in the x-axis direction x And the error e of the disinfection robot in the y-axis direction y The error of the disinfection robot during its movement can be described; its input data and error parameters are concise and accurate, which saves computing resources while being able to accurately describe the error of the disinfection robot during its movement.

[0102] Specifically, in step S2, the fuzzy rule selection model of the obstacle avoidance path is:

[0103]

[0104] Where:

[0105] P(x) is the function set of the fuzzy rule selection model of the obstacle avoidance path;

[0106] O f is the set of barrier-free zones, is the membership function within the barrier-free interval, Specifically:

[0107]

[0108] Among them, P={p1,p n}, representing the obstacle-free path set of the disinfection robot;

[0109] O O is the set of intervals containing obstacles, is the membership function within the obstacle interval, Specifically:

[0110]

[0111] Among them, P={p1,p n}, representing the set of obstacle paths of the disinfection robot.

[0112] After constructing a coordinate system in the place where the disinfection robot is required to disinfect, obstacles are identified through the visual sensors, ultrasonic sensors and photoelectric sensors on each disinfection robot. Obstacles can also be identified through cameras and visual sensors in the place. After the obstacle is identified, the coordinates with the obstacle are dynamically added to the obstacle-containing interval set, and the coordinates without the obstacle are dynamically added to the obstacle-free interval set, and the obstacle-containing interval set and the obstacle-free interval set are dynamically refreshed. When the planned path of the disinfection robot passes through an obstacle-containing interval, the path is dynamically added to the obstacle-containing path set. When the planned path of the disinfection robot does not pass through any obstacle-containing interval set (the path only passes through the obstacle-free interval), the path is dynamically added to the obstacle-free path set, and the obstacle-containing path set and the obstacle-free path set are dynamically refreshed according to changes in the obstacle-containing interval set and the obstacle-free interval set.

[0113] Specifically, in step S2, the specific method for obtaining the optimal operation path set is:

[0114] S2.1. Expand the function set P(x) of the fuzzy rule selection model for the obstacle avoidance path into a matrix function, expressed as follows:

[0115]

[0116] Where: O fi＝1,2；j＝1,...,n and O Oi＝1,2；j＝1,...,n A specific path value selected for the path of the disinfection robot;

[0117] S2.2. The matrix function of the function set P(x) of the fuzzy rule selection model of the obstacle avoidance path and the membership function in the obstacle-free interval And the membership function in the interval containing obstacles The optimal running path set is obtained as follows:

[0118]

[0119] Specifically, in step S3, the dynamic error parameter model is:

[0120]

[0121] Where: e x is the error of the disinfection robot in the x-axis direction, for e x The derivative of y is the error of the disinfection robot in the y-axis direction, for e y The derivative of ω is the angular error of the disinfection robot during operation, for e ω The derivative of

[0122] u1 is the system input of the disinfection robot, which is defined as u1 = -v + v r cose3;

[0123] u2 is also the system input of the disinfection robot, which is defined as u2 = ω r -ω.

[0124] The dynamic error parameter model includes the error parameter model e corresponding to the kinematic model of the disinfection robot. x 、e y as well as It can describe the error of the disinfection robot during movement, and also includes the function set P(x) of the optimal running path set of the disinfection robot, which can perform dynamic fuzzy obstacle avoidance for the disinfection robot.

[0125] Specifically, in step S4, the high-order sliding mode controller corresponding to the dynamic error parameter model is a fractional-order high-order sliding mode controller, specifically:

[0126]

[0127] Where: is the derivative of the system input of the disinfection robot, μ i1 and μ i2 is the control gain of the disinfection robot, α i1 and α i2 is the fractional order of the high-order sliding mode controller, δ i＝1,2 and υ i is an intermediate variable, and δ1=e1+k1e2, δ2=e3-k2arctan(e2), sgn() is a sign function.

[0128] Specifically, in step S5, the operation model constraints of the disinfection robot during operation are specifically:

[0129]

[0130]

[0131] Where: v r min is the minimum reference value of the input linear velocity of the disinfection robot, ω max is the maximum angular velocity of the disinfection robot, P is a symmetric positive definite matrix and satisfies:

[0132]

[0133] Among them are, Mi＝1,2,3 are all symmetric positive definite matrices, and Q and R are matrices of appropriate dimensions.

[0134] Specifically, in step S5, the control gain of the disinfection robot during operation is:

[0135]

[0136] μ i =[μ i1 ,μ i2 ] T =P -1 Q;

[0137] Where: μ i is the set of constraints of the running model.

[0138] This disinfection robot control method that takes into account fuzzy obstacle avoidance paths has a dynamic error parameter model that includes an error parameter model corresponding to the kinematic model of the disinfection robot, which can describe the error of the disinfection robot during movement. It also includes an optimal operation path set for the disinfection robot, which can perform dynamic fuzzy obstacle avoidance on the disinfection robot. A high-order sliding mode controller based on this dynamic error parameter model can perform dynamic fuzzy obstacle avoidance and posture control on the disinfection robot. The present invention realizes real-time selection of obstacle avoidance paths, real-time derivation of obstacle avoidance errors, and precise tracking of paths during the operation of the disinfection robot, and obtains a diversified and highly integrated dynamic error parameter model, which improves the control accuracy and robustness of the disinfection robot in the obstacle avoidance and error tracking processes.

[0139] Example 2:

[0140] See also Figure 2 This embodiment provides a disinfection robot control system that considers a fuzzy obstacle avoidance path, which is applied to a disinfection robot that uses a high-order sliding mode controller for motion control.

[0141] The disinfection robot control system considering the fuzzy obstacle avoidance path includes:

[0142] Kinematic model of the disinfection robot;

[0143] an error parameter model corresponding to a kinematic model of the disinfection robot;

[0144] A fuzzy rule selection model is used to obtain an optimal operation path set of the disinfection robot;

[0145] a dynamic error parameter model, which is constructed and extended by the error parameter model and the optimal operation path set;

[0146] a high-order sliding mode controller, corresponding to the dynamic error parameter model, for performing motion control operations on the disinfection robot;

[0147] control gain, used to perform motion control operations on the disinfection robot; and

[0148] The operating model constraints are used to perform motion control operations on the disinfection robot.

[0149] Specifically, the kinematic model of the disinfection robot is:

[0150]

[0151] Where: is the derivative of the x-axis coordinate of the current position of the disinfection robot, is the derivative of the y-axis coordinate of the current position of the disinfection robot, is the deflection angle of the current disinfection robot during operation, l is the center point position offset of the disinfection robot, v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot, g is the intermediate variable, and the matrix form of g is:

[0152]

[0153] After constructing a coordinate system in the place where the disinfection robot is required to disinfect, the x-axis coordinate and y-axis coordinate of the current position of the disinfection robot can be obtained through the position recording device of the disinfection robot; the deflection angle of the disinfection robot The position offset l from the center point can be obtained through the posture sensor of the disinfection robot; the linear velocity v and angular velocity ω of the disinfection robot can be obtained through the speed sensor of the disinfection robot.

[0154] After constructing the kinematic model, the x-axis coordinate and its derivative, y-axis coordinate and its derivative of the current position of the disinfection robot, and the deflection angle of the disinfection robot can be used. The center point position offset l, linear velocity v and angular velocity ω are used to describe the entire motion process of the disinfection robot, providing a mathematical basis for constructing the error parameter model of the disinfection robot.

[0155] Specifically, the specific construction method of the error parameter model is:

[0156] S1.1. Based on the kinematic model of the disinfection robot, the input equation of the disinfection robot is solved as follows:

[0157]

[0158] In this step, after solving the kinematic model of the disinfection robot, the linear velocity v and angular velocity ω of the disinfection robot, the x-axis coordinates and y-axis coordinates of the current position of the disinfection robot, and the deflection angle of the disinfection robot can be obtained. The relationship between the linear velocity v and the center point position offset l can be described by the readings of the position recording device and posture sensor of the disinfection robot.

[0159] S1.2. Based on the input equation of the disinfection robot, the error parameter model is designed as follows:

[0160]

[0161] Where: l(x) is the time-varying parameter of the tracking error, the point coordinates (x, y) are the actual coordinates of the disinfection robot at present, and the point coordinates (x r ,y r ) is the current reference coordinate of the disinfection robot, e x is the error of the disinfection robot in the x-axis direction, e y is the error of the disinfection robot in the y-axis direction;

[0162] e x and e y The definition of is: x =x r -x,e y =y r -y;

[0163] S1.3. Based on the error parameter model, the system error equation of the disinfection robot is obtained as follows:

[0164]

[0165] Where: e=[e x ,e y ] T , is the derivative of the error of the disinfection robot, f(e) is the set of parameter errors of the disinfection robot, specifically f(e)=[l(x)e x ,l(x)e y ] T , is an intermediate variable.

[0166] The error parameter model is obtained by combining the readings of the position recording device and the posture sensor of the disinfection robot with the reference coordinates (x r ,y r ), the error e of the disinfection robot in the x-axis direction x And the error e of the disinfection robot in the y-axis direction yThe error of the disinfection robot during its movement can be described; its input data and error parameters are concise and accurate, which saves computing resources while being able to accurately describe the error of the disinfection robot during its movement.

[0167] Specifically, the fuzzy rule selection model of the obstacle avoidance path is as follows:

[0168]

[0169] Where:

[0170] P(x) is the function set of the fuzzy rule selection model of the obstacle avoidance path;

[0171] O f is the set of barrier-free zones, is the membership function within the barrier-free interval, Specifically:

[0172]

[0173] Among them, P={p1,p n}, representing the obstacle-free path set of the disinfection robot;

[0174] O O is the set of intervals containing obstacles, is the membership function within the obstacle interval, Specifically:

[0175]

[0176] Among them, P={p1,p n}, representing the set of obstacle paths of the disinfection robot.

[0177] After constructing a coordinate system in the place where the disinfection robot is required to disinfect, obstacles are identified through the visual sensors, ultrasonic sensors and photoelectric sensors on each disinfection robot. Obstacles can also be identified through cameras and visual sensors in the place. After the obstacle is identified, the coordinates with the obstacle are dynamically added to the obstacle-containing interval set, and the coordinates without the obstacle are dynamically added to the obstacle-free interval set, and the obstacle-containing interval set and the obstacle-free interval set are dynamically refreshed. When the planned path of the disinfection robot passes through an obstacle-containing interval, the path is dynamically added to the obstacle-containing path set. When the planned path of the disinfection robot does not pass through any obstacle-containing interval set (the path only passes through the obstacle-free interval), the path is dynamically added to the obstacle-free path set, and the obstacle-containing path set and the obstacle-free path set are dynamically refreshed according to changes in the obstacle-containing interval set and the obstacle-free interval set.

[0178] Specifically, the specific method for obtaining the optimal operation path set is:

[0179] S2.1. Expand the function set P(x) of the fuzzy rule selection model for the obstacle avoidance path into a matrix function, expressed as follows:

[0180]

[0181] Where: O fi＝1,2；j＝1,...,n and O Oi＝1,2；j＝1,...,n A specific path value selected for the path of the disinfection robot;

[0182] S2.2. The matrix function of the function set P(x) of the fuzzy rule selection model of the obstacle avoidance path and the membership function in the obstacle-free interval And the membership function in the interval containing obstacles The optimal running path set is obtained as follows:

[0183]

[0184] Specifically, the dynamic error parameter model is:

[0185]

[0186] Where: e x is the error of the disinfection robot in the x-axis direction, for e x The derivative of y is the error of the disinfection robot in the y-axis direction, for e y The derivative of ω is the angular error of the disinfection robot during operation, for e ω The derivative of

[0187] u1 is the system input of the disinfection robot, which is defined as u1 = -v + v r cose3;

[0188] u2 is also the system input of the disinfection robot, which is defined as u2 = ω r -ω.

[0189] The dynamic error parameter model includes the error parameter model e corresponding to the kinematic model of the disinfection robot. x 、e y as well as It can describe the error of the disinfection robot during movement, and also includes the function set P(x) of the optimal running path set of the disinfection robot, which can perform dynamic fuzzy obstacle avoidance for the disinfection robot.

[0190] Specifically, the high-order sliding mode controller corresponding to the dynamic error parameter model is a fractional-order high-order sliding mode controller, specifically:

[0191]

[0192] Where: is the derivative of the system input of the disinfection robot, μ i1 and μ i2 is the control gain of the disinfection robot, α i1 and α i2 is the fractional order of the high-order sliding mode controller, δ i＝1,2 and υ i is an intermediate variable, and δ1=e1+k1e2, δ2=e3-k2arctan(e2), sgn() is a sign function.

[0193] Specifically, the operation model constraints of the disinfection robot during operation are as follows:

[0194]

[0195]

[0196] Where: v r min is the minimum reference value of the input linear velocity of the disinfection robot, ω max is the maximum angular velocity of the disinfection robot, P is a symmetric positive definite matrix and satisfies:

[0197]

[0198] Among them are, M i＝1,2,3 are all symmetric positive definite matrices, and Q and R are matrices of appropriate dimensions.

[0199] Specifically, the control gain of the disinfection robot during operation is:

[0200]

[0201] μ i =[μ i1 ,μ i2 ] T =P -1 Q;

[0202] Where: μ i is the set of constraints of the running model.

[0203] This disinfection robot control system that takes into account fuzzy obstacle avoidance paths has a dynamic error parameter model that includes an error parameter model corresponding to the kinematic model of the disinfection robot, which can describe the errors of the disinfection robot during movement. It also includes an optimal operation path set for the disinfection robot, which can perform dynamic fuzzy obstacle avoidance on the disinfection robot. A high-order sliding mode controller based on this dynamic error parameter model can perform dynamic fuzzy obstacle avoidance and posture control on the disinfection robot. The present invention realizes real-time selection of obstacle avoidance paths, real-time derivation of obstacle avoidance errors, and precise tracking of paths during the operation of the disinfection robot, and obtains a diversified and highly integrated dynamic error parameter model, which improves the control accuracy and robustness of the disinfection robot during obstacle avoidance and error tracking.

[0204] Example 3:

[0205] See also Figure 3 This embodiment provides a disinfection robot control method considering a fuzzy obstacle avoidance path, which is applied to a disinfection robot that uses a high-order sliding mode controller for motion control.

[0206] The control method of the disinfection robot considering the fuzzy obstacle avoidance path includes the following steps:

[0207] S1. Construct an error parameter model corresponding to the kinematic model of the disinfection robot;

[0208] S2. Selecting a model based on the fuzzy rules of the obstacle avoidance path to obtain an optimal operation path set for the disinfection robot;

[0209] S3. Constructing and expanding a dynamic error parameter model of the disinfection robot according to the error parameter model and the optimal operation path set;

[0210] S4, constructing a high-order sliding mode controller corresponding to the dynamic error parameter model;

[0211] S5. Perform motion control operations on the disinfection robot according to the high-order sliding mode controller.

[0212] Specifically, in step S1, the kinematic model of the disinfection robot is:

[0213]

[0214] Where: is the derivative of the x-axis coordinate of the current position of the disinfection robot, is the derivative of the y-axis coordinate of the current position of the disinfection robot, is the deflection angle of the current disinfection robot during operation, l is the center point position offset of the disinfection robot, v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot, g is the intermediate variable, and the matrix form of g is:

[0215]

[0216] After constructing a coordinate system in the place where the disinfection robot is required to disinfect, the x-axis coordinate and y-axis coordinate of the current position of the disinfection robot can be obtained through the position recording device of the disinfection robot; the deflection angle of the disinfection robot The position offset l from the center point can be obtained through the posture sensor of the disinfection robot; the linear velocity v and angular velocity ω of the disinfection robot can be obtained through the speed sensor of the disinfection robot.

[0217] After constructing the kinematic model, the x-axis coordinate and its derivative, y-axis coordinate and its derivative of the current position of the disinfection robot, and the deflection angle of the disinfection robot can be used. The center point position offset l, linear velocity v and angular velocity ω are used to describe the entire motion process of the disinfection robot, providing a mathematical basis for constructing the error parameter model of the disinfection robot.

[0218] Specifically, in step S1, the specific method for constructing the error parameter model is:

[0219] S1.1. Based on the kinematic model of the disinfection robot, the input equation of the disinfection robot is solved as follows:

[0220]

[0221] In this step, after solving the kinematic model of the disinfection robot, the linear velocity v and angular velocity ω of the disinfection robot, the x-axis coordinates and y-axis coordinates of the current position of the disinfection robot, and the deflection angle of the disinfection robot can be obtained. The relationship between the linear velocity v and the center point position offset l can be described by the readings of the position recording device and posture sensor of the disinfection robot.

[0222] S1.2. Based on the input equation of the disinfection robot, the error parameter model is designed as follows:

[0223]

[0224] Where: l(x) is the time-varying parameter of the tracking error, the point coordinates (x, y) are the actual coordinates of the disinfection robot at present, and the point coordinates (x r ,y r ) is the current reference coordinate of the disinfection robot, e xis the error of the disinfection robot in the x-axis direction, e y is the error of the disinfection robot in the y-axis direction;

[0225] e x and e y The definition of is: x =x r -x,e y =y r -y;

[0226] S1.3. Based on the error parameter model, the system error equation of the disinfection robot is obtained as follows:

[0227]

[0228] Where: e=[e x ,e y ] T , is the derivative of the error of the disinfection robot, f(e) is the set of parameter errors of the disinfection robot, specifically f(e) = [l(x)e x ,l(x)e y ] T , is an intermediate variable.

[0229] The error parameter model is obtained by combining the readings of the position recording device and the posture sensor of the disinfection robot with the reference coordinates (x r ,y r ), the error e of the disinfection robot in the x-axis direction x And the error e of the disinfection robot in the y-axis direction y The error of the disinfection robot during its movement can be described; its input data and error parameters are concise and accurate, which saves computing resources while being able to accurately describe the error of the disinfection robot during its movement.

[0230] Specifically, in step S2, the fuzzy rule selection model of the obstacle avoidance path is:

[0231]

[0232] Where:

[0233] P(x) is the function set of the fuzzy rule selection model of the obstacle avoidance path;

[0234] O f is the set of barrier-free zones, is the membership function within the barrier-free interval, Specifically:

[0235]

[0236] Among them, P={p1,p n}, representing the obstacle-free path set of the disinfection robot;

[0237] O O is the set of intervals containing obstacles, is the membership function within the obstacle interval, Specifically:

[0238]

[0239] Among them, P={p1,p n}, representing the set of obstacle paths of the disinfection robot.

[0240] After constructing a coordinate system in the place where the disinfection robot is required to disinfect, obstacles are identified through the visual sensors, ultrasonic sensors and photoelectric sensors on each disinfection robot. Obstacles can also be identified through cameras and visual sensors in the place. After the obstacle is identified, the coordinates with the obstacle are dynamically added to the obstacle-containing interval set, and the coordinates without the obstacle are dynamically added to the obstacle-free interval set, and the obstacle-containing interval set and the obstacle-free interval set are dynamically refreshed. When the planned path of the disinfection robot passes through an obstacle-containing interval, the path is dynamically added to the obstacle-containing path set. When the planned path of the disinfection robot does not pass through any obstacle-containing interval set (the path only passes through the obstacle-free interval), the path is dynamically added to the obstacle-free path set, and the obstacle-containing path set and the obstacle-free path set are dynamically refreshed according to changes in the obstacle-containing interval set and the obstacle-free interval set.

[0241] Specifically, in step S2, the specific method for obtaining the optimal operation path set is:

[0242] S2.1. Expand the function set P(x) of the fuzzy rule selection model for the obstacle avoidance path into a matrix function, expressed as follows:

[0243]

[0244] Where: O fi＝1,2；j＝1,...,n and O Oi＝1,2；j＝1,...,n A specific path value selected for the path of the disinfection robot;

[0245] S2.2. The matrix function of the function set P(x) of the fuzzy rule selection model of the obstacle avoidance path and the membership function in the obstacle-free interval And the membership function in the interval containing obstacles The optimal running path set is obtained as follows:

[0246]

[0247] Specifically, in step S3, the dynamic error parameter model is:

[0248]

[0249] Where: e x is the error of the disinfection robot in the x-axis direction, for e x The derivative of y is the error of the disinfection robot in the y-axis direction, for e y The derivative of ω is the angular error of the disinfection robot during operation, for e ω The derivative of

[0250] u1 is the system input of the disinfection robot, which is defined as u1 = -v + v r cose3;

[0251] u2 is also the system input of the disinfection robot, which is defined as u2 = ω r -ω.

[0252] The dynamic error parameter model includes the error parameter model e corresponding to the kinematic model of the disinfection robot. x 、e y as well as It can describe the error of the disinfection robot during movement, and also includes the function set P(x) of the optimal running path set of the disinfection robot, which can perform dynamic fuzzy obstacle avoidance for the disinfection robot.

[0253] Specifically, in step S4, the high-order sliding mode controller corresponding to the dynamic error parameter model is a fractional-order high-order sliding mode controller, specifically:

[0254]

[0255] Where: is the derivative of the system input of the disinfection robot, μ i1 and μ i2 is the control gain of the disinfection robot, α i1 and α i2 is the fractional order of the high-order sliding mode controller, δ i＝1,2 and υ i is an intermediate variable, and δ1=e1+k1e2, δ2=e3-k2arctan(e2), sgn() is a sign function.

[0256] This disinfection robot control method that takes into account fuzzy obstacle avoidance paths has a dynamic error parameter model that includes an error parameter model corresponding to the kinematic model of the disinfection robot, which can describe the error of the disinfection robot during movement. It also includes an optimal operation path set for the disinfection robot, which can perform dynamic fuzzy obstacle avoidance on the disinfection robot. A high-order sliding mode controller based on this dynamic error parameter model can perform dynamic fuzzy obstacle avoidance and posture control on the disinfection robot. The present invention realizes real-time selection of obstacle avoidance paths, real-time derivation of obstacle avoidance errors, and precise tracking of paths during the operation of the disinfection robot, and obtains a diversified and highly integrated dynamic error parameter model, which improves the control accuracy and robustness of the disinfection robot in the obstacle avoidance and error tracking processes.

[0257] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A disinfection robot control method considering fuzzy obstacle avoidance path, characterized in that: The steps include: S1. Construct an error parameter model corresponding to the kinematic model of the disinfection robot; S2. Selecting a model based on the fuzzy rules of the obstacle avoidance path to obtain an optimal operation path set for the disinfection robot; S3. Constructing and expanding a dynamic error parameter model of the disinfection robot according to the error parameter model and the optimal operation path set; S4, constructing a high-order sliding mode controller corresponding to the dynamic error parameter model; S5. Obtaining control gains and operation model constraints of the disinfection robot during operation according to the high-order sliding mode controller; S6. Perform motion control operations on the disinfection robot according to the high-order sliding mode controller, the control gain, and the operation model constraints; In step S2, the fuzzy rule selection model of the obstacle avoidance path is specifically: Where: P(x) is the function set of the fuzzy rule selection model of the obstacle avoidance path; O f is the set of barrier-free zones, is the membership function within the barrier-free interval; O O is the set of intervals containing obstacles, is the membership function within the interval containing obstacles; In step S2, the specific method for obtaining the optimal operation path set is: S2.

1. Expand the function set P(x) of the fuzzy rule selection model for the obstacle avoidance path into a matrix function, expressed as follows: Where: O fi＝1，2；j＝1，...，n and O Oi＝1，2；j＝1，...，n A specific path value selected for the path of the disinfection robot; S2.

2. The matrix function of the function set P(x) of the fuzzy rule selection model of the obstacle avoidance path and the membership function in the obstacle-free interval And the membership function in the interval containing obstacles The optimal running path set is obtained as follows: In step S3, the dynamic error parameter model is specifically: Where: e x is the error of the disinfection robot in the x-axis direction, for e x The derivative of y is the error of the disinfection robot in the y-axis direction, for e y The derivative of ω is the angular velocity error of the disinfection robot during operation, for e ω The derivative of u1 is the system input of the disinfection robot, which is defined as u1 = -v + v r cose3; u2 is also the system input of the disinfection robot, which is defined as u2 = ω r -ω; v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot; In step S4, the high-order sliding mode controller corresponding to the dynamic error parameter model is a fractional-order high-order sliding mode controller, specifically: Where: is the derivative of the system input of the disinfection robot, μ i1 and μ i2 is the control gain of the disinfection robot, α i1 and α i2 is the fractional order of the high-order sliding mode controller, δ i＝1，2 and υ i is an intermediate variable, and δ1=e1+k1e2, δ2=e3-k2arctan(e2), sgn() is a sign function; In step S5, the operation model constraints of the disinfection robot during operation are specifically as follows: Where: v rmin is the minimum reference value of the input linear velocity of the disinfection robot, ω max is the maximum angular velocity of the disinfection robot, P is a symmetric positive definite matrix and satisfies: Among them are, M i＝1，2，3 are all symmetric positive definite matrices, Q and R are matrices of appropriate dimensions; In step S5, the control gain of the disinfection robot during operation is specifically: m i =[μ i1 ,m i2 ] T =P -1 Q; Where: μ i is the set of constraints of the running model.

2. The disinfection robot control method considering fuzzy obstacle avoidance path according to claim 1 is characterized in that: In step S1, the kinematic model of the disinfection robot is specifically: Where: is the derivative of the x-axis coordinate of the current position of the disinfection robot, is the derivative of the y-axis coordinate of the current position of the disinfection robot, is the deflection angle of the current disinfection robot during operation, l is the center point position offset of the disinfection robot, v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot, g is the intermediate variable, and the matrix form of g is:

3. The disinfection robot control method considering fuzzy obstacle avoidance path according to claim 2 is characterized in that: In step S1, the specific method for constructing the error parameter model is: S1.

1. Based on the kinematic model of the disinfection robot, the input equation of the disinfection robot is solved as follows: S1.

2. Based on the input equation of the disinfection robot, the error parameter model is designed as follows: Where: l(x) is the time-varying parameter of the tracking error, the point coordinates (x, y) are the actual coordinates of the disinfection robot at present, and the point coordinates (x r ,y r ) is the current reference coordinate of the disinfection robot, e x is the error of the disinfection robot in the x-axis direction, e v is the error of the disinfection robot in the y-axis direction; e x and e y The definition of is: x =x r -x,e y =y r -y; S1.

3. Based on the error parameter model, the system error equation of the disinfection robot is obtained as follows: Where: e=[e x ,e y ] T , is the derivative of the error of the disinfection robot, f(e) is the set of parameter errors of the disinfection robot, specifically f(e)=[l(x)e x , l(x)e y ] T , is an intermediate variable.

4. A disinfection robot control system considering fuzzy obstacle avoidance path, characterized in that: include: Kinematic model of the disinfection robot; an error parameter model corresponding to a kinematic model of the disinfection robot; A fuzzy rule selection model is used to obtain an optimal operation path set of the disinfection robot; a dynamic error parameter model, which is constructed and extended by the error parameter model and the optimal operation path set; a high-order sliding mode controller, corresponding to the dynamic error parameter model, for performing motion control operations on the disinfection robot; control gain, used to perform motion control operations on the disinfection robot; and An operating model constraint is used to perform motion control operations on the disinfection robot; The fuzzy rule selection model of the obstacle avoidance path is specifically as follows: Where: P(x) is the function set of the fuzzy rule selection model of the obstacle avoidance path; O f is the set of barrier-free zones, is the membership function within the barrier-free interval; O O is the set of intervals containing obstacles, is the membership function within the interval containing obstacles; The specific method for obtaining the optimal operation path set is: S2.

1. Expand the function set P(x) of the fuzzy rule selection model for the obstacle avoidance path into a matrix function, expressed as follows: Where: O fi＝1，2；j＝1，...，n and O Oi＝1，2；j＝1，...，n A specific path value selected for the path of the disinfection robot; S2.

2. The matrix function of the function set P(x) of the fuzzy rule selection model of the obstacle avoidance path and the membership function in the obstacle-free interval And the membership function in the interval containing obstacles The optimal running path set is obtained as follows: The dynamic error parameter model is specifically: Where: e x is the error of the disinfection robot in the x-axis direction, for e x The derivative of y is the error of the disinfection robot in the y-axis direction, for e y The derivative of ω is the angular error of the disinfection robot during operation, for e ω The derivative of u1 is the system input of the disinfection robot, which is defined as u1 = -v + v r cose3; u2 is also the system input of the disinfection robot, which is defined as u2 = ω r -ω; v is the linear velocity of the disinfection robot, ω is the angular velocity of the disinfection robot; The high-order sliding mode controller corresponding to the dynamic error parameter model is a fractional-order high-order sliding mode controller, specifically: Where: is the derivative of the system input of the disinfection robot, μ i1 and μ i2 is the control gain of the disinfection robot, α i1 and α i2 is the fractional order of the high-order sliding mode controller, δ i＝1，2 and υ i is an intermediate variable, and δ1=e1+k1e2, δ2=e3-k2arctan(e2), sgn() is a sign function; The operation model constraints of the disinfection robot during operation are specifically as follows: Where: v rmin is the minimum reference value of the input linear velocity of the disinfection robot, ω max is the maximum angular velocity of the disinfection robot, P is a symmetric positive definite matrix and satisfies: Among them are, M i＝1，2 , 3 are all symmetric positive definite matrices, Q and R are matrices with appropriate dimensions; The control gain of the disinfection robot during operation is specifically: m i =[μ i1 ,m i2 ] T =P -1 Q; Where: μ i is the set of constraints of the running model.

Images