Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a moving robot track tracking method based on a backstepping sliding mode, which comprises the steps of firstly carrying out kinematics and dynamics modeling on a moving robot, then realizing track tracking of the moving robot by adopting the backstepping method, then designing a speed tracking control law by utilizing a sliding mode variable structure method based on a dynamics model of the moving robot, and finally realizing accurate track tracking of the moving robot according to the designed control law. The method can effectively weaken the buffeting phenomenon of the control system, improve the motion performance of the mobile robot when the expected path has an inflection point, and improve the anti-interference capability of the mobile robot and the motion precision of track tracking.
In order to achieve the above purpose, the invention adopts the following technical scheme:
a mobile robot track tracking method based on a backstepping sliding mode comprises the following steps:
Step 1, establishing a kinematic model and a dynamic model of a mobile robot;
Step 2, designing a kinematic controller of the mobile robot based on a backstepping method;
Step 3, designing a dynamic controller of the mobile robot based on a sliding mode variable structure of a combined approach law;
And 4, according to the kinematic model of the robot, obtaining the actual pose and the actual speed of the mobile robot, inputting the expected pose and the actual pose of the mobile robot into a backstepping kinematic controller to obtain a virtual control variable, inputting the virtual control variable and the actual speed into a dynamics controller to obtain a control moment, inputting the control moment and external disturbance into the dynamics model, and outputting acceleration so that the actual pose of the mobile robot reaches the expected pose.
The technical scheme is further optimized, and the kinematic model of the mobile robot is as follows:
the actual pose of the mobile robot at any moment is as follows:
x, y and in formula (1) Respectively representing the pose of the transverse direction, the longitudinal direction and the course angle of the mobile robot. The actual speed of the mobile robot at any moment is:
v=[v,ω]T (26)
v in the formula (2) is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot.
When the contact surface of the wheels and the ground has no sliding or sideslip phenomenon, the motion model of the mobile robot can be expressed as follows:
in formula (3), a (q) is:
equation (4) is a jacobian matrix representing the relative relationship between pose and velocity.
According to the technical scheme, the dynamic model of the mobile robot is further optimized:
the mass m and moment of inertia I of the mobile robot, the kinetic model can be expressed as follows:
in the formula (5) of the present invention, For the acceleration vector, τ and τ d are input torque and disturbance, respectively, and M is:
according to further optimization of the technical scheme, the kinematic controller of the mobile robot is designed as follows:
The goal of the kinematic controller is to move the actual pose of the robot when the time t tends to infinity under the action of the control law v c Gradually converging to the expected pose, the expected speed v r=[vr,ωr]T, and the expected poseWherein v r and ω r are the desired linear and angular speeds, x r,yr and ω r, respectively, of the mobile robotThe expected position and the expected position of the heading angle of the mobile robot are respectively;
the expected speed of the mobile robot in the global coordinate system is therefore:
The pose error is defined as:
wherein e x,ey and The pose error is respectively the error components along the X direction, the Y direction and the course angle direction, so that the pose error vector can be obtained as follows:
obtaining a pose error differential equation from the formula (8) and the formula (9):
Wherein the method comprises the steps of AndThe differential forms of pose errors in the transverse direction, the longitudinal direction and the course angle direction are respectively adopted, and a Liapunov function is constructed based on a backstepping method design idea in order to keep the system stable:
deriving the formula (11) and substituting the formula (10) into the formula to obtain:
to sum up, to ensure Designing a back-stepping method track tracking controller based on kinematics:
where v c and ω c are output virtual control amounts, and k 1,k2,k3 are both positive real numbers.
The technical scheme is further optimized, and the dynamics controller is as follows:
the speed tracking error of the mobile robot is defined as:
considering that the kinetic model of a wheeled mobile robot is a first order nonlinear function, here a proportional-integral slip-plane is chosen:
Where μ 1 and μ 2 are integral parameters of the slip plane and are both greater than 0, this can improve the robustness of the system when the system state moves to the slip plane at s=0:
Where μ=diag (μ 1,μ2) >0, so selecting an appropriate μ can lead to e v towards 0 when time goes to infinity, deriving equation (15), and combining equation (5) to get the following:
let equation (17) be 0, the resulting equivalent control law be:
the vector is therefore expressed as:
However, the actual mobile robot system generally has external interference, so that the switching control law τ sw is introduced to convert the formula (19) into the following form:
Where β=diag (β 1,β2)>0,sgn(s)=[sgn(s1),sgn(s2) ].
(3) In the early stage of sliding mode movement, in order to ensure that the system state quickly approaches the sliding mode surface to weaken buffeting, the following exponential approach law is selected:
The control law is as follows:
Where η=diag (η 1,η2)>0,q=diag(q1,q2) >0. The eta, q and epsilon are all adjustable parameters of an approach law, and the quality of an arrival process can be ensured and the buffeting phenomenon in a variable structure system can be weakened by reasonably adjusting the parameters;
(4) In the later and stable stages of the sliding mode movement, in order to weaken the buffeting phenomenon in the stable state, the following variable speed approach law is selected:
The control law is as follows:
Wherein the method comprises the steps of ε=diag(ε1,ε2)>0,vs(S,ε)=[ε1|ev|sgn(s1),-ε2|eω|sgn(s2)]T.
Compared with the prior art, the beneficial effects of the technical scheme are as follows:
The invention can realize the motion control of the mobile robot, effectively weaken buffeting phenomenon existing in the system, improve the track tracking and anti-interference capabilities of the mobile robot under the condition that the expected track has inflection points, and improve the motion precision of the track tracking of the mobile robot.
Detailed Description
In order to describe the technical content, constructional features, achieved objects and effects of the technical solution in detail, the following description is made in connection with the specific embodiments in conjunction with the accompanying drawings.
The invention discloses a mobile robot track tracking control method based on a back-stepping sliding mode method, wherein the mobile robot is a typical nonlinear system, has strong coupling, time-varying and nonlinear dynamics characteristics, and usually inevitably has uncertain factors such as parameter errors, unmodeled dynamics, observation noise, uncertain external interference and the like, so that a kinematic controller and a dynamics controller of the mobile robot are respectively designed based on the mobile robot, the problems are effectively solved, and the motion precision of track tracking of the mobile robot is improved.
The wheel type mobile robot is driven in a differential mode, and is simple in structure and flexible in steering. The invention aims at a differential-drive mobile robot, and solves the technical problems that when an inflection point exists in a motion path or an initial error of the motion path of the mobile robot is larger, the final track tracking effect of the mobile robot is poor due to the generated speed jump, and in addition, as a wheel type mobile robot system is a typical nonlinear system, the dynamic characteristics of strong coupling, time variation and nonlinearity are provided, uncertain factors such as parameter errors, unmodeled dynamics, observation noise, uncertain external interference and the like are inevitably present, and the performance of track tracking of the mobile robot is adversely affected. Thus, in the above state, how to control the motion state of the mobile robot so that it better tracks a predetermined time-varying path, thereby stably traveling according to the expected state.
A mobile robot track tracking control method based on a backstepping sliding mode method comprises the following steps:
Step 1, performing kinematic and dynamic modeling on a mobile robot;
referring to fig. 1, a simplified model diagram of a mobile robot is shown. The global coordinate system XOY is a Cartesian inertial coordinate system, allows the mobile robot to move in the Cartesian inertial coordinate system, is used for representing environmental information, and is characterized in that the center of mass of the mobile robot and the center of the axis of the driving wheel are coincided with a point P, the mass is m, the moment of inertia is I, the forward linear speed of the mass movement of the mobile robot is v, the rotation angle speed of the mass movement is omega, and the included angle between the movement direction of the mobile robot and the X-axis direction is formed Is a heading angle, and thus the centroid P of the mobile robot is physically analyzed.
Step 1.1, establishing a kinematic model of the mobile robot
The actual pose of the mobile robot at any moment is as follows:
x, y and in formula (1) Respectively representing the pose of the transverse direction, the longitudinal direction and the course angle of the mobile robot. The actual speed of the mobile robot at any moment is:
V=[v,ω]T (50)
v in the formula (2) is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot.
When the contact surface of the wheels and the ground has no sliding or sideslip phenomenon, the motion model of the mobile robot can be expressed as follows:
in formula (3), a (q) is:
equation (4) is a jacobian matrix, representing the relative relationship between the actual pose and the actual velocity.
Step 1.2, establishing a dynamic model of the mobile robot
Considering the mass m and moment of inertia I of the mobile robot, the kinetic model can be expressed as follows:
in the formula (5) of the present invention, For the acceleration vector, τ and τ d are control moment and disturbance, respectively, and the mass and moment of inertia matrix M of the mobile robot is:
Step 2, designing a mobile robot kinematics controller based on a backstepping method
Referring to fig. 2, in the motion process of the mobile robot, the reference speed is used as input, the actual motion pose error of the mobile robot is obtained through a kinematic model, namely a formula (3), a feedback controller is obtained through recursively constructing a Lyapunov function of a closed-loop system by a backstepping method, a control law is selected to enable a derivative of the Lyapunov function along a track of the closed-loop system to have certain performance, the bounded nature and convergence of the track of the closed-loop system to be guaranteed, the selected control law is a solution of the track tracking problem, the method can decompose a complex nonlinear system into subsystems which do not exceed the system order, then a part of the Lyapunov function and an intermediate virtual control quantity are designed for each subsystem, the whole system is always 'backed-off', and the Lyapunov function and the intermediate virtual control quantity are integrated to complete the design of the whole control law, and the basic design method is that the design of a kernel of a high-order system is started. The method comprises the steps of designing a virtual control law to ensure certain performance of a kernel system, such as stability, and the like, then gradually correcting an algorithm for the obtained virtual control law, but ensuring the set performance, and further designing a real calm controller to realize global regulation or tracking of the system so as to enable the system to reach expected performance indexes. The back-step control method is suitable for an uncertain nonlinear system which can be linearized in a state or has strict parameter feedback, and has obvious superiority in the aspect of designing a robust or self-adaptive controller of the uncertain system, especially when interference or uncertainty does not meet a matching condition. The kinematic controller is therefore designed for the mobile robot based on this method.
The goal of the kinematic controller is to move the actual pose of the robot when the time t tends to infinity under the action of the control law v c Gradually converging to the expected pose, the expected speed v r=[vr,ωr]T, and the expected poseWherein v r and ω r are the desired linear and angular speeds, x r,yr and ω r, respectively, of the mobile robotThe position and the orientation of the mobile robot are respectively a horizontal expected position and a longitudinal expected position and an orientation angle expected position of the mobile robot.
The expected speed of the mobile robot in the global coordinate system is therefore:
The pose error is defined as:
wherein e x,ey and The pose error is respectively the error components along the X direction, the Y direction and the course angle direction, so that the pose error vector can be obtained as follows:
obtaining a pose error differential equation from the formula (8) and the formula (9):
Wherein the method comprises the steps of AndThe differential forms of pose errors in the transverse direction, the longitudinal direction and the course angle direction are respectively adopted, and a Liapunov function is constructed based on a backstepping method design idea in order to keep the system stable:
deriving the formula (11) and substituting the formula (10) into the formula to obtain:
to sum up, to ensure Designing a back-stepping method track tracking controller based on kinematics:
Where v c and ω c are output virtual control amounts, and k 1,k2,k3 are both positive real numbers. Thereby ensuring that the actual pose q also converges to the expected pose q r when the time t approaches infinity under the action of the control law v c. It can be seen that v c is suddenly changed when the expected track has an inflection point or the initial moment tracking error is large, and a moment with a large value is required for the mobile robot, but the moment is difficult to realize in the control of the actual mobile robot. Therefore, in the actual track tracking control process, the speed tracking v-v r is the premise of q-q r, and only the kinematic controller is designed, so that the high-quality speed tracking cannot be ensured, and the dynamic controller is required to be designed.
And 3, a sliding mode variable structure control method based on a combined approach law.
The sliding mode variable structure control method is essentially a special nonlinear control, the nonlinearity of which is represented by control discontinuity, and the control method is different from other controls in that the structure of the system is not fixed, but the system can be purposefully and continuously changed in a dynamic process according to the current state of the system, such as deviation and various derivatives thereof, so that the system moves according to the state track of a preset sliding mode. The sliding mode control has the advantages of overcoming the uncertainty of the system, having strong robustness to disturbance and unmodeled dynamics, and particularly having good control effect on the control of a nonlinear system. The variable structure control system has simple algorithm, high response speed and robustness to external noise interference and parameter perturbation, so the variable structure control system is widely applied to related researches of mobile robots. However, the disadvantage of this method is that when the state trace reaches the sliding surface, it is difficult to slide along the sliding surface strictly toward the balance point, but the state trace passes back and forth on both sides of the sliding surface, so that chatter, i.e., a buffeting problem, is generated. Therefore, the sliding mode variable structure control does not need to establish an accurate model of a controlled object, has insensitivity to external bounded interference and parameter change, and is gradually applied to motion control of a mobile robot in recent years. However, the sliding mode variable structure control inevitably generates buffeting, and unstable system is caused when serious, so that the track tracking is failed. The invention provides a sliding mode control method based on a combined approach law based on a sliding mode variable structure, which can effectively weaken the buffeting phenomenon of a control system and improve the stability and track tracking performance of the control system. As shown in fig. 2, the sliding mode control uses the virtual output variable v c of the back-step control method in step 2 and the difference value of the actual speed v of the mobile robot as input, so as to make v converge to v c, and make the system reach the stability of speed tracking, thereby ensuring the accuracy of system track tracking. It is therefore necessary to design a dynamic controller based on a sliding mode variable structure.
First, the speed tracking error of the mobile robot is defined as:
considering that the kinetic model of a wheeled mobile robot is a first order nonlinear function, here a proportional-integral slip-plane is chosen:
Where μ 1 and μ 2 are integral parameters of the slip plane and are both greater than 0, this can improve the robustness of the system when the system state moves to the slip plane at s=0:
Where μ=diag (μ 1,μ2) >0, so selecting an appropriate μ can trend 0 when time goes to infinity, e v. Deriving the formula (15) and combining the formula (5) to obtain the following:
let equation (17) be 0, the resulting equivalent control law be:
the vector is therefore expressed as:
However, the actual mobile robot system generally has external disturbance in order to weaken the influence of the disturbance in the system, so that the switching control law τ sw is introduced to convert the formula (19) into the following form:
Wherein the adjustable parameter of the system β=diag (β 1,β2)>0,sgn(s)=[sgn(s1),sgn(s2) ].
However, the discontinuous term sgn(s) in the control law τ may cause the system to toggle back and forth between different control logics, thereby causing the system to buffeting. Therefore, the combined approach law is introduced into the tracking problem of the mobile robot, the buffeting problem in the sliding mode movement is weakened by replacing the sign function sgn(s), the rapidity and the stability of the tracking process are ensured, namely, the system state can be ensured to quickly approach the sliding mode surface and weaken buffeting by adopting the exponential approach law in the early stage of the sliding mode movement, and the buffeting in the steady state is effectively reduced by adopting the variable speed approach law in the later stage and the steady stage of the sliding mode movement, so that the good steady state performance is ensured, and the specific implementation process comprises the following two stages:
(5) In the early stage of sliding mode movement, in order to ensure that the system state quickly approaches the sliding mode surface to weaken buffeting, the following exponential approach law is selected:
The control law is as follows:
Where η=diag (η 1,η2)>0,q=diag(q1,q2) >0. The eta, q and epsilon are all adjustable parameters of the approach law, and the quality of the arrival process can be ensured and the buffeting phenomenon in the variable structure system can be weakened by reasonably adjusting the parameters.
(6) In the later and stable stages of the sliding mode movement, in order to weaken the buffeting phenomenon in the stable state, the following variable speed approach law is selected:
The control law is as follows:
Wherein the method comprises the steps of ε=diag(ε1,ε2)>0,vs(S,ε)=[ε1|ev|sgn(s1),-ε2|eω|sgn(s2)]T.
Stability analysis was then performed, using the lyapunov function to demonstrate the effectiveness of the proposed kinetic controller:
Differentiating the formula (25) and combining the formula (21) can obtain:
deriving equation (25) and combining equation (23) yields:
is available in the form of The dynamics control system is thus stable, ensuring a fast and stable convergence of the actual speed v to the virtual control speed v c.
Step 2 and step 3 finish the design of the kinematic and dynamic controllers of the mobile robot, are suitable for a typical complex system with strong coupling, time varying and nonlinearity such as a wheeled mobile robot system, and effectively reduce the buffeting problem in track tracking motion.
And 4, realizing accurate track tracking of the mobile robot by combining the designed control system according to the method. The final control system block diagram is shown in fig. 2.
According to this control method, the mobile robot trajectory tracking effect is shown in fig. 3.
According to the final algorithm detection result, the method for tracking the track of the mobile robot provided by the invention can realize accurate track tracking of the mobile robot with uncertain factors under the condition that the track has inflection points, has a good tracking effect, effectively weakens the buffeting phenomenon of a system, ensures the stability and rapidity of track tracking, and realizes the motion control of the mobile robot.
It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or terminal that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or terminal. Without further limitation, elements defined by the phrases "including" or "comprising" do not exclude the presence of additional elements in a process, method, article, or terminal device that includes the elements. In addition, herein, "greater than", "less than", "exceeding" and the like are understood to exclude the present number, and "above", "below", "within" and the like are understood to include the present number.
While the embodiments have been described above, other variations and modifications will occur to those skilled in the art once the basic inventive concepts are known, and it is therefore intended that the foregoing description and drawings illustrate only embodiments of the invention and not limit the scope of the invention, and it is therefore intended that the invention not be limited to the specific embodiments described, but that the invention may be practiced with their equivalent structures or with their equivalent processes or with their use directly or indirectly in other related fields.