Motion Control of Redundant Robots under Joint Constraints: Saturation in the Null Space Fabrizio Flacco * Alessandro De Luca * Oussama Khatib ** Abstract— We present a novel efficient method addressing the inverse differential kinematics problem for redundant manip- ulators in the presence of different hard bounds (joint range, velocity, and acceleration limits) on the joint space motion. The proposed SNS (Saturation in the Null Space) iterative algorithm proceeds by successively discarding the use of joints that would exceed their motion bounds when using the minimum norm solution and reintroducing them at a saturated level by means of a projection in a suitable null space. The method is first defined at the velocity level and then moved to the acceleration level, so as to avoid joint velocity discontinuities due to the switching of saturated joints. Moreover, the algorithm includes an optimal task scaling in case the desired task trajectory is unfeasible under the given joint bounds. We also propose the integration of obstacle avoidance in the Cartesian space by properly modifying on line the joint bounds. Simulation and experimental results reported for the 7-dof lightweight KUKA LWR IV robot illustrate the properties and computational efficiency of the method. I. I NTRODUCTION Inversion of first-order (velocity level) or second-order (ac- celeration level) differential kinematics is the standard way to execute a desired Cartesian motion of redundant robots [1]. Control methods typically use a generalized inverse (most often, the pseudoinverse) of the manipulator Jacobian in order to convert velocity or acceleration commands from the task space to the joint space, where actuation takes place. Kinematic redundancy is exploited for collision avoidance, for joint motion optimization, or by augmenting the primary task with multiple additional ones, possibly prioritized [2]. These approaches can be casted as the selection of suitable joint commands in the null space of the Jacobian matrix. An important problem of all the above methods in their simplest form is that constraints in the joint space (e.g., bounds on the joint range, velocity, acceleration, or even torque) are not taken explicitly into account. The underlying assumption is that either the joint motion and/or the actuator capabilities can be considered unlimited in practice, or that the robot task has been smoothly tailored in space and scaled in time so as to fit to the robot limitations. However, for sensor-driven robotic tasks in dynamic environments, in particular during physical human-robot interaction (pHRI), it is not unlikely that large instantaneous task velocities or accelerations are suddenly requested in response to an unexpected situation. These may lead to nominal commands * Dipartimento di Ingegneria Informatica, Automatica e Gestionale, Uni- versit` a di Roma “La Sapienza”, Via Ariosto 25, 00185 Rome, Italy ({fflacco,deluca}@dis.uniroma1.it). ** Artificial Intelligence Laboratory, Stanford University, Stanford, CA 94305, USA (khatib@cs.stanford.edu). in the joint space exceeding some bounds, with an as- sociated saturation that makes the resulting robot motion unpredictable. Simple scaling of the task commands recovers motion feasibility but may no longer satisfy the primary intention, e.g., avoid a collision with a fast moving human. Before doing so, it would be useful to verify whether there exist alternative joint motions satisfying the hard bounds in the joint space and still executing the original task command. Stated in this way, the problem is intrinsically local to the current robot state, i.e., needs to be solved on line with no future information. A common on-line approach to deal with limited joint ranges is to convert these hard bounds into soft ones, resolving redundancy by optimization (e.g., based on the Projected Gradient algorithm) of an objective function that keeps the joints closer to the center of their ranges (see [3], [4]). However, since the Cartesian task has always the highest priority, satisfaction of joint limits is not guaranteed. A method that enhances the capability of avoiding joint limits by using a suitably weighted pseudoinverse has been proposed in [5]. Other similar solutions for handling the presence of joint ranges have been proposed in the context of visual servoing tasks (e.g., [6], [7]). In the presence of joint velocity or joint acceleration bounds, simple software saturation of the joint-space com- mand results in the lack of execution of the desired motion in the task space. In [8], a redundancy resolution method was proposed that minimizes the infinity-norm (i.e., the maxi- mum absolute value of the components) of the command vector in the joint space, with the norm being weighted by the available actuation ranges. Nonetheless, satisfaction of the original hard bounds is again not guaranteed. In [9], the velocity term in the one-dimensional null space of a 7-dof robot is scaled so as to satisfy the joint velocity bounds, if at all possible. On the other hand, task relaxation by time scaling can be used for satisfying joint velocity [10] and/or acceleration [11] bounds. This approach has been extended to the redundant case, specifically in [12] to a team of mobile robots executing multiple tasks with priority. Lower-priority task velocity commands are scaled in the null space of higher-priority Jacobians so that the hierarchy of task priorities is preserved despite actuator saturations. It should be mentioned that the considered problem fits into the framework of constrained minimization of a quadratic objective function under linear equality and inequality constraints, possibly having different priorities. Once the problem is properly formulated, the use of a general-purpose optimization algorithm is suggested in [13]. 2012 IEEE International Conference on Robotics and Automation RiverCentre, Saint Paul, Minnesota, USA May 14-18, 2012 978-1-4673-1404-6/12/$31.00 ©2012 IEEE 285