Trajectory tracking control method for mobile robot in sliding and sideslip states

Abstract

The invention discloses a trajectory tracking control method for a mobile robot in sliding and sideslip states, which comprises the following steps of: firstly, constructing a model related to sliding and sideslip conditions, then establishing a matched kinematics controller by using a backstepping method, and determining a virtual speed related to calculation; and then designing a dynamic controller based on a nonlinear extended state observer and an integral sliding mode control idea, and analyzing the stability of the whole closed-loop system. The control method provided by the invention can effectively improve the tracking precision and robustness.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory tracking control, and in particular to a trajectory tracking control method for a mobile robot in a sliding and side-slipping state. Background Art

[0002] In recent years, wheeled mobile robots (WMR) have been widely used in the fields of planetary exploration, military and search and rescue, and can be used to perform dangerous tasks in specific occasions. However, due to the basic characteristics of wheeled mobile robots such as high coupling and nonlinearity, traditional control schemes are difficult to achieve the desired effect. At present, the control schemes for WMR mainly include adaptive, inversion, neural network, anti-disturbance, fuzzy and sliding mode control. In an ideal environment, the movement of WMR needs to meet non-holonomic constraints, that is, to maintain pure rolling for a long time without sliding. However, in relevant real scenarios, due to the influence of the actual environment, lateral sliding, longitudinal sliding and other situations may occur, which will destroy the ideal constraints of the robot and cause the robot to fail to move normally. Therefore, it is necessary to conduct in-depth analysis and research on the trajectory tracking control of WMR when there is sliding and side sliding. Summary of the invention

[0003] Aiming at the problem of poor trajectory tracking control performance of mobile robots, a trajectory tracking control method for mobile robots under unknown slip is provided. The method comprises:

[0004] The kinematic model of the mobile robot under unknown slip and the dynamic model of the robot are established. The unknown longitudinal slip and side slip disturbances, model parameter uncertainty and unknown input disturbances in the dynamic model are called lumped disturbances.

[0005] Design a kinematic controller based on the position tracking error of the mobile robot to output the virtual linear velocity and angular velocity of the mobile robot;

[0006] According to the speed tracking error and dynamic model of the virtual linear velocity and angular velocity of the mobile robot and the actual linear velocity and angular velocity, an anti-disturbance integral sliding mode controller is designed to determine the control torque of the left and right driving wheel driving motors of the mobile robot, and control the actual linear velocity and angular velocity of the mobile robot to converge asymptotically to the virtual linear velocity and angular velocity;

[0007] According to the anti-disturbance integral sliding mode controller and the actual linear velocity and angular velocity of the mobile robot, a nonlinear extended state observer is designed to estimate the velocity state and lumped disturbance of the mobile robot, and to feedforward compensate the lumped disturbance to eliminate the influence of the disturbance on the control performance of the system.

[0008] In one embodiment, the kinematic model of the mobile robot under unknown slip is expressed as:

[0009] Assume that the pose q = [x yθθ r θ l ] T , then in the case of sliding and sideslipping, the WMR non-holonomic constraint can be expressed as:

[0010]

[0011] Where (x, y) represents the coordinates of point p in the world coordinate system, which describes the position of the robot in space; θ represents the azimuth of WMR, indicating the orientation of the robot; θ l and θ r Respectively represent the rotation angles of the left and right driving wheels, which directly affect the vehicle's forward movement and steering. l , r and u represent the angular velocity disturbance and longitudinal sliding disturbance of the left and right driving wheels of the WMR when they slip, respectively, reflecting the external disturbance to which the vehicle is subjected during its motion, the magnitude of which is related to the combined influence of various factors such as wheels and road conditions. The non-holonomic constraint formula can be rewritten as follows in a compact form: In the formula, Λ=[u-rξ r -rξ l ] T .

[0012] Here we choose a full rank matrix C(q), which is a basis of the null space of A(q), so that A(q)C(q)=0, where

[0013] The kinematic equation of the wheeled mobile robot in the case of sliding and sideslipping can be expressed as:

