Interaction-adaptive service robot track and speed simultaneous safety control method

By establishing a dynamic model of human-machine system with uncertain interactive force and designing an adaptive controller, the simultaneous safety control problem of the motion trajectory and speed of the service robot is solved, and high-precision and safety tracking are achieved when human-machine interactive force exists. Ensures user safety.

CN119002271APending Publication Date: 2024-11-22SHENYANG UNIVERSITY OF TECHNOLOGY
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411081734.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-08
Publication Date
2024-11-22

AI Technical Summary

Technical Problem

The prior art cannot effectively solve the simultaneous safety control problem of service robot motion trajectory and speed, especially when there is uncertainty in human-computer interaction force, resulting in unsatisfactory tracking accuracy and security threats.

Method used

By establishing a dynamic model of human-machine system with uncertain interaction force based on the dynamic model of the service robot, design an uncertain interaction force observer and adaptive controller to suppress the impact of interactive force on the robot motion and to convert the safety trajectory and speed. The constraints are converted into secondary planning problems, and the optimal safety controller is obtained to ensure the stability and safety of the movement trajectory and speed of the human-machine system.

Benefits of technology

When human-computer interaction force exists, the service robot's motion trajectory and speed are simultaneously tracked, improving tracking accuracy and system stability, and ensuring user safety.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119002271A_ABST
    Figure CN119002271A_ABST
Patent Text Reader

Abstract

The invention relates to control of a service robot, in particular to a method for controlling simultaneous safety of a track and a speed of a service robot adapting to interaction. According to the method, constraint conditions of a safe trajectory and a safe speed are constructed, control barrier functions are designed respectively, controller design of simultaneous safety of the trajectory and the speed is converted into solving of a quadratic programming problem, the optimal safety controller of the man-machine system is obtained, and the safety of the man-machine system is guaranteed. Comprising the following steps: 1) based on a service robot dynamics model, establishing a man-machine system dynamics model of uncertain interaction force by decomposing generalized input force, and designing an uncertain interaction force observer; and 2) establishing constraint conditions of the safe trajectory and the safe speed of the man-machine system, respectively designing control barrier functions of the safe trajectory and the safe speed according to the constraint conditions, and converting the controller design of simultaneously safe trajectory and speed into solving of a quadratic programming problem.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the control of a service robot, and in particular to a method for controlling the trajectory and speed of a service robot that is adaptable to interactive forces and is safe at the same time. Background Art

[0002] Since the elderly and people with lower limb disabilities are unable to complete their daily independent lives, they bring heavy burdens to family caregivers and society. Sitting service robots can replace the walking function of the elderly and people with lower limb disabilities. They can complete simple daily life activities by sitting on the robot and using their healthy upper limbs, which brings a new life paradigm to the elderly and people with lower limb disabilities. Therefore, the research on service robots has received widespread attention from scholars. Since the user will generate interactive forces with the robot during the operation of various daily actions, this human-machine interactive force seriously interferes with the robot's tracking motion, and even produces unsafe motion trajectories and motion speeds, threatening the safety of the user. Therefore, it is of great significance to solve the problem of safe tracking and control of the motion trajectory and motion speed of the service robot.

[0003] Some research results have been achieved on the tracking control of service robots. However, these results have ignored the uncertain interaction forces generated during human-machine cooperative operation. More importantly, the existing results have not solved the problem of simultaneous safety of the motion trajectory and motion speed of the human-machine system, which not only leads to unsatisfactory tracking accuracy, but also excessive trajectory tracking errors and speed tracking errors will threaten the safety of users. So far, there is no control method for simultaneous safety of trajectory and speed that adapts to human-machine interaction forces. Therefore, it is of great significance to study how to suppress the influence of human-machine uncertain interaction forces on tracking accuracy and solve the problem of simultaneous safety control of trajectory and speed to improve the performance of service robots. Summary of the invention

[0004] The present invention aims at the defects of the prior art and provides a method for controlling the trajectory and speed of a service robot which is adaptable to interactive forces and safe at the same time.

[0005] To achieve the above objectives, the present invention adopts the following technical solutions, a method for safely controlling the trajectory and speed of a service robot adapted to interactive forces, including:

