Fast nonlinear model predictive planner and control for an unmanned ground vehicle in the presence of disturbances and dynamic obstacles.

This paper presents a solution for the tracking control problem, for an unmanned ground vehicle (UGV), under the presence of skid-slip and external disturbances in an environment with static and moving obstacles. To achieve the proposed task, we have used a path-planner which is based on fast nonlinear model predictive control (NMPC); the planner generates feasible trajectories for the kinematic and dynamic controllers to drive the vehicle safely to the goal location. Additionally, the NMPC deals with dynamic and static obstacles in the environment. A kinematic controller (KC) is designed using evolutionary programming (EP), which tunes the gains of the KC. The velocity commands, generated by KC, are then fed to a dynamic controller, which jointly operates with a nonlinear disturbance observer (NDO) to prevent the effects of perturbations. Furthermore, pseudo priority queues (PPQ) based Dijkstra algorithm is combined with NMPC to propose optimal path to perform map-based practical simulation. Finally, simulation based experiments are performed to verify the technique. Results suggest that the proposed method can accurately work, in real-time under limited processing resources.

Introduction

In recent years, unmanned ground vehicles (UGVs) have been applied extensively in agriculture, construction, mining and military operations, etc. The UGVs have lightweight, fast speed, low energy consumption, high maneuverability, convenient drive, and simple mechanism[1-3]. Consequently, these advantages make UGVs suitable for indoor[4,5] and outdoor[6,7] applications. However, the ground vehicles can be perturbed due to uncertainties in models, and description of the context of operation[8], skidding and slipping[9], noisy sensor’s measurements,[10], and failures in electromechanical components[11], making the navigation and control of the platform a challenging task[12,13].

Motion planning and control are the two crucial factors for a vehicle to complete an assigned task safely and effectively[14]. First, it should navigate in an environment with obstacles. Second, it must satisfy the requirement of reaching a specified goal, constantly dealing with perturbations[15]. Thus, the first requirement implied using a path-planner to plan the vehicle’s motion, while the second requirement can be treated by a controller to control the kinematics and dynamics of the vehicle.

In the past few decades, research on motion planning and control of UGVs has witnessed remarkable development. In particular, trajectory tracking and target posture stabilization problems on kinematic models were investigated[16-19]. Additionally, many researchers developed motion control at dynamic levels (such as torque controllers[20,21] or voltage controllers[22,23]). Moreover, there are some methods developed on model predictive control (MPC), which considers the constraints (such as[24-26]). Similarly, many path-planners and path-followers were developed to reach the specified final location safely[27-30]. From the perspective of UGVs, some studies consider the presence of disturbances, and uncertainties[31-33]; some others assume pure rolling without skid-slip disturbances[34,35]. In addition, some autonomous system use safe switched tracking[36] and neural network based techniques[37]. Thus, for practical applications, it is important to consider those disturbances.

Overall, most of the existing techniques mentioned above assume that the UGVs satisfy pure rolling conditions (without skid-slip). However, pure rolling may not always hold due to the terrain, tire dynamics, voltage drive circuitry, etc. Thus, making the control problem challenging for the UGVs in the presence of external disturbances and skid-slip. Nevertheless, on the other hand, it allows solving these challenges with advanced planning and control strategies. In[38], kinematic models that are explicitly linked to skid-slip are presented, which considers the design from a controller’s perspective. An analysis of the hybrid MPC for stabilization of robot under the presence of wheel slippage is discussed in[39]. In[40], a nonlinear disturbance observer is used for a self-balancing mobile wheeled mobile robot. Additionally, this work focuses on a robust tracking control problem with unknown disturbances. In[41], an NDO and extended Kalman filter (EKF) are combined to perform the trajectory tracking control for wheeled mobile robots. First, a kinematic model excluding perturbation and distorted dynamic model is discussed. Then, NDO is used to observe the external disturbances, and EKF is used to estimate the platform’s states. In addition, robust constrained control is developed by observing the disturbances using disturbance observer in[42].

