LiDAR Based Trajectory-Tracking of an Autonomous Differential Drive Mobile Robot Using Fuzzy Sliding Mode Controller

This study aims to achieve the trajectory-tracking of an autonomous differential drive mobile robot (ADDMR) in the presence of friction torques using the proposed fuzzy sliding-mode control (FSMC). First, the complete model of the ADDMR is established including kinematic, dynamic and actuator models. Then, the desired trajectory is planned and generated via Bézier curve combined with cubic time mapping. The localization of the ADDMR is performed using the Monte Carlo Localization (MCL) technique and LiDAR point cloud data. Subsequently, the proposed FSMC utilizes nonlinear sliding surfaces inferred based on fuzzy logic to asymptotically attain the pose convergence. To investigate the feasibility and robustness of the proposed FSMC, simulations and experiments are conducted under different operation conditions. The results show that the proposed FSMC using LiDAR data in the feedback leads to excellent control performance even in the presence of friction torques. It was also shown through simulations and experiments that the established ADDMR model fits the actual ADDMR model very well.


I. INTRODUCTION
O NLINE shopping has been drastically increasing over the past year, especially during COVID-19 pandemics [1], [2]. Factories are racing to equip their warehouses with automated logistic handling technologies [3], [4] or contactless delivery service [5], [6]. For these reasons, there has been numerous research on the control of mobile robots. The commonly used configurations are robots driven by wheels speed differences and they are controlled to follow a prescribed path from initial to final position in a determinate time. The type of robots driven by wheel velocity differences are called Differential Drive Mobile Robot. The challenge of controlling Autonomous Differential Drive Mobile Robots (ADDMRs) comes from its nonlinearity. Specifically, its nonholonomic property constrains its side movements. Many control techniques are proposed to handle it, but the important issue is that most of them only consider kinematics [7]- [22]. It is understandable because the consideration of its dynamic model will increase the complexity and dimensionality of the model which leads to difficulties for a controller design standpoint. However, kinematics does not consider mass and when an ADDMR is driven at high speed, its dynamic model becomes a necessity in addition to the kinematic model. Thus, some controller design techniques have been taking the dynamic model of the ADDMR into consideration. For example, the works in [23]- [34] considered both kinematics and dynamics for their vehicle models. A significant task for the above-mentioned works is trajectory tracking in which the robot must follow a predefined path. The studies in [24] and [28] have positioned their ADDMRs on the path initially. Keighobadi and Menhaj [24] stated that their proposed method could not fully compensate initial position outside the path. In [25]- [27], the results demonstrate tracking error convergence at arbitrary initial positions; however, the circular path just has a constant velocity reference. Xu et al. [27] showed that it is important to consider speed transition of the robot movement. In most previous works, the ADDMR was given a path without time constraints; therefore, the ADDMR approaches each point in the path incrementally. This is an important issue that the time arrangement along the planned path can lead to speed variations that the ADDMR has to follow.
The most recent developments in trajectory-tracking using fuzzy logic [14]- [22], [24], [30], [33]- [35] and sliding mode control techniques [13], [29]- [32], [35]- [39] are the motivation behind the work presented herein, which takes advantage of findings from both methods. Therefore, the FSMC based on a LiDAR point cloud data is proposed here to control an ADDMR system to follow a trajectory at prescribed time from arbitrary initial positions under torque disturbances. The contributions of this work are as follows: 1) A comprehensive ADDMR model consists of the kinematic and dynamic models is presented here. The model is augmented with the actuator dynamics for a more realistic representation of the motion of the ADDMR which has not been addressed by Refs. [7]- [36]. 2) A desired trajectory is planned using Bézier curve.
The n + 1 way-points on the trajectory are determined using an nth order Bézier curve. Unlike work in [27], the dynamic velocity command is embedded in the trajectory itself rather than in the control process. The advantage of the proposed approach is that it lends itself to possible combination with other control techniques in the closed-loop control design process. It also enables the trajectory to have zero velocity at the beginning and the end whereas previous works, such as those in [7]- [36] did not consider this. The closeloop performance of the ADDMR using the proposed control technique is validated experimentally using an 8th-order Bézier trajectory with cubic time mapping for the time arrangement along the path. 3) A fuzzy logic control design with 9 inference rules only combining a sliding mode control scheme leads to effective closed-loop control of the ADDMR. Fuzzy logic control offers multi-input, multi-output, model free controllers for trajectory tracking (see for example. [21] and [22]). In addition, the interpolative reasoning provides resilience against noisy measurement in the ADDMR system. However, unlike existing unlike existing techniques (e.g. [14]- [22], [30], [33]- [35]) where complex inference rules and membership functions are used, , relatively simpler inference rules are used in the proposed control scheme. These lead to computational cost reduction and provide flexibility in the choice of computing devices used in the AD-DMR testbed; a major advantage in the implementation phase. 4) LiDAR based Monte Carlo Localization (MCL) is used for feedback control to improve the control performance of the closed-loop system. The common ap-proaches described in the literature (see for example [12], [29], [31]- [34]) use angular acceleration and torque feedback which renders the implementation a little more challenging. Instantaneous measurement of the orientation and position are easily accessible and implementable on an actual testbed [22]. Therefore, a LiDAR is used here for pose feedback due to its accuracy compared to GPS or odometry sensors by Refs. [40]- [43]. 5) Experiments and simulations are conducted to validate the theoretical developments. Both the simulations and experiments are conducted using different initial conditions which have not been done in previous works (see for example [10], [12], [13], [16], [17], [19]- [21], [23], [23], [25]- [33], [35], [36]). The rest of the paper is arranged as follows: In Section 2, the system description, modelling, and problem formulation are given. In Section 3, the trajectory planning, pose measurement, and FSMC design are discussed. Discussions of the simulation and experiment results are presented in Section 4. Finally, concluding statements are made in Section 5.