[0006] 1) Based on the service robot dynamics model, by decomposing the generalized input force, a human-machine system dynamics model with uncertain interaction forces is established, and an uncertain interaction force observer is designed. At the same time, an adaptive controller is constructed to suppress the influence of the interaction force on the robot motion and ensure the stability of trajectory tracking and velocity tracking of the human-machine system;

[0007] 2) Establish the constraints of the safe trajectory and safe speed of the human-machine system, design the control obstacle functions of the safe trajectory and safe speed respectively according to the constraints, and transform the design of the controller with safe trajectory and speed into solving a quadratic programming problem, so as to obtain the optimal safety controller for stable tracking of the service robot, and ensure that the human-machine system has a safe motion trajectory and speed at the same time.

[0008] Here are the steps:

[0009] Step 1) By decomposing the generalized input force, a human-machine system dynamics model of uncertain interaction force is established, and an uncertain interaction force observer is designed. At the same time, an adaptive controller is constructed to suppress the influence of the interaction force on the robot motion, ensuring the stability of the trajectory tracking and speed tracking of the human-machine system. The characteristics are as follows: The service robot dynamics model is described as follows:

[0010]

[0011] in

[0012]

[0013] u(t)=[f 1 f 2 f 3 ] T

[0014] M represents the quality of the service robot, m represents the quality of the user; X(t) represents the actual motion trajectory of the service robot; I 0 represents the moment of inertia of the service robot, r 0 represents the distance between the center of the system and the center of gravity. u(t) represents the generalized input force, f 1 , f 2 and f 3 Represent the generalized input force of each omnidirectional wheel. 0 , B(θ) represents the coefficient matrix, θ represents the angle between the horizontal axis and the line connecting the center of the robot and the center of the first wheel, and l represents the distance from the center of the robot to each wheel.

[0015] In practical applications, when users perform daily actions, uncertain interaction forces will be generated and enter the system from the control channel to affect the tracking performance of the service robot. Therefore, the generalized input force u(t) is decomposed as follows:

[0016]

[0017] where u 0 (t) represents the control input force of the service robot, u 1 (t) represents the dissipative force of the human-machine system.

[0018] make Represents the uncertain interaction force of the human-machine system, then equation (2) is transformed into the following form:

[0019]

[0020] Among them, d(t) is bounded according to the physical meaning, that is, ||d(t)||≤ρ, and ρ is a positive constant.

[0021] Design the observer of the uncertain interaction force of the human-machine system as:

[0022]

[0023] where d(t) = [d 1 (t)d 2 (t)d 3 (t)] T are the uncertain interaction forces in the x-axis, y-axis and rotation angle directions, is the estimated value of d(t), is the estimation error, z is the intermediate auxiliary variable, and P is the observer gain matrix.

[0024] Using formula (4), The derivative is:

[0025]

[0026] Substituting equation (3)-equation (4) into equation (5), we can obtain:

[0027]

[0028] By taking the derivative of formula (6), we can get:

[0029]

[0030] Let x 1 (t) = X(t), Convert formula (3) into the following form:

[0031]

[0032] where x 1 (t) = [x 11 (t) x 12 (t) x 13 (t)] T are the actual motion trajectories in the x-axis, y-axis and rotation angle directions respectively; 2 (t) = [x 21 (t) x 22 (t) x 23 (t)] TThey are the actual movement speeds in the x-axis, y-axis and rotation angle directions respectively.

[0033] Let x d (t) represents the specified motion trajectory, then the trajectory tracking error e 1 (t) is:

[0034] e 1 (t) = x 1 (t)-x d (t) (9)

[0035] where e 1 (t) = [e 11 (t) e 12 (t) e 13 (t)] T are the trajectory tracking errors in the x-axis, y-axis and rotation angle directions respectively. d (t) = [x d1 (t) x d2 (t) x d3 (t)] T The specified motion trajectories are respectively the x-axis, y-axis and rotation angle directions.

[0036] Define the dummy variable α 1 (t) is:

[0037]

[0038] where α 1 (t) = [α 11 (t)α 12 (t)α 13 (t)] T are dummy variables for the x-axis, y-axis and rotation angle directions, respectively, and λ>0 indicates an adjustment parameter.

[0039] Assume that the speed tracking error e 2 (t) is:

[0040]

[0041] where e 2 (t) = [e 21 (t) e 22 (t) e 23 (t)] T They are the velocity tracking errors in the x-axis, y-axis and rotation angle directions respectively.