In terms of path-tracking control problem, an obstacle avoidance mobile robot is introduced in[26]. Firstly, a CasAdi-based[43] single shooting NMPC path-planner is designed. Secondly, a velocity-based virtual control and saturated torque control are designed to apply control actions to the mobile robotic platform. Finally, an extended state observer (ESO) is designed to estimate the disturbances. This technique only considers a single shooting (SS) optimization problem, which requires a larger prediction horizon to compute the NMPC-based path planner. A higher prediction horizon can be understandable in the presence of a complex control problem. However, in the simulations, only a simple point with one dynamic obstacle is avoided. The processing time of the onboard computer is not considered in the simulations, which can cause an issue in the real experiment with the emphasis on the processing time per iteration of the computer. One way to fix the problem is to introduce the multiple shooting(MS) optimization problems, which takes a small prediction horizon. Alternatively, the proximal averaged Newton-type method of optimal control (PANOC) can be used[24]. In terms of voltage control strategies, an adaptive perturbation rejection technique is proposed in[44]. Although they have not considered skid-slip explicitly, the proposed voltage control strategy can solve the trajectory tracking control problem.

This paper focuses on the tracking control problem of a UGV to safely reach a goal location in the presence of moving and static obstacles, skid-slip, and external disturbances. An NMPC-based path-planner, voltage driving control, kinematic and dynamic control, and NDO are used to stabilize the target posture, while PPQ-Dijkstra is used to generate optimal path for the map-based simulation, which is also used in[45]. The main contributions of this research are: (1) In contrast to[26], the proposed NMPC-based path planner is designed to propose a suitable trajectory to achieve the goal location and avoid the vehicle from colliding with multiple dynamic and static obstacles. Additionally, the MS optimization problem and voltage control strategy are proposed in this paper, which improves the processing time of a single or multicore CPU. (2) A novel EP tuning method is used to tune the gains of the KC to generate velocity commands to the dynamic controller. A dynamic controller and NDO are then proposed to provide voltage-based control actions to the UGV and compensate for the lumped disturbances, respectively. (3) An investigation is performed to measure the single-core and multi-core CPUs processing time per iteration using the proposed MS-based NMPC with the SS method in[26]. In this paper, the proposed control scheme is designed to cope with the dynamic environment. To verify the working of the proposed scheme, extensive simulations have been carried out to show the vehicle’s behavior in the presence of disturbances in a realistic framework.

Problem formulation

In this paper we use dynamic and kinematic models which consider disturbances, including skidding and slippage; those models are the following ones[46,47]with,Consider the UGV represented in Fig. 1 is driven by two DC motors attached on each side with same mechanical properties with . An expression for the angular velocities of motor could have the following expression:For simplicity, we have assumed that the torque generated by the motors are explicitly depended on the armature currentwhere is the torque constant. Now, we can obtain the armature voltage as followswhere . Now, we can convert our torque dynamics into the voltage driving circuit by using the above expression as follows:

Figure 1

UGV configuration.

Methods

In this Section, first, an NMPC-based path planner is to generate reference trajectory to reach the current destination pose, additionally satisfying the collision avoidance constrain. Second, an EP-based gain tuning method is designed to generate KC-based velocity commands. Third, a dynamic control is designed with the NDO to mitigate the effects of disturbances and treat the skid-slip effect. Finally, the stability analysis of the proposed controller is discussed. Figure 2 illustrates the overall close-loop structure of the proposed technique.

Figure 2

Proposed planner and control structure diagram for the UGV.

NMPC-based path planner

we will consider NMPC-based path planner to generate a reference trajectory for the UGV from a starting pose q(0). In particular, the the presences of static and dynamic objects are considered in the environment of the UGV. For the reference posture of the UGV, we consider that the platform operates free of skid-slip, which corresponds to the assumption of pure rolling. Thus, by using the pure rolling, the kinematics and dynamics of the UGV can be expressed as[48]where and . Until now, we have taken we have modelled the platform by a continuous time model. as a continuous-time system. However, it is essential for a computer to generate discrete signals. Therefore, by using Euler’s approximation with , we can approximate our nonlinear continuous-time system by a discrete-time one, as followsThe discrete indexing , while is a set of positive natural numbers.

