Efficient Calculation of Minimum Distance Between Capsules and Its Use in Robotics

The problem of minimum distance calculation between line-segments/capsules, in 3D space, is an important subject in many engineering applications, spanning CAD design, computer graphics, simulation, and robotics. In the latter, the human-robot minimum distance is the main input for collision avoidance/detection algorithms to measure collision imminence. Capsules can be used to represent humans and objects, including robots, in a given dynamic environment. In this scenario, it is important to calculate the minimum distance between capsules efficiently, especially for scenes (situations) that include a high number of capsules. This paper investigates the utilization of QR factorization for performing efficient minimum distance calculation between capsules. The problem is reformulated as a bounded variable optimization in which an affine transformation, deduced from QR factorization, is applied on the region of feasible solutions. A geometrical approach is proposed to calculate the solution, which is achieved by computing the point closest to the origin from the transferred region of feasible solutions. This paper is concluded with numerical tests, showing that the proposed method compares favorably with the most efficient method reported in the literature.