[0042] Design an adaptive controller u 0 (t) are as follows:

[0043]

[0044] Where c>0 represents the controller adjustment parameter.

[0045] In order to suppress the influence of uncertain human-machine interaction force on the motion of the service robot, the adaptive law of interaction force observation is designed as follows:

[0046]

[0047] Where r>0 is the adaptive law adjustment parameter.

[0048] By taking the derivative of formula (11), we can get:

[0049]

[0050] Further combining equation (9) and equation (11), we can obtain:

[0051]

[0052] The Lyapunov function V(t) is designed as follows:

[0053]

[0054] Deriving equation (16) and substituting equation (14)-(15) into it, we get:

[0055]

[0056] Substituting equation (7) and equation (12) into equation (17), we can obtain:

[0057]

[0058] Taking the derivative of formula (13) and substituting it into formula (18), we can get:

[0059]

[0060] From formula (19), we can see that the designed adaptive controller (12) suppresses the influence of the interaction force on the robot motion, making the trajectory tracking and velocity tracking of the human-machine system asymptotically stable, and at the same time making the observation error of the uncertain interaction force asymptotically stable. The designed observer (4) obtains the uncertain interaction force of the human-machine system

[0061] Step 2) Establish the constraints of the safe trajectory and safe speed of the human-machine system, design the control obstacle functions of the safe trajectory and safe speed respectively according to the constraints, and transform the design of the controller for safe trajectory and speed into solving a quadratic programming problem, thereby obtaining the optimal safe controller for stable tracking of the service robot. It is characterized in that: the service robot dynamic model (1) is transformed into a nonlinear affine system as follows:

[0062]

[0063] in

[0064] The safety trajectory constraint conditions for the service robot to maintain a certain distance from obstacles are designed as follows:

[0065] x 1i (t)-x ki (t)≥D i (twenty one)

[0066] where x k (t) = [x k1 (t) x k2 (t) x k3 (t)] T Respectively represent the position of the obstacle in the x-axis, y-axis and rotation angle directions, and D=[D 1 D 2 D 3 ] T , D i ≥0, i=1,2,3.

[0067] In order to ensure that the service robot has a safe motion trajectory, the trajectory control obstacle function is designed as follows:

[0068]

[0069] From formula (22), we can know that h x (x)≥0, and for h x Taking the partial derivative of (x) along x(t) yields:

[0070]

[0071] For h x (x) Taking the Lie derivative along g(x), we have Established, so the trajectory control obstacle function h x (x) is then differentiated and we get The control input set A of the safe trajectory is defined as:

[0072] A={u(t)∈U|L 2 f h x (x)+L g L f h x (x)u(t)+κ[h x (x)]…0} (24)

[0073] in is the set of allowed control inputs, and κ[h x(x)]=k 0 h x (x)+k 1 L f h x (x), k 0 >0 and k 1 >0 indicates adjustment parameters, L f h x (x) represents h x Lie derivative of f(x) along f(x), L 2 f h x (x) indicates L f h x Lie derivative of f(x) along f(x).

[0074] The constraints of the safe speed of each axis of the service robot are designed as follows:

[0075] |x 2i (t)|≤v i (25)

[0076] where v i >0 indicates the safe speed value in the x, y axis and rotation angle directions.

[0077] In order to ensure that the service robot has a safe movement speed, the speed control obstacle function is designed as follows:

[0078]

[0079] From formula (25), we can get h v (x)≥0, and for h v Taking the partial derivative of (x) along x(t) yields:

[0080]

[0081] For h v (x) Taking the Lie derivative along g(x), we get So the control input set B of safe speed is defined as:

[0082] B={u(t)∈U|L f h v (x)+L g h v (x)u(t)+k 3 h v (x)…0} (28)

[0083] where k 3 >0 indicates adjustment parameters, L f h v (x) represents h vLie derivative of f(x) along f(x).

[0084] According to the service robot trajectory control obstacle function and speed control obstacle function, the design of the controller with both trajectory and speed safety is transformed into solving the following quadratic programming problem:

[0085]

[0086] Where H represents a given positive definite matrix, μ>0 is a penalty factor, and δ is a slack variable.