In terms of collision avoidance, the UGV must move with some restrictions. Therefore, we have set constraints on the platform’s physical position as follows:where obstacles are ; I represent number of currently known obstacles. Additionally, constraints on the velocities and accelerations are considered, as follows:Now, we can compute the cost-function for our NMPC-based path planner as follows:subject to following constraintswhere , , , The nonlinear problem proposed in (10) and (11) can be transformed into optimal control problem by using the multiple shooting numerical method proposed by[43]. Alternatively, the single shooting method, as proposed in[26], can be used. However, it would be computationally more complex.

Kinematic controller

The evolutionary strategies narrow the degree of freedom in the instantiating of meta-heuristic algorithms. Thus, the main idea here is first to take the error between the reference and actual positions and orientation.Now, by taking the global transformation mentioned in[49], we take the following posture error of the real UGV from (12):The time derivative of the tracking error of (13) is as followsNow, a back-stepping method (as that presented in[49]) is used to get a control signals excluding skid-slip.

In EP, we only require to know the fitness function of an individual population. Consider P as a population of all parameters involved in Eq. (1) and . Now, the main idea here is to find sub-optimal scaling factors and . These sub-optimal scaling factors can be achieved by applying the EP searching algorithm shown in Algorithm 1.The error function of velocities can also be computed as follows

Dynamic controller with NDO

The dynamics in Eq. (2) can be transferred into the followingwithThe parameter M is unknown. However, the nominal is known, and taking the velocity tracking error , we havewhere . In addition, we can take to handle the saturation constraints. This will also gives an intuition that is related to unknown skid-slip. Therefore the above expression becomes as followsTo accomplish the NDO, we first need to lumped the estimated disturbances as followswhere L(q) is the observer gain. Now, we can define the auxiliary variable to obtain an improved disturbance observer design as follows:where the function is selected from[50].where K is the constant matrix. Now, by taking the time derivative of (22)By combining the expressions Eqs. (21) and (22)Thus, we select the observer gain matrix as . Now, using Eqs. (19) and (22) the expression can be re-written as follows:For the system proposed in (26) the applied torque can be achieved as follows:The stability analysis of the observer is performed as per[26]. Therefore, we will discuss the closed loop stability analysis of the controller scheme in the following subsection.

Stability analysis

To provide the stability analysis of the closed loop system in Fig. 3, we will first define an important assumption as follows:

Figure 3

(a) Path-tracking of the UGV with a birds eye-view of the UNSW map. (b) Collision avoidance of incoming MO as well as the prediction of future state. (c) Applied longitudinal and angular velocity of the UGV. (d) Tracking error representation of the vehicle’s actual states.

Assumption 1[49]

Consider a first order continuous differential variable , which has a limit with . Thus, the second derivative of does existed and bounded for all such that first derivative holds the condition .

Consider the model of UGV Eqs. (1) and (2), the posture control error Eqs. (17) and (20), the NDO Eqs. (21)–(26), and controllers Eqs. (5) and (27). Then a compensated error-inequality as followscan be refined and we can find error-based asymptotic stability.

while,.Consider the following Lyapunov functionwhere . In addition, the Lyapunov function and when and . Now, by taking the following derivativeLet the function of Eq. (20) has an error , and controller of Eq. (27), we can differentiate V2 and getIt is important to notice here that skew symmetry property makes the first term zero, which allows us to rewrite the aswhere is the maximum NDO estimation error. Now, we can substitute the inequality and the derivatives of the velocity error in Eq. (17), we haveNow, we will introduce EPKC from Eq. (22), defining and substitute in Eq. (32).In which it can be seen that the first two terms of the above equation are negative, which also guarantees negative as long as the following is satisfiedTherefore, we can write-down the velocity error between EPKC and the estimation velocity asThe error functions is bounded, therefore, both and are bounded. In addition, can be obtained from the Assumption 1. Thus, it can be concluded that posture and velocity errors of the UGV are asymptotically stable.

Results and discussions

In this section, we discuss the simulation-based experiments performed in MATLAB. The physical parameters of UGV are set as follows: , , , , , . The skid-slip are considered as . The external torque-based disturbance .

For NMPC gain tuning, we have selected the weighting matrices , , and . The prediction horizon of for the simulations related to path-tracking, and , the simulations related to tracking a circle. Additionally, the sample time of is selected. The input torque is saturated and assumed to be and . Finally, voltages on the right and left motors are , , and , respectively.

Simulation 1: Path-tracking in a large map using PPQ-Dijkstra

