ترکیب برنامه ریزی مسیر محلی و کنترل مسیر یابی برای وسایل نقلیه خودکار در امتداد یک مسیر مرجع Combining Local Trajectory Planning and Tracking Control for Autonomous Ground Vehicles Navigating along a Reference Path

The past three decades have witnessed the rapid development in the research field of autonomous driving, which attracts considerable research interest and efforts from both academia and industry. During the famous AGV competitions, the DARPA Grand Challenges and the DARPA Urban Challenge, autonomous ground vehicles had demonstrated their great potentials to improve driving safety, efficiency and comfort in both off-road and on-road environments [1]-[3]. These competitions showed significant advances over the-state-of-the-art in autonomous driving technology and stimulated extensive interest in the AGV research field as well. Recently, partially automated control functions have already been applied in the driver assistant systems and several automotive companies even make their research and development plans for producing future autonomous cars. Nevertheless, there is still a number of challenges to be faced for developing truly reliable and robust fully-autonomous driving vehicles to handle various realistic situations in the real world. As is well-known that the development and application of AGVs require the integration of the-state-of-the-art technologies, ranging from perception, localization to navigation and control [4]. As the core modules, both local motion planner and path tracking controller play a significant role in guaranteeing safety and improving driving comfort. In order to track the reference path accurately and robustly, many researchers developed Lyapunov-based feedback control laws by considering vehicle kinematics and dynamics, such as sliding model control, backstepping control and so on. To adapt high speed and varying terrain conditions and improve control accuracy and robustness, some researchers explore cascaded or multi-tiered control strategies and focusing on minimizing lateral errors in the outer loop, and stabilizing yaw motions via steering actions in the inner loop [5]. To reject model uncertainties and external disturbances, tire sideslip angles and inertial effects are taken into account. However, most of these controllers formulate the tracking control problem into a regulation problem, which primarily concerns with current cross-track error information (such as lateral and heading deviations from the reference path) to generate the immediate control action instead of a sequence of optimal or sub-optimal control actions within a finite horizon. In addition, state and control constraints are often ignored. Therefore, it may easily result in abrupt steering actions when vehicles deviate far from the reference path or negotiate a tight turn. Some researchers utilizes the gain-scheduling or variable structure control approaches to avoid abrupt control actions to achieve graceful motions at the expense of reducing tracking accuracy or error convergent speed [6]. Based on the comparison of a variety of path tracking controllers, [7] concluded that path tracking control performance is strongly dependent on both vehicle dynamics and smoothness of the reference path. Essentially, most of these kinematics-based and/or dynamics-based controller primarily focused on eliminating the current cross-track errors instead of taking advantage of the predictive information ahead to integrally optimize a sequence of control actions and its corresponding trajectory, which smoothly regulates the vehicle from the current state onto the sampling states aligned with the reference path ahead. There exists a large amount of research on integrated planning and control approaches for AGVs by applying optimization techniques. One of the most attractive methods is the Model Predictive Control (MPC), which is capable of formulating the vehicle navigation problem into a finite-horizon constrained optimization control problem [8]-[10]. MPC approach uses the vehicle kinematic or dynamic model to predict its future state evolution based on the current measured states. In each control cycle it generates a sequence of control actions, which minimizes a specific objective function within a finite horizon and satisfies the control constraints as well. Then, the first control action is issued to be executed by the low-level actuator. The process is repeated at subsequent time steps. The MPC scheme has the capabilities to systematically deal with system state and control constraints. However, it often assumes the reference path is known and the speed is constant over a short-term finite horizon ahead at each time. When vehicles drive either in on-road or off-road dynamic environments, due to the existence of unexpected obstacles and localization errors, the collision-free assumption of the reference path may be impractical. Solving the optimization problem via MPC scheme may involve non-convex constraints when obstacles are considered. In this case, computational burden and limited on-board computational resources may become a barrier preventing MPC approaches from generating a collision-free and feasible trajectory in real time. To deal with obstacle avoidance and achieve safe and graceful motions along the reference route, we introduce a computationally efficient local trajectory planner between the reference path and the path following controller based on a hierarchical framework. To generate a sufficiently long, collision-free and curvature-continuous trajectory in real-time, the planner is required to consider the reference route and surrounding environmental information from on-board perceptual system, as well as system model and constraints. To some extent, the local trajectory planner performs the function of bridging the gap between high-level rough reference path planning and low-level tracking control. In addition, it enables the vehicle to handle dynamic environments deliberatively and reactively. In addition, corresponding control inputs of the planned trajectory could be used as feedforward control commands by the low-level tracking controller and allow the feedback controller to focus on handling model uncertainties and disturbances. To solve the local trajectory generation problem, several well-known sampling-based motion planning approaches have been extensively studied. Most of them follow a discrete optimization scheme. More specifically, a set of trajectory candidates is generated via forward simulation based on the system model. Then a best trajectory is selected according to an objective function. This sampling-based motion planning scheme can be roughly classified into two categories, one is control space sampling-based method and the other is state space sampling-based method [11]. The former scheme discretizes the control input space (such as constant-curvature arcs [12], clothoids [13], or concatenation of these short-term motions [14]) to generate a set of trajectory candidates via numerical forward integration of differential equations which governs vehicle kinematics or dynamics [15]. Therefore, the generated trajectory candidates are inherently drivable. Due to its simplicity and computational efficiency, the scheme has been widely applied for the local navigation purpose, especially suitable for finding a collision-free path in the far less constrained environment. Based on the symmetric nature of mechanical system, some researchers generate a motion primitive library offline in the body-centered coordinate and use them online via ration and translation [16]. Nonetheless, since the motion primitives are generated by discretizing the control input space, the motion primitives are often not well-separated. A large amount of computational resource will be consumed on exhaustive collision-test and evaluation process. By contrast, instead of sampling discrete control inputs, the state space sampling-based motion planning scheme sample a set of terminal states by using the information of guidance path and environments. It not only considers the position constraints, but also accounts for heading and curvature states constraints imposed by the reference path. To be aligned with the reference path and obtain a collision-free and relatively smooth trajectory, a set of terminal states are sampled laterally offset along the reference path. Several approaches have been proposed to generate trajectories, which connect the vehicle current state with the terminal states aligned with the reference path. Based on cubic Bézier curves, [17] developed an efficient and analytical path smoothing algorithm to generate continuous-curvature path, which considers an upper-bound curvature constraint. Regarding the reference path as a baseline, [18] introduced a geometric method to generate multiple path candidates. The baseline is required to be smooth enough to ensure the smoothness of generated candidates. Instead of using geometric methods, [19] and [20] proposed local dynamically-feasible trajectory planners involving both close-loop control laws (kinematic-based nonlinear control law and pure pursuit control law, respectively) and system, state and control constraints, regardless of the smoothness of the baseline. [21] presented a model predictive trajectory generation scheme, which transforms the local trajectory generation problem into a two-point boundary value problem (BVP), subject to high-fidelity vehicle dynamic constraints. Due to use of the numerical solving method, it achieves a high degree of efficiency and generality. The low-level controller applies an open-loop control strategy for tracking control. The remainder of the paper is organized as follows: in Section II we introduce the structure of the proposed framework. Then, Trajectory generation method and low-level path tracking controller are described in detail in Section III and Section IV respectively. Section V presents the simulation results. Followed by concluding remarks and future work in Section VI.