[0087] By solving the above optimization problem (29), we can obtain the optimal controller u for the service robot's motion trajectory to avoid obstacles and with speed constraints: * (t), the simultaneously safe motion trajectory and motion speed of the human-machine system are obtained.

[0088] Step 3) Based on the MSP430 series single-chip microcomputer, the output PWM signal is provided to the motor drive module, so that the service robot can stably track the specified motion trajectory and ensure that the human-machine system has a safe motion trajectory and motion speed. It is characterized by: the MSP430 series single-chip microcomputer is used as the main controller, the input of the main controller is connected to the motor speed measurement module, and the output is connected to the motor drive module; the motor drive circuit is connected to the DC motor; the power supply system supplies power to each electrical device. The control method of the main controller is to read the feedback signal of the motor encoder and the control command signal x given by the main controller 1 (t) and And calculate the error signal. According to the error signal, the main controller calculates the control amount of the motor according to the predetermined control algorithm and sends it to the motor drive module. The motor rotates to drive the wheel to maintain its own balance and move in a specified manner.

[0089] Compared with the prior art, the present invention has beneficial effects.

[0090] The present invention is a method for tracking and controlling the trajectory and speed of a service robot that is adaptable to uncertain human-machine interaction forces and is simultaneously safe, and has the following advantages: a human-machine system dynamics model with uncertain interaction forces is established, and an observer is designed to estimate the interaction forces; an adaptive controller is designed to suppress the influence of the interaction forces on the motion of the service robot and ensure the stability of the tracking error system; safe trajectory and safe speed constraints are constructed and control obstacle functions are designed respectively, and the design of a controller for simultaneous safety of trajectory and speed is converted into a quadratic programming problem, and the optimal safety controller of the human-machine system is obtained, thereby ensuring the safety of the human-machine system. BRIEF DESCRIPTION OF THE DRAWINGS

[0091] The present invention is further described below in conjunction with the accompanying drawings and specific implementation methods. The protection scope of the present invention is not limited to the following description.

[0092] Figure 1 This is a working block diagram of the controller of the present invention;

[0093] Figure 2 is the coordinate diagram of the system of the present invention;

[0094] Figure 3 It is the minimum system of MSP430 single-chip microcomputer of the present invention;

[0095] Figure 4 The peripheral expansion circuit of the main controller of the present invention;

[0096] Figure 5 This is the overall principle circuit of the hardware of the present invention. DETAILED DESCRIPTION

[0097] In order to make the purpose, technical scheme and beneficial effects of the embodiments of the present invention clearer, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.

[0098] like Figure 1-5 As shown, a service robot trajectory and speed tracking control method that adapts to the uncertain human-machine interaction force and is safe at the same time.

[0099] 1) Based on the service robot dynamics model, by decomposing the generalized input force, a human-machine system dynamics model with uncertain interaction forces is established, and an uncertain interaction force observer is designed. At the same time, an adaptive controller is constructed to suppress the influence of the interaction force on the robot motion and ensure the stability of trajectory tracking and velocity tracking of the human-machine system;

[0100] 2) Establish the constraints of the safe trajectory and safe speed of the human-machine system, design the control obstacle functions of the safe trajectory and safe speed respectively according to the constraints, and transform the design of the controller with safe trajectory and speed into solving a quadratic programming problem, so as to obtain the optimal safety controller for stable tracking of the service robot, and ensure that the human-machine system has a safe motion trajectory and speed at the same time.

[0101] in:

[0102] Step 1) Based on the service robot dynamics model, by decomposing the generalized input force, a human-machine system dynamics model of uncertain interaction force is established, and an uncertain interaction force observer is designed. At the same time, an adaptive controller is constructed to suppress the influence of the interaction force on the robot motion, and ensure the stability of the trajectory tracking and speed tracking of the human-machine system. The characteristics are as follows: The service robot dynamics model is described as follows:

[0103]

[0104] in

[0105]

[0106] u(t)=[f 1 f 2 f 3 ] T

[0107] M represents the quality of the service robot, m represents the quality of the user; X(t) represents the actual motion trajectory of the service robot; I 0 represents the moment of inertia of the service robot, r 0 represents the distance between the center of the system and the center of gravity. u(t) represents the generalized input force, f 1 , f 2 and f 3 Represent the generalized input force of each omnidirectional wheel. 0 , B(θ) represents the coefficient matrix, θ represents the angle between the horizontal axis and the line connecting the center of the robot and the center of the first wheel, and l represents the distance from the center of the robot to each wheel.

