NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators

The real world is not static and consequently, manipulators must be up to the challenge of reacting to the unpredictable nature of the environment they work in. Getting a manipulator's end-effector from point A to point B is a fundamental problem in robotic control. While the task is seemingly simple, there can be numerous causes of failure, especially as the environment becomes cluttered and dynamic. Dynamic obstacle avoidance is essential for manipulators to be robust in such environments.

Each iteration of the control loop must be able to consider the state of both environment and robot to guarantee safe and reliable robot operation. The current focus in obstacle avoidance for manipulators has come at the cost of greater up-front computational load, leading to open-loop motion plans. Planners compute a sequence of joint coordinates which at run time become joint velocities. In contrast, a reactive approach capable of avoiding non-stationary obstacles works directly with joint velocities.

We consider the differential kinematics and compute a set of joint velocities that steer the end-effector towards the goal in task space. This classic and purely reactive approach is known as resolved-rate motion control (RRMC) and is cheap to compute and easily able to run at over 1000 Hz. In this paper we add additional capabilities to the RRMC approach while maintaining its highly reactive capability.

Differential kinematics also allows us to capture the rate of change of the distance between any part of the robot and any obstacle present in the environment. By exploiting such relationships, a reactive controller can be developed that will avoid colliding with obstacles. The resulting controller may be over-constrained and unable to achieve the goal pose, but we can employ two strategies to resolve this.

Firstly, we could employ a kinematically redundant manipulator with more degrees of freedom than is necessary to reach any pose within its task space – these are now increasingly common. Secondly, we could relax the path specification to allow for intentional error, called slack which allows the end-effector to diverge from the straight-line trajectory to dodge obstacles.

To provide the best chance for the robot to be able to react in a volatile environment, we must consider the robot's conditioning. A measure of manipulability, devised in [1], describes how well-conditioned a manipulator is to achieve any arbitrary velocity. Therefore, by also maximising the manipulability of the robot, we can decrease the likelihood of robot failure due to singularity while also improving robot obstacle avoidance.

The contributions of this paper are:

1. 1.

   a reactive motion controller for serial-link manipulators (fully actuated or redundant) that can achieve a desired end-effector pose with the ability to dodge stationary and non-stationary obstacles, avoid joint limits and maximise manipulability.

2. 2.

   experimental validation in simulation on a published motion planning benchmark as well as on a physical Franka-Emika Panda robot.

3. 3.

   an open-source Python library providing all tools necessary to implements our controller on any serial-link manipulator [2]. Benchmark code and implementation details are provided at jhavl.github.io/neo.

Classical planning methods will find the optimal path according to the supplied criteria. However, their computation time means they are restricted to use as open-loop planners. Motion planners such as STOMP [3], CHOMP [4] and Trajopt [5] use sampling-based methods to solve general high-dimensional motion planning problems not just for manipulators. Motion planners can typically incorporate collision avoidance as well as other constraints such as end-effector orientation throughout the motion. In some cases, motion-planners may need post-processing to remove jerky or extraneous motions due to sampling.

STOMP uses a stochastic and gradient-free scheme to optimise a trajectory while minimising a cost function. The stochastic nature of the scheme means that it can overcome local minima which purely gradient-based approaches like CHOMP and Trajopt can be trapped by. CHOMP and Trajopt both utilise novel methods for solving optimisation problems while incorporating obstacles.

Planning times of these algorithms preclude their use in dynamic environments. In one benchmark [5] planning time varies from 0.19 seconds (for Trajopt) to 4.91 seconds (for CHOMP).

Path planners output a sequence of poses for which the inverse kinematic (IK) solution is computed to obtain a sequence of joint coordinates. IK can be solved analytically on most robots, but it is more commonly solved through an optimisation framework [6]. The optimisation problem can be augmented, through the cost function, with equality or inequality constraints to provide additional functionality. IK solvers can therefore provide joint configurations which are free from collisions and avoid joint limits. Typically only redundant robots with an exploitable null space can achieve useful benefits.