In this path-tracking experiment, the tracking process is combined with a 2 DoF optimal planner (PPQ-Dijkstra), and considered the University of New South Wale’s evacuation map. The map has a size of (1350 m 600 m) which consists of the buildings of the campus. The intuition behind the experiment is to avoid moving obstacles (MOs) and static obstacles (SOs) in a large map. For this purpose, three destinations are provided as shown in the Fig. 3a, while UGV able to select for selecting the shortest path from the PPQ-Dijkstra (which can deal with multiple destinations). The color lines in the Fig. 3a represents how expensive it is to reach the goal. For example, red has the highest value of 300 which refers to a higher cost, so it is necessary to follow the vehicle to follow blue color (0 value) in order to follow the lowest cost. The NMPC is looking after the non-holonomic constraints of the vehicle as well as the collision avoidance. Figure 3b represents the UGV avoiding the incoming MO, additionally, providing the prediction of the vehicle’s state.

In terms of the performance of the proposed scheme, Fig. 3c represents the velocity profile applied to the real-platform. Clearly, the chattering in both longitudinal velocity and yaw rate represents the collision avoidance in the environment. As far as tracking errors are concerned, Fig. 3d shows that the actual states of the UGV has a good accuracy.

Simulation 2: Circle trajectory tracking

For this simulation experiment, we have considered the following expression to form a circle:where , for the above expression, the initial conditions are aligned with the reference trajectory as .The prediction horizon for this experiment is .The input constraints are selected differently for this experiment. We have the following input constraints:

We have considered four SOs and three MOs as shown in the Fig. 4. The SOs are located at , , , and . All the SOs are having a radius of 0.5m. For MOs, the starting locations are , , and , while these MOs are subject to linear obstacles with linear velocities of , and , respectively. The intention for this simulation is to avoid collisions and follow the reference trajectory. For first collision avoidance, the UGV predicts SO and MO to divert from the reference trajectory as shown in the Fig. 4a. Since, the longitudinal velocity of the vehicle is , it increases speed to avoid the MO safely as shown in the Fig. 4b. In particular, this is an example of a vehicle over taking another vehicle in the environment. The UGV tracks back on the proposed nominal trajectory, as shown in the Fig. 4c, in which the achieved platform’s trajectory does converge to the nominal trajectory, after a short transient during the collision avoidance event. Soon after synchronizing with the nominal trajectory, the platforms detects the presence of an unexpected SO, which requires another collision avoidance manoeuvre, as shown in Fig. 4d. In Fig. 4e the platform properly continues the trajectory tracking process, for finally dealing with a second incoming DO and an additional SO, both requiring an avoidance treatment, which is successfully performed, for finally completing the required trajectory as shown in Fig. 4f.

Figure 4

(a) Avoided first SO and MO by predicting them. (b) Speeds up to safely overtake the incoming vehicle. (c) Catching up the RT (d) Avoiding another SO and predicting incoming MO to slow down. (e) Approaching to the goal location. (f) Safely reached to the goal.

In terms of meeting the input constraints imposed by the NMPC, Fig. 5 illustrates the applied velocities and accelerations to the UGV. The reference velocity is considered as 0.7m/s, which the vehicles tries to match. However, since the environment is filled with obstacles, it can only reach the reference velocity once the obstacles are avoided as shown in the Fig. 5a. Additionally, the longitudinal velocity increases in the time instances , which indicates avoidance of obstacles safely and then converging to the reference velocity. For the angular velocity, a similar trend can be observed in the Fig. 5b. For the rate of change in the longitudinal and angular velocities in Fig. 5c,d, chattering are visible in the signal. However, it always converges to zero once it meets the reference trajectories. The reason for having chattering are due to the continuous occurrence of multiple obstacles, which diverts the UGV from it’s nominal trajectory.

Figure 5

(a) Longitudinal velocity applied to the UGV in the simulation experiment 3. (b) Angular velocity applied to the UGV in the simulation experiment 3. (c) Rate of change in the longitudinal velocity applied to the UGV in the simulation experiment 3. (d) Rate of change in the angular velocity applied to the UGV in the in the simulation experiment 3.