[0108] In practical applications, when users perform daily actions, uncertain interaction forces will be generated and enter the system from the control channel to affect the tracking performance of the service robot. Therefore, the generalized input force u(t) is decomposed as follows:

[0109]

[0110] where u 0 (t) represents the control input force of the service robot, u 1 (t) represents the dissipative force of the human-machine system.

[0111] make Represents the uncertain interaction force of the human-machine system, then equation (2) is transformed into the following form:

[0112]

[0113] Among them, d(t) is bounded according to the physical meaning, that is, ||d(t)||≤ρ, and ρ is a positive constant.

[0114] Design the observer of the uncertain interaction force of the human-machine system as:

[0115]

[0116] where d(t) = [d 1 (t)d 2 (t)d 3 (t)] T are the uncertain interaction forces in the x-axis, y-axis and rotation angle directions, is the estimated value of d(t), is the estimation error, z is the intermediate auxiliary variable, and P is the observer gain matrix.

[0117] Using formula (4), The derivative is:

[0118]

[0119] Substituting equation (3)-equation (4) into equation (5), we can obtain:

[0120]

[0121] By taking the derivative of formula (6), we can get:

[0122]

[0123] Let x 1 (t) = X(t), Convert formula (3) into the following form:

[0124]

[0125] where x 1 (t) = [x 11 (t) x 12 (t) x 13 (t)] T are the actual motion trajectories in the x-axis, y-axis and rotation angle directions respectively; 2 (t) = [x 21 (t) x 22 (t) x 23 (t)] T They are the actual movement speeds in the x-axis, y-axis and rotation angle directions respectively.

[0126] Let x d (t) represents the specified motion trajectory, then the trajectory tracking error e 1 (t) is:

[0127] e 1 (t) = x 1 (t)-x d (t) (9)

[0128] where e 1 (t) = [e 11 (t) e 12 (t) e 13 (t)] T are the trajectory tracking errors in the x-axis, y-axis and rotation angle directions respectively. d (t) = [x d1 (t) x d2 (t) x d3(t)] T The specified motion trajectories are respectively the x-axis, y-axis and rotation angle directions.

[0129] Define the dummy variable α 1 (t) is:

[0130]

[0131] where α 1 (t) = [α 11 (t)α 12 (t)α 13 (t)] T are dummy variables for the x-axis, y-axis and rotation angle directions, respectively, and λ>0 indicates an adjustment parameter.

[0132] Assume that the speed tracking error e 2 (t) is:

[0133] e 2 (t) = x 2 (t)-α 1 (t) (11)

[0134] where e 2 (t) = [e 21 (t) e 22 (t) e 23 (t)] T They are the velocity tracking errors in the x-axis, y-axis and rotation angle directions respectively.

[0135] Design an adaptive controller u 0 (t) are as follows:

[0136]

[0137] Where c>0 represents the controller adjustment parameter.

[0138] In order to suppress the influence of uncertain human-machine interaction force on the motion of the service robot, the adaptive law of interaction force observation is designed as follows:

[0139]

[0140] Where r>0 is the adaptive law adjustment parameter.

[0141] By taking the derivative of formula (11), we can get:

[0142]

[0143] Further combining equation (9) and equation (11), we can obtain:

[0144]

[0145] The Lyapunov function V(t) is designed as follows:

[0146]

[0147] Deriving equation (16) and substituting equation (14)-(15) into it, we get:

[0148]

[0149] Substituting equation (7) and equation (12) into equation (17), we can obtain:

[0150]

[0151] Taking the derivative of formula (13) and substituting it into formula (18), we can get:

[0152]

[0153] From formula (19), we can see that the designed adaptive controller (12) suppresses the influence of the interaction force on the robot motion, making the trajectory tracking and velocity tracking of the human-machine system asymptotically stable, and at the same time making the observation error of the uncertain interaction force asymptotically stable. The designed observer (4) obtains the uncertain interaction force of the human-machine system

