Trajectory tracking and obstacle avoidance method for robots based on fast terminal sliding mode

CAO Zhi-bin, YANG Wei, SHAO Xing-ling, LIU Ning

(Key Lab of Instrumentation Science & Dynamic Measurement (North University of China)，Ministry of Education, Taiyuan 030051， China)

Abstract： For accurate trajectory tracking and obstacle avoidance in finite time of a nonholonomic mobile robot, a trajectory tracking controller based on global fast terminal sliding mode method is proposed, which has the advantages of chattering-free and adjustable convergence time. First of all, the kinematics model of the robot is established in mobile carrier coordinates. Secondly, the global structure including terminal attractor and exponential convergence of the fast terminal sliding mode trajectory tracking controller is proved by Lyapunov stability theory, ensuring that the trajectory and heading angle tracking error converges to a smaller zero range in finite time. Finally, the artificial potential field obstacle avoidance method is introduced to make the robot not only track the reference trajectory strictly, but also avoid the obstacles. The simulation results show that the proposed method can achieve a stable tracking control in finite time for a given reference trajectory.

Key words： trajectory tracking; global fast terminal sliding mode; adjustable convergence time; chattering free; artificial potential field method

0 Introduction

Wheeled robots have been widely used in military and civilian fields, including situational awareness, anti-terrorism and riot prevention, space exploration, topographic mapping and so on owing to their advantages of light weight, large load, simple mechanism, flexibility, high efficiency, easy driving and control[1-3]. A wheeled robot is a typical multi-input multi-output, under-actuated and nonlinear system with nonholonomic constraints, which makes it difficult to track precisely[4-5].

In recent years, the trajectory tracking control of nonholonomic wheeled robots has attracted wide attention. In Ref.[6], a feedback stable trajectory tracking control law based on small disturbance linearized dynamic model is presented, which can obtain local stability. The smoothing controller presented in Ref.[7] can achieve exponential stability for any initial condition, thus improving the convergence speed of the algorithm. In Ref.[8], the trajectory tracking control of the nonholonomic wheeled robot is realized by using computational moment method and robust sliding mode control theory. In Ref.[9], considering that the angular velocity of the reference model is non-zero constant, a continuous finite-time state feedback tracking control law is proposed for the global finite-time tracking of the desired trajectory which satisfies the assumption that the angular velocity does not tend to zero. In Ref.[10], a four-rotor unmanned aerial vehicle (UAV) flight controller based on global fast terminal sliding mode control is proposed for chattering-free high-precision tracking control of finite-time position and attitude, which can ensure that the position and velocity tracking error converge to zero in finite time. A finite-time cascade trajectory tracking controller for nonholonomic mobile robots is presented in Refs.[11-12]. The error model of mobile robots is divided into two subsystems. A finite-time control law is designed for the first-order subsystem to stabilize the angle error. A finite-time sliding mode controller is used for the second-order subsystem to improve the convergence speed and overcome the sliding mode chattering of control system.

The global fast terminal sliding mode not only ensures that the system can reach the sliding surface in finite time, but also makes the error converge to zero quickly and achieve global fast convergence, which has the advantages of reducing the chattering and improving the stability of the system[13-15]. By designing the potential energy function of the target point and the obstacle in a moving environment, the artificial potential field method enables the robot to avoid obstacles under the action of the artificial potential field, which is convenient to adjust parameters and has a small amount of calculation. It is suitable for the path planning and safe obstacle avoidance of the robot[16]. In this paper, a global fast terminal sliding mode controller with terminal attractor and exponential convergence term is presented for the finite-time precise trajectory tracking of a nonholonomic wheeled robot kinematics model. At the same time, the artificial potential field obstacle avoidance algorithm is introduced, which can make the robot realize stable trajectory tracking and obstacle avoidance control in finite time in the presence of obstacles.

1 Nonholonomic wheeled mobile robots