II. SYSTEM DESCRIPTION, MODELING, AND PROBLEM FORMULATION
The hardware configuration for the ADDMR experimental platform is described here for which the equations of motion will be derived. The system modeling is divided into 3 subsystems, i.e., kinematic, dynamic, and actuator models. The details of each subsystem are explained in the following subsections. The problem formulation for the ADDMR is subsequently delineated using the equations of motion of all subsystems.

A. SYSTEM DESCRIPTION
The experimental platform of the ADDMR and its diagram are shown in Fig. 1. The main CPU from ADLINK AmITX-SL-G with Intel i5-6500TE and 8GB Random Access Memory (RAM) is where the proposed controller is computed and all the data from the STM32F microcontroller is stored. Two A4950 DMOS Full-Bridge motor drivers generate voltages to actuate two DC motors responsible for the motion of the robot. The provided sensors are Quadrature hall effect encoder and Inertial Measurement Unit (IMU) used to measure the corresponding position and orientation of the ADDMR; A LiDAR is also provided for measurements of distance around the robot. The wheels are centered and with center of mass is halfway in in between. In addition, two castor wheels are attached for the balance of the ADDMR.

B. SYSTEM MODELLING
The ADDMR is a type of mobile robots that the speed differences for two actuated wheels cause forward, backward, or turning movement. The illustration can be seen in the following  origin of body frame {B} is located on the center of mass of itself as described in Fig. 2. The orientation of the robot is θ with respect to the inertial frame which is calculated by taking angle measurement between the longitudinal axes of body frame and inertial frame. The linear and angular velocities of the ADDMR are calculated with respect to the point o, i.e. the origin of body frame. Assume that there is a point of contact between each wheel and the floor, there exists resulting tangential velocities from their angular velocities (φ r ,φ l ). The angular and linear velocities of the wheels are proportional to the ratio of the wheel radius R. Hence, the ADDMR's velocities with respect to the inertial frame can be obtained from its wheel angular rates [44]- [48] as where θ is the orientation of the ADDMR. Moreover, the relationship between the velocity of mass ( Bẋ ,θ) with respect to body frame and wheels' angular velocities are given by: In addition to the kinematic model of Eqs. (1) and (2), the Newton-Euler approach is used to build up the dynamic model of the robot. The ADDMR is assumed to have a unified rigid body and the equations of motion are determined with respect to reference {B} as shown in Fig. 2. Moreover, two motors provide the ADDMR with longitudinal wheel forces (F lwx , F rwx ) created by their corresponding torques (τ l , τ r ) [45], [46], [49]. Hence, the linear and angular accelerations of the robot with respect to the body frame can be represented as That means that the dynamic model of the ADDMR generates linear velocity ( Bẋ ) and angular velocity (θ) after integration when motor torques τ r and τ l are applied.
Combining the aforementioned kinematic with dynamic models, the actuator model is derived to complete the system's equations of motion. However, designing the corresponding controller is very challenging due to the high dimensionality of the system's model. The actuated system is comprised of two identical DC motors and the relations of voltage and torque of the motor are formulated as follows: where R a , L a , V a , i a , and N are armature resistance, inductance, voltage, current, and gear ratio, respectively. The back emf e a is proportional to the rotational velocity ω m with back emf constant K b . Moreover, the motor torque τ m is proportional to the armature current i a with constant K t and the output torque τ is proportional to τ m with gear ratio N . Based on Eq. (5), an actuator model can be obtained. In the subsystem, the two respective inputs are voltage V al of the left motor and V ar for the right motor. Here, τ r and τ l stand for torques of the left and right motors which are the outputs of the actuator model. From the kinematic, dynamic, and actuator models, a comprehensive model is built for subsequently designing the trajectory-tracking controller. In addition, τ dr and τ dl which denote torque disturbances, are added to the dynamic model to investigate the robustness of the closed-loop control system when the proposed controller is applied.

