New Control Approaches for Trajectory Tracking and Motion Planning of Unmanned Tracked Robot

This work proposes new control approaches for tracking and motion planning of Unmanned Ground Vehicles (UGVs) that utilize skid steering system. This work proposes an energy based Variable Structure Control (VSC) scheme, in which two independent Sliding Control Surfaces (SCS)s are designed based on the system states. Particularly, the controller is designed based on the assessment and the minimization of the systems total energy by finding an explicit relation between the controller gains and the slope of the sliding surface. The work also discusses a new fuzzy potential approach for motion planning of UGV. The Fuzzy system generates an attractive force that pulls the UGV effectively toward a moving or stationary target, and a repulsive force, which is required to avoid any stationary or moving obstacles. Both, the VSC and the motion planning were validated by a nonlinear model of an Unmanned Tracked Robot (UTR) on different trajectories, and was compared with different control schemes. Simulation results show superiority of the proposed VSC over other methods with less control effort. Furthermore, the new motion planning controller proved its high capacity in producing a smooth and dynamic trajectory to allow an UGV to track a target and to avoid obstacles.


Introduction
Unmanned Ground Vehicles (UGVs) are becoming widely used in a variety of applications for their robustness and maneuverability. Moreover, the need to keep human away from danger in many applications has given researchers great attention in recent years [1]. Nowadays, UGVs are present in search and rescue, nuclear plant operations, space exploring and mining [2] and [3]. In such applications, UGV is required to rapidly change its directions without a considerable changes in speed. Hence, various steering systems have been developed for UGVs and the Skid-Steering (SS) is the most widely used mechanism in aforementioned applications because of its high maneuverability and robust structure [4].
Relatively to traditional steering systems such as single axial and double axial steering, the motion control in skid steering mechanisms is particularly challenging because of braking and excessive slip in tires [5]. Moreover, SS results in complex track-terrain interactions where the wheels/track must skid latterly to follow a curved path which might lead to motion instability or what is referred to as skid ding [6] and [7]. In addition to uncertainty with respect to vehicle dynamics due to immeasurable friction coefficient, this makes the modeling of the vehicle, for the purposes of motion control a cumbersome task [8] and [9].
The steering of a tracked vehicle using skid steering approach is achieved by individually varying the thrust of the tracks. Consequently, a turning moment is created to overcome the moment of turning resistance due to skidding of the tracks on the ground and the rotational inertia of the vehicle [10] and [5]. In other words, the skid steering approach is based on varying the relative velocities of the two tracks which in turn will result in slippage and soil shearing in order to achieve the steering. These phenomena of slippage and soil shearing are highly nonlinear and difficult to reliably quantify, and cause inaccurate tracked vehicle kinematics and dynamics. The problem is further aggravated by the fact that as the vehicle navigates over differing terrain, the magnitude of these nonlinear quantities vary significantly [11], [12] and [13].
Within a navigational mission for a surveillance vehicle, the changes tend to be rapid and frequent. These inaccuracies make it difficult to predict the exact motion of the tracked vehicle and increase the complexity of the task of controlling their motion. Although various controller design approaches were proposed such as in [14], [15], [16] and [17], control problems associated with system uncertainties, presence of high-order dynamics, and system inherent nonlinearities remain a big challenge.
Variable Structure Control (VSC) opened up a wide area of development for controller design [15]. It provides robustness in the presence of modeling uncertainties, nonlinearities, and disturbances. Furthermore, the researchers in [13] proposed robust algorithm to address the uncertainties in the dynamic parameters of the robots. In [18], the trajectory-tracking control problem of a non-holonomic skid steering was solved using a practical stabilizer with an oscillator. A nonholonomic constraints was used with the kinematic and dynamic models for the Sliding Mode Control (SMC) for robust trajectories tracking in [19] and [20].
SMCs are based on the theory of VSC which first appeared in the late fifties in Russia as a special class of nonlinear systems [16]. Design theory and procedures are outlined in [17], [21], [22], [23] and [24], and other literature dealing with SMCs tend to be specific to a narrow class of plant structures. SMC has an advantage of adaptability and robustness over other control schemes [23]. Nevertheless, the high-frequency switching between the sliding surfaces produces chattering [25]. In [26] and [27] SMC was used for skidsteered robots, however, there was wheel slipping during high-speed turns.
To eliminate the chattering phenomeno, a boundary layer was introduced in [28] for smoothing the switching between sliding surfaces, however, this boundary layer may result in steady-state errors. To overcome this drawback, methods developed in other publications [16], [17] and [21] involve integrating the classical theories used for designing the sliding mode surface and the switching control law with other computational intelligence-based systems such as Fuzzy Logic (FL), Neural Networks (NN), and Genetic Algorithms (GA) controllers. Many researchers investigated such con-trol schemes for SS robots [29], [30], [31], [32] and [33]. However, such systems are not recommended to be implemented for low-level control structure due to their high computing demand while operating in portable embedded systems. A new adaptive SMC method was proposed to guide a mobile robot in [34]. The work in [35] solved the problem with a super-twisting sliding mode control, which enforces a second-order sliding mode, integrated with a PD controller, to achieve good robustness.
In this paper we adapt an energy based VSC first proposed in [10], where the work and energy of the system are used for the SMC. Energy based VSC scheme is used in which two independent sliding surfaces are designed based on the system states. In other words, the controller is designed based on the assessment and the minimization of the system's total energy by finding an explicit relation between the controller gains and the slope of the sliding surface. Later, the two sliding surfaces for both control signals, the lateral position and the heading angle, were combined to form a hyper sliding control surface to control the motion of the UTR. The hyper VSC was compared with different control schemes to validate its effectiveness.
The path planning for robots is treated as a highlevel controller that does not consider the dynamics, control inputs, motion constraints, and duration of motion [36]. The goal of this controller is to plan a path towards a desired goal, and to identify all potential obstacles in order to search for a collision free path. Most of the path planning methods focused on solving the motion planning problem in a stationary environment, where both targets and obstacles are fixed. Some of these methods have been recently used such as the Voronoi diagrams [37], fuzzy logic controller [38], [39] and [40].
The most challenging issue in path planning is to avoid unknown obstacles that can be dynamic. Reactive planning needs to be fast and works in realtime which allows robot to react effectively to obstacles in vicinity [41]. Therefore, researchers extend the problem in a dynamic environment where the target and the obstacles are moving [42] and [43]. A recent work has implemented the Voronoi diagram and computational geometry to check for collisions using a small rectangular region around the robot [44]. Other works adopted the Artificial Potential Field (APF) method for its mathematical elegance and simplicity to determine the path in minimum computation time [45] and [46]. The major problem in this method is that the robot can get stuck in the local minima of the potential field at a point is far from the actual goal.
Recently, Fuzzy based motion planner for mobile robots have been discussed in literature. The bene-fit of fuzzy logic is the absence of new changes between states, improved continuous executions, and it is free from local minima problem [47]. Therefore, this work proposes a new fuzzy controller approach based on the potential field method [43] which defines virtual attractive and repulsive forces. These forces are used to generate a smooth and dynamic trajectory for the mobile robot path planning. The proposed method produces an efficient and accurate low-frequency pathway in order to reduce the computational burden on the embedded resources. In addition, the fuzzy-based controller can be applied practically on real robots as was proved in [48].
In order to study the performance of the robot with the low-level and high-level controllers, a nonlinear and dynamic simulation environment is prepared. In the first stage, the path tracking of the tracked robot is tested using the proposed energy based variable structure controller and compared with different control schemes. In the next stage, the simulation results will discuss the robot's performance in reaching and tracking the target in an environment containing moving and static obstacles. A new fuzzy potential approach for motion planning of the UGV is used. The Fuzzy system generates an attractive force that pulls the UGV effectively toward a moving or stationary target, and a repulsive force, which is required to avoid any stationary or moving obstacles.
After what was mentioned in the Introduction, the remainder of this work is as follows. The kinematic and the dynamic models of the UTR are discussed in Sec. 2.
The concept of the energy based VSC is presented in Sec. 3. and the controller is derived in Sec. 4. Then, Sec. 5. shows a new approach for robot motion planning based on Fuzzy potential forces. The simulation results are discussed in Sec. 6. Finally, conclusion remarks and future work orientations are summarized in Sec. 7. .