The wheeled mobile robot studied is a small four-wheeled unmanned vehicle. Its front wheel controls the direction of movement through the steering gear, and the servo motor drives four wheels through the drive shaft. The chassis of the robot and the external equipment exchange data through CAN bus. When the chassis controller receives the control signal from the external equipment, it transmits it to the driver. The driver controls the starting, stopping and rotating speed of the servo motor, and protects the motor from overload, short circuit and under voltage. The speed reducer reduces the speed of the motor and enlarges the torque. The speed and torque are transferred to the differential on the front and rear sides of the chassis synchronously through the output shafts at both ends. The differential is then transmitted to the front and rear wheels to drive the car forward. When turning, the inner driving wheel and the outer driving wheel can rotate at different speeds. At the same time, the controller transfers the control signal to the steering gear to control the steering of the trolley. The robot is composed of chassis frame, suspension system, steering gear, drive motor, deceleration and differential system, drive shaft, control drive module, data transmission system module, power supply battery and external protection mechanism. MPU-9150 motion sensing and tracking module, embedded with three-axis MEMS gyroscope, three-axis MEMS accelerator, three-axis MEMS electronic compass and a digital motion sensing processing engine capable of handling complex nine-axis motion sensing fusion algorithm, has the characteristics of low power consumption, low cost and high performance.

Fig.1 Wheeled mobile robots

The hardware structure is divided into two parts: the upper control system and the lower control system. The controller local network is connected by CAN bus between the upper control system and the lower control system, which can realize reliable data communication and real-time and efficient task scheduling. The hierarchical structure of hardware and the use of CAN bus local network not only can improve the real-time data processing ability of the robot, but also reduce the weight and volume of the mobile platform, the energy consumption and the overall cost of the mobile robot.

The upper control system can be divided into main control module, serial port expansion module, dual channel radio frequency wireless communication module, GPS module, electronic compass and so on.

The main control module needs to store and process a large amount of data. The data processor of robot colony cooperative system uses a high performance 16-bit digital signal control dsPIC33FJ256MC710 which is produced by Micro-Chip Company of America. It is a hybrid chip with DSP processing and MCU control functions. Flash has a capacity of 256 KB, supports two UART interfaces and DMA transmission, and the internal processing frequency of the system can be as high as 150 MHz. It adopts an improved Harvard architecture with low power consumption, small size and high speed.

Assuming that the front and rear wheels are not deformed and that the contact between the wheel and the ground is a pure rolling, non-sliding mode, its linear velocity is orthogonal to the rear axle. The inertial coordinate system {OI-XIYI} and the body coordinate system {B} of vehicle motion are established. The inertial coordinate system is dark and the body coordinate system is light. The position and attitude of the vehicle can be expressed by the body coordinate system {B}. The origin of the coordinate is located in the center of the rear wheel, and the speed in thex-axis direction of the rear wheel isv. Its motion in the horizontal plane can be represented by Fig.2.

Fig.2 Posture errors of the coordinates of a mobile robot

For the wheeled robot in the inertial coordinate system, the velocity expression in theYdirection can be written as

(1)

It cannot be transformed into integralx,yandθ, which is a nonholonomic constraint.

The kinematics model of first-order nonholonomic wheeled robot with the Jacobi matrix is expressed as

(2)

The reference signal in inertial coordinate system for the mobile robot is generated bypr=(xr,yr,θr)T. The velocity vector isqr=(vr,wr)T. The error coordinates of wheeled robot moving fromp=(x,y,θ)Ttopr=(xr,yr,θr)TarePe=(xe,ye,θe)T. The pose error equation by the coordinate transformation of the wheeled robot is

(3)

The differential equation of pose error is

(4)

2 Design of global fast terminal sliding mode controller

The structure of global fast terminal sliding mode controller of the wheeled robot is shown in Fig.3. The system input is the reference robot posepr, and the output is the current moment positionpof the robot. The controller obtains the robot control amountqfrom the pose errorpeand the velocity vectorqr, and obtains the posepof the current moment by means of the constraint condition and kinematic equation.

Fig.3 Control structure diagram

2.1 Switching function

For anyx∈Rand |x|<∞, there existsφ(x)=xsin(arctanx)≥0, and if and only ifx=0, “=” holds. Using the theorem, the back-step method can be used to design the switching function of the sliding mode controller.

Whenxe=0, considering the Lyapunov function

(5)

letθe=-arctan(vrye)， then

-yexew+vryesin(-arctan(vrye))=

-yexew-vryesin(arctan(vrye)).

(6)

It can be seen that as long asxeconverges to zero andθeconverges to -arctan(vrye), the system stateyeconverges to zero.

Based on the above conclusion, the following switching function is designed as

(7)

2.2 Controller

Adding terminal attractor in the control system can improve the convergence speed of the control system and make the control system reach a stable state in a limited time. Its mathematical model is

(8)

wherexis the state variable of the controlled system,η0andμ0are positive numbers withp0>q0.

