Visual Servoing Path Planning via Homogeneous Forms and LMI Optimizations

Path planning is a useful technique for visual servoing as it allows one to take into account system constraints and achieve desired performances during the camera motion. In this paper, we propose a new framework for path planning based on the use of homogeneous forms and linear matrix inequalities (LMIs). Specifically, we introduce a general parametrization of the trajectories from the initial to the desired location based on homogeneous forms and a parameter-dependent version of the Rodrigues formula. This allows us to impose typical constraints (field of view, workspace, joint, avoidance of collision, and occlusion) via positivity conditions on suitable homogeneous forms. Then, we reformulate the problem of finding a trajectory in the 3-D space satisfying all these constraints as an LMI optimization that can handle the maximization of typical performances (e.g., visibility margin, similarity to a straight line). The planned camera path is tracked by using an image-based controller. The proposed approach is illustrated and validated through simulations and experiments.