In terms of trajectory tracking error, we have computed a case with no obstacles and compared it with the one, which we have simulated in this experiment as shown in the Fig. 6a. The mean error for both situations are respectable. However, it is appreciable that without obstacles the tracking error is very small, while the error with obstacles are still having reasonable values. Additionally, for control action applied to the UGV, the applied voltages are converging to a steady-state as shown in the Fig. 6b, which proves that motor would work adequately despite these challenging conditions of obstacles and skid-slip.

Figure 6

(a) Box plot to distinguish the actual state tracking errors; where pose (x,y) are in c m and heading is in rad. Additionally, and represents obstacles case. (b) Applied voltages to the UGV in the simulation experiment 3.

Performance evaluation of the proposed method

To validate the robustness of the proposed method, we have evaluated our method with[26]. The comparison is performed as per the error accuracy, capability of the NMPC to run on a single core and multiple cores CPU, and average processing time of the NMPC to compute a single optimization iteration. As mentioned earlier in the paper, our numerical method is based on multiple shooting problem, which is tested in different prediction horizon of time.

The ability to perform any optimization method on an on-board processor must be feasible. For instance, a single-core CPU with limited resources must be able to solve the optimization problem effectively. For this comparison, Table 1 discusses the proposed method utilizes the average processing time per iteration (PT/I) for this paper (using MS) and compared with SS. In terms of computing NMPC on single or multi-core CPUs, the average PT/I in this paper is relatively better than SS method. In addition, even if we increase the prediction horizon to a larger number , the PT/I always feasible to run this on the CPU with limited resources. This also enables to perform a real experiment with an on board processor with limited processing capabilities.

Table 1

NMPC average PT/I for single and multi-core CPUs.

N_p	Single core this paper (ms)	Single core[26] (ms)	Multi-core this paper (ms)	Multi-core[26] (ms)
15	1.2	10.3	1	10
50	3.32	21.47	3	21
100	10.6	38.61	10	38
150	17.8	51.82	17	51

In contrast to[26], a remarkable finding is that accuracy in the MS method is optimal for a small and a large prediction horizon. Although, an increase in the prediction horizons may cause a slower processing but a lower error as shown in the Table 2. However, the issue with increasing a too large prediction horizon isn’t feasible for some cases. For instance, in[26], a prediction horizon of was used to run the path planner, which has an accuracy of 2cm. However, this can lead to a larger processing time. Thus, based on these findings, the proposed method is reasonable for real-time applications.

Table 2

Comparison of Euclidean distance accuracy among MS and SS numerical methods.

N_p	This paper (accuracy (cm))	Accuracy (cm)[26]
15	− 3.0 to 3.0	− 10.0 to 10.0
50	− 0.5 to 0.5	− 2.5 to 2.5
100	− 0.05 to 0.05	− 0.25 to 0.25
150	− 0.01 to 0.01	− 0.01 to 0.01

Skid-slip analysis

In terms of including the skid-slip applied to simulations, we have also used the PWL-based function proposed in Fig. 7a for the circle trajectory simulation. Furthermore, the side-slip angle of front wheels is within a reasonable range (slip angle) as shown in Fig. 7b,c.

Figure 7

(a) PWL function-based slippage and skidding velocities. (b) Side slip angle of the front wheels for the simulation 1. (c) Side slip angle of the front wheels for the simulation 2.

Conclusions

In this paper, we propose a tracking control and planner for a UGV in the presence of a skid-slip and dynamic and static objects. The proposed technique consists of a fast NMPC-based path-planner, novel evolutionary programming-based kinematic controller, a dynamic controller with a nonlinear disturbance observer, and a voltage driving circuit. We have performed simulation experiments by considering actual physical parameters to drive the vehicle safely to the goal location. First, a large map of UNSW is used, which is fed to PPQ-Dijkstra algorithm, and NMPC is combined with the map to avoid collisions and non-holonomic constraints. Finally, a test in which the UGV must follow a sizeable circular trajectory under the presence of static and moving objects is performed. The results obtained from these experiments validate that the proposed method can safely drive the vehicle to the goal location. To validate the performance of our proposed technique, we have performed the numerical multiple-shooting method, being restricted to use only one core and running in multiple cores. Compared to the existing literature, the proposed technique has improved the processing capability of the NMPC to run in a CPU with limited resources and improves the error accuracy.