C. PROBLEM FORMULATION
The objective of the proposed FSMC is to control the ADDMR from an arbitrary initial position along a desired trajectory in the presence of torque disturbances τ dr and τ dl . The pose information obtained by an MCL technique using LiDAR information to form nonlinear sliding surfaces. Then, the nonlinear sliding surfaces are reasoned to satisfy the reaching law (i.e., σσ < 0, where σ is a sliding surface) via the designed fuzzy inference rules. When control law is VOLUME 4, 2016 applied to the ADDMR system, the tracking errors asymptotically converge to zero. The overall control block diagram is shown in Fig. 3.

III. TRAJECTORY PLANNING, POSE MEASUREMENT, AND FSMC DESIGN A. TRAJECTORY PLANNING
It is common in the robotics community to differentiate between path and trajectory. A path is a spatial function that connects an initial pose to a final one where as a trajectory is a path which is a function of time [50]. In this work, a path is a spatial function of arc length s where s = 0 is the initial pose and s = 1 is the final pose. One of the well-known ways to generate paths is Bézier curves where the path is described using a parametric equation. The n + 1 parameter points P i with i = 0, 1, . . . , n are used to form an nth-order Bézier curve B(s) as: where B n i (s) is a Bernstein polynomial given by and n i is a polynomial coefficient given by Because a Bézier curve only goes through the first and the last parameter points, the other points are used to determine the shape of the curve. These points V i are spaced points in an equally divided arc length such that This yields a Bézier curve that will always go through all the V i . The polynomial form of the Bézier curve in Eq. (6) can be expanded into a matrix form of where A(s) is an 1 × (n + 1) column vector that maps parameter points P i to Bézier points at arc length s. The corresponding P i to the designed V i can be calculated by expanding (9) as follows: To form a smooth trajectory from the planned Bézier curve path, a time mapping function of the arc length s(t) is designed using a cubic polynomial as follows: where the coefficients a i , i ∈ {0, 1, 2, 3} are calculated based on the intended initial and final conditions of s, anḋ s. By combining the path with a cubic time mapping, the desired trajectory is obtained for tracking control by the ADDMR system.