Eq.(8) can be rewritten as

(9)

Simultaneous integration is performed on both sides, then

(10)

wheretris the convergence time of state variablex, andx(0) is the initial value of state variablexof the system. According to the formula, the terminal attractor can not only make the state variable of the system converge to zero in a certain time, but also make the convergence speed faster when the state variable is at the equilibrium point.

A hybrid reaching law between terminal attractor and exponential convergence term is constructed as

(11)

wheresis the sliding mode switching surface, andα0,β0>0.

When the state variable of the controlled object is located farther away from the switching surface, the state variable of the controlled object will gradually approach the equilibrium point according to the terminal attraction approach and exponential approach. The exponential reaching law in sliding mode will no longer play any role when the state variable of the controlled object is located near the switching surface. The exponential reaching law will gradually approach the equilibrium point in the form of terminal attraction, and the position of the state variable in the controlled system will be between the equilibrium point and the position of the state variable in the process of reaching the equilibrium point. As the distance between the state variables and the switching surface becomes closer and closer, the approaching speed decreases with the distance between the state variables and the switching surface. Therefore, the impact strength of the state variables entering the switching surface can be reduced, the buffeting can be weakened, and the dynamic performance of the controlled system can be improved.

Based on the sliding mode controller switching function, we can get

(12)

Letα=arctan(vrye),s0=xe,s1=θe+arctan(vrye) ands2=0， substituting Eq.(4) into Eq.(7), we get

(13)

Then the linear velocity and angular velocity control laws are

(14)

2.3 Analysis of convergence time

From Eq.(11), we obtain

(16)

(17)

We further obtain

(18)

Whent=0，C=y(0)， there is

y=

(19)

Lety=0，t=ts， we have

(20)

with

(21)

Therefore, the time from the arbitrary initial states0(0) to the equilibrium states0=0 in the sliding mode is

(22)

In this part, the proposed control method iscompared with exponential reaching law to show its superiority in tracking performance. Its form is

(23)

If the state variables of the system move from one side ofs>0 to the switching surface, the upper form can be simplified to

(24)

Therefore

(25)

When the state variables of the system move to the switching surface, there ares(t)=0, the convergence time of exponential reaching law is

(26)

Supposngtsis equal to the convergence time of exponential reaching law, there is

(27)

We further obtain

(28)

Obviously, the coefficient of the exponential part in this method is obviously much larger than that of the exponential part in the exponential reaching law, which results in the convergence time of this method is obviously shorter than that of the exponential reaching law.

3 Obstacle avoidance algorithm based on artificial potential field method

Artificial potential field method is a virtual force method. Since the robot is small compared to the external environment, it can be regarded as a point. During the movement, the robot is affected by the repulsive force generated by the obstacle and the gravitational force generated by the target point, and finally moves to the target point under the action of the resultant force.

The gravitational potential field function is defined as

(29)

wherexris the reference signal in inertial coordinate system for the mobile robot,xis the Cartesian position coordinate of the robot center, andδ>0 is the gravitational field coefficient constant. Then the gravitational potential of the artificial potential field is given by

F1=A1=δ(x-xr).

(30)

The repulsion potential field function is defined as

(31)

whereλis the distance between the robot and the obstacle;λ0is the length of the obstacle repulsion field, that is, the safety distance;ais the obstacle repulsion field coefficient, and it is a constant. The repulsive force of artificial potential field is

(32)

with

(33)

The repulsion function divides the repulsive force of the robot under the artificial potential field into two parts. One line along the robot and the target point points from the robot to the target point, and the other force points to the robot along the robot and the obstacle. The algorithm is prone to converge at the local minimum point, therefore the robot may not reach the target position and the trajectory jitters in the narrow region. The proposed method can solve this problem by improving the traditional algorithm.

4 Simulation

4.1 Experiment in barrier free environment

In this section, the nonholonomic mobile robot experimentally tracks a desired trajectory in an obstacle-free environment. The initial position of the robot is (1, 2)m, the initial azimuth angle of the robot is 0 rad, and the initial line speed is 1 m/s. The desired trajectory is

The controller parameters are chosen as follows:wr=1.0，vr=1.0,α0=α1=1,β0=β1=5,η0=η1=7,μ0=μ1=9. The simulation results are shown in Figs.4-6.

Fig.4 Trajectory tracking results