[0014]

[0015] In the formula, ζ=[ζ v ζ w ] T , z=[vw] T ,

[0016] f(q,u)=[-usinθucosθ000] T .

[0017] Among them: v and w They represent the linear velocity disturbance and yaw rate disturbance caused by longitudinal sliding, v and w represent the linear velocity and angular velocity of the wheeled mobile robot, respectively. f(q,u) is the disturbance vector caused by lateral sliding of the wheel.

[0018] In one embodiment, the dynamic model of the mobile robot under slipping is expressed as:

[0019]

[0020] In the formula, B1(q)=C T M,B2(q)=C T V, I=I c +2I m +2m w b 2 .

[0021] After considering parameter uncertainty and external disturbances, the robot dynamics model is rewritten as

[0022]

[0023] Since the WMR runs on the horizontal ground, G(q) = 0, the robot dynamics model can be simplified as

[0024]

[0025] In the formula,

[0026]

[0027] d is the total disturbance of the sideslip disturbance f(q,u) and the sliding disturbance ζ. Assuming that d is bounded and has a first-order derivative, it means that when the system is disturbed, the disturbance will not increase infinitely and the rate of change of the disturbance is limited.

[0028] In one embodiment, the specific method of designing a kinematic controller according to the posture tracking error of the mobile robot and outputting the virtual linear velocity and angular velocity of the mobile robot includes:

[0029] Taking the actual linear velocity and angular velocity of the mobile robot as inputs of the kinematic model of the mobile robot to obtain the actual position and posture of the mobile robot;

[0030] Obtain a posture tracking error between a reference posture of the mobile robot and the actual posture, and based on the posture tracking error, execute the step of obtaining a virtual linear velocity and an angular velocity of the mobile robot according to the kinematic controller.

[0031] In one embodiment, the lumped disturbance is determined by the mass, moment of inertia, torque, longitudinal slip speed, sideslip speed, interference angular velocity vector caused by the longitudinal slip of the mobile robot's driving wheels, non-matching disturbance vector caused by the sideslip of the mobile robot, and the azimuth angle of the mobile robot.

[0032] In one of the embodiments, the specific method of controlling the actual speed of the mobile robot to converge to the virtual speed includes: inputting the control torque of the left and right driving wheel driving motors of the mobile robot into the dynamic model of the mobile robot, thereby determining the actual speed of the mobile robot.

[0033] The above-mentioned mobile robot trajectory tracking control method under unknown slippage constructs a dual closed-loop control strategy of an outer-loop kinematic controller and an inner-loop sliding mode controller. By obtaining the posture tracking error between the reference posture and the actual posture of the mobile robot, a kinematic controller based on the backstepping method is designed to obtain the virtual linear velocity and angular velocity, so as to achieve asymptotic convergence of the posture tracking error and improve the system state response speed; according to the speed tracking error and dynamic model between the virtual linear velocity and angular velocity and the actual linear velocity and angular velocity of the mobile robot, an anti-disturbance integral sliding mode controller is designed to determine the control torque of the left and right driving wheel driving motors of the mobile robot, and control the actual linear velocity and angular velocity of the mobile robot to converge asymptotically to the virtual linear velocity and angular velocity; a nonlinear extended state observer is designed to estimate the speed state and lumped disturbance of the mobile robot, and the lumped disturbance is feedforward compensated to eliminate the influence of the disturbance on the control performance of the system; by executing the above-mentioned dual closed-loop control strategy, the trajectory tracking control of the mobile robot is realized, and the control performance of the mobile robot under wheel slippage is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 A motion model of a mobile robot in one embodiment;

[0035] Figure 2 A block diagram of a trajectory tracking control system for a mobile robot under unknown slip in one embodiment;

[0036] Figure 3 A schematic diagram of the trajectory tracking control effect of a mobile robot under a circular reference trajectory in one embodiment;

[0037] Figure 4 Schematic diagram of the X-direction posture tracking error of a mobile robot under a circular reference trajectory in one embodiment;