SS Robot Kinematic and Dynamic Modelling
Here we will discuss the kinematic and dynamic model of the unmanned tracked robot, the robot utilizes a skid steering mechanism, which makes it highly nonlinear.

Kinematic Model
Skid steering of Fig. 1 is achieved by applying an appropriate force forward or backward to the tracks in order to steer the vehicle along a prescribed path. Depending on whether a forward thrust or a backward force is applied, a slip or skid may occur to the track.
Taking this action into consideration, the turning radius of curvature and the rotational velocity equations of the vehicle will change. The radius of the curvature ρ as shown in Fig. 1 is given by: where B is the distance between the tracks, v out = rω out and v in = rω in are the outer and inner track velocities, relative to the center of rotation O. The ω out and ω in are the outer and inner tracks rotational speeds, and r is the track radius. The robot turning velocity is given by: where is the robot average velocity. Taking slippage in the outer and inner wheels, i out and i in , respectively, Eq. (1) and Eq. (2) can be given as: The values of the slip and skid (i out , i in ) of the outer track and inner track respectively depend on the nature of the tracks and terrain as well as the thrust forces; and can be determined experimentally.

Dynamic Model
The turning behavior of a tracked robot using skid steering depends on the thrust forces of the outside and inside tracks F out and F in , the resultant resisting force F res , the moment or turning resistance M res exerted on the tracks by ground, and the vehicle parameters as shown in Fig. 1 assuming the simple case of steering at low vehicle speeds. The centrifugal force may be neglected and the behavior of the vehicle can be described by the following equations of motion: where m is the mass of the robot, s is the displacement of the center of mass of the robot, I is the mass moment of inertia about the vertical axis passing through the center of mass of the robot. If the tracked vehicle is turning at high speed, the centrifugal force may be significant in this case and its effect should be taken into consideration. If the center of mass is higher than the plane forming the sprockets, load transfer between the two tracks will also occur.  The generated forces for the inside and outside tracks are required to maintain the vehicle at steady state while turning at high speed. By taking into account the friction and centrifugal forces [10] and [14] as shown in Fig. 2, the final forms of thrust forces of the inside and outside tracks are given as follows: where f r is the coefficient of resistance to motion of the vehicle in the longitudinal direction, and a y is the lateral acceleration of the center of mass of the vehicle; W is the weight of the vehicle, and µ t is the coefficient of lateral resistance. The gravity acceleration is represented by g and h, B, l and s o are the dimensions related to the vehicle as shown in Fig. 1 and Fig. 2.
The value of f r depends on the vehicle translational velocity v and the tracks contour. Also, the value of µ t depends on the nature of the ground and on the track contour.