From Fig.4, we can see that the wheeled robot tracks the desired trajectory accurately and steadily within a limited time. To verify the superiority of the proposed algorithm in convergence speed and tracking accuracy, the proposed algorithm is compared with the exponential reaching law based sliding mode controller trajectory tracking algorithm (SMC-ERL). The simulation results are shown in Figs.5-6.

Fig.5 Trajectory tracking errors

Fig.6 Control inputs

As seen from Fig.5, the convergence rates of errors inxdirection,ydirection and angular velocity of the proposed method are obviously better than those of the sliding mode control algorithm based on exponential reaching law. Because the sliding mode variable structure control algorithm contains discontinuous switching functions, if the sliding mode variable structure control algorithm is applied to the actual project, the chattering phenomenon will be caused by the algorithm itself. In Fig.6, the control inputvand control inputwof the sliding mode control algorithm based on exponential reaching law exhibit obvious chattering phenomena in the process of continuous maneuvering of the control object, while the control inputvand control inputwof the proposed method are smooth without chattering.

4.2 Experiment in obstacle environment

In this section, we consider the motion of nonholonomic mobile robots in the presence of obstacles. The initial position of the robot is (4, 35) m. In the presence of obstacles, the wheeled robot is safe and collision-free to the target point position (43, 20) m under the action of the artificial potential field. In the simulation, the parameters are chosen as follows:α0=α1=1,β0=β1=5,η0=η1=7,μ0=μ1=9,λ=0.5,δ=50,a=5. The obtained simulation result are shown in Figs.7-9.

Fig.7 Vector field

Fig.8 Trajectory tracking results

Fig.7 is a vector field view of the artificial potential field in the presence of an obstacle environment. It can be seen that under the action of the artificial potential field, a vector field from the arbitrary position to the position of the target is generated, and a vector deviating from the obstacle is generated in a small range near the obstacle. The moving wheeled robot can bypass the obstacle to reach the target position from any starting point under the action of artificial potential field and vector. In Fig.8, the wheeled robot accurately and stably tracks the desired trajectory in a limited time, and realizes the obstacle avoidance control in the presence of an obstacle. In addition, when the wheeled robot moves to a position where the obstacle is close, it can still pass safely and smoothly, avoiding the problem that easily appears in the traditional artificial potential field.

To further illustrate the tracking performance of the proposed controller, a comparison is made between the proposed controller and the SMC-ERL. Under the condition of the same controller gain, the control performance of the two controllers is shown in Fig.9.

Fig.9 Trajectory tracking errors

It can be seen from Fig.9 that thexdirection,ydirection and direction angle errors of the proposed method can converge smoothly to zero in a short time, and the steady-state error is small and there is no oscillation. In order to quantify the tracking performance of the controller, the average error and convergence time of the position and attitude tracking of the wheeled robot are calculated under dynamic response, as shown in Table 1.

Table 1 Tracking performance comparison of controllers

It can be seen that the average tracking error of the proposed method has been reduced by 100 times than that of the sliding mode controller based on exponential reaching law, and the convergence time increased by 51.12%. This fully demonstrates the superiority of the proposed control algorithm.

In summary, the designed control method can ensure strong and stable tracking control for the desired trajectory, and at the same time it can achieve safe obstacle avoidance during the movement. In addition, the proposed method has fast response without chattering, which is more suitable for engineering applications.

5 Conclusion

A trajectory tracking controller based on global fast terminal sliding mode method is designed. The terminal attractor and exponential convergence term are constructed, so that the tracking errors of the trajectory and heading angle can converge to a small zero domain within a limited time. At the same time, the artificial potential field method is introduced to make the wheeled robot better adapt to the fast and accurate trajectory tracking in the obstacle environment, ensuring that the robot can reach the target point smoothly. The method has precise control and fast error convergence without no chattering, therefore it has certain application prospects. The proposed method can be applied to the motion control of a mobile robot such as a four-wheeled trolley by using a differential drive of the left and right wheels to detect the distance between the trolley and the obstacle through a device such as a laser radar or an infrared sensor. Under the action of the artificial potential field method, if the trolley is far away from the obstacle, the trolley is only subjected to the gravitational effect from the target point. If the distance between the car and the obstacle is less than the set safety distance, the car will be affected by the repulsion from the obstacle and the gravitation of the target point. The algorithm is used to calculate the direction of the resultant force of the car, and the trajectory tracking control algorithm is used to quickly and stably adjust the driving direction.