[0154] Step 2) Establish the constraints of the safe trajectory and safe speed of the human-machine system, design the control obstacle functions of the safe trajectory and safe speed respectively according to the constraints, and transform the design of the controller for safe trajectory and speed into solving a quadratic programming problem, thereby obtaining the optimal safe controller for stable tracking of the service robot. It is characterized in that: the service robot dynamic model (1) is transformed into a nonlinear affine system as follows:

[0155]

[0156] in

[0157] The safety trajectory constraint conditions for the service robot to maintain a certain distance from obstacles are designed as follows:

[0158] x 1i (t)-x ki (t)≥D i (twenty one)

[0159] where x k (t) = [x k1 (t) x k2 (t) x k3 (t)] T Respectively represent the position of the obstacle in the x-axis, y-axis and rotation angle directions, and D=[D 1D 2 D 3 ] T , D i ≥0, i=1,2,3.

[0160] In order to ensure that the service robot has a safe motion trajectory, the trajectory control obstacle function is designed as follows:

[0161]

[0162] From formula (22), we can know that h x (x)≥0, and for h x Taking the partial derivative of (x) along x(t) yields:

[0163]

[0164] For h x (x) Taking the Lie derivative along g(x), we have Established, so the trajectory control obstacle function h x (x) is then differentiated and we get The control input set A of the safe trajectory is defined as:

[0165] A={u(t)∈U|L 2 f h x (x)+L g L f h x (x)u(t)+κ[h x (x)]…0} (24)

[0166] in is the set of allowed control inputs, and κ[h x (x)]=k 0 h x (x)+k 1 L f h x (x), k 0 >0 and k 1 >0 indicates adjustment parameters, L f h x (x) represents h x Lie derivative of f(x) along f(x), L 2 f h x (x) indicates L f h x Lie derivative of f(x) along f(x).

[0167] The constraints of the safe speed of each axis of the service robot are designed as follows:

[0168] |x 2i (t)|≤v i (25) where v i >0 indicates the safe speed value in the x, y axis and rotation angle directions.

[0169] In order to ensure that the service robot has a safe movement speed, the speed control obstacle function is designed as follows:

[0170]

[0171] From formula (25), we can get h v (x)≥0, and for h v Taking the partial derivative of (x) along x(t) yields:

[0172]

[0173] For h v (x) Taking the Lie derivative along g(x), we get So the control input set B of safe speed is defined as:

[0174] B={u(t)∈U|L f h v (x)+L g h v (x)u(t)+k 3 h v (x)…0} (28)

[0175] where k 3 >0 indicates adjustment parameters, L f h v (x) represents h v Lie derivative of f(x) along f(x).

[0176] According to the service robot trajectory control obstacle function and speed control obstacle function, the design of the controller with both trajectory and speed safety is transformed into solving the following quadratic programming problem:

[0177]

[0178] Where H represents a given positive definite matrix, μ>0 is a penalty factor, and δ is a slack variable.

[0179] By solving the above optimization problem (29), we can obtain the optimal controller u for the service robot's motion trajectory to avoid obstacles and with speed constraints: * (t), the simultaneously safe motion trajectory and motion speed of the human-machine system are obtained.

[0180] Step 3) Based on the MSP430 series single-chip microcomputer, the output PWM signal is provided to the motor drive module, so that the service robot can stably track the specified motion trajectory and ensure that the human-machine system has a safe motion trajectory and motion speed. It is characterized by: the MSP430 series single-chip microcomputer is used as the main controller, the input of the main controller is connected to the motor speed measurement module, and the output is connected to the motor drive module; the motor drive circuit is connected to the DC motor; the power supply system supplies power to each electrical device. The control method of the main controller is to read the feedback signal of the motor encoder and the control command signal x given by the main controller 1 (t) and And calculate the error signal. According to the error signal, the main controller calculates the control amount of the motor according to the predetermined control algorithm and sends it to the motor drive module. The motor rotates to drive the wheel to maintain its own balance and move in a specified manner.

[0181] In summary, the present invention establishes a dynamic model of the uncertain interactive force human-machine system of the service robot, designs an observer to realize interactive force estimation, and constructs an adaptive controller to eliminate the influence of interactive force on the robot motion, with the aim of improving tracking accuracy and ensuring the stability of the human-machine system motion. Furthermore, the human-machine system safe trajectory and safe speed constraints are designed, and the safe trajectory and safe speed control obstacle functions are designed respectively according to the constraints, thereby obtaining a novel optimal tracking controller with safe trajectory and speed at the same time, which improves the safety performance of the service robot.