[0038] Figure 5 Schematic diagram of the Y-direction posture tracking error of a mobile robot under a circular reference trajectory in one embodiment;

[0039] Figure 6 A schematic diagram of the azimuth tracking error of a mobile robot under a circular reference trajectory in one embodiment;

[0040] Figure 7 A schematic diagram of the control torque of the left driving wheel motor of a mobile robot under a circular reference trajectory in one embodiment;

[0041] Figure 8 Schematic diagram of the control torque of the right driving wheel motor of the mobile robot under the circular reference trajectory in one embodiment;

[0042] Fig. 9 A schematic diagram of linear velocity tracking of a mobile robot under a circular reference trajectory in one embodiment;

[0043] Fig.10 A schematic diagram of angular velocity tracking of a mobile robot under a circular reference trajectory in one embodiment;

[0044] Fig.11 It is a schematic diagram of the estimation effect of the nonlinear extended state observer of the mobile robot on the lumped disturbance under the circular reference trajectory in one embodiment; DETAILED DESCRIPTION

[0045] In order to more clearly understand the above-mentioned objects, features and advantages of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present application and the features in the embodiments can be combined with each other without conflict.

[0046] Many specific details are explained in the following description in order to fully understand the present invention. However, the present invention may also be implemented in other ways than those described herein. Therefore, the scope of protection of the present invention is not limited to the specific embodiments disclosed below. The trajectory tracking control method of a mobile robot under unknown slippage described in some embodiments of the present invention is described below with reference to the accompanying drawings. The method of the present invention can be applied to the cloud or server, and can also be applied to a terminal that can perform trajectory tracking control, such as a sweeping robot, a sweeping and mopping robot, a two-wheel differential drive service robot and other mobile robots. The following is an example of applying this method to a mobile robot.

[0047] The mobile robot trajectory tracking control method under unknown slip of the present invention can be implemented by a pre-designed dual closed-loop control system of the mobile robot. The dual closed-loop control system controls the convergence of the speed error and the posture error based on the control strategy. The dual closed-loop control system is independent of the initial state of the mobile robot and can achieve convergence through the parameters of the controller. Figure 2 shown.

[0048] Specifically, in the outer loop control, a kinematic controller based on backstepping is designed through the kinematic model of the mobile robot to generate the virtual velocity of the mobile robot. In the inner loop control, the unknown longitudinal and lateral slip disturbances, model parameter uncertainty and unknown input disturbances of the mobile robot are regarded as lumped disturbances, and a nonlinear extended state observer is designed to estimate the lumped disturbance of the system in real time. By designing an auto-disturbance rejection integral sliding mode controller, the mobile robot is controlled to track the virtual velocity generated by the outer loop, and the observed lumped disturbances are compensated in real time to improve the robustness of the dual closed-loop control system.

[0049] In order to build a dual closed-loop control system for a mobile robot, two controllers, an inner loop and an outer loop, can be constructed:

[0050] Step 1: Establish the kinematic model of the mobile robot under unknown sliding and sideslipping.

[0051] This study uses robots whose geometric center coincides with their center of mass for correlation analysis. Figure 1 For easy control, two coordinate systems, the world and the robot, are established. The world coordinate system is represented by XOY, and the robot coordinate system is represented by X c -O c -Y c Indicates.

[0052] The specific structure of the wheeled mobile robot is shown in the attached figure. Figure 1 As shown in the figure, the distance between the two driving wheels of the mobile robot is 2b, the radius of the driving wheel is r, and the distance from the center of mass of the mobile robot to the geometric center of the wheeled mobile robot is d. Assume that P is the reference point of the wheeled mobile robot, and its posture is represented by the vector (x, y, θ) T , where x and y represent the horizontal and vertical coordinates of the reference point P in the global coordinate system XOY of the mobile robot, and θ is the direction angle of the reference point P.

[0053] Assume that the pose q = [x yθθ r θ l ] T , then in the case of sliding and sideslipping, the WMR non-holonomic constraint can be expressed as:

