Predictive Motion Planning with Pipelined Feature-Based Obstacle Avoidance

An efficient method for the approximate solution of a finite-horizon constrained optimal control problem was proposed that uses quantized and discretized control candidates in such a way that a real-time implementation on a small onboard computer is feasible. For obstacle or collision avoidance, an algorithm using predicted trajectory points and visible three-dimensional features in a point cloud was presented that is suitable for a parallel and pipelined implementation on a field-programmable gate array. This allows for a very large number of obstacles to be checked by using raw sensor data only without further processing. The second option of an obstacle cost function uses a barrier function approach. It is designed so that the cost tends to infinity if the distance between a predicted trajectory point and an obstacle approaches the minimum distance. Choosing this approach helps to influence the flown trajectory, even when the vehicle has a much greater distance to possible obstacles than the minimum distance, since features always introduce costs on a trajectory and not only if the predicted trajectory violates the minimum distance requirement.