Energy Based Variable
Structure Controller As shown in Fig. 3, it is required to move the system from location (1) to (2). The principle of energy conservation in the presence of a dissipative load is given in Eq. (6). The work done by the controller U 1→2 is less than that consumed by the dissipative load U R(1→2) and is related to the energy of the system as follows: where KE and P E are the kinetic and potential energy of the system, respectively. U 1→2 is the work done by the controller between the two positions, and U R(1→2) is the work lost to the viscous damper between the two positions. For a general system, Eq. (6) becomes: where β and α are the inertial and potential energy storage element constants, respectively. The linear dissipative element constant is represented by Γ and k is the controller gain. Then, the energy balance at any point along the trajectory of the SMC phase portrait is given by: For the linear sliding surface shown in Fig. 3, By substituting for x into Eq. (8) gives: Equation (9) relates the controller gain to the slope of the sliding surface as follows: The gain in Eq. (10) can be used for SISO control systems. An extended design methodology can be achieved to include SIMO control systems. If a fourstate-variables system with a single input is considered, two sliding surfaces can be designed as follows: where x 1 , x 2 , x 3 and x 4 are the states of the system and x 2 =ẋ 1 and x 4 =ẋ 3 . The input control signal is a relay type signal and is given by: where: where λ 1 and λ 2 are the slopes of the sliding surfaces, and k 1 and k 2 are the controller gains, η 1 and η 2 are the magnitudes of the controller gains. Finally, Eq. (10) can be used to find the magnitudes of the controller gains as well as the sliding surfaces slopes as follows: Gains in Eq. (14) show that the ranges of the SMC parameters (λ 1 , λ 2 , k 1 , k 2 ) are very wide such that additional constraints have to be imposed on the choice of the parameters. Sensitivity analysis [17] is introduced to find such constraints.