[0182] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, it should be understood by those skilled in the art that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features therein may be replaced by equivalents. Therefore, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope defined by the claims of the present invention.

Claims

1. A method for controlling the trajectory and speed of a service robot that is adaptive to interactive forces and is safe and reliable, characterized in that: Includes steps: S1. Based on the service robot dynamics model, by decomposing the generalized input force, a human-machine system dynamics model of uncertain interaction force is established, and an uncertain interaction force observer is designed. At the same time, an adaptive controller is constructed to suppress the influence of interaction force on robot motion and ensure the stability of trajectory tracking and velocity tracking of the human-machine system. S2. Establish the constraints of the safe trajectory and safe speed of the human-machine system, design the control obstacle functions of the safe trajectory and safe speed respectively according to the constraints, and transform the design of the controller with safe trajectory and speed into solving the quadratic programming problem, so as to obtain the optimal safety controller for stable tracking of the service robot, and ensure that the human-machine system has a safe motion trajectory and motion speed.

2. The method for controlling the trajectory and speed of a service robot that is adaptive to interactive forces and is safe according to claim 1, characterized in that: Step S1 is specifically divided into: The dynamic model of the service robot is established as follows: in u(t)=[f1 f2 f3] T M represents the mass of the service robot, m represents the mass of the user; X(t) represents the actual motion trajectory of the service robot; I0 ​​represents the moment of inertia of the service robot, r0 represents the distance between the center of the system and the center of gravity; u(t) represents the generalized input force, f1, f2 and f3 represent the generalized input forces of each omnidirectional wheel respectively; M0, B(θ) represents the coefficient matrix, θ represents the angle between the horizontal axis and the line connecting the center of the robot and the center of the first wheel, and l represents the distance from the center of the robot to each wheel; In the application, when the user performs life actions, uncertain interaction forces will be generated, and they will enter the system from the control channel and affect the tracking performance of the service robot. Therefore, the generalized input force u(t) is decomposed as follows: Where u0(t) represents the control input force of the service robot, and u1(t) represents the dissipative force of the human-machine system; make Represents the uncertain interaction force of the human-machine system, then formula (2) is transformed into the following form: Among them, d(t) is bounded according to the physical meaning, that is, ||d(t)||≤ρ, ρ is a positive constant; Design the observer of the uncertain interaction force of the human-machine system as: where d(t) = [d1(t)d2(t)d3(t)] T are the uncertain interaction forces in the x-axis, y-axis and rotation angle directions, is the estimated value of d(t), is the estimation error; z is the intermediate auxiliary variable, P is the observer gain matrix; Using formula (4), The derivative is: Substituting equation (3)-equation (4) into equation (5), we can obtain: By taking the derivative of formula (6), we can get: Let x1(t)=X(t), Convert formula (3) into the following form: where x1(t) = [x 11 (t)x 12 (t)x 13 (t)] T are the actual motion trajectories in the x-axis, y-axis and rotation angle directions respectively; x2(t)=[x 21 (t)x 22 (t)x 23 (t)] T The actual movement speeds in the x-axis, y-axis and rotation angle directions respectively; Let x d (t) represents the specified motion trajectory, then the trajectory tracking error e1(t) is: e1(t)=x1(t)-x d (t) (9) where e1(t) = [e 11 (t)e 12 (t)e 13 (t)] T are the trajectory tracking errors in the x-axis, y-axis and rotation angle directions respectively; d (t) = [x d1 (t)x d2 (t)x d3 (t)] T The specified motion trajectories are respectively the x-axis, y-axis and rotation angle directions; Define the dummy variable α1(t) as: where α1(t)=[α 11 (t)α 12 (t)α 13 (t)] T are dummy variables for the x-axis, y-axis and rotation angle directions, respectively, and λ>0 indicates an adjustment parameter; Assume the velocity tracking error e2(t) is: e2(t)=x2(t)-α1(t) (11) where e2(t) = [e 21 (t)e 22 (t)e 23 (t)] T They are the velocity tracking errors in the x-axis, y-axis and rotation angle directions respectively; Design the adaptive controller u0(t) as follows: Where c>0 represents the controller adjustment parameter; In order to suppress the influence of uncertain human-machine interaction force on the motion of the service robot, the adaptive law of interaction force observation is designed as follows: Where r>0 is the adaptive law adjustment parameter; By taking the derivative of formula (11), we can get: Combining equation (9) and equation (11), we can get: The Lyapunov function V(t) is designed as follows: Deriving equation (16) and substituting equation (14)-(15) into it, we get: Substituting equation (7) and equation (12) into equation (17), we can obtain: Taking the derivative of formula (13) and substituting it into formula (18), we can get: From formula (19), we can see that the designed adaptive controller (12) suppresses the influence of the interaction force on the robot motion, making the trajectory tracking and velocity tracking of the human-machine system asymptotically stable, and at the same time making the observation error of the uncertain interaction force asymptotically stable. The designed observer (4) obtains the uncertain interaction force of the human-machine system 3. The method for controlling the trajectory and speed of a service robot that is adaptive to interactive forces and is safe according to claim 1, characterized in that: Step S2 is specifically divided into: in The safety trajectory constraints for the service robot to maintain a certain distance from obstacles are designed as follows: x 1i (t)-x ki (t)≥D i (21) where x k (t) = [x k1 (t)x k2 (t)x k3 (t)] T Represents the position of the obstacle in the x-axis, y-axis and rotation angle directions respectively, and D=[D1 D2 D3] T , D i ≥0, i=1,2,3; In order to ensure that the service robot has a safe motion trajectory, the trajectory control obstacle function is designed as follows: From formula (22), we can know that h x (x)≥0, and for h x Taking the partial derivative of (x) along x(t) yields: For h x (x) Taking the Lie derivative along g(x), we have Established, so the trajectory control obstacle function h x (x) is then differentiated and we get The control input set A of the safe trajectory is thus defined as: A={u(t)∈U|L 2 f h x (x)+L g L f h x (x)u(t)+κ[h x (x)]…0} (24) in is the set of allowed control inputs, and κ[h x (x)] = k0h x (x)+k1L f h x (x), k0>0 and k1>0 represent adjustment parameters, L f h x (x) represents h x Lie derivative of f(x) along f(x), L 2 f h x (x) indicates L f h x (x) Lie derivative along f(x); The constraints of the safe speed of each axis of the service robot are designed as follows: |x 2i (t)|≤v i (25) where v i >0 indicates the safe speed value in the x, y axis and rotation angle directions; In order to ensure that the service robot has a safe movement speed, the speed control obstacle function is designed as follows: From formula (25), we can get h v (x)≥0, and for h v Taking the partial derivative of (x) along x(t) yields: For h v (x) Taking the Lie derivative along g(x), we get So the control input set B of safe speed is defined as: B={u(t)∈U|L f h v (x)+L g h v (x)u(t)+k3h v (x)…0} (28) Where k3>0 represents the adjustment parameter, L f h v (x) represents h v (x) Lie derivative along f(x); According to the service robot trajectory control obstacle function and speed control obstacle function, the design of the controller with both trajectory and speed safety is transformed into solving the following quadratic programming problem: Where H represents a given positive definite matrix, μ>0 is a penalty factor, and δ is a slack variable; By solving the above optimization problem (29), we can obtain the optimal controller u for the service robot's motion trajectory to avoid obstacles and with speed constraints: * (t), the simultaneously safe motion trajectory and motion speed of the human-machine system are obtained.

4. The method for controlling the trajectory and speed of a service robot that is adaptive to interactive forces and is safe according to claim 1, characterized in that: Also includes: Based on the MSP430 series single-chip microcomputer, the output PWM signal is provided to the motor drive module, so that the service robot can stably track the specified motion trajectory and ensure that the human-machine system has a safe motion trajectory and motion speed at the same time; specifically, the MSP430 series single-chip microcomputer is used as the main controller, the input of the main controller is connected to the motor speed measurement module, and the output is connected to the motor drive module; the motor drive circuit is connected to the DC motor; the power supply system supplies power to various electrical equipment; the main controller control method is to read the feedback signal of the motor encoder and the control command signal x1(t) and And calculate the error signal; based on the error signal, the main controller calculates the control amount of the motor according to the predetermined control algorithm and sends it to the motor drive module. The motor rotates to drive the wheel to maintain its own balance and move in a specified manner.