CN112034842B - Speed constraint tracking control method of service robot applicable to different users - Google Patents
Speed constraint tracking control method of service robot applicable to different users Download PDFInfo
- Publication number
- CN112034842B CN112034842B CN202010794629.3A CN202010794629A CN112034842B CN 112034842 B CN112034842 B CN 112034842B CN 202010794629 A CN202010794629 A CN 202010794629A CN 112034842 B CN112034842 B CN 112034842B
- Authority
- CN
- China
- Prior art keywords
- speed
- random
- robot
- different users
- service robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 104
- 230000006641 stabilisation Effects 0.000 claims abstract description 38
- 238000011105 stabilization Methods 0.000 claims abstract description 38
- 230000008859 change Effects 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 48
- 230000003595 spectral effect Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 10
- 230000009471 action Effects 0.000 claims description 5
- 230000003750 conditioning effect Effects 0.000 claims description 5
- 238000005457 optimization Methods 0.000 claims description 5
- 230000000452 restraining effect Effects 0.000 abstract description 2
- 230000006735 deficit Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 208000037265 diseases, disorders, signs and symptoms Diseases 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 230000000474 nursing effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
- Numerical Control (AREA)
Abstract
The speed constraint tracking control method of the service robot suitable for different users comprises the following steps: 1) Establishing a random differential equation for describing the quality changes of different users; 2) Restricting the movement speed of the robot in the axial and rotational angle directions; 3) And (3) establishing a tracking error system by utilizing the constrained motion speed in the step (2) and combining the random differential equation in the step (1), and establishing an exponential stabilization condition of the tracking error system based on a random stabilization theory to realize a speed constraint tracking control method suitable for different users. The controller is simple in design and easy to realize, and quality information of a user does not exist in the controller, so that the service robot can be applied to different users, and the track tracking precision is improved; meanwhile, a method for restraining the movement speed is provided for the service robot system described by the random differential equation, abrupt change of the robot speed is avoided, and safety of a user is guaranteed.
Description
Technical field:
the invention relates to the field of control of service robots, in particular to a control method of a wheeled life service robot.
The background technology is as follows:
traffic accidents and population aging increase patients with walking disorders year by year, and the patients with walking disorders cannot get timely and effective exercise training due to the lack of professional rehabilitation staff in China, so that the walking functions are gradually lost, and daily independent life cannot be realized. With the application of the service robot in places such as home, nursing homes and the like, the self-standing living problem of patients with walking impairment is effectively solved. However, in practical applications, users with different masses may deviate the robot from the indoor motion track, seriously affect the tracking accuracy of the robot, and even cause the robot to collide with surrounding obstacles. In addition, the uncertain external environment in the movement process of the robot inevitably causes abrupt change of the speed, thereby threatening the safety of the user. Therefore, it is important to study the control method of the service robot so that it can adapt to users of different qualities and help patients with walking impairment to realize daily independent life with a restricted movement speed.
In recent years, there have been many studies on tracking control of service robots, but these results cannot solve the problem of random variation in quality of different users. If the robot cannot adapt to users with different qualities, not only the tracking accuracy is affected, but also the robot collides with surrounding objects due to excessive tracking errors, thereby threatening the safety of the users. Meanwhile, the service robot dynamics system described by the random differential equation cannot directly restrict the movement speed. Therefore, no tracking control method with randomly changed quality and constrained speed for different users exists so far, and the method for improving the tracking precision of the service robot is researched based on a new view angle, so that the method has important significance for ensuring that patients with dyssynchrony can safely realize daily independent life.
The invention comprises the following steps:
the invention aims to:
in order to solve the problems, the invention provides a speed constraint tracking control method of a service robot applicable to different users, and aims to improve the tracking precision of the robot so as to ensure the safety of the users.
The technical scheme is as follows:
a speed constraint tracking control method of a service robot suitable for different users,
the method comprises the following steps:
1) According to the dynamic model of the service robot, the coefficient matrix M is obtained 0 The mass m of the middle trainer is decomposed into a constant value and a random variable, and a random differential equation for describing the mass change of different users is established;
2) Based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, so as to restrict the robot to x-axis, y Speed of movement of shaft and rotational angular direction
3) And (3) establishing a tracking error system by utilizing the constrained motion speed in the step (2) and combining the random differential equation in the step (1), and establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory to realize a speed constraint tracking control method suitable for different users.
In step 1): the kinetic model is described as follows:
wherein the method comprises the steps of
X (t) represents the actual walking track of the service robot, u (t) represents the control input force, f i (i=1, 2, 3) represents the input force to each wheel, M represents the mass of the robot, M represents the mass of the user, I 0 Represent moment of inertia, M 0 B (theta) is a coefficient matrix; θ represents the angle between the horizontal axis and the connection between the robot center and the center of the first wheel, l represents the distance from the center of the system to the center of each wheel,representing the moment of inertia of the user;
matrix the coefficients M 0 The mass m of the user in (a) is decomposed into m=m r +Δm, where m r Representing the set mass constant value, Δm represents the random variation value of the mass, and Δm is converted into random noise η (t), resulting in the following equation:
wherein the method comprises the steps ofIs M R And (2) inverse matrix of (2)
According to equation (2), the random noise η (t) is expressed asWherein v represents a 3-dimensional independent random process, to obtain
Order the(v=1, 2,3; σ=1, 2, 3) and calculates
Further, the formula (3) is
Let the spectral density of random noise eta (t) beI.e. < ->Is true, where Λ represents the spectral density matrix, +.>Representing a random process with a spectral density distribution, thus yielding a random differential equation for the service robot
In step 2): the kinematic model of the system is described as follows:
wherein B is G (t) represents a coefficient matrix, and
V(t)＝[v 1 (t) v 2 (t) v 3 (t)] T ，v i (t) (i=1, 2, 3) represents the movement speed of each wheel;
as further obtained from the equation (7),
discretizing equation (8), letting Y (t) =x (t) represent the system output, and writing the velocity input V (t) into an incremental expression form to obtain a predictive model as follows
Where k=0, 1, …, N-1, N represents the prediction horizon; x (k) and X (k+1) respectively represent the motion trail of the robot at the current moment and the later moment; y (k) represents the current robot motion position output; deltaV (k) represents the current time speed increment and V (k-1) represents the previous time speed input; a=i 3 ，T represents sampling time, I 3 An identity matrix representing the appropriate dimension;
next, an x-axis is constructed, y Shaft and screwSpeed of prediction of the direction of rotationAnd a predictive input for each wheel speed +.>The constraints of (2) are as follows:
wherein the method comprises the steps ofPredictive input representing speed, N C To control the time domain +.>An upper bound and a lower bound of the predicted input representing the speed, respectively;Representing the predicted actual movement speed,/->Respectively representing an upper bound and a lower bound of the actual movement speed;
according to equation (9), the predicted speed model is obtained as follows:
wherein the method comprises the steps ofΦ＝B p L 0 ，G＝B p L 1 ，
B (k+δ), δ=0, 1 … N-1 represents the value of the coefficient matrix B at different sampling instants in equation (9);
substituting the formula (11) into the constraint condition (10), and conditioning the constraint condition into a speed input incrementThe form is as follows
Wherein the method comprises the steps of
b 1min And b 1max Respectively representLower and upper limits of the constraint; b 2min And b 2max Respectively indicate->Lower and upper limits of the constraint;
and further has
Wherein the method comprises the steps of
The objective function J is established as follows:
wherein the method comprises the steps ofRepresent a specified speed of movement, Q 1 And Q 2 Respectively positive fixed adjustment matrixes; substituting the formula (11) into the formula (14), the objective function is expressed as
Wherein the method comprises the steps of
Thus, by solving the quadratic programming optimization problem equations (15) and (13), the speed input increment is obtainedWill->Is the first speed increment of +.>Substituting the motion velocity into a prediction model (9) to obtain a velocity input V (k) of each wheel of the robot, and controlling a motion velocity system (8) of the service robot by using the V (k) so as to restrict the motion velocity system in an x-axis, y Actual speed of shaft, rotation angle direction
Step (a)3) In (a): constrained speed of motionThe method comprises the steps of combining a random differential equation, establishing a tracking error system, establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory, obtaining a speed constraint tracking controller suitable for different users, serving an actual walking track X (t) of a robot, and designating a training track X by a doctor d (t) restricted movement speed +.>Designated movement speed +.>Set track tracking error e 1 (t) and velocity tracking error e 2 (t) are respectively
e 1 (t)＝X(t)-X d (t) (16)
Wherein alpha represents a parameter to be designed, and a tracking error system is obtained according to a random differential equation of the rehabilitation walking robot, and is as follows:
de 1 (t)＝[e 2 (t)+αe 1 (t)]dt (18)
designing Lyapunov function as
Based on the random stabilization theory, obtain
Wherein I represents an identity matrix of appropriate dimension; according to Young's inequality, for a given constant ρ 1 ＞0,ρ 2 > 0, have
Wherein,representing the F norm of the matrix, and the upper bound is h;
further, the design controller u (t) is as follows:
wherein the parameters to be designedλ 1 ＞0,ρ 1 ＞0,λ 2 > 0 represents a controller parameter;
thus, under the action of the controller (24), the tracking error systems (16) (17) are enabled to realize random exponential stabilization according to the formula (21). The advantages and effects are that:
a speed constraint tracking control method of a service robot suitable for different users,
1) According to the dynamic model of the service robot, the coefficient matrix M 0 The mass m of the middle trainer is decomposed into a constant value and a random variable, and a random differential equation for describing the mass change of different users is established;
2) Based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, so as to restrict the robot to x-axis, y Shaft and rotarySpeed of movement in the direction of rotation
3) And establishing a tracking error system by utilizing the constrained motion speed and combining a random differential equation, establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory, and obtaining a speed constraint tracking controller suitable for different users.
The method comprises the following steps:
step 1) decomposing the quality m of a trainer into a constant value and a random variable based on a dynamic model of a service robot, and establishing a random differential equation for describing quality changes of different users, wherein the method is characterized in that: the kinetic model of the system is described below
Wherein the method comprises the steps of
X (t) represents the actual walking track of the service robot, u (t) represents the control input force, f i Representing the input force to each wheel, M representing the mass of the robot, M representing the mass of the user, I 0 Represent moment of inertia, M 0 B (θ) is a coefficient matrix. θ represents the angle between the horizontal axis and the connection between the robot center and the center of the first wheel, l represents the distance from the center of the system to the center of each wheel,representing the moment of inertia of the user;
matrix the coefficients M 0 The mass m of the user in (a) is decomposed into m=m r +Δm, where m r Representing a specified mass constant value, Δm represents a random variation value of the mass, and Δm is converted into random noise η (t), resulting in the following equation:
wherein the method comprises the steps of
According to equation (2), the random noise η (t) is expressed asWherein v represents a 3-dimensional independent random process, can be obtained
Order the(v=1, 2,3; σ=1, 2, 3) and calculates
Further, formula (3) is convertible into
Let the spectral density of random noise eta (t) beI.e. < ->Is true, where Λ represents the spectral density matrix, +.>Representing a random process with spectral density distribution, then a random differential equation of the available service robot
Step 2) based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, and the robot is constrained in the x-axis, y The motion speed of the shaft and the rotation angle direction is characterized in that: the kinematic model of the system is described below
Wherein the method comprises the steps of
V(t)＝[v 1 (t) v 2 (t) v 3 (t)] T ，v i (t) (i=1, 2, 3) represents the movement speed of each wheel.
As further obtained from the equation (7),
discretizing equation (8), letting Y (t) =x (t) represent the system output, and writing the velocity input V (t) into an incremental representation, the predictive model is obtained as follows
Where k=0, 1, …, N-1, N represents the prediction horizon; x (k) and X (k+1) respectively represent the motion trail of the robot at the current moment and the later moment; y (k) represents the current robot motion position output; deltaV (k) represents the current time speed increment and V (k-1) represents the previous time speed input; a=i 3 ，T represents sampling time, I 3 Representing an identity matrix of appropriate dimensions.
Next, an x-axis is constructed, y Predicted speed of shaft and rotation angle directionAnd a predictive input for each wheel speed +.>The constraints of (2) are as follows:
wherein the method comprises the steps ofPredictive input representing speed, N C To control the time domain +.>An upper bound and a lower bound of the predicted input representing the speed, respectively;Representing the predicted actual movement speed,/->Representing the upper and lower bounds, respectively, of the actual movement speed.
According to equation (9), the predicted speed model can be obtained as follows
Wherein the method comprises the steps ofΦ＝B p L 0 ，G＝B p L 1 ，
B (k+δ), δ=0, 1 … N-1 represents the value of the coefficient matrix B at different sampling instants in equation (9).
Substituting the formula (11) into the constraint condition (10), and conditioning the constraint condition into a speed input incrementThe form is as follows
Wherein the method comprises the steps of
And further has
Wherein the method comprises the steps of
The objective function J is established as follows
Wherein the method comprises the steps ofRepresent a specified speed of movement, Q 1 And Q 2 The adjustment matrices are positive. Substituting equation (11) into equation (14), the objective function can be expressed as
Wherein the method comprises the steps of
Thus, by solving quadratic programming optimization problem equations (15) and (13), the speed input delta can be obtainedWill->Is the first speed increment of +.>Substituting into the predictive model (9) to obtain the speed input V (k) of each wheel of the robot, and then utilizing V (k) to control the movement speed system (8) of the service robot, thereby obtaining the speed input V (k)Beam it is in x-axis, y Actual speed of the shaft, rotation angle direction +.>Step 3) use of the restricted movement speed +.>And combining a random differential equation to establish a tracking error system, and establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory to obtain a speed constraint tracking controller suitable for different users, wherein the method is characterized in that: the actual walking track X (t) of the service robot, and the doctor designates the training track X d (t) restricted movement speed +.>Designated movement speed +.>Set track tracking error e 1 (t) and velocity tracking error e 2 (t) are respectively
e 1 (t)＝X(t)-X d (t) (16)
Wherein alpha represents a parameter to be designed, and a tracking error system is obtained according to a random differential equation of the rehabilitation walking robot, and is as follows:
de 1 (t)＝[e 2 (t)+αe 1 (t)]dt (18)
designing Lyapunov function as
Based on the random stabilization theory, obtain
Where I represents an identity matrix of appropriate dimension. According to Young's inequality, for a given constant ρ 1 ＞0,ρ 2 > 0, have
Wherein,the F-norm of the matrix is represented and its upper bound is h.
Further, the design controller u (t) is as follows:
wherein the parameters to be designedλ 1 ＞0,ρ 1 ＞0,λ 2 And > 0 represents the controller parameter.
Thus, under the action of the controller (24), the tracking error systems (16) (17) can be stabilized by the formula (21) to realize random index stabilization. Since there is no user quality information in the controller u (t), and e 2 (t) contains a constrained motion speed so that the service robot can track indoor motion trajectories at the constrained motion speed for different users with randomly varying masses.
Step 4) providing output PWM signals to electricity based on MSP430 series single-chip microcomputerThe machine drive module enables the service robot to help different people and track indoor motion trail at constrained motion speed, and is characterized in that: taking an MSP430 series singlechip as a main controller, wherein the input of the main controller is connected with a motor speed measuring module, and the output of the main controller is connected with a motor driving module; the motor driving module is connected with the direct current motor; the power supply system supplies power to the respective electrical devices. 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 d (t) andan error signal is calculated. According to the error signal, the main controller calculates the control quantity of the motor according to a preset control algorithm and sends the control quantity to the motor driving module, and the motor rotates to drive the wheels to maintain self balance and move in a specified mode.
In summary, the invention relates to a speed constraint tracking control method of a service robot applicable to different users, which has the following advantages:
the invention combines a dynamics model, decomposes the user mass m into a constant value and a random variable, and establishes a random differential equation for describing the mass change of different users; based on a kinematic model of the service robot, a model prediction control method for restricting the motion speed of the robot is provided; the method for designing the controller suitable for the random variation of the quality of different users is provided by utilizing the constrained motion speed and combining a random differential equation, an exponential stabilization condition of a tracking error system is constructed by adopting a random Lyapunov stabilization theory, and a speed constraint tracking controller suitable for different users is obtained. The controller is simple in design and easy to realize, and quality information of a user does not exist in the controller, so that the service robot can be applied to different users, and the track tracking precision is improved; meanwhile, a method for restraining the movement speed is provided for the service robot system described by the random differential equation, abrupt change of the robot speed is avoided, and safety of a user is guaranteed.
Description of the drawings:
FIG. 1 is a block diagram of the operation of a controller according to the present invention;
FIG. 2 is a graph of a system of the present invention;
FIG. 3 is a schematic diagram of a MSP430 single-chip microcomputer minimal system according to the present invention;
FIG. 4 is a main controller peripheral expansion circuit of the present invention;
fig. 5 is a circuit of the general principles of the hardware of the present invention.
The specific embodiment is as follows:
a speed constraint tracking control method of a service robot suitable for different users,
the method comprises the following steps:
1) According to the dynamic model of the service robot, the coefficient matrix M is obtained 0 The mass m of the middle trainer is decomposed into a constant value and a random variable, and a random differential equation for describing the mass change of different users is established;
2) Based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, so as to restrict the robot to x-axis, y Speed of movement of shaft and rotational angular direction
3) And (3) establishing a tracking error system by utilizing the constrained motion speed in the step (2) and combining the random differential equation in the step (1), and establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory to realize a speed constraint tracking control method suitable for different users.
In step 1): the kinetic model is described as follows:
wherein the method comprises the steps of
X (t) represents the actual walking track of the service robot, u (t) represents the control input force, f i (i=1, 2, 3) represents the input force to each wheel, M represents the mass of the robot, M represents the mass of the user, I 0 Represent moment of inertia, M 0 B (θ) is a coefficient matrix. θ represents the angle between the horizontal axis and the connection between the robot center and the center of the first wheel, l represents the distance from the center of the system to the center of each wheel,representing the moment of inertia of the user;
matrix the coefficients M 0 The mass m of the user in (a) is decomposed into m=m r +Δm, where m r Representing the set mass constant value, Δm represents the random variation value of the mass, and Δm is converted into random noise η (t), resulting in the following equation:
wherein the method comprises the steps ofIs M R And (2) inverse matrix of (2)
According to equation (2), the random noise η (t) is expressed asWherein v represents a 3-dimensional independent random process, can be obtained
Order the(v=1, 2,3; σ=1, 2, 3), β represents an element in the inverse matrix, and is calculated
Further, formula (3) is convertible into
Let the spectral density of random noise eta (t) beI.e. < ->Is true, where Λ represents the spectral density matrix, +.>Representing a random process with spectral density distribution, then a random differential equation of the available service robot
In step 2): the kinematic model of the system is described as follows:
wherein B is G (t) represents a coefficient matrix, and
V(t)＝[v 1 (t) v 2 (t) v 3 (t)] T ，v i (t) (i=1, 2, 3) represents the movement speed of each wheel.
As further obtained from the equation (7),
discretizing equation (8), letting Y (t) =x (t) represent the system output, and writing the velocity input V (t) into an incremental representation, the predictive model is obtained as follows
Where k=0, 1, …, N-1, N represents the prediction horizon; x (k) and X (k+1) respectively represent the motion trail of the robot at the current moment and the later moment; y (k) represents the current robot motion position output; deltaV (k) represents the current time speed increment and V (k-1) represents the previous time speed input; a=i 3 ，T represents sampling time, I 3 Representing an identity matrix of appropriate dimensions.
Next, an x-axis is constructed, y Predicted speed of shaft and rotation angle directionAnd a predictive input for each wheel speed +.>The constraints of (2) are as follows:
wherein the method comprises the steps ofPredictive input representing speed, N C To control the time domain +.>An upper bound and a lower bound of the predicted input representing the speed, respectively;Representing the predicted actual movement speed,/->Representing the upper and lower bounds, respectively, of the actual movement speed.
According to equation (9), the predicted speed model is obtained as follows:
wherein the method comprises the steps ofΦ＝B p L 0 ，G＝B p L 1 ，
B (k+δ), δ=0, 1 … N-1 represents the value of the coefficient matrix B at different sampling instants in equation (9).
Substituting the formula (11) into the constraint condition (10), and conditioning the constraint condition into a speed input incrementThe form is as follows
Wherein the method comprises the steps of
b 1min And b 1max Respectively representLower and upper limits of the constraint; b 2min And b 2max Respectively indicate->Lower and upper limits of the constraint.
And further has
Wherein the method comprises the steps ofG L 、b m Is a coefficient matrix of (13)
The objective function J is established as follows:
wherein the method comprises the steps ofRepresent a specified speed of movement, Q 1 And Q 2 The adjustment matrices are positive. Substituting equation (11) into equation (14), the objective function can be expressed as
Wherein the method comprises the steps of
Thus, by solving quadratic programming optimization problem equations (15) and (13), the speed input delta can be obtainedWill->Is the first speed increment of +.>The velocity input V (k) of each wheel of the robot can be obtained by substituting the velocity input V (k) into a prediction model (9), and the motion velocity system (8) of the service robot is controlled by using the V (k), so that the robot can be restrained in the x-axis, y Actual speed of the shaft, rotation angle direction +.>
In step 3): constrained speed of motionThe method comprises the steps of combining a random differential equation, establishing a tracking error system, establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory, obtaining a speed constraint tracking controller suitable for different users, serving an actual walking track X (t) of a robot, and designating a training track X by a doctor d (t) restricted movement speed +.>Designated movement speed +.>Set track tracking error e 1 (t) and velocity tracking error e 2 (t) are respectively
e 1 (t)＝X(t)-X d (t) (16)
Wherein alpha represents a parameter to be designed, and a tracking error system is obtained according to a random differential equation of the rehabilitation walking robot, and is as follows:
de 1 (t)＝[e 2 (t)+αe 1 (t)]dt (18)
designing Lyapunov function as
Based on the random stabilization theory, obtain
Where I represents an identity matrix of appropriate dimension. According to Young's inequality, for a given constant ρ 1 ＞0,ρ 2 > 0, have
Wherein,the F-norm of the matrix is represented and its upper bound is h.
Further, the design controller u (t) is as follows:
wherein the parameters to be designedλ 1 ＞0,ρ 1 ＞0,λ 2 And > 0 represents the controller parameter.
Thus, under the action of the controller (24), the tracking error systems (16) (17) can be stabilized by the formula (21) to realize random index stabilization.
Since there is no user quality information in the controller u (t), and e 2 (t) contains a constrained motion speed so that the service robot can track indoor motion trajectories at the constrained motion speed for different users with randomly varying masses.
The invention uses the coefficient matrix M according to the dynamic model of the service robot 0 The mass m of the middle trainer is decomposed into a constant value and a random variable, and a random differential equation of the service robot is established; based on a kinematic model of the service robot, a model predictive control method is proposed for constraining the movement speed of the robot by controlling the speed of each wheel; further, a tracking error system is established by utilizing the constrained motion speed and combining a random differential equation, a controller design method suitable for random variation of quality of different users is provided, an exponential stabilization condition of the tracking error system is established by adopting a random Lyapunov stabilization theory, a speed constraint tracking controller suitable for different users is obtained, the tracking precision of a service robot system is improved, and the safety of the users is ensured.
The invention provides an output PWM signal for a motor driving module based on an MSP430 series singlechip, so that a service robot can help different people and track indoor motion tracks at constrained motion speeds, and the MSP430 series singlechip is used as a main controller, wherein the input of the main controller is connected with a motor speed measuring module and the output of the main controller is connected with the motor driving module; the motor driving module is connected with the direct current motor; the power supply system supplies power to the respective electrical devices. 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 d (t) andan error signal is calculated. According to the error signal, the main controller calculates the control quantity of the motor according to a preset control algorithm and sends the control quantity to the motor driving module, and the motor rotates to drive the wheels to maintain self balance and move in a specified mode.
The present invention will be further described with reference to the accompanying drawings, but the scope of the present invention is not limited by the examples.
The speed constraint tracking control method of the service robot suitable for different users is characterized by comprising the following steps of:
1) According to the dynamic model of the service robot, the coefficient matrix M 0 The mass m of the middle trainer is decomposed into a constant value and a random variable, and a random differential equation for describing the mass change of different users is established;
2) Based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, so as to restrict the movement speeds of the robot in the directions of the x axis, the y axis and the rotation angle
3) And establishing a tracking error system by utilizing the constrained motion speed and combining a random differential equation, establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory, and obtaining a speed constraint tracking controller suitable for different users.
The method comprises the following steps:
step 1) decomposing the quality m of a trainer into a constant value and a random variable based on a dynamic model of a service robot, and establishing a random differential equation for describing quality changes of different users, wherein the method is characterized in that: the kinetic model of the system is described below
Wherein the method comprises the steps of
X (t) represents the actual walking track of the service robot, u (t) represents the control input force, f i Representing the input force to each wheel, M representing the mass of the robot, M representing the mass of the user, I 0 Represent moment of inertia, M 0 B (θ) is a coefficient matrix. θ represents the angle between the horizontal axis and the connection between the robot center and the center of the first wheel, l represents the distance from the center of the system to the center of each wheel,representing the moment of inertia of the user.
Matrix the coefficients M 0 The mass m of the user in (a) is decomposed into m=m r +Δm, where m r Representing a specified mass constant value, Δm represents a random variation value of the mass, and Δm is converted into random noise η (t), resulting in the following equation:
wherein the method comprises the steps of
According to equation (2), the random noise η (t) is expressed asWherein v represents a 3-dimensional independent random process, can be obtained
Order the(v=1, 2,3; σ=1, 2, 3) and calculating +.>
Further, formula (3) is convertible into
Let the spectral density of random noise eta (t) beI.e. < ->Is true, where Λ represents the spectral density matrix, +.>Representing a random process with spectral density distribution, then a random differential equation of the available service robot
Step 2) based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, and the robot is constrained in the x-axis, y The motion speed of the shaft and the rotation angle direction is characterized in that: the kinematic model of the system is described below
Wherein the method comprises the steps of
V(t)＝[v 1 (t) v 2 (t) v 3 (t)] T ，v i (t) (i=1, 2, 3) represents the movement speed of each wheel.
As further obtained from the equation (7),
discretizing equation (8), letting Y (t) =x (t) represent the system output, and writing the velocity input V (t) into an incremental representation, the predictive model is obtained as follows
Where k=0, 1, …, N-1, N represents the prediction horizon; deltaV (k) represents the current time speed increment and V (k-1) represents the previous time speed input; a=i 3 ，T represents sampling time, I 3 Unit moment representing proper dimensionAn array.
Next, an x-axis is constructed, y Predicted speed of shaft and rotation angle directionAnd a predictive input for each wheel speed +.>The constraints of (2) are as follows:
wherein the method comprises the steps ofPredictive input representing speed, N C To control the time domain +.>An upper bound and a lower bound of the predicted input representing the speed, respectively;Representing the predicted actual movement speed,/->Representing the upper and lower bounds, respectively, of the actual movement speed.
According to equation (9), the predicted speed model can be obtained as follows
Wherein the method comprises the steps ofΦ＝B p L 0 ，G＝B p L 1 ，
B (k+δ), δ=0, 1 … N-1 represents the value of the coefficient matrix B at different sampling instants in equation (9).
Substituting the formula (11) into the constraint condition (10), and conditioning the constraint condition into a speed input incrementThe form is as follows
Wherein the method comprises the steps of
And further has
Wherein the method comprises the steps of
The objective function J is established as follows
Wherein the method comprises the steps ofRepresent a specified speed of movement, Q 1 And Q 2 The adjustment matrices are positive. Substituting equation (11) into equation (14), the objective function can be expressed as
Wherein the method comprises the steps of
Thus, by solving quadratic programming optimization problem equations (15) and (13), the speed input delta can be obtainedWill->Is the first speed increment of +.>The velocity input V (k) of each wheel of the robot can be obtained by substituting the velocity input V (k) into a prediction model (9), and the motion velocity system (8) of the service robot is controlled by using the V (k), so that the robot can be restrained in the x-axis, y Actual speed of the shaft, rotation angle direction +.>Step 3) use of the restricted movement speed +.>And combining a random differential equation to establish a tracking error system, establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory, obtaining a speed constraint tracking controller suitable for different users,the method is characterized in that: the actual walking track X (t) of the service robot, and the doctor designates the training track X d (t) restricted movement speed +.>Designated movement speed +.>Set track tracking error e 1 (t) and velocity tracking error e 2 (t) are respectively
e 1 (t)＝X(t)-X d (t) (16)
Wherein alpha represents a parameter to be designed, and a tracking error system is obtained according to a random differential equation of the rehabilitation walking robot, and is as follows:
de 1 (t)＝[e 2 (t)+αe 1 (t)]dt (18)
designing Lyapunov function as
Based on the random stabilization theory, obtain
Where I represents an identity matrix of appropriate dimension. According to Young's inequality, for a given constant ρ 1 ＞0,ρ 2 > 0, have
Wherein,the F-norm of the matrix is represented and its upper bound is h.
Further, the design controller u (t) is as follows:
wherein the parameters to be designedλ 1 ＞0,ρ 1 ＞0,λ 2 And > 0 represents the controller parameter.
Thus, under the action of the controller (24), the tracking error systems (16) (17) can be stabilized by the formula (21) to realize random index stabilization. Since there is no user quality information in the controller u (t), and e 2 (t) contains a constrained motion speed so that the service robot can track indoor motion trajectories at the constrained motion speed for different users with randomly varying masses.
Step 4) based on MSP430 series singlechip provide output PWM signal to motor drive module, make service robot can help different users and follow indoor motion trail with constrained motion speed, its characterized in that: taking an MSP430 series singlechip as a main controller, wherein the input of the main controller is connected with a motor speed measuring module, and the output of the main controller is connected with a motor driving module; the motor driving module is connected with the direct current motor; the power supply system supplies power to the respective electrical devices. 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 d (t) andan error signal is calculated. According to the error signal, the main controller calculates the control quantity of the motor according to a preset control algorithm and sends the control quantity to the motor driving module, and the motor rotates to drive the wheels to maintain self balance and move in a specified mode.
Conclusion:
the invention solves the problem of speed constraint tracking control of the service robot with the quality of different users changing randomly, and establishes a random differential equation for describing the quality change of different users based on a dynamic model of the service robot; a model prediction control method for restricting the movement speed of a robot is provided; the method for designing the controller suitable for the random variation of the quality of different users is provided by utilizing the constrained motion speed and combining a random differential equation, an exponential stabilization condition of a tracking error system is constructed based on a random Lyapunov stabilization theory, and a speed constraint tracking controller suitable for different users is obtained. The method effectively inhibits the influence of the quality change of different users on the tracking performance of the system, avoids the abrupt change of the speed of the robot, improves the tracking precision of the service robot and ensures the safety of the users.
Claims (3)
1. The speed constraint tracking control method of the service robot suitable for different users is characterized by comprising the following steps of:
the method comprises the following steps:
1) According to the dynamic model of the service robot, the coefficient matrix M is obtained 0 The mass m of the middle trainer is decomposed into a constant value and a random variable, and a random differential equation for describing the mass change of different users is established;
2) Based on the kinematic model of the service robot, a model predictive control method for controlling the movement speed of each wheel is provided, so as to restrict the movement speeds of the robot in the directions of the x axis, the y axis and the rotation angle
3) Establishing a tracking error system by utilizing the constrained motion speed in the step 2) and combining the random differential equation in the step 1), and establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory to realize a speed constraint tracking control method suitable for different users;
in step 1): the kinetic model is described as follows:
wherein the method comprises the steps of
X (t) represents the actual walking track of the service robot, u (t) represents the control input force, f i (i=1, 2, 3) represents the input force to each wheel, M represents the mass of the robot, M represents the mass of the user, I 0 Represent moment of inertia, M 0 B (theta) is a coefficient matrix; θ represents the angle between the horizontal axis and the connection between the robot center and the center of the first wheel, l represents the distance from the center of the system to the center of each wheel,representing the moment of inertia of the user;
matrix the coefficients M 0 The mass m of the user in (a) is decomposed into m=m r +Δm, where m r Representing the set mass constant value, Δm represents the random variation value of the mass, and Δm is converted into random noise η (t), resulting in the following equation:
wherein the method comprises the steps ofIs M R And (2) inverse matrix of (2)
According to equation (2), the random noise η (t) is expressed asWherein v represents a 3-dimensional independent random process, to obtain
Order the(v=1, 2,3; σ=1, 2, 3) and calculates
Further, the formula (3) is
Let the spectral density of random noise eta (t) beI.e. < ->Is true, where Λ represents the spectral density matrix, +.>Representing a random process with a spectral density distribution, thus yielding a random differential equation for the service robot
2. A service robot speed constraint tracking control method applicable to different users according to claim 1, wherein: in step 2): the kinematic model of the system is described as follows:
wherein B is G (t) represents a coefficient matrix, and
V(t)＝[v 1 (t) v 2 (t) v 3 (t)] T ，v i (t) (i=1, 2, 3) represents the movement speed of each wheel;
as further obtained from the equation (7),
discretizing equation (8), letting Y (t) =x (t) represent the system output, and writing the velocity input V (t) into an incremental expression form to obtain a predictive model as follows
Where k=0, 1, …, N-1, N represents the prediction horizon; x (k) and X (k+1) respectively represent the motion trail of the robot at the current moment and the later moment; y (k) represents the current robot motion position output; deltaV (k) represents the current time speed increment and V (k-1) represents the previous time speed input; a=i 3 ，T represents sampling time, I 3 An identity matrix representing the appropriate dimension;
next, predicted speeds in the x-axis, y-axis, and rotational angle directions are constructedAnd a predictive input for each wheel speed +.>The constraints of (2) are as follows:
wherein the method comprises the steps ofPredictive input representing speed, N C In order to control the time domain of the signal,an upper bound and a lower bound of the predicted input representing the speed, respectively;Representing the predicted actual movement speed,/->Respectively represent realUpper and lower bounds of the inter-motion velocity;
according to equation (9), the predicted speed model is obtained as follows:
wherein the method comprises the steps ofΦ＝B p L 0 ，G＝B p L 1 ，
B (k+δ), δ=0, 1 … N-1 represents the value of the coefficient matrix B at different sampling instants in equation (9);
substituting the formula (11) into the constraint condition (10), and conditioning the constraint condition into a speed input incrementThe form is as follows
Wherein the method comprises the steps of
b 1min And b 1max Respectively representLower and upper limits of the constraint; b 2min And b 2max Respectively indicate->Lower and upper limits of the constraint;
and further has
Wherein the method comprises the steps of
The objective function J is established as follows:
wherein the method comprises the steps ofRepresent a specified speed of movement, Q 1 And Q 2 Respectively positive fixed adjustment matrixes; substituting the formula (11) into the formula (14), the objective function is expressed as
Where Θ=2 (G T Q 1 G+Q 2 ),
Thus, by solving the quadratic programming optimization problem equations (15) and (13), the speed input increment is obtainedWill->Is the first speed increment of +.>Substituting into a prediction model (9) to obtain the speed input V (k) of each wheel of the robot, and controlling the movement speed system (8) of the service robot by using the V (k) so as to restrict the actual speed +.>
3. A service robot speed constraint tracking control method applicable to different users according to claim 1, wherein:
in step 3): constrained speed of motionThe method comprises the steps of combining a random differential equation, establishing a tracking error system, establishing an exponential stabilization condition of the tracking error system based on a random Lyapunov stabilization theory, obtaining a speed constraint tracking controller suitable for different users, serving an actual walking track X (t) of a robot, and designating a training track X by a doctor d (t) restricted movement speed +.>Designated movement speed +.>Set track tracking error e 1 (t) and velocity tracking error e 2 (t) are respectively
e 1 (t)＝X(t)-X d (t) (16)
Wherein alpha represents a parameter to be designed, and a tracking error system is obtained according to a random differential equation of the rehabilitation walking robot, and is as follows:
de 1 (t)＝[e 2 (t)+αe 1 (t)]dt (18)
designing Lyapunov function as
Based on the random stabilization theory, obtain
Wherein I represents an identity matrix of appropriate dimension; according to Young's inequality, for a given constant ρ 1 ＞0,ρ 2 > 0, have
Wherein,representing the F norm of the matrix, and the upper bound is h;
further, the design controller u (t) is as follows:
wherein the parameters to be designedλ 1 ＞0,ρ 1 ＞0,λ 2 > 0 represents a controller parameter;
thus, under the action of the controller (24), the tracking error systems (16) (17) are enabled to realize random exponential stabilization according to the formula (21).
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN2020100761510 | 2020-01-23 | ||
| CN202010076151 | 2020-01-23 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112034842A CN112034842A (en) | 2020-12-04 |
| CN112034842B true CN112034842B (en) | 2024-03-26 |
Family
ID=73576819
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010794629.3A Active CN112034842B (en) | 2020-01-23 | 2020-08-10 | Speed constraint tracking control method of service robot applicable to different users |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112034842B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3718026B1 (en) | 2017-12-01 | 2023-11-29 | 1QB Information Technologies Inc. | Systems and methods for stochastic optimization of a robust inference problem |
| EP3891668A4 (en) | 2018-12-06 | 2023-01-25 | 1QB Information Technologies Inc. | QUANTUM CALCULATION DRIVEN BY ARTIFICIAL INTELLIGENCE |
| WO2021111368A1 (en) | 2019-12-03 | 2021-06-10 | 1Qb Information Technologies Inc. | System and method for enabling an access to a physics-inspired computer and to a physics-inspired computer simulator |
| CN113377115B (en) * | 2021-07-05 | 2023-10-20 | 沈阳工业大学 | Stable control method for service robot to autonomously learn transient movement time |
| CN116257044B (en) * | 2021-12-09 | 2025-10-10 | 灵动科技(安徽)有限公司 | Multi-robot trajectory planning method and computer program product |
| EP4599309A1 (en) * | 2022-10-06 | 2025-08-13 | Irreversible Inc. | Systems and methods for simulating at least one solution of a stochastic differential equation and methods for using thereof for generative machine learning |
| CN116627138B (en) * | 2023-05-29 | 2024-09-06 | 东北大学 | Adaptive Heavy Object Mass Control Method for Mobile Robot with Speed Range Constraints |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104932488A (en) * | 2015-06-30 | 2015-09-23 | 南京工业大学 | Model predictive control performance evaluation and diagnosis method |
| KR20160024110A (en) * | 2014-08-25 | 2016-03-04 | 주식회사 바로텍시너지 | Legs rehabilitation robot capable of movable gait training and stationary gait training |
| CN107272640A (en) * | 2017-06-12 | 2017-10-20 | 华中科技大学 | A kind of modeling quality control method and system based on model predictive controller |
| CN108245380A (en) * | 2018-03-13 | 2018-07-06 | 西安交通大学 | A kind of human body lower limbs recovery exercising robot |
-
2020
- 2020-08-10 CN CN202010794629.3A patent/CN112034842B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20160024110A (en) * | 2014-08-25 | 2016-03-04 | 주식회사 바로텍시너지 | Legs rehabilitation robot capable of movable gait training and stationary gait training |
| CN104932488A (en) * | 2015-06-30 | 2015-09-23 | 南京工业大学 | Model predictive control performance evaluation and diagnosis method |
| CN107272640A (en) * | 2017-06-12 | 2017-10-20 | 华中科技大学 | A kind of modeling quality control method and system based on model predictive controller |
| CN108245380A (en) * | 2018-03-13 | 2018-07-06 | 西安交通大学 | A kind of human body lower limbs recovery exercising robot |
Non-Patent Citations (6)
| Title |
|---|
| Direct force control of upper-limb exoskeleton based on fuzzy adaptive algorithm;Jing Tang;《JOURNAL OF VIBROENGINEERING》;全文 * |
| Finite-time tracking control with velocity constraints for the stochastic rehabilitative training walker systems considering different rehabilitee masses;孙平;《ORIGINAL PAPER》;全文 * |
| 不确定康复训练机器人速度与加速度同时约束的跟踪控制;孙平;;北京理工大学学报(10);全文 * |
| 全方向康复步行训练机器人的迭代学习控制研究;单芮;《中国优秀硕博士学位论文全文数据库》;第3章 * |
| 助行康复机器人减重支撑单元控制仿真研究;张立勋;《康复工程》;全文 * |
| 考虑人机作用力的康复训练机器人各运动轴最优轨迹跟踪预测控制;孙平;《北京理工大学学报》;第1076-1079页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112034842A (en) | 2020-12-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112034842B (en) | Speed constraint tracking control method of service robot applicable to different users | |
| Venkatesh et al. | Enhancing Stability in Autonomous Control Systems Through Fuzzy Gain Scheduling (FGS) and Lyapunov Function Analysis | |
| Pan et al. | A new predictive approach for bilateral teleoperation with applications to drive-by-wire systems | |
| CN112417755A (en) | A master-slave surgical robot trajectory prediction control method | |
| CN102902203A (en) | Time series prediction and intelligent control combined online parameter adjustment method and system | |
| CN110989589A (en) | A tracking control method for a rehabilitation walking robot with random changes in the quality of different trainers | |
| CN113093538A (en) | Non-zero and game neural-optimal control method of modular robot system | |
| CN112433495B (en) | Fast finite time control of rehabilitation robot based on SCN human-machine uncertainty model | |
| CN113419433B (en) | Design method of tracking controller of under-actuated system of self-balancing electric wheelchair | |
| CN106329986B (en) | A kind of supersonic motor contragradience adaptive servo control method | |
| Deshmukh et al. | An improvement to the conventional PD scheme for the speed control of a two-wheeled differential steering mobile robot | |
| CN114019791A (en) | Adaptive neural network dynamic surface control method of photovoltaic power generation servo system | |
| CN119937612A (en) | Human-machine coordinated motion control method for active walking speed prediction of rehabilitation patients | |
| Antić et al. | Optimal Design of the Fuzzy Sliding Mode Control for a DC Servo Drive. | |
| Wu et al. | Fuzzy control of rider-motorcycle system using genetic algorithm and auto-tuning | |
| Noor et al. | STUDY THE PERFORMANCE OF TWO-WHEELED BALANCING MOBILE ROBOT USING FUZZY PD CONTROLLER | |
| Duan et al. | Application of PID tracking control in inverted pendulum system | |
| Kim et al. | Experimental evaluation of adaptive and robust schemes for robot manipulator control | |
| Hashim et al. | Optimized Type-2 Fuzzy Logic Control for Low-Speed Vehicle Pedal Pressing Automation Using Hybrid Spiral Sine Cosine Algorithm | |
| CN116000917A (en) | Motion trail safety triggering data driving control method of rehabilitation walking robot | |
| Deng et al. | Simulating optimal control of an inertial platform stabilization loop using genetic algorithms | |
| Farooq et al. | Design and implementation of a fuzzy logic controller for two wheeled self balancing robot | |
| Xue | Dual-Loop MPC Architecture for Quadrotor Control | |
| Salem | Mechatronics motion control design of electric machines for desired deadbeat response specifications, supported and verified by new matlab built-in function and simulink model | |
| Mala et al. | Design of PID controller for directon control of robotic vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |