Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles

Abstract

This study proposes a novel integrated local trajectory planning and tracking control (ILTPTC) framework for autonomous vehicles driving along a reference path with obstacles avoidance. For this ILTPTC framework, an efficient state-space sampling-based trajectory planning scheme is employed to smoothly follow the reference path. A model-based predictive path generation algorithm is applied to produce a set of smooth and kinematically-feasible paths connecting the initial state with the sampling terminal states. A velocity control law is then designed to assign a speed value at each of the points along the generated paths. An objective function considering both safety and comfort performance is carefully formulated for assessing the generated trajectories and selecting the optimal one. For accurately tracking the optimal trajectory while overcoming external disturbances and model uncertainties, a combined feedforward and feedback controller is developed. Both simulation analyses and vehicle testing are performed to verify the effectiveness of the proposed ILTPTC framework, and future research is also briefly discussed.

Introduction

Autonomous ground vehicles (AGVs) have great potential to improve driving safety, comfort and efficiency and can be widely applied in a variety of fields, such as road transportation, agriculture, planetary exploration, military purpose and so on [1], [2], [3], [4]. The past three decades have witnessed the rapid development of AGV technologies, which have attracted considerable interest and efforts from academia, industry, and governments. Particularly in the past decade, contributing to significant advances in sensing, computer technologies, and artificial intelligence, the AGV has become an extraordinarily active research field. During this period, several well-known projects and competitions for AGVs have already exhibited AGV׳s great potentials in the areas ranging from unstructured environments to the on-road driving environments [5], [6].

The development and application of AGVs requires a variety of the-state-of-the-art technologies, among which, motion planning and control play a critical role in improving safety and comfort for autonomous driving. There has been substantial research on motion planning and path tracking control for AGVs [7], [8], [9], [10], [11], [12], [13], [14], [15], [16], [17], [18], [19], [20], [21], [22]. Due to the limited on-board computational resources, most of the previous approaches address the motion planning and tracking control problems separately [8]. More specifically, the high-level motion planner often takes advantage of powerful discrete graph search-based algorithms based upon a simplified point-mass vehicle motion model to compute a long-term and collision-free path. The speed law is often assumed to be a constant value. Then the low-level path tracking controller focuses on regulating the vehicle onto the planned path using kinematic and/or dynamic lateral controllers [12], [13], [14].

As shown in Fig. 1, most of the conventional Lyapunov-based linear and nonlinear feedback path tracking controllers derive the steering control laws by relying on cross-track errors without explicitly considering the prediction of future motions or environmental constraints. The steering control command δ is derived by minimizing the lateral error ∆y and heading error ∆θ. In addition, the collision avoidance problem is also neglected. It is known that the smoothness of the reference path has a significant impact on the control performance. If the reference path is collision-free and smooth, only using gain-scheduling or sliding-model feedback control laws can achieve fairly satisfactory results. However, in practice, the reference paths from the high-level path planner are generally not smooth or even violate the vehicle kinematic and/or dynamic constraints, and tracking the reference paths directly using cross-track error-based feedback controllers can therefore easily result in response overshoot, oscillation or even instability, which is especially critical when the reference path is highly curvy or curvature-discontinuous. To alleviate these negative impacts, optimization techniques and path deformation algorithms have been developed to refine and smoothen the planned path for execution [23]. Smoothening a path with considering vehicle constraints and obstacles avoidance often involves a complex nonlinear optimization process, which makes computation prohibitively expensive. Besides, due to localization errors and unpredictably changing outdoor environments, the initially planned collision-free path may still collide with obstacles at the execution stage. Therefore, when a vehicle drives in a dynamic environment along a guidance path, it is necessary to develop a local motion planner to handle the online updating surrounding perceptional information.

The literature indicates that a better communication or integration between the high-level motion planning and the low-level tracking control would be very valuable for enhancing the overall performance of AGVs. To bridge this gap, a real-time trajectory planner is necessary and is required to be capable of taking both the information of the guidance path and vehicle motion constraints into account. Also, it should generate expressive drivable trajectories to make full use of vehicles׳ maneuverability to handle dynamic environments. To ensure that the generated trajectory could be smoothly and accurately tracked, instead of solely using the cross-track error, the low-level trajectory tracking controller should take advantage of the information provided by the trajectory planner to derive the feedforward control input to stabilize the vehicle and guide it along the planned trajectory as well as employ the feedback controller to overcome the model uncertainties and external noises, while guaranteeing the control stability [12], [13], [14]. In this study, we focus on developing an efficient local trajectory planner and the corresponding controller for AGVs to smoothly follow the reference path while avoiding unexpected obstacles. During the trajectory planning process, the vehicle kinematic model and control constraints are explicitly considered to generate smooth and curvature-continuous spatial paths. Furthermore, to ensure driving safety and comfort, velocity profiles are also carefully generated and assigned along the trajectory. After that, vehicle dynamics is also taken into account during the tracking control process to enhance the tracking control performance.