UTR Control Using VSC and Hyper Schemes
The UTR is SIMO system given that the speed is fixed; hence, the controller signal can be given in the form of the required rotational velocity of the vehicle as shown in Fig. 4. From the transnational and rotational velocities, the required angular speeds of the left and right tracks DC motors can be calculated from the vehicle kinematic equations.  The lateral position error is calculated as the difference in the y-direction between the recovery path and the current location of the vehicle with respect to the local coordinates of the vehicle. The heading angle error is calculated as the difference between the angle found from the recovery path slope and the current direction of the vehicle with respect to the world coordinates. The sliding surfaces equations are formulated as follows: where e p and e θ are the lateral position and the heading angle errors of the robot, respectively, andė p andė θ are their rates of change. λ 1 and λ 2 are the slopes of the sliding surfaces related to the vehicle position and heading angle, respectively. The proposed control law shown in Fig. 4 can be written as: where k 1 and k 2 are the controller gains for the vehicle position and heading angle errors, respectively. These gains are defined as follows: Chattering is undesirable phenomenon which is produced from control discontinuities and switching action and should be minimized or eliminated for practical controller implementations [49] and [28]. This is achieved by using a thin boundary layer on either sides of the sliding surfaces and using a saturation function instead of the sign function used in the Eq. (17). In this approach, the switching control law is replaced by a saturation function which approximates the sign term by a boundary layer neighboring the sliding surface s = 0. If the state of the system is outside the boundary layer, the control law u is chosen as before. If the state of the system is inside the boundary layer [−φ, φ], the control law u is interpolated by replacing the sign function in the expression of u by s φ .
Using Eq. (14) along with the dynamic model of the robot dynamics, Eq. (5), the controller gains are given as follows: For each of the two relationships, one of the parameters, either λ i or k i has to be chosen and the other calculated. Alternatively, additional constraints have to be found in order to guide the choice of the parameters. The system sensitivity analysis again is used here to find such constraints. The rotational velocity of the vehicle is given by: Taking the partial derivatives of the lateral position error e p and the direction error e θ of the robot with respect to the DC motors angular speeds give: In order that the UTR errors are rendered insensitive to the motors angular speed variations, the magnitude of the denominator in Eq. (20) must be larger than that of the numerators, that is: Finally, the sliding controller is designed such that both sliding surfaces of the lateral position and the heading angle which are given by Eq. (15) of the UTR are coupled together to form a single hyper sliding surface: The control law and gains are the same as in Eq. (16).