B. POSE MEASUREMENT
To localize and control the ADDMR, its pose measurement for feedback is required. In the experiment, in addition to odometry, MCL is used to improve the accuracy of the pose measurement via a LiDAR. The MCL technique is inspired by a recursive Bayes filter based on importance sampling to estimate probability of robot poses using sensory measurement data [51]. The states of the robot can be defined in a probabilistic approach with the help of two distribution models i.e. motion and observation models [52]. The motion model p(x t | x t−1 , u t ) represents the transition of probability distribution of state x at time t based on the previous state x t−1 and the control input u t . The observation model p(z t | x t ) incorporates reading of sensors z t to calculate the likelihood. The whole MCL algorithm is described in Table 1. The continuous belief of states bel(x t ) in MCL is represented by a discrete number of M particles [53]. The weight w (i) t is approximately proportional to the probability of robot at states x (i) t . The initial belief bel(x 0 ) is uniformly distributed. Hence, the initial weights w 0 are also uniform and the states x 0 are initially sampled from all possible state values with uniform distribution [54]. Line 4 in Table 1 propagates the particles using the system dynamics to generate hypothetical state x t . This process represents a sampling from distribution p(x t | x t−1 , u t ). Line 5 in Table 1 calculates the likelihood representing measurement probability z t from particle x In this study, LiDAR beams are utilized to measure the likelihood of each particle. Line 6 in Table 1 assigns the states and likelihoods to a temporary particlesX . Line 8 in Table 1 commonly referred to as re-sampling step in which j ∈ [1, M ] integers are drawn proportionally to the likelihoods w t . The newly selected particles are assigned to the reference of j in Line 10. Particles with bigger likelihood are more likely to be drawn multiple times. Hence, more particles are distributed to the most probable states. ine Monte Carlo Localization Algorithm ine 1: Input: X t−1 , ut, zt 2: Output: Xt 3: for i = 1 to M do 4: x draw new indices j = {1, 2, . . . , M } with probability ∝ wt 9: for j = 1 to M do 10: assign x (j) t to Xt 11: endfor ine Here, a simulated example is displayed. The result shown in Fig. 4 indicates particles (blue dots) convergence history. The robot moves in a counterclockwise direction and the position is marked in green dashed line as a ground truth. The mean position of particles is taken as a predicted location and marked by an orange line. As seen from the figure, the particles gradually concentrate and converge to the real location of the robot.

C. FUZZY SLIDING-MODE CONTROL DESIGN
Based on the established comprehensive model and localization of the ADDMR, the proposed FSMC is designed to control an ADDMR to track the desired trajectory. The proposed FSMC has two inputs; i.e., heading error e θ and position error e p , and two outputs; i.e., V al and V ar . The details are further described in the following subsections.

1) Nonlinear sliding surfaces design
Prior to designing the FSMC, nonlinear sliding surfaces are first designed. The distance error e p and heading error e θ information are defined to construct nonlinear sliding surfaces. To clearly understand how both errors are calculated, a geometrical schematic shown in Fig. 5 describes the relation between the inertial frame {I}, robot frame {B}, and the planned Bézier trajectory T = B(s(t)). Vector I P B and I P T are the robot position and trajectory position relative to the inertial frame. The trajectory position relative to the body frame is denoted by B P T . In this way, it can be calculated by  13) and the position error is the magnitude of B P T as follows: Using Eq. (13), the heading error e θ is where atan2(·) is a full-quadrant inverse tangent with output between [−π, π] [55]- [58]. The function atan2(·) is preferred over the tan −1 (·) = atan(·) function for converting Cartesian coordinates to their corresponding polar coordinates. The control objective is to make e p and e θ asymptotically converge to zero. Firstly, the following nonlinear sliding surfaces s p and s θ constructed by tracking errors are designed. Here, the sliding surface s p corresponds to trajectory tracking and s θ corresponds to robot's heading tracking represented as where k p , k θ , f p , and f θ are positive constants and sign(·) is a signum function [13], [15], [21], [26], [29], [31], [32], [35], [36]. Eq. (16) can be proved that once the sliding surfaces approach to zero, the corresponding tracking errors also approach to zero. Then, the fuzzy logic is proposed to regulate nonlinear sliding surfaces towards zero and the details are described in next subsection.

