Local trajectory planning for autonomous vehicle with static and dynamic obstacles avoidance

Trajectory planning is one of the most complex tasks that should be accomplished in order to ensure vehicle autonomous driving. Trajectory planning can be classified into local and global planning. The purpose of local trajectory planning is to find the optimal trajectory to follow a global reference trajectory while avoiding obstacles in a smooth and comfortable way, within the constraints of road driving. This paper presents a trajectory planning algorithm that calculates a path according to a set of predefined way-points describing a global map. The predefined way-points provide the basic reference frame of a curvilinear coordinate system to generate candidate paths, which start with a transient phase, followed by a curve parallel to the road. Each candidate path, associated to a desired velocity profile, is evaluated via a cost function against several criteria including passenger's comfort, static and dynamic obstacles avoidance and overall trajectory tracking. The chosen trajectory is then applied to a full vehicle model using a coupled longitudinal/lateral controller validated on SCANeR studio (OKtal) simulator. A challenging test scenario of SCANeR studio is used to validate the proposed algorithm under Matlab.