However, IK alone cannot provide a valid path to the goal. Rather than consider the pose or joint configuration at each time step we can consider velocity. This approach, using differential kinematics, is commonly referred to as Resolved-Rate Motion Control (RRMC) [7].

RRMC uses the derivative of the forward kinematics to choose joint velocities which will drive the robot in a straight line from the starting end-effector pose to the desired end-effector pose. Unlike the previously mentioned approaches, RRMC is purely reactive and provides the joint velocities for the next time instant rather than for the whole trajectory.

If the robot is redundant, there are an infinite set of joint velocities which will achieve the desired end-effector velocity. Typically an additional constraint such as minimising the norm of joint velocity is applied. Alternatively, through gradient projection and exploitation of the null space, the robot can complete sub-tasks while achieving its motion.

One such subtask is maximising the manipulability of the robot [8]. By projecting the manipulability Jacobian into the null space of the differential kinematic equation, the manipulator will perform the straight-line end-effector motion towards the goal pose, but also choose joint velocities such that the robot manipulability is maximised at each step. This is beneficial for the robustness of the robot operation as it decreases the likelihood the robot will approach a singular configuration. While there are numerous measures for kinematic sensitivity, the Yoshikawa manipulability index [1] is the most widely accepted and used measure for kinematic manipulability. The manipulator Jacobian contains rows relating to translational and rotational velocities. Additionally, manipulators comprising mixed prismatic and revolute actuators have non-homogeneous Jacobians due to the different units used. Therefore, when using Jacobian-based manipulability measures such as [1] care must be taken to consider issues including length scaling and mixed units [9]. In a dynamic environment, manipulability optimisation enables the robot to achieve arbitrary velocity at the next time step in order to meet the needs of unpredictable scene dynamics.

RRMC can be reformulated as a quadratic programming (QP) optimisation problem. QPs provide the ability to consider constraints and costs which provide additional functionality. The work in [10] incorporated the physical joint limits of a mobile manipulator into a QP. Recent work in [11] introduced slack into the typical straight-line approach of RRMC – effectively introducing extra redundancy to the problem which is especially useful for robots with six or fewer degrees-of-freedom. This work then exploited the extra redundancy to maximise the manipulability and avoid physical joint limits of manipulators through a reactive QP controller. The velocity damper approach described in [12] was used to avoid joint limits but could equally be used to avoid obstacles.

A classical and alternate approach to reactive motion control is through potential fields where the robot is repelled from joint limits and obstacles [13, 14]. This guides the tool on a collision-free path to the goal but does not explicitly maximise the manipulability or conditioning of the robot.

Reactive control is essential for important sensor-based motion robotics problems such as visual servoing [15] or closed-loop visual grasping [16]. Both determine the end-effector velocity for the next control loop iteration, and this is challenging to achieve with planning-based methods.

This paper proposes a novel real-time motion controller which can avoid moving obstacles (for the whole robot, not just the end-effector), avoid physical joint limits and maximises the manipulability of a manipulator in a purely reactive manner.

In Section III we incorporate the differential kinematics and manipulability maximisation into a general QP problem. We extend this to handle obstacles in Section IV, then velocity dampers and slack variables in Section V. We present the NEO controller in Section VI and our experimental methodology in Section VII. We conclude in Section VIII with a discussion of our experimental results and insights.

The generic form of a QP is [17]

where $f_{o}$ is the objective function to minimise; ${{\boldsymbol{A}}}_{1}$, ${\mathitbf{b}}_{1}$ define the equality constraints; ${{\boldsymbol{A}}}_{2}$, ${\mathitbf{b}}_{2}$ define the inequality constraints, and ${\mathitbf{d}}$ and ${\mathitbf{e}}$ define the lower and upper bounds of the optimisation variable ${\mathitbf{x}}$. When the matrix ${{\boldsymbol{Q}}}$ is positive definite the QP is strictly convex [17].