Since the AGV trajectory generation and tracking control problem involves dealing with constraints imposed by the vehicle control model, it can be naturally formulated into an constrained optimal control problem. Applying optimization techniques to solve these problems is not new. Very recently, some researchers employed linear and nonlinear model predictive control (MPC) approaches to address vehicle trajectory generation and tracking problems [24], [25], [26], [27]. Due to its capabilities of systematically handling system nonlinearities, state and control constraints, MPC has become a well-known method to solve trajectory generation and tracking control problems. However, solving the constrained optimization problem in the continuous control space over a long-term prediction horizon often involves a complex optimization process, which may easily result in excessive computational burden. Besides, constraints imposed by irregularly distributed obstacles in outdoor environments are difficult to be handled. To overcome these difficulties, a great deal of research on sampling-based local trajectory planning approaches has been conducted. They are capable of generating feasible trajectories along with the corresponding control inputs. Most of these methods follow a discrete optimization scheme, i.e. generation, collision test and evaluation. These approaches can be roughly classified into two types: control-space sampling and state-space sampling [15].

The control-space sampling method refers to generating a finite control input subset based on the parameterized control input space, for instance, parameterized curvature, such as arcs, clothoids, polynomial spirals. Then, based on the sampled control inputs, the trajectories are generated through forward simulation of the differential equations, respecting vehicle system constraints. Therefore, the produced paths are intrinsically drivable. Owing to its simplicity and efficiency, the method has been widely applied for local navigation. To reduce online computational complexity, some researchers proposed approaches by generating a set of body-centered motion primitives offline, applied them via translation and rotation online, and then evaluated the primitives via the convolution of the vehicle shape along these primitives on a cost-map [15], [16]. However, the control-space sampling methods also suffer from shortcomings: it does not exploit the information of environments and the reference path during the sampling process. As the purple dash dot line curves illustrate in Fig. 2, the uniformly sampled trajectories are not aligned with the reference path. As a consequence, it may result in discrepancy between the consecutive plans and cause overshoot and oscillation in the tracking control process. Secondly, due to the nonlinearities of the system constraints, though the uniform sampling performs in the control input space, the generated paths may not be well separated. Consequently, it results in computational complexity during the collision-test and evaluation process.

In contrast, instead of sampling in the control space, the state-space sampling method requires generated trajectories to satisfy the constraints imposed by the terminal states and the vehicle model. This approach samples a set of terminal states firstly, then computes the trajectories, which connect the initial state with the sampling states while respecting system constraints. Furthermore, the state-space sampling scheme is able to exploit the information of environmental structure and guidance path or from sampling or collision-test results to bias sampling toward the space, where the potential solutions are most likely to exist. In this way, it can significantly reduce unnecessary samples. Pure pursuit control is a well-known preview path tracking controller, which is essentially a simplified version of state-based sampling planning approach. As the orange dash line curves show in Fig. 2, the essential idea of the pure pursuit approach is regulating the vehicle onto the reference path with satisfying the position constraints. However, it does not explicitly consider the constraints imposed by heading and curvature states. Based upon the state-space sampling strategy [18], [28], [29], computed system-compliant trajectories via forward simulation using the vehicle system model and associated close-loop feedback control laws.

Howard et al. [7], formulated the trajectory generation problem into a two-point boundary value problem (BVP). Since it involves inverting nonlinear differential equations, solving the BVP directly will be nontrivial. The well-known Dubins curves transformed the BVP into a path-length optimization problem and used an analytical approach to generate path by using circular arcs and line segments. However, curvature discontinuities at the conjunction of the line segments and arcs may cause stop and reorientation actions. Another popular analytical method, Bézier curves are able to ensure the curvature continuity, but they cannot satisfy the constraints of curvature limits and its rates limits [30]. Montemerlo et al. [6] proposed a geometrical path generation method, which generated a finite set of paths with terminal states aligned with a baseline (reference path). The method is especially efficient when a curvature-continuous reference path is available. However, it did not explicitly take vehicle control constraints into account.

To explicitly deal with vehicle model and control constraints, Howard et al. [7] developed a general and efficient model-predictive trajectory generation approach via numerical linearization and optimization techniques. The approach can also be used for generating feasible edges to construct global search graphs, such as regular state lattice [17]. It has been successfully applied in a variety of mobile robotic platforms. Combining the trajectory generation method and the conformal spatiotemporal lattice, McNaughton et al. proposed a powerful Graphic Processor Unit (GPU) based motion planner to solve several challenging motion planning problems in on-road driving scenarios [16].

It can be seen from the aforementioned research, most of them deal with the trajectory generation and tracking control problem separately. In this paper, a new integrated local trajectory planning and tracking control (ILTPTC) framework is developed for online autonomous driving with obstacles avoidance to bridge the gap between planning and control. The presented ILTPTC framework consists of an efficient sampling-based local trajectory planning algorithm which explicitly considers the time parameterized states. Subsequently, a tightly associated tracking controller directly exploits the rich information from the trajectory planner by combining feedforward and feedback controllers.