[0054]

[0055] Where (x, y) represents the coordinates of point p in the world coordinate system, which describes the position of the robot in space; θ represents the azimuth of WMR, indicating the orientation of the robot; θ l and θ r Respectively represent the rotation angles of the left and right driving wheels, which directly affect the vehicle's forward movement and steering. l , rand u represent the angular velocity disturbance and longitudinal sliding disturbance of the left and right driving wheels of the WMR when they slip, respectively, reflecting the external disturbance to which the vehicle is subjected during its motion, the magnitude of which is related to the combined influence of various factors such as wheels and road conditions. The non-holonomic constraint formula can be rewritten as follows in a compact form: In the formula, Λ=[u-rξ r -rξ l ] T .

[0056] Here we choose a full rank matrix C(q), which is a basis of the null space of A(q), so that A(q)C(q)=0, where

[0057] The kinematic equation of the wheeled mobile robot in the case of sliding and sideslipping can be expressed as:

[0058]

[0059] In the formula, ζ=[ζ v ζ w ] T , z=[vw] T ,

[0060] f(q,u)=[-usinθucosθ000] T .

[0061] Among them: v and w They represent the linear velocity disturbance and yaw rate disturbance caused by longitudinal sliding, v and w represent the linear velocity and angular velocity of the wheeled mobile robot, respectively. f(q,u) is the disturbance vector caused by lateral sliding of the wheel.

[0062] Step 2: Establish a dynamic model of the mobile robot considering the dynamic characteristics of the mobile robot.

[0063]

[0064] In the formula, B1(q)=C T M,B2(q)=C T V, I=I c +2I m +2m w b 2 .

[0065] After considering parameter uncertainty and external disturbances, the robot dynamics model is rewritten as

[0066]

[0067] Since the WMR runs on the horizontal ground, G(q) = 0, the robot dynamics model can be simplified as

[0068]

[0069] In the formula,

[0070]

[0071] d is the total disturbance of the sideslip disturbance f(q,u) and the sliding disturbance ζ. Assuming that d is bounded and has a first-order derivative, it means that when the system is disturbed, the disturbance will not increase infinitely and the rate of change of the disturbance is limited.

[0072] Step three, the outer loop designs a kinematic controller based on the backstepping control method, and the kinematic controller is used to generate a virtual speed, which includes a virtual linear speed and a virtual angular speed.

[0073] In the design of motion control law, the kinematic model of the robot reference trajectory is assumed to be:

[0074]

[0075] In the formula, v r is the expected linear velocity of the robot, w r is the desired angular velocity, (x r ,y r ,θ r ) is the desired posture of the robot. Let the desired posture of WMR be q r =(x r ,y r ,θ r ) T , the expected speed is z r =[v r w r ] T , define the pose tracking error q e =(x e ,y e ,θ e ) T , the velocity tracking error is z e =[v e w e ] T .

[0076] The trajectory error is defined as:

[0077]

[0078] Taking the derivative of the above formula, we can get:

[0079]

[0080] In one embodiment, the kinematic controller is implemented by the following formula, and the following auxiliary kinematic control law is designed:

[0081]

[0082] In the formula, v c is the robot auxiliary linear speed, w c is the auxiliary angular velocity, k1>0, k2>0, k3>0. Through the design of auxiliary kinematic control law, the actual position of the robot can converge to the desired position asymptotically.

[0083] In order to prove the convergence of the control law, the following candidate Lyapunov function is considered:

[0084]

[0085] V1≥0 always holds. When V1=0, x e =0,y e =0,θ e = 0 holds. Taking the derivative of both ends of the above equation with respect to time, we can get

[0086]

[0087] Where β1=k1,β2=k3v r , according to the stability criterion, So V1 is bounded, x e ,y e ,θ e is also bounded, the closed-loop system is asymptotically stable, and the position tracking error x e ,y e ,θ e It converges to zero asymptotically under the action of the kinematic controller. Step 4: Design the nonlinear extended state observer in the inner loop to observe the lumped disturbance in the dynamic model. The design process of the nonlinear extended state observer (NESO) is as follows.