A point in 3D space can be represented as ${\mathitbf{p}}\in\mathbb{R}^{3}$. The distance $d$ between a point ${\mathitbf{p}}_{r}$ on a robot and a point ${\mathitbf{p}}_{o}$ on an obstacle is

and a unit vector ${\hat{{\mathitbf{n}}}}_{ro}$ pointing from ${\mathitbf{p}}_{r}$ to ${\mathitbf{p}}_{o}$ is

The time derivative of (8) is

We know from differential kinematics that the end-effector velocity is ${\mathitbf{\nu}}(t)={{{{\boldsymbol{J}}}}\left({\mathitbf{q}}\right)}{\dot{{\mathitbf{q}}}}(t)$. Furthermore, we can calculate the velocity of any point fixed to the robot by taking the translational-velocity component of a modified manipulator Jacobian that considers the point as the robot's end-effector

where $k$ is the index of the link to which $p_{r}$ is attached, ${{\boldsymbol{J}}}_{p}\in\mathbb{R}^{3\times k}$ is the Jacobian relating the velocity of the point ${\mathitbf{p}}$ to the velocities of joints 0 to $k$, ${{}^{0}{{\boldsymbol{T}}}_{{\mathitbf{q}}_{k}}}$ describes the pose of link $k$ with respect to the base frame of the robot, $\bullet$ represents composition, and $\Lambda(\cdot)$ is a function that converts the partial derivative of the pose from a tensor $\mathbb{R}^{6\times k\times k}$ to the translational velocity Jacobian $\in\mathbb{R}^{3\times k}$. Note that this Jacobian is only a function of the $k$ joints preceding the link to which the point is attached. We denote the set of joints which ${{\boldsymbol{J}}}_{p}$ depends on as ${\tilde{{\mathitbf{q}}}}$ which is of a variable size – if the point is attached to the end-effector then $k=n$.

The pose ${{}^{{\mathitbf{q}}_{k}}{{\boldsymbol{T}}}_{P}}$ a frame $P$ fixed to link $k$, with respect to the reference frame of link $k$, and whose origin is ${\mathitbf{p}}_{r}$. The orientation of this frame is arbitrary, although the direction ${\hat{{\mathitbf{n}}}}$ is expressed in this frame, so we disregard the angular velocity component from the partial derivative in (IV). More details on calculating this Jacobian can be found in [19]. Our provided software package [2] makes it trivial to calculate these Jacobians.

Using (IV), we can now write

and the rate of change of distance becomes

The coefficient of $\dot{{\tilde{{\mathitbf{q}}}}}(t)$ is the distance Jacobian

and substituting (13) into (IV) we obtain the scalar

which is now in a form usable for our QP.

Our proposed controller, NEO, exploits the QP described by (20) within a position-based servoing (PBS) scheme. This controller seeks to drive the robot's end-effector in a straight line from its current pose to a desired pose. PBS is formulated as

where $\beta\in\mathbb{R}^{+}$ is a gain term, $\,{}^{\scriptscriptstyle 0}\!\,{{\boldsymbol{T}}}_{e}\in\mathrm{{\bf SE}(3)}$ is the current end-effector pose in the robot's base frame, $\,{}^{\scriptscriptstyle 0}\!\,{{\boldsymbol{T}}}_{e^{*}}\in\mathrm{{\bf SE}(3)}$ is the desired end-effector pose in the robot's base frame, and $\psi(\cdot):\mathbb{R}^{4\times 4}\mapsto\mathbb{R}^{6}$ is a function which converts a homogeneous transformation matrix to a spatial twist.

By setting ${\mathitbf{\nu}}$ in (20) equal to ${\mathitbf{\nu}}_{e}$ in (30) the robot will be driven towards the goal, however, not necessarily in a straight line. The robot will diverge from the path to improve manipulability, avoid joint velocity limits, avoid joint position limits, and dodge both static and dynamic obstacles. We detail the effects of changing the controller parameters in Table I.