2) Fuzzy inference logic design
In fuzzy inference logic design, the sliding surfaces are inputs and the actuator voltages are outputs to control the left and right wheels. In the beginning, sliding surfaces are normalized using gains k np and k nθ yieldingŝ p = k np s p andŝ θ = k nθ s θ . The normalized values are then evaluated using triangular fuzzy membership functions shown in Fig.  6. For each sliding surface, there are 3 membership functions comprising {N : N egative, Z : Zero, P : P ositive}. In addition to the fuzzified inputs, this work also uses fuzzified voltage outputs for each actuator. The fuzzification of the outputs are the triangular type with 3 linguistic voltage representations {N : N egative, Z : Zero, P : P ositive} shown in Fig. 7. Then, fuzzy rules are designed in the inference engine as follows: The t-norm AN D operator in this work is M IN (·) function. The consequence of every rule is aggregated using t-conorm M AX(·) function as The complete list of the rules is displayed in the following Table 2. A crisp value from each fuzzified voltage is obtained ine by means of the centroid method expressed as follows: Based on the proposed FSMC design, discussion of the corresponding simulation and experiment results is presented in the next section.

IV. DISCUSSION OF SIMULATION AND EXPERIMENT RESULTS
The proposed method applied to the ADDMR system is tested to follow the planned Bézier trajectory. The trajectory is designed using the following via points as The via points are chosen to mimic a sinusoidal path. Using Eq. (11), we find the parameter points and the corresponding matrix form of the Bézier curve as follows: The resulting path can be seen in Fig. 8. To follow a speed variation in the trajectory, the path is designed to end at 60 s with a nonlinear arc length function as follows: (22) and the evolution of s(t) andṡ(t) can be seen in Fig. 9. In practical situation, there are some torque disturbances generated from frictions or uneven floors. In order to investigate this effect, friction torque τ f and uncertainty torque τ u are applied to the ADDMR control system. Both disturbances are formulated as follows: The friction torque is based on friction model with stribeck effect [59], [60] where T v , T c , T s , andφ s are viscous friction, coulomb friction, stiction friction, and stribeck velocity constants, respectively. The friction model gives variation of disturbance torque with respect to angular velocity of the wheelsφ. Furthermore, the uncertainty torque gives variation of disturbance torque with respect to time which includes low and high frequency components. The system and control parameters in the simulation are listed in Table  3    It is clearly seen from Fig. 11 that the proposed FSMC can achieve excellent trajectory-tracking of the ADDMR despite VOLUME 4, 2016 the presence of disturbance torques. Similarly, the orientation tracking is also good and displayed in Fig. 12 where the rise time of e p is 2 s and the rise time of e θ is 1.26 s. Moreover, Fig. 13 shows that the sliding surface of case 1 converges to zero as the robot successfully follows the planned trajectory. The provided simulations validate that the proposed method is capable of controlling the ADDMR to follow the planned trajectory under torque disturbances.
In the experiments, the proposed FSMC is implemented on ADLINK NeuronBot. The robot follows the same trajectory at the same initial conditions as the simulations. The implementation result can be seen in Fig. 14 where the trajectory-tracking is satisfactory despite the presence of the floor disturbances. Compared to the simulations shown in Fig. 11, the curvatures are smaller when approaching the desired trajectory. This is because the robot operates indoors and its speed limit is needed for safety consideration. As seen in Fig. 15, the linear velocity of the robot does not exceed 0.4 m/s. As a result, e θ is reduced much faster than e p as shown in Fig. 16 where the robot made a sharper move compared to the simulations. Furthermore, it can be seen from error responses that the robot completes the trajectory-tracking in a specified time.

V. CONCLUSIONS
In this study, the FSMC technique is proposed to control an ADDMR subject to torque disturbances. In addition, the comprehensive model of the ADDMR consisting of kinematics, dynamics, and actuator dynamics is established. The trajectory is planned via Bézier curve. In addition, the pose measurement of the ADDMR is measured through MCL technique based on a LiDAR point cloud data. The numerical and experimental validation results prove the effectiveness and robustness of the proposed control scheme. HSIU-MING WU received Ph.D. degree from department of electrical engineering, National Taiwan University of Science and Technology in 2009. Next, he served the Opto-Mechatronics Technology center, National Taiwan University of Science and Technology as a postdoctoral fellow. In early 2013, he worked in Texas A&M University at Qatar as a postdoctoral fellow for 4 years. In early 2018, he joined in the department of mechanical engineering at National Chin-Yi University of Technology in Taichung for two years. Currently, he is an assistant professor in the department of intelligent automation engineering, National Taipei University of Technology. His research interests include mechatronics, nonlinear control, and robotic control. VOLUME 4, 2016