Distance Calculations in Motion Planning Problems With Interference Situations

Abstract

Abstract This paper discusses distance calculations for three dimensional polyhedra with the assumption of convex bodies. An n-surface convex polyhedron is viewed as the intersection of n half-spaces and is represented by n linear inequality equations while the square of the distance between two points is of a quadratic form in terms of two sets of x-y-z coordinates. The static distance-to-contact between two non-interfering convex polyhedral shapes is then directly solvable by quadratic programming. Based on the concept of distance-past-contact, distance calculations for situations with interference are presented and tested in optimization based robot path planning examples. The distance evaluation is further investigated for the dynamic situations by a swept volume computation strategy. The approach is illustrated in examples with a moving robot link and a fixed obstacle.