[0088] For the trajectory tracking of WMR under sliding and sideslipping, a nonlinear extended state observer (NESO) is designed to estimate the total disturbance d. Define the state variables x1 = z, x2 = d, The corresponding state space description is:

[0089]

[0090] NESO's design takes the following form:

[0091]

[0092] Defined as sig γ (x)=|x γ sgn(x).

[0093] For the differential wheeled robot system, a nonlinear extended state observer is used to accurately estimate the system state variable z and external disturbance d within a finite time.

[0094] Step 5: Based on the observation results of the nonlinear extended state observer, the inner loop designs an active disturbance rejection integral sliding mode controller to control the mobile robot to perform inner loop velocity tracking, thereby achieving trajectory tracking. The steps for designing the active disturbance rejection integral sliding mode controller are as follows:

[0095] Define the virtual speed tracking error as The following integral sliding surface is introduced:

[0096]

[0097] In the formula, λ>0. The designed dynamic controller is

[0098] In the formula, k>0.

[0099] For the differential wheeled robot system, the velocity tracking error can converge to zero asymptotically by using a dynamic controller.

Claims

1. A trajectory tracking control method for a mobile robot in a sliding and side-slipping state, characterized in that: The specific steps include: Step 1, establishing a trajectory tracking control system model for a wheeled mobile robot considering lateral and longitudinal slippage of the wheels; Step 2, according to the established control system model, a kinematic controller is designed based on the backstepping method to provide the corresponding virtual speed; Step 3, according to the speed tracking error and dynamic model of the virtual linear speed and angular speed of the mobile robot and the actual linear speed and angular speed, design an auto-disturbance rejection integral sliding mode controller to control the actual linear speed and angular speed of the mobile robot to converge asymptotically to the virtual linear speed and angular speed; Step 4: Based on the anti-disturbance integral sliding mode controller and the actual linear velocity and angular velocity of the mobile robot, a nonlinear extended state observer is designed to estimate the velocity state and lumped disturbance of the mobile robot, and the lumped disturbance is feedforward compensated to eliminate the influence of the disturbance on the control performance of the system.

2. According to the trajectory tracking control method of a mobile robot in sliding and sideslipping states, the kinematic controller designed based on the backstepping method can make the actual position of the robot converge to the desired position asymptotically by designing an auxiliary kinematic control law. For the trajectory tracking of WMR under sliding and sideslipping, the total disturbance is estimated by designing a nonlinear extended state observer.

3. The method for tracking the trajectory of a mobile robot in a sliding and side-slipping state according to claim 1, characterized in that: The specific method of designing a kinematic controller according to the posture tracking error of the mobile robot and outputting the virtual linear velocity and angular velocity of the mobile robot includes: Taking the actual linear velocity and angular velocity of the mobile robot as inputs of the kinematic model of the mobile robot to obtain the actual position and posture of the mobile robot; Obtain the reference posture of the mobile robot and the posture tracking error of the actual posture, and according to the posture tracking error, execute the step of obtaining the virtual linear velocity and angular velocity of the mobile robot according to the kinematic controller based on the backstepping method.

4. The method for tracking the trajectory of a mobile robot in a sliding and sideslipping state according to claim 1, characterized in that: The specific method of controlling the actual speed of the mobile robot to converge to the virtual speed includes: inputting the control torque of the left and right driving wheel driving motors of the mobile robot into the dynamic model of the mobile robot, thereby determining the actual speed of the mobile robot.

5. The method for tracking the trajectory of a mobile robot in a sliding and sideslipping state according to claim 1, characterized in that: The lumped disturbance is determined by the mass, moment of inertia, torque, longitudinal slip speed, sideslip speed, interference angular velocity vector caused by longitudinal slip of the mobile robot's driving wheel, non-matching disturbance vector caused by sideslip of the mobile robot, and azimuth of the mobile robot.

Images