The proposed controller is subject to local minima which can prevent it from reaching the goal pose if the direct path is blocked by an obstacle. We coded a simple retreat heuristic that has proven effective for many common cases: if the direct path between the end-effector and the goal is blocked by obstacles and the closest obstacle is within the influence distance $d_{i}$, then we bias $v_{x}$ and $v_{y}$ components of the desired end-effector velocity ${\mathitbf{\nu}}_{e}$ to make the arm retract towards the robot's base frame.

We use our Python library, the Robotics Toolbox for Python [2], to implement our controller. We use the Python library qpsolvers which implements the quadratic programming solver [21] to optimise (20).

We validate and evaluate our controller by testing on a real manipulator as well as in simulation. In these experiments, NEO has full state information of the environment which otherwise would need to be provided by eye in hand and/or a third-person-view camera with depth sensing capabilities.

We use the following values for the controller parameters: $\xi=1$, $d_{i}=0.3\mbox{$\mathrm{\,m}$}$, and $d_{s}=0.05\mbox{$\mathrm{\,m}$}$ in (26), $\eta=1$, $\rho_{i}=50\mbox{${}^{\circ}$}$, and $\rho_{s}=2\mbox{${}^{\circ}$}$ in (29), $\beta=1$ in (30), and $\lambda_{q}=0.01$, and $\lambda_{\delta}=\frac{1}{e}$ in (22), where $e$ represents the total error between the current and desired end-effector pose. By setting $\lambda_{\delta}$ to be inversely proportional to pose error, the optimiser has freedom to maximise manipulability at the beginning of the trajectory while the increasing restriction ensures the end-effector continues to approach the goal. Table I describes each of the gains within NEO, as well as the consequence of making them too large or too small. In practice we found that performance was generally sound within $50\%$ to $200\%$ of the chosen gain values. As discussed in Section VI, a velocity damper is incorporated into the controller when the minimum distance between the robot collision model and the obstacle is less than the influence distance $d_{i}$. This means there may be several velocity dampers for a single collision object, each with respect to a different robot link collision model.

For the simulated experiments, we use our open-source simulator Swift [20] with our Python library [2] to simulate the Willow Garage PR2. For the physical experiments, we use our Python library [2], ROS middleware to interface with the robot, and the 7 degree-of-freedom Franka-Emika Panda robot.

The average execution time of the NEO controller during the experiments was $9.8$$\mathrm{\,ms}$ using an Intel i7-8700K CPU with 12 cores at 3.70GHz in a single threaded process – a control rate just above $100$$\mathrm{\,Hz}$. Each collision model to obstacle pair being considered in the controller (i.e. the minimum distance between them is less than $d_{i}$) takes $0.5$$\mathrm{\,ms}$. While this is an order-of-magnitude slower than vanilla RRMC it provides significant additional capability at a rate well in excess of typical vision sensors which have a refresh rate of $30$$\mathrm{\,Hz}$ – our controller is much faster than the slowest part of the pipeline. Reducing the execution time is possible since many parts of the controller, such as collision checking, are parallelizable.

In this paper we have presented NEO, a novel Expeditious Optimisation Algorithm for manipulator motion control while dodging dynamic and static obstacles, avoiding joint limits, and maximising manipulability. Our experiments show the purely reactive and real-time NEO controller is reliable in environments containing dynamic obstacles reflective of workspaces containing multiple robots working with humans. Additionally, NEO is shown to be viable for general environments containing benches or shelves but will struggle in environments containing small confined spaces with confined spaces and is an area for future work. In such scenarios, NEO is better employed as a local controller when dynamic obstacles are encountered. NEO has a strong advantage in terms of low upfront computation time.

Our approach requires the computation of the manipulability Jacobian, the manipulator Jacobian (between any points on the robot), the manipulator Hessian, and distance Jacobians. The associated Python library, the Robotics Toolbox for Python [2], can conveniently compute all these values. The only required input for our library is a kinematic robot model which can be in standard or modified Denavit–Hartenberg notation, a URDF/XACRO model, or the ETS format [19].