Fuzzy Potential Motion Planning System
This section discusses a new motion planning system for UGVs based on fuzzy controller. The goal of this system is to allow the robot to track a moving target in a dynamic environment with obstacles. In other words, the proposed approach provides the ground vehicle with virtual repulsive and attractive forces that regenerate real-time and dynamic trajectory that gives the robot the ability to track a target and to avoid obstacles. Mamdani inference fuzzy system is used to create the two nonlinear models for attractive and repulsive forces.
The attractive force model is provided by the relative position and velocity vectors between UGV and the target. This force defines the dynamics between the UGV and the target and can be defined as follows: where δ and δ v are the relative position and velocity between the robot and the target, respectively, with n rt and n v(rt) being unit vectors pointing from the robot to the target for position and velocity, respectively. As seen in Eq. (23), the virtual attractive force A consists of two components which are A 1 and A 2 . The first force component, A 1 , pushes the robot to the target to reduce the distance between them, where the second component, A 2 , keeps the robot move at the same velocity of the target. The attractive force for each motion direction (i.e. x and y) can be given as follows: where δ x and δ y are the relative position of the robot with respect to the target, and δ vx and δ vy are the relative velocities in x and y directions, respectively. These variables represent the inputs to the attractive fuzzy model that generates the appropriate force components (i.e. A 1 , A 2 ), which are summed for the total attractive forces A x and A y , as shown in Fig. 5. The output forces are tuned carefully by scaling parameters k p and k v . The UGV will also be provided by an extended potential force when it reaches an obstacle. Therefore, another Fuzzy model is adopted to produce appropriate repulsive forces. These forces are calculated according to the following function: The relationship between the repulsive force components in a 2D space is shown in Fig. 6, where R 2 keeps the robot away from the obstacle and second repulsive force component, R 1 , acts as a steering force. The repulsive force components in xy plane can be defined as follows: where η is a positive constant and ρ s is the shortest distance between the center of the robot and the center of the obstacle. v is a component of the relative velocity between the robot and the obstacle pointing from the robot to the obstacle as shown in Fig. 6. The deceleration, a max , is applied to the robot to reduce its velocity, then the distance traveled by the robot before v approaches to zero is defined by ρ m and is given by: Also, α is the angle between the x axis and the vector v , and it is defined as follows: where, x and y are the relative positions between the robot and the obstacle in x and y directions, respectively. As shown in Fig. 5, the inputs to the repulsive-fuzzy model are x , y , vx , and vy . Furthermore, R x and R y are the repulsive forces which are calculated according to the output (R 1 , R 2 ) of the Fuzzy model. After the calculations of the attractive and repulsive forces from Eq. (23) and Eq. (25), respectively, the overall virtual force on the robot is the summation of the calculated attractive and repulsive forces, and it is given as follows: The repulsive force model can be extended to be valid for multiple obstacles n obs , so the following relation is given: The process of designing a fuzzy logic controller is performed in three main steps. The first step is called fuzzification; in this step, the inputs are mapped onto membership functions. The membership values are then quantified from a rule base. This step is called rule evaluation. The last step is called defuzzification; here, the membership values are converted into crisp outputs.  To configure the repulsive force model, two Fuzzy inference systems were built. The first one is designed to obtain the repulsive force in the x direction and a second one for the repulsive force in y direction. Each model takes two inputs ( x , y ) which are the relative position between obstacle and UGV in xy directions. Each input, in any repulsive inference system, has five triangular membership functions with fuzzy terms (−2, −1, 0, 1, 2). Moreover, each output has 21 triangular membership functions which have 21 fuzzy terms (i.e. −2, −1.8, −1.6, −1.4, . . . , 1.4, 1.6, 1.8, 2). The fi-nal set of data consisting of 25 data points is converted to 25 if-then fuzzy rules in each repulsive fuzzy model.

