CN115718421A - Mobile robot track tracking control method based on double closed-loop control - Google Patents
Mobile robot track tracking control method based on double closed-loop control Download PDFInfo
- Publication number
- CN115718421A CN115718421A CN202211261623.5A CN202211261623A CN115718421A CN 115718421 A CN115718421 A CN 115718421A CN 202211261623 A CN202211261623 A CN 202211261623A CN 115718421 A CN115718421 A CN 115718421A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- speed
- pose
- controller
- tracking error
- 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.)
- Pending
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a mobile robot trajectory tracking control method based on double closed-loop control. By constructing a double closed-loop control strategy of an outer-loop kinematics controller and an inner-loop terminal sliding-mode controller, the outer loop acquires the pose tracking error of the mobile robot, and the pose tracking error is input into the outer-loop kinematics controller to obtain the auxiliary speed, so that the finite-time convergence of the pose tracking error is realized, and the response speed of the system state is improved; the inner ring utilizes the linear extended state observer to estimate the system state and the lumped disturbance in real time, the speed tracking error is used as the input of the inner ring sliding mode controller, the control voltage is obtained through the inner ring sliding mode controller, the mobile robot is driven, and the actual speed of the mobile robot is gradually converged to the auxiliary speed. By executing the double closed-loop control strategy, the track tracking control of the mobile robot in limited time is realized, and the track tracking control precision of the mobile robot with unknown wheels slipping is improved.
Description
Technical Field
The invention relates to the technical field of robot trajectory control, in particular to a mobile robot trajectory tracking control method based on double closed-loop control.
Background
The application of the mobile robot is more and more common, and the research on the motion control of the mobile robot is increasingly deep. In the actual operation of the mobile robot, the wheel slip situation of the mobile robot is easy to occur due to the factors of tire deformation, wet or frozen road surface, rapid turning and the like, the non-integrity constraint of a control system of the mobile robot can be damaged, and the control precision of the mobile robot is reduced.
At present, the influence of a driving motor is researched or ignored, or the influence of longitudinal slipping (longitudinal slipping) and sideslip (lateral slipping) disturbance and uncertain model parameters on the motion control of the mobile robot is not considered, and a controller is designed to only realize the asymptotic stability of a closed loop state of the system, so that the infinite time convergence of the closed loop state of the system is caused theoretically, and the time optimal control is not realized.
Disclosure of Invention
Therefore, the mobile robot track tracking control method based on the double closed loop control is provided for solving the problem that the mobile robot has poor control effect under the condition of wheel slip. The method comprises the following steps:
the method comprises the following steps:
acquiring pose tracking errors of a reference pose and an actual pose of the mobile robot;
inputting the pose tracking error into an outer ring kinematics controller, and obtaining an auxiliary speed according to the output of the outer ring kinematics controller;
acquiring speed tracking errors of the auxiliary speed and the actual speed;
and taking the speed tracking error as the input of an inner ring terminal sliding mode controller, obtaining the control voltage of a driving motor of a driving wheel of the mobile robot through the inner ring sliding mode controller, and further driving the mobile robot to enable the actual speed of the mobile robot to gradually converge to the auxiliary speed.
In one embodiment, the acquiring pose tracking errors of the reference pose and the actual pose of the mobile robot includes:
determining a kinematic equation of the mobile robot;
and obtaining the pose tracking error of the mobile robot according to the reference pose, the actual pose and the kinematic equation of the mobile robot.
In one embodiment, the bitAttitude tracking error e q Calculated by the following algorithm:
wherein e is q ＝[e x e y e θ ] T Pose tracking error, e x 、e y 、e θ Tracking errors of the mobile robot in the directions of x, y and theta are respectively; (x, y) represents the position of the geometric center of the mobile robot in the global coordinate system, and theta is the course angle of the mobile robot; the subscript r denotes a reference state of the mobile robot.
In one embodiment, the outer loop kinematics controller is a finite time kinematics controller; the finite time kinematic controller is realized by the following formula:
wherein z is c ＝[v c ω c ] T To assist speed, v c To assist the linear velocity, ω c To assist angular velocity; k is a radical of x 、k y 、k θ Is the controller gain and is a normal number; beta is a x 、β y 、β θ Is the controller gain, and 0 < beta x ,β y ,β θ ＜1；e x 、e y 、e θ Tracking errors of the mobile robot in the directions of x, y and theta are respectively; v. of r Reference linear velocity, omega, for the advance of a mobile robot r Is the reference angular velocity of the mobile robot body around the geometric center.
In one embodiment, the inner-ring sliding-mode controller comprises a terminal sliding-mode surface, a controller module, a linear extended state observer, and a dynamic model; wherein, the first and the second end of the pipe are connected with each other,
the dynamic model is used for obtaining the actual speed of the mobile robot according to the control voltage of the driving motor of the driving wheel of the mobile robot;
the linear extended state observer is used for obtaining an actual speed estimation value and a disturbance estimation value of the mobile robot according to the actual speed and the control voltage of a driving motor of a driving wheel of the mobile robot;
the terminal sliding mode surface is used for designing a sliding surface according to the speed tracking error of the actual speed and the auxiliary speed as input, so that the speed tracking error index is converged to zero;
and the controller module is used for obtaining the control voltage of a driving wheel driving motor of the mobile robot according to the terminal sliding mode surface and the lumped disturbance estimation value, and the control voltage is used as the input of a dynamic model of the mobile robot to control the actual speed of the mobile robot to gradually converge to the auxiliary speed.
In one embodiment, the dynamic model is obtained according to a kinematic model and a driving motor model of the mobile robot; wherein the content of the first and second substances,
the kinematic model is realized by the following formula:
wherein q = [ x y θ] T Is the pose of the mobile robot; z = [ v ω ]] T The vector is a velocity vector of the mobile robot, and v and omega are respectively a linear velocity and an angular velocity of the mobile robot; eta = [ eta ] v η ω ] T ，η v ＝r(ξ r +ξ l ) [ eta ] is the longitudinal slip speed ω ＝r(ξ r -ξ l ) V (2 b) yaw rate disturbance caused by longitudinal sliding, r is the radius of the driving wheels of the mobile robot, 2b is the distance between the two driving wheels, xi l 、ξ r Respectively representing interference angular velocity vectors caused by the longitudinal sliding of the left driving wheel and the right driving wheel of the mobile robot;
the vector is a non-matching disturbance vector caused by sideslip of the mobile robot, and mu is the sideslip speed of the mobile robot; s (q) is a transformation matrix having
The driving motor model is realized by the following formula:
wherein L is a 、R a 、k t 、k b Armature inductance, armature resistance, torque constant and back electromotive force constant of the driving motor, respectively, N is a mechanical gear reduction ratio, and τ = [ τ = r τ l ] T Indicating the input torque of the right and left drive wheels, u, of the mobile robot a ＝[u ar u al ] T ,I a ＝[I ar I al ] T Control voltage and armature current, omega, of the drive motor for the right and left drive wheels, respectively w ＝[ω R ω L ] T The right and left driving wheels drive the angular speed of the motor;
and under the condition that the mobile robot takes the driving voltage of the direct current motor as a control input, the dynamic model is expressed as follows:
wherein
K 1 ＝Nk t /R a ，K 2 ＝N 2 k t k b /R a R is the radius of the driving wheel of the mobile robot, 2b is the distance between the two driving wheels, m and J are the mass and the moment of inertia of the mobile robot respectively, R a 、k t 、k b The armature resistance, the torque constant and the back electromotive force constant of the driving motor respectively, N is the mechanical gear reduction ratio, u a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels; d is a model containing unknown longitudinal and lateral slip disturbances of the systemUncertain parameters and lumped disturbances of unknown input disturbances.
In one embodiment, the linear extended state observer is implemented by the following formula:
wherein the content of the first and second substances,
are respectively a state vector x 1 、x 2 Estimate of beta 1 、β 2 For the gain of the extended state observer, the values are:
ω o the bandwidth of the observer is more than 0;
K 1 ＝Nk t /R a ,K 2 ＝N 2 k t k b /R a r is the radius of the driving wheel of the mobile robot, 2b is the distance between the two driving wheels, m and J are the mass and the moment of inertia of the mobile robot respectively, R a 、k t 、k b The armature resistance, the torque constant and the back electromotive force constant of the driving motor respectively, N is the mechanical gear reduction ratio, u a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels.
In one embodiment, the terminal sliding surface is implemented by the following formula:
e z ＝z-z c
wherein e is z For velocity tracking error, z = [ v ω ]] T For the velocity vector of the mobile robot, v andomega is the linear and angular velocity of the mobile robot, respectively, z c Is the auxiliary velocity vector of claim 4; s is a terminal sliding mode surface vector; q. q of 1 、p 1 Is positive odd and satisfies q 1 ＜p 1 ＜2q 1 ，λ 1 、λ 2 A diagonal matrix is determined for the positive to be designed.
In one embodiment, the finite time dynamics control law of the controller is implemented by the following formula:
wherein u is a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels;
wherein k is the boundary layer thickness and the value k =0.1.
In one embodiment, the method further comprises:
taking the actual speed as the input of a kinematic model of the mobile robot to obtain the actual pose of the mobile robot;
and acquiring a reference pose of the mobile robot and a pose tracking error of the actual pose, and executing the step of acquiring the auxiliary speed of the mobile robot according to the pose tracking error.
According to the mobile robot track tracking control method based on the double closed-loop control, a double closed-loop control strategy of an outer-loop kinematics controller and an inner-loop sliding mode controller is constructed, the pose tracking error is input into the outer-loop kinematics controller by acquiring the pose tracking error of a reference pose and an actual pose of the mobile robot, the auxiliary speed is obtained, the pose tracking error is converged within a limited time, the response speed of the system state is improved, the speed tracking error obtained according to the auxiliary speed and the actual speed is used as the input of the inner-loop sliding mode controller, and the actual speed is controlled by the inner-loop sliding mode controller to be gradually converged to the auxiliary speed. By executing the double closed-loop control strategy, the tracking control of the mobile robot in a limited time track is realized, and the control effectiveness of the mobile robot under the condition of wheel slip is improved.
Drawings
FIG. 1 is a diagram of a double closed loop control system for an unknown slip-down mobile robot in one embodiment;
FIG. 2 is a flow diagram illustrating trajectory tracking control of a mobile robot based on dual closed-loop control without slip-down in an embodiment;
fig. 3 is a schematic diagram of the motion trajectory of the down-sweeping robot according to different control schemes in one embodiment;
fig. 4 is a schematic diagram of the pose tracking effect of the robot sweeper under different control schemes in one embodiment;
FIG. 5 is a schematic diagram of pose tracking errors of the robot sweeper under different control schemes in one embodiment;
fig. 6 is a schematic diagram of actual speeds of the down-sweeping robot under different control schemes in one embodiment;
FIG. 7 is a schematic diagram of the collective disturbance estimation of the linear extended state observer on the sweeping robot in one embodiment;
fig. 8 is a schematic diagram of control voltages of the floor sweeping robot according to different control schemes in one embodiment.
Detailed Description
For a more clear understanding of the above objects, features and advantages of the present invention, reference is now made to the following detailed description taken in conjunction with the accompanying drawings. It should be noted that the embodiments and features of the embodiments of the present application may be combined with each other without conflict.
The following description sets forth numerous specific details for a thorough understanding of the present invention, however, the present invention may be practiced otherwise than as specifically described herein, and the scope of the present invention is not limited by the specific embodiments disclosed below.
When the mobile robot is actually operated, longitudinal slipping (longitudinal slipping) or lateral slipping (side slipping) of wheels easily occurs due to the influence of factors such as tire deformation, wet or frozen road surface, quick turning and the like. In other words, the mobile robot is actually operating under unknown skidding. The following describes a mobile robot trajectory tracking control method based on dual closed-loop control according to some embodiments of the present invention, with reference to the accompanying drawings, to solve the problem of poor control effect of a mobile robot in a wheel slipping situation. The method can be applied to a cloud or a server, and can also be applied to terminals capable of performing track tracking control, such as a sweeping robot, a sweeping and mopping integrated robot, a two-wheel differential drive service robot and other mobile robots. The following description will be given taking an example in which the method of the present invention is applied to a mobile robot terminal.
The method is realized by depending on a double closed-loop control system, and in order to construct the double closed-loop control system, a kinematics model of the mobile robot under unknown longitudinal slip and sideslip and a dynamics model considering the dynamics characteristics of a driving motor need to be established in advance.
In order to construct a kinematic model of wheels of the mobile robot under longitudinal sliding and side sliding disturbance, the pose of the mobile robot is defined as q = [ x y theta ]] T ∈R 3×1 . Wherein, (x, y) represents the position of the geometric center of the mobile robot in the global coordinate system, and theta represents the heading angle of the mobile robot. Thus, in the presence of longitudinal and lateral slip disturbances, the mobile robot kinematics model can be expressed as:
wherein z = [ v ω ]] T V and omega are respectively the linear velocity and the angular velocity of the mobile robot; eta = [ eta ] v η ω ] T ，η v ＝r(ξ r +ξ l ) [ eta ] is the longitudinal slip speed ω ＝r(ξ r -ξ l ) V (2 b) yaw rate disturbance caused by longitudinal sliding, r is the radius of the driving wheels of the mobile robot, 2b is the distance between the two driving wheels, xi l 、ξ r Respectively representing interference angular velocity vectors caused by the longitudinal sliding of the left driving wheel and the right driving wheel of the mobile robot;
the method comprises the following steps of (1) obtaining a non-matching disturbance vector caused when the mobile robot sideslips, wherein mu is the sideslip speed of the mobile robot; s (q) is a transform matrix having the form:
in the pure rolling no-slip ideal case, the dynamic model of the mobile robot can be described as:
wherein
Positively determining a symmetric inertia matrix for the system, wherein m is the mass of the mobile robot, and J is the rotational inertia of the mobile robot;
the method is a system centrifugal force and Goldfish force matrix, and the item is an all-zero matrix because the mass center of the mobile robot is coincident with the geometric center; g (q) ∈ R 3×1 The term is a zero vector for the gravity vector of the system and for the mobile robot moving in a plane;
controlling a moment transformation matrix for the system; τ = [ τ ] r τ l ] T ∈R 2×1 Is an input torque vector; a (q) = [ -sin θ cos θ 0]∈R 1×3 Is a systematic incomplete constraint vector, and S T (q)A T (q)＝0∈R 2×1 ；
Is a system constraint force term.
Formula (1) and formula (3) are combined to obtain:
wherein the content of the first and second substances,
F 2 ＝S T V＝0∈R 2×3 ；
considering the dynamics model parameter uncertainty and unknown dynamics input disturbance, equation (4) can be rewritten as:
wherein Δ M s ∈R 2×2 Is M s Amount of change of (c), τ d ∈R 2×1 The bounded conductive input is unknown.
Further define the
Equation (5) can be further expressed as:
because unmodeled high-frequency disturbance also exists in motor dynamics, the influence of the motor dynamics needs to be considered in control design in order to improve the control precision of a system. Assuming that the mobile robot is driven by two identical brushed direct current motors, the dynamic equation of the right and left driving wheel driving motors is described as follows:
wherein L is a 、R a 、k t 、k b The armature inductance, the armature resistance, the torque constant and the back electromotive force constant of the driving motor respectively, N is the mechanical gear reduction ratio, and tau = [ tau = [ r τ l ] T Representing input torque, u, of the mobile robot a ＝[u ar u al ] T ,I a ＝[I ar I al ] T Control voltage and armature current, omega, of the drive motor for the right and left drive wheels, respectively w ＝[ω R ω L ] T For driving the angular velocity of the motor, and
wherein z = [ v ω ]] T V and omega are respectively the linear velocity and the angular velocity of the mobile robot,
is a transformation matrix.
Considering that the driving motor is an actuating mechanism of the system, compared with the system as a whole, the response speed is high, so that a driving motor model can be properly simplified, and the inductance is ignored and can be obtained by sorting:
τ＝K 1 u a -K 2 Xz (9)
wherein, K 1 ＝Nk t /R a ，K 2 ＝N 2 k t k b /R a 。
The dynamic model of the mobile robot with the driving voltage of the direct current motor as the control input can be expressed as follows:
wherein the content of the first and second substances,
lumped disturbances including unknown longitudinal and lateral slip disturbances of the system, uncertain model parameters, and unknown bounded guided input disturbances.
The control task is to design the control voltage u of the driving motor of the right driving wheel and the driving motor of the left driving wheel aiming at the mobile robot system with uncertain model parameters and unknown externally-bounded conductive disturbance a Therefore, the mobile robot can accurately complete the track tracking task under the condition of unknown longitudinal sliding and sideslip, and the track tracking error is converged in limited time.
On the basis of the dynamic model and the kinematic model of the mobile robot, a double closed-loop control system shown in fig. 1 can be constructed, and the accurate trajectory tracking control of the mobile robot considering the motor dynamics in the limited time under the unknown longitudinal and lateral sliding disturbance, the uncertain model parameters and the unknown input disturbance is realized. The double closed-loop control system structure comprises an outer-loop kinematics controller and an inner-loop sliding-mode controller. The outer ring kinematics controller can be configured as a finite time kinematics controller, the pose tracking error of the reference pose and the actual pose of the mobile robot is used as input, the finite time Lyapunov method is used for designing the auxiliary kinematics controller, the auxiliary speed is generated, the finite time convergence of the pose tracking error is ensured, and the response speed of the system state is improved. The inner loop is a terminal sliding mode controller based on LESO (linear extended state observer), the error between the auxiliary speed and the actual speed is used as input, real-time estimation and feedforward compensation are carried out on dynamic lumped disturbance by using LESO, the disturbance resistance of the system is improved, the actual movement speed of the mobile robot is enabled to gradually converge to the auxiliary movement speed, and track tracking control of the mobile robot is achieved. The mobile robot terminal can pre-construct the control strategies of an outer ring kinematics controller and an inner ring LESO-based terminal sliding mode controller.
In one embodiment, to configure the outer loop kinematics controller, a mobile robot reference pose is defined as q r ＝[x r y r θ r ] T ∈R 3×1 The subscript r denotes a reference state of the mobile robot, and the reference pose satisfies the following kinematic equation:
wherein z is r ＝[v r ω r ] T For reference velocity, v r Reference linear velocity, omega, for the advance of a mobile robot r Is the reference angular velocity of the mobile robot body around the geometric center.
Further, the pose tracking error of the mobile robot is defined as follows:
wherein e is q ＝[e x e y e θ ] T For pose tracking error, e x 、e y 、e θ The tracking errors of the mobile robot in the x, y and theta directions are respectively.
The pose tracking error differential equation derived from the derivation of equation (12) is:
based on equation (13), the outer loop kinematics controller may design the following finite time aided kinematics control inputs:
wherein z is c ＝[v c ω c ] T To assist speed, v c To assist the linear velocity, ω c To assist angular velocity; k is a radical of x 、k y 、k θ Is the controller gain and is a normal number; beta is a beta x 、β y 、β θ Is the gain of the controller, and 0 < beta x ,β y ,β θ ＜1。
To demonstrate the convergence of the kinematic control law, consider the following candidate Lyapunov function:
V 1 ＝V 11 +V 12 +V 13 (15)
wherein
By deriving formula (16) and substituting formulae (13) and (14)
Based on the finite time stability theory, due to V 1 Is not less than 0 and
the auxiliary kinematics controller asymptotically stabilizes for a finite time.
In some embodiments, the mobile robot acquires a reference pose q r ＝[x r y r θ r ] T ∈R 3×1 And actual pose q = [ x y θ] T ∈R 3×1 Pose tracking error e q Tracking the pose with error e q The above equation (14) is input to obtain the assist velocity z c . As input data for the outer loop kinematics controller, and also a reference velocity z of the mobile robot r 。
The outer ring kinematics controller takes the pose tracking error of the reference pose and the actual pose of the mobile robot and the reference speed of the mobile robot as input, utilizes a finite time Lyapunov method to design an auxiliary kinematics controller, outputs an auxiliary linear speed and an auxiliary angular speed, ensures the pose tracking error to be converged in finite time, and improves the response speed of the state of the double closed-loop control system.
In one embodiment, an inner ring sliding mode controller is designed based on a mobile robot dynamic model, and accurate tracking of the mobile robot to the auxiliary speed is achieved. The inner ring sliding mode controller comprises a terminal sliding mode surface, a controller module, a linear expansion state observer and a dynamic model; the dynamic model is used for obtaining the actual speed of the mobile robot according to the control voltage of the mobile robot; the linear extended state observer is used for obtaining an actual speed estimation value and a total disturbance estimation value of the mobile robot according to the actual speed and the control voltage of the mobile robot; the terminal sliding mode surface is used for designing a sliding surface according to the speed tracking error of the actual speed and the auxiliary speed as input so that the speed tracking error index is converged to zero; and the controller module is used for obtaining a control voltage according to the terminal sliding mode surface and the lumped disturbance estimation value, and the control voltage is used as a dynamic model input to drive the mobile robot to track the auxiliary speed.
The mobile robot terminal may previously construct a Linear Extended State Observer (LESO). Defining a state vector x 1 ＝z,x 2 = d, mobile robot dynamics model equation (10) may be described as state space form:
the linear extended state observer is designed for equation (18):
wherein, the first and the second end of the pipe are connected with each other,
are respectively a state vector x 1 、x 2 An estimated value of (d); beta is a 1 、β 2 For the gain of the extended state observer, the values are:
wherein ω is o The bandwidth of the observer is more than 0;
K 1 ＝Nk t /R a ,K 2 ＝N 2 k t k b /R a r is the radius of the driving wheel of the mobile robot, 2b is the distance between the two driving wheels, m and J are the mass and the moment of inertia of the mobile robot respectively, R a 、k t 、k b Armature resistance, torque constant and back electromotive force constant of the driving motor are respectively, and N is a mechanical gear reduction ratio; u. of a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels.
The mobile robot terminal may also pre-construct a Terminal Sliding Mode Controller (TSMC). Defining velocity tracking error e z ：
e z ＝z-z c (21)
Wherein z and z c Respectively the actual velocity vector of the mobile robot and the auxiliary velocity vector generated by the kinematics controller.
Taking the derivative of equation (21):
to ensure that the state variables converge quickly to the equilibrium point, the following terminal sliding mode surface can be designed:
wherein s = [ s ] 1 s 2 ] T For a mobile robot terminal sliding mode surface vector, s 1 Is a linear velocity terminal sliding mode surface; s 2 Is the angular velocity terminal slip form face, q 1 、p 1 Is positive odd and satisfies q 1 ＜p 1 ＜2q 1 ，λ 1 ＞0，λ 2 More than 0 is a positive definite diagonal matrix to be designed; e.g. of the type z The error vector is tracked for velocity.
The derivation of equation (23) can be:
the united type (10), (22), the formula (24) can be written as
Power approximation law of selected formula (26)
Wherein, the first and the second end of the pipe are connected with each other,
λ 3 more than 0 is positive definite diagonal matrix to be designed, beta is more than 0 s ＜1。
The finite time dynamics control law of the inner-loop sliding-mode controller can be designed as follows (i.e. the controller module):
in order to restrain the buffeting problem in sliding mode control, sign () functions in the controller are replaced by saturation functions sat (), wherein the saturation functions have the following forms:
wherein k is the boundary layer thickness, and the value k =0.1.
Consider the following candidate Lyapunov function:
by taking the derivative of equation (29) and combining equations (25) and (27), the following can be obtained:
wherein l i (i =1,2) is the upper bound of the absolute value of the lumped disturbance observation error of the linear extended state observer. Determining a normal number gamma < min [ lambda ] 31 ,λ 32 The above formula is rewritten as:
when in use
And (2) the method is easy to obtain:
where κ = min { λ 3i -γ}。
Through parameter configuration of the controller, the system sliding mode surface can be converged into a very small neighborhood of the origin within limited time, and the speed tracking error is converged into the very small neighborhood of the origin within limited time by combining the characteristic of the limited time convergence of the terminal sliding mode surface.
In some embodiments, a mobile machineThe human terminal takes the auxiliary speed output by the outer ring kinematics controller and the speed tracking error of the actual speed of the mobile robot as the input of a terminal sliding mode surface, takes the output of the terminal sliding mode surface as the input of an inner ring sliding mode controller module, and obtains a motor control voltage u for controlling the mobile robot according to the formula (27) a . The control voltage and the actual speed of the mobile robot are used as the input of the LESO to obtain the estimated value of the actual speed of the mobile robot
And lumped disturbance estimates
The lumped disturbance estimated value can be used as the input of a controller module of the inner ring sliding mode controller, and the feed-forward compensation of the lumped disturbance is realized.
The mobile robot terminal controls the voltage u according to the control voltage a After the processing of the motor model formula (9), the driving wheel input torque tau = [ tau ] of the mobile robot is obtained r τ l ] T 。
Further, the mobile robot terminal can collect the driving wheel input torque of the mobile robot, and the dynamics unknown input disturbance tau d Interference angular velocity vector xi caused by longitudinal slip of left and right driving wheels of mobile robot l 、ξ r And the side-slip velocity mu of the mobile robot is used as the input of the dynamic model formula (10) to obtain the velocity z of the mobile robot processed by the inner ring sliding mode controller. The speed z can be used as the input of an outer ring kinematics model to obtain the actual pose q of the mobile robot after adjustment. Can also be used as an input to the LESO to derive an actual speed estimate for the inner loop control input
And system lumped disturbance estimate
The inner ring sliding mode controller takes the error between the auxiliary speed and the actual speed as the input of the terminal sliding mode surface, utilizes the LESO to estimate the dynamic lumped disturbance in real time and carry out feedforward compensation in the controller, improves the disturbance resistance of the system, enables the actual speed of the mobile robot to converge to the auxiliary speed gradually, and realizes the track tracking control of the mobile robot.
In one embodiment, as shown in fig. 2, a mobile robot trajectory tracking control method based on dual closed-loop control is provided, a dual closed-loop control system applying the above-mentioned embodiments is used for executing control of a mobile robot, and the application of the control system on a mobile robot terminal is described for executing trajectory tracking of the mobile robot. The method comprises the following steps:
and step S210, acquiring pose tracking errors of the reference pose and the actual pose of the mobile robot.
Wherein, the reference pose q r The target pose pre-configured by the mobile robot is a convergence target of the actual pose of the mobile robot when the trajectory tracking of the mobile robot is executed.
The actual pose q refers to the pose of the mobile robot at a certain time point, and the actual pose dynamically changes in the process of performing track tracking control. The mobile robot terminal can detect the actual pose in real time and can also acquire the actual pose at certain intervals.
The reference pose and the actual pose can be defined by the position of the geometric center of the mobile robot in the global coordinate system and the heading angle of the mobile robot. For example, the reference pose may be represented as q r ＝[x r y r θ r ] T ∈R 3×1 The actual pose can be expressed as q = [ x y θ = [ x y ]] T ∈R 3×1 。
In specific implementation, when the track control of the mobile robot is executed, the mobile robot terminal obtains the pose tracking error according to the position relation between the reference pose and the actual pose of the mobile robot.
In some cases, the mobile robot terminal may determine the pose tracking error from the reference pose, the actual pose, and the robot kinematic equation, in conjunction with equation (12).
And step S220, inputting the pose tracking error into an outer ring kinematics controller to obtain an auxiliary speed.
The outer ring kinematics controller refers to a finite time kinematics controller, namely the control relationship determined by the formula (14) can realize the finite time convergence of the pose tracking error based on a finite time auxiliary kinematics control law.
Wherein the auxiliary speed z c The method can be used as the input of an inner ring sliding mode controller and used for executing an inner ring sliding mode control process, so that the actual speed of the mobile robot is converged to the auxiliary speed. The assist speed includes an assist linear speed and an assist angular speed.
In step S230, the velocity tracking error of the auxiliary velocity and the actual velocity is acquired.
In specific implementation, the mobile robot terminal can acquire the actual speed and obtain the speed tracking error of the auxiliary speed and the actual speed.
For example, the mobile robot terminal may detect the actual speed of the mobile robot in real time, or may collect the actual speed z of the mobile robot at a certain time interval, and collect the auxiliary speed at a corresponding time, so as to determine the speed tracking error.
And S240, taking the speed tracking error as the input of the inner ring sliding mode controller, obtaining the control voltage of the driving motor of the driving wheel of the mobile robot through the inner ring sliding mode controller, and further driving the mobile robot to enable the actual speed of the mobile robot to gradually converge to the auxiliary speed.
During specific implementation, the mobile robot terminal can take a speed tracking error as an input of the inner ring sliding mode controller, obtain an actual speed after the processing of the terminal sliding mode surface, the controller module, the motor module and the dynamic model, and obtain an actual speed estimation value and a total disturbance estimation value by processing the actual speed through the linear extended state observer. The lumped disturbance estimated value can be used as the input of the controller module to realize the feedforward compensation of the lumped disturbance.
According to the method, a double closed-loop control strategy of an outer-loop kinematics controller and an inner-loop sliding-mode controller is constructed, the pose tracking error of the reference pose and the actual pose of the mobile robot is obtained, the pose tracking error is input into the outer-loop kinematics controller to obtain the auxiliary speed, the limited-time convergence of the pose tracking error is realized, the response speed of the system state is improved, the speed tracking error obtained according to the auxiliary speed and the actual speed is used as the input of the inner-loop sliding-mode controller, and the mobile robot is driven by the inner-loop sliding-mode controller to enable the actual speed to gradually converge to the auxiliary speed. By executing the double closed-loop control strategy, the finite-time track tracking control of the mobile robot is realized, and the control precision of the mobile robot under the condition that the wheels slip is unknown is improved.
In one embodiment, the mobile robot terminal may pre-configure a reference pose and a reference velocity of the mobile robot so that when the mobile robot is started, input parameters are provided to implement the double closed loop control according to the detected actual pose and actual velocity of the mobile robot.
In one embodiment, the actual speed obtained by the inner ring sliding mode control can be processed by a kinematics model to obtain a new actual pose, the actual pose and a reference pose are processed, and the obtained pose tracking error can be used for executing outer ring control, so that double closed-loop control is realized, the pose tracking error of the outer ring is limited in time and the pose tracking error of the inner ring is limited in time, and the speed of the inner ring is converged, so that the track tracking control of the mobile robot is finally realized.
In order to verify the effectiveness of the control method, a trajectory tracking control simulation test of the sweeping robot is carried out under the conditions of unknown longitudinal and lateral sliding disturbance, unknown bounded conductible input interference and uncertain model parameters.
Taking a laser radar navigation sweeping robot product as an example, the physical parameters and the related parameters of the driving motor are as follows:
m＝2.7kg,J＝0.033kg·m 2 ,r＝0.034m,b＝0.11m,
R a ＝13.5Ω,k b ＝0.0239V/rad·s -1 ,k t ＝0.0239(N·m)/A,N＝54.5
assuming that the reference track of the sweeping robot is a circular track, the simulation is set as follows: simulation time of 30s, sampling frequency of 100Hz and reference pose q of sweeping robot r (0)＝[0 0 0] T Initial pose q (0) = [ 0.1.1 pi/10 =] T Reference velocity v r ＝0.28m/s,ω r =1rad/s, initial robot velocity v =0m/s, ω =0rad/s.
For further study on the robustness of the proposed controller, it is assumed that the unknown input perturbation of the dynamics of the sweeping robot follows a normal distribution function:
in order to test the control effect of the designed controller on the disturbance containing different frequencies and amplitude information, the longitudinal slip and sideslip disturbance is given in an attenuation sine form. Left and right driving wheel longitudinal slip angular velocity disturbance
The disturbance of the sideslip linear speed of the sweeping robot is defined as follows:
wherein, t s Is the disturbance start time.
In order to verify the robustness of the designed control algorithm, the designed control method is respectively subjected to simulation comparison with an algorithm (FTCTSMC) combining traditional Double-closed-loop PID (digital-closed-loop PID) control, outer ring Finite time Lyapunov tracking control and inner ring terminal sliding mode control. The adopted terminal sliding mode surface and auxiliary speed control law of the FTCTSMC are the same as those of the FTCTSMC + LESO method provided by the invention, and the dynamic control law of the FTCTSMC is designed as follows:
the outer loop controller parameters of the DCLPID method are set to:
the inner loop controller parameter is
k p ＝diag([40,10]),k i ＝diag([1,1]),k d ＝diag([8,8])。
The system auxiliary speed control law parameters in the FTCTSMC + LESO algorithm designed by the invention are set as follows: k is a radical of formula x ＝6,k y ＝5,k θ ＝2,β x ＝2/3,β y ＝1/3,β θ =2/3, and the linear extended state observer parameters are set to ω o =100; the dynamics controller parameter is set to q 1 ＝3,p 1 ＝5,λ 1 ＝diag(20,20),λ 2 ＝diag(15,15),λ 3 ＝diag(1,1),β s =1/8, the FTCTSMC controller parameters are the same as the FTCTSMC + LESO controller parameters described above.
When the sweeping robot is in t = 5-8 s, the load changes slowly, and the variation quantity delta M s ＝0.1M s Longitudinal slip occurs in 10-13 s, sideslip occurs in 15-18 s, and lumped disturbance is added in 20-23 s.
As can be seen from fig. 3, 4, and 5, when the actual initial pose of the sweeping robot is different from the reference pose, the DCLPID has a large steady-state error and a long convergence time, and the finite-time trajectory tracking controller of the present invention can effectively shorten the convergence time and reduce the steady-state error. Under the action of lumped disturbance, the FTCTSMC + LESO method has stronger disturbance resistance and smaller pose tracking error fluctuation.
As shown in fig. 6, for the actual linear velocity and actual angular velocity change conditions of the sweeping robot under three control algorithms, the speed change will be more gradual at the initial time based on the control law designed by the finite time method, the initial speed change of the DCLPID algorithm is more severe, and the motor performance may be damaged in practice. When disturbance exists, the disturbance is accurately estimated by the FTCTSMC + LESO method, and feedforward compensation is carried out in the terminal sliding mode controller, so that the movement of the sweeping robot is smoother.
As shown in fig. 7, the LESO of the FTCTSMC + LESO method of the present invention can make real-time, accurate estimation of lumped disturbances.
As shown in fig. 8, a schematic diagram of the control voltage variation with time under the three control schemes is shown, from which it can be known that the FTCTSMC + LESO method designs a more gradual control voltage variation at the initial start.
The technical features of the embodiments described above can be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered to fall within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the present invention. It should be noted that, for a person skilled in the art, many variations and modifications can be made without departing from the spirit of the invention, which falls within the scope of the invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.
Claims (10)
1. A mobile robot track tracking control method based on double closed-loop control is characterized by comprising the following steps:
acquiring pose tracking errors of a reference pose and an actual pose of the mobile robot;
inputting the pose tracking error into an outer ring kinematics controller, and obtaining an auxiliary speed according to the output of the outer ring kinematics controller;
acquiring speed tracking errors of the auxiliary speed and the actual speed;
and taking the speed tracking error as the input of an inner ring sliding mode controller, obtaining the control voltage of a driving motor of a driving wheel of the mobile robot through the inner ring sliding mode controller, and further driving the mobile robot to enable the actual speed of the mobile robot to gradually converge to the auxiliary speed.
2. The method of claim 1, wherein the acquiring pose tracking errors for the reference pose and the actual pose of the mobile robot comprises:
determining a kinematic equation of the mobile robot;
and obtaining the pose tracking error of the mobile robot according to the reference pose, the actual pose and the kinematic equation of the mobile robot.
3. The method according to claim 2, characterized in that the pose tracking error e q Calculated by the following algorithm:
wherein e is q ＝[e x e y e θ ] T For pose tracking error, e x 、e y 、e θ Tracking errors of the mobile robot in the directions of x, y and theta are respectively; (x, y) represents the position of the geometric center of the mobile robot in the global coordinate system, and theta is the course angle of the mobile robot; the subscript r denotes a reference state of the mobile robot.
4. The method of claim 1, wherein the outer loop kinematics controller is a finite time kinematics controller; the finite time kinematic controller is realized by the following formula:
wherein z is c ＝[v c ω c ] T As an auxiliary velocity vector, v c To assist the linear velocity, ω c To assist angular velocity; k is a radical of x 、k y 、k θ Is the controller gain and is a normal number; beta is a beta x 、β y 、β θ Is the controller gain, and 0 < beta x ,β y ,β θ ＜1；e x 、e y 、e θ Tracking errors of the mobile robot in the directions of x, y and theta are respectively; v. of r Reference linear velocity, ω, for moving the robot forward r Is the reference angular velocity of the mobile robot body around the geometric center.
5. The method of claim 4, wherein the inner ring sliding-mode controller comprises a terminal sliding-mode face, a controller module, a linear extended state observer, and a dynamic model; wherein, the first and the second end of the pipe are connected with each other,
the dynamic model is used for obtaining the actual speed of the mobile robot according to the control voltage of the driving motor of the driving wheel of the mobile robot;
the linear extended state observer is used for obtaining an actual speed estimation value and a total disturbance estimation value of the mobile robot according to the actual speed and the control voltage of a driving motor of a driving wheel of the mobile robot;
the terminal sliding mode surface is used for designing a sliding surface according to the speed tracking error of the actual speed and the auxiliary speed as input, so that the speed tracking error index is converged to zero;
and the controller module is used for obtaining the control voltage of a driving wheel driving motor of the mobile robot according to the terminal sliding mode surface and the lumped disturbance estimation value, and the control voltage is used as the input of a dynamic model of the mobile robot to control the actual speed of the mobile robot to gradually converge to the auxiliary speed.
6. The method of claim 5, wherein the dynamical model is derived from a kinematic model and a drive motor model of the mobile robot; wherein the content of the first and second substances,
the kinematic model of the mobile robot is realized by the following formula:
wherein，q＝[x y θ] T Is the pose of the mobile robot; z = [ v ω ]] T The vector is a velocity vector of the mobile robot, and v and omega are respectively a linear velocity and an angular velocity of the mobile robot; eta = [ eta ] v η ω ] T ，η v ＝r(ξ r +ξ l ) [ eta ] is the longitudinal slip speed ω ＝r(ξ r -ξ l ) V (2 b) yaw rate disturbance caused by longitudinal sliding, r is the radius of the driving wheels of the mobile robot, 2b is the distance between the two driving wheels, xi l 、ξ r Respectively representing interference angular velocity vectors caused by the longitudinal sliding of the left driving wheel and the right driving wheel of the mobile robot;
the vector is a non-matching disturbance vector caused by sideslip of the mobile robot, and mu is the sideslip speed of the mobile robot; s (q) is a transform matrix having the form:
the driving motor model is realized by the following formula:
wherein L is a 、R a 、k t 、k b The armature inductance, the armature resistance, the torque constant and the back electromotive force constant of the driving motor respectively, N is the mechanical gear reduction ratio, and tau = [ tau = [ r τ l ] T Representing input torque, u, of the mobile robot a ＝[u ar u al ] T ,I a ＝[I ar I al ] T Control voltage and armature current, omega, of the drive motor for the right and left drive wheels, respectively w ＝[ω R ω L ] T The right and left driving wheels drive the angular speed of the motor;
and under the condition that the mobile robot takes the driving voltage of the direct current motor as a control input, the dynamic model is expressed as follows:
wherein
K 1 ＝Nk t /R a ，K 2 ＝N 2 k t k b /R a R is the radius of the driving wheel of the mobile robot, 2b is the distance between the two driving wheels, m and J are the mass and the moment of inertia of the mobile robot respectively, R a 、k t 、k b The armature resistance, the torque constant and the back electromotive force constant of the driving motor respectively, N is the mechanical gear reduction ratio, u a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels; d is a lumped disturbance including unknown longitudinal and lateral slip disturbances of the system, uncertain model parameters and unknown input disturbances.
7. The method of claim 5, wherein the linear extended state observer is implemented by the following equation:
wherein, the first and the second end of the pipe are connected with each other,
are respectively a state vector x 1 、x 2 Estimate of beta, beta 1 、β 2 For the gain of the extended state observer, the values are:
ω o the bandwidth of the observer is more than 0;
K 1 ＝Nk t /R a ,K 2 ＝N 2 k t k b /R a r is the radius of the driving wheel of the mobile robot, 2b is the distance between the two driving wheels, m and J are the mass and the moment of inertia of the mobile robot respectively, R a 、k t 、k b The armature resistance, the torque constant and the back electromotive force constant of the driving motor are respectively, and N is a mechanical gear reduction ratio; u. of a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels.
8. The method of claim 5, wherein the terminal sliding surface is implemented by the formula:
wherein e is z For velocity tracking error, z = [ v ω ]] T For the velocity vector of the mobile robot, v and ω are the linear velocity and angular velocity, respectively, of the mobile robot, z c Is the auxiliary velocity vector of claim 4; s is a terminal sliding mode surface vector; q. q.s 1 、p 1 Is positive odd and satisfies q 1 ＜p 1 ＜2q 1 ，λ 1 、λ 2 A positive definite diagonal matrix is to be designed.
9. The method of claim 5, wherein the finite time dynamics control law of the controller module is implemented by the following equation:
wherein u is a ＝[u ar u al ] T The control voltage of the driving motor of the right and left driving wheels;
K 1 ＝Nk t /R a ,K 2 ＝N 2 k t k b /R a r is the radius of the driving wheel of the mobile robot, 2b is the distance between the two driving wheels, m and J are the mass and the moment of inertia of the mobile robot respectively, R a 、k t 、k b The armature resistance, the torque constant and the back electromotive force constant of the driving motor are respectively, and N is a mechanical gear reduction ratio;
is a state vector x 2 An estimated value of (d); q. q of 1 、p 1 Is positive odd and satisfies q 1 ＜p 1 ＜2q 1 ；e z Is the velocity tracking error; z is a radical of c Is the auxiliary speed of claim 4;
is the derivative of the auxiliary speed; z = [ v ω ]] T V and ω are the linear velocity and angular velocity of the mobile robot, respectively; lambda 1 、λ 2 、λ 3 Determining a diagonal matrix for the positive to be designed; s is a terminal sliding mode surface vector; beta is a s For the gain of the controller, satisfy 0 < beta s ＜1；
Is a saturation function, where k is the boundary layer thickness, and takes the value k =0.1.
10. The method of claim 1, further comprising:
taking the actual speed as the input of a kinematic model of the mobile robot to obtain the actual pose of the mobile robot;
and acquiring a pose tracking error of the reference pose and the actual pose of the mobile robot, and acquiring the auxiliary speed of the mobile robot according to a kinematics controller according to the pose tracking error.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211261623.5A CN115718421A (en) | 2022-10-14 | 2022-10-14 | Mobile robot track tracking control method based on double closed-loop control |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211261623.5A CN115718421A (en) | 2022-10-14 | 2022-10-14 | Mobile robot track tracking control method based on double closed-loop control |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115718421A true CN115718421A (en) | 2023-02-28 |
Family
ID=85254247
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211261623.5A Pending CN115718421A (en) | 2022-10-14 | 2022-10-14 | Mobile robot track tracking control method based on double closed-loop control |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115718421A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116540723A (en) * | 2023-05-30 | 2023-08-04 | 南通大学 | Underwater robot sliding mode track tracking control method based on artificial potential field |
-
2022
- 2022-10-14 CN CN202211261623.5A patent/CN115718421A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116540723A (en) * | 2023-05-30 | 2023-08-04 | 南通大学 | Underwater robot sliding mode track tracking control method based on artificial potential field |
| CN116540723B (en) * | 2023-05-30 | 2024-04-12 | 南通大学 | Underwater robot sliding mode track tracking control method based on artificial potential field |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108008720B (en) | Fuzzy sliding mode trajectory tracking control and method for wheeled mobile robot | |
| Boukattaya et al. | Adaptive nonsingular fast terminal sliding-mode control for the tracking problem of uncertain dynamical systems | |
| Zhang et al. | Learning deep control policies for autonomous aerial vehicles with mpc-guided policy search | |
| CN106483964B (en) | Robot compliance control method based on contact force observer | |
| Subudhi et al. | Sliding-mode-observer-based adaptive slip ratio control for electric and hybrid vehicles | |
| Fang et al. | Trajectory tracking control of farm vehicles in presence of sliding | |
| Garimella et al. | Neural network modeling for steering control of an autonomous vehicle | |
| Ryu et al. | Differential flatness-based robust control of mobile robots in the presence of slip | |
| Wen et al. | Elman fuzzy adaptive control for obstacle avoidance of mobile robots using hybrid force/position incorporation | |
| Yoo et al. | Adaptive formation tracking control of electrically driven multiple mobile robots | |
| Park et al. | Adaptive leader-follower formation control of mobile robots with unknown skidding and slipping effects | |
| CN112558468B (en) | Launching platform adaptive robust output feedback control method based on double observers | |
| CN115718421A (en) | Mobile robot track tracking control method based on double closed-loop control | |
| Kim et al. | Path tracking for a skid-steer vehicle using model predictive control with on-line sparse gaussian process | |
| CN110109363B (en) | Neural network self-adaptive control method for wheeled mobile robot formation | |
| Płaskonka | The path following control of a unicycle based on the chained form of a kinematic model derived with respect to the Serret-Frenet frame | |
| Hill et al. | Online gain setting method for path tracking using CMA-ES: Application to off-road mobile robot control | |
| Cheng et al. | A game theoretical chassis domain approach to trajectory tracking for automated vehicles | |
| Tao et al. | Control of a flexible air-breathing hypersonic vehicle with measurement noises using adaptive interval type-2 fuzzy logic system | |
| CN112147894B (en) | Wheel type mobile robot active control method based on kinematics and dynamics model | |
| CN112051736B (en) | Trajectory tracking control system based on single controller | |
| Li et al. | Neural adaptive tracking control for wheeled mobile robots | |
| Miah et al. | Universal dynamic tracking control law for mobile robot trajectory tracking | |
| CN116736748A (en) | Method for constructing controller of robot and robot | |
| CN114296466A (en) | Trajectory tracking control method and device and mobile robot |
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 |