Time-optimized 3D Path Smoothing with Kinematic Constraints

Abstract

This paper proposes a novel time-optimized smoothing algorithm, that optimizes a path planner output, considering three dimensional (3D) kinematic constraints. First, a path from the start to goal position is obtained using a path planner algorithm. To find a locally time-optimal smooth trajectory between path planner output nodes, an optimization of Bezier curve control point positions is employed. The optimization method considers the obstacle avoidance as well as kinematic constraints, and minimizes the arc length of the proposed trajectory section with constant maximal linear velocity model. Bezier curves are calculated in a piece-wise manner and connected with a G2 continuity to obtain a continuous trajectory. Simulation experimental results are included to verify the feasibility of the proposed method and express the improvement with regards to the arc length over similar path smoothing approaches, while satisfying more complex full three dimensional kinematic constraints, instead of maximal curvature. The proposed method produces shorter constrained paths in uncluttered environment as well as in environments with obstacles.