Simulation Results and Discussions
In this section, the proposed control systems are evaluated and implemented for UTR. In particular, the energy based VSC scheme is adopted as a low-level controller for trajectory tracking, and the Fuzzy potential motion planning system is used as a high-level controller for free obstacle avoidance path generation. The integrated control system is illustrated in Fig. 8. The simulation environment includes 3 UTR platforms designed in CAD environment and integrated with Simulink/Matlab models using the Simmechanics toolbox.    The errors between the desired and the achieved path using the four different control methods are illustrated in Fig. 11 and summarized in Tab. 2. The results clearly demonstrate the superiority of our proposed controller over other types. The hyper VSC controller provides best performance with small errors in different slip values as shown in Tab. 2.  In order to verify the effectiveness of the VSC, another challenging scenario was simulated where the robot needs to continue following a square path while keeping its speed significantly constant, particularly at corners. In this case, a velocity of 1.5 m·s −1 was required. In this test, the sliding surface slopes were λ 1 = 1.8 and λ 2 = 18, and the controller gains were k 1 = ±4 and k 2 = ±4. With 3 % slip ratio, the performance of the UTR on a squared path is illustrated in Fig. 12. The figure shows also the results of the path tracking using different controllers. It can be seen that the UTR with hyper VSC can accurately track the desired trajectory and perform the best at sharp corners. The position error was less than 10 cm, and the steering error was less than 1 • .   Fig. 13(a) and Fig. 13(b) express the tracking trajectory of x, y and θ respectively. Furthermore, Fig. 13(c) shows the linear and the angular velocities of the robot. It can be found that the robot was able to follow the required speed of 1.5 m·s −1 along the path with small deviations in corners. Furthermore, the angular speeds at corners were fast enough to keep the tracking of steering angle of the robot.   Finally, the robot tracking ability is evaluated with a dynamic environment which included stationary and moving robots. The Fuzzy potential control system, discussed in Sec. 5. , was implemented for robot motion planning. In the simulation environment, the planned trajectory was provided for one robot platform, where two robots were used as a target and an obstacle. The motion of the robot was tested in two dif-  Fig. 14(a), the robot needed to follow a stationary robot located at point (5,5). Meanwhile, an obstacle robot was located at point (2,2) which is a potential point on the shortest line between the robot and the target. The figure shows how the robot was able to avoid collision with the obstacle and to continue tracking the target. It is clear from the figure that the robot also avoided collision with the target and stopped before reaching it. The second scenario is illustrated in Fig. 14(b), where the target and the obstacle were moving. The target was following a circular path which started at point (0, 5), and the obstacle was moving from the point (2,2) to the point (4,2). The robot started tracking the target from the point (0, 0), then when it reached the obstacle influence, it made an appropriate detouring to avoid collision. After that, the robot resumed its mission in tracking the target. Figure 14(b) shows that there are points of intersection between the paths of the main robot and the target robot. These intersections do not necessarily represent the collisions between the two robots. The Fuzzy potential control system provides less force to the main robot when it is reaching the target, which results in slowing down the robot speed (see Fig. 16) to avoid collisions. This concept also applies to the obstacle robot with the main robot. To clarify what has been mentioned, the paths for all robots were plotted over time as shown in Fig. 15. For a collision in the x-y plane to occur, the intersections between paths in the x and y axes must be met simultaneously. Therefore, as shown in the figure, the trajectories of the robots are free of collisions.  Figure 16 shows the average velocity (i.e. v = v x 2 + v y 2 ) for the target and the robot. The robot was attracted to the target quickly at the beginning due to the large distance between them. Then, it was decelerating while approaching the obstacle to avoid collision. Furthermore, the main robot was able to track the target robot.

Conclusion
This work proposed new control approaches for UTR considering a nonlinear and dynamic motion models.
In particular, the energy based VSC scheme is adopted as a low-level controller for trajectory tracking. In order to provide a fully integrated robot control system, a high-level motion planning system also proposed for free obstacle-collision path generation. For these purposes, a new approach Fuzzy potential motion planning system was designed and implemented. The UTR model is highly nonlinear and the effect of the slip of the tracks makes it impossible to exactly model the system. The obtained simulation results proved best performances for the low-level variable structure controller which also presented superiority over the other control methods. The robot was able to effectively track different trajectories and velocity profiles, achieving relatively small errors. Furthermore, the motion planning for the robot was tested in different scenarios, where the target and the robot are stationary or moving. The results presented high efficient planning system for obstacle avoidance and target tracking.
This work opens future orientations to implement the proposed integrated control system on real UTR platforms. For a fully autonomous robot, a visual based feedback guidance system can be added for mapping and localization.