Obstacle Avoidance in Operational Configuration Space Kinematic Control of Redundant Serial Manipulators

Abstract

Kinematic control of redundant serial manipulators has been carried out for the past half century based primarily on a generalized inverse velocity formulation that is known to have mathematical deficiencies. A recently developed inverse kinematic configuration mapping is employed in an operational configuration space differentiable manifold formulation for redundant-manipulator kinematic control with obstacle avoidance. This formulation is shown to resolve deficiencies in the generalized inverse velocity formulation, especially for high-degree-of-redundancy manipulators. Tracking a specified output trajectory while avoiding obstacles for four- and twenty-degree-of-redundancy manipulators is carried out to demonstrate the effectiveness of the differentiable manifold approach for applications with a high degree of redundancy and to show that it indeed resolves deficiencies of the conventional generalized inverse velocity formulation in challenging applications.

1. Introduction

1.1. Basics of Redundant-Serial-Manipulator Kinematics

A serial manipulator is comprised of a chain of bodies that are connected by single-degree-of-freedom joints. Joint relative input coordinates $y_{\mathrm{i}}$ between bodies in the chain define the configuration of outboard bodies relative to their inboard counterparts. The terminal body in the chain is the end effector, whose output coordinates characterize manipulator working capability and are defined as twice continuously differentiable functions of input coordinates, in the form of forward kinematic mapping.

$$\mathit{z}=\mathit{G}(\mathit{y})$$

(1)

Input coordinates $\mathit{y}\in {\mathrm{R}}^{\mathrm{n}}$ are independent generalized coordinates [1] that define the configuration of the underlying mechanism, and output coordinates $\mathit{z}\in {\mathrm{R}}^{\mathrm{m}}$ of the end effector, with $\mathrm{m}<\mathrm{n}$, define functionality of the kinematically redundant manipulator. Here, ${\mathrm{R}}^{\mathrm{k}}$ refers to k-dimensional Euclidean vector space with elements $\mathit{x}\in {\mathrm{R}}^{\mathrm{k}}$ in the form of column vectors $\mathit{x}={\left[\begin{array}{ccc}{x}_1& \cdots & x_{\mathrm{k}}\end{array}\right]}^{\mathrm{T}}$, where superscript ${}^{\mathrm{T}}$ denotes matrix transpose. Bold characters denote vectors and matrices.

An input–output pair $(\mathit{y},\mathit{z})$ that satisfies Equation (1) defines a manipulator configuration, denoted $\mathit{x}={\left[\begin{array}{cc}{\mathit{y}}^{\mathrm{T}}& {\mathit{z}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in {\mathrm{R}}^{\mathrm{n}+\mathrm{m}}$. The manipulator configuration space is defined as

$$\mathrm{X}=\left\{\mathit{x}\in {\mathrm{R}}^{\mathrm{n}+\mathrm{m}}:\mathit{G}(\mathit{y})-\mathit{z}=\mathbf{0}\right\}$$

(2)

As observed in [2], X is the graph of Equation (1). As such, under moderate regularity conditions, it is a differentiable manifold with the single chart $({\mathrm{R}}^{\mathrm{n}},\mathit{\varphi }(\mathit{x}))$, where $\mathit{\varphi }(\mathit{x})=\mathit{y}$ [2]. This manifold, however, contains information only on forward kinematics and no information regarding inverse kinematics.

The manipulator configuration space often contains singular configurations relative to inverse kinematics. If the rank of the Jacobian matrix of derivatives of $\mathit{G}(\mathit{y})$ with respect to $\mathit{y}$, defined as the $m\times n$ matrix ${\mathit{G}}^{\prime }(\mathit{y})\equiv \left[\mathsf{\partial }{\mathrm{G}}_{\mathrm{i}}(\mathit{y})/\mathsf{\partial }{y}_{\mathrm{j}}\right]$, is less than m at a configuration $\overline{\mathit{x}}={\left[\begin{array}{cc}{\overline{\mathit{y}}}^{\mathrm{T}}& {\overline{\mathit{z}}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in \mathrm{X}$, then in a neighborhood of $\overline{\mathit{x}}$, there exists no continuously differentiable set-valued inverse kinematic mapping with $\mathrm{n}-\mathrm{m}$ arbitrary parameters [3,4]. To avoid difficulties associated with inverse kinematic singular configurations in X, the regular configuration space is defined as

$$\tilde{\mathrm{X}}=\left\{\mathit{x}\in \mathrm{X}: \mathrm{rank}({\mathit{G}}^{\prime }(\mathit{y}))=\mathrm{m}\right\}$$

(3)

In $\tilde{X}$, the manipulator degree of redundancy is $r=n-m$.

1.2. A Four-Degree-of-Redundancy Manipulator

Most applications of redundant manipulators have just one degree of redundancy, i.e., r = $\mathrm{n}-\mathrm{m}=1$. To relate the mathematical formulation to a real manipulator, consider the planar serial manipulator of Figure 1 that has $\mathrm{n}=6$ inputs, $\mathit{y}\in {\mathrm{R}}^6$, and two outputs, $\mathit{z}\in {\mathrm{R}}^2$, hence $\mathrm{r}=6-2=4$ degrees of redundancy.

Using the notation $y_{\mathrm{ij}}=y_{\mathrm{i}}+\cdots +y_{\mathrm{j}}$, the forward kinematic mapping of Equation (1) is

$$\mathit{z}=\mathit{G}(\mathit{y})=\left[\begin{array}{c}{y}_1+{\cos \mathrm{y}}_3+{\cos \mathrm{y}}_{34}+{\cos \mathrm{y}}_{35}+{\cos \mathrm{y}}_{36}\\ y_2+{\sin \mathrm{y}}_3+{\sin \mathrm{y}}_{34}+{\sin \mathrm{y}}_{35}+{\sin \mathrm{y}}_{36}\end{array}\right]$$

(4)

with the full-rank Jacobian, for all $\mathit{x}\in \mathrm{X}=\tilde{\mathrm{X}}$,

$$\begin{array}{l}{\mathit{G}}^{\prime }(\mathit{y})=\left[\begin{array}{cccccc}1& 0& a& b& -({\sin \mathrm{y}}_{35}+{\sin \mathrm{y}}_{36})& -{\sin \mathrm{y}}_{36}\\ 0& 1& c& d& ({\cos \mathrm{y}}_{35}+{\cos \mathrm{y}}_{36})& {\cos \mathrm{y}}_{36}\end{array}\right]\\ a=-({\sin \mathrm{y}}_3+{\sin \mathrm{y}}_{34}+{\sin \mathrm{y}}_{35}+{\sin \mathrm{y}}_{36})\\ b=-({\sin \mathrm{y}}_{34}+{\sin \mathrm{y}}_{35}+{\sin \mathrm{y}}_{36})\\ c=({\cos \mathrm{y}}_3+{\cos \mathrm{y}}_{34}+{\cos \mathrm{y}}_{35}+{\cos \mathrm{y}}_{36})\\ d=({\cos \mathrm{y}}_3+{\cos \mathrm{y}}_{34}+{\cos \mathrm{y}}_{35})\end{array}$$

(5)

1.3. Generalized-Inverse-Velocity-Based Redundancy Resolution

Differentiating Equation (1) with respect to time yields the kinematic velocity equation:

$$\dot{\mathit{z}}={\mathit{G}}^{\prime }(\mathit{y})\dot{\mathit{y}}$$

(6)

Since Equation (6) is linear in velocity, for a given configuration, it may be thought that analysis in the linear velocity space is easier than in the nonlinear regular configuration space of Equation (3). When $\mathrm{m}=\mathrm{n}$ and the manipulator is nonredundant, this may in fact be the case. In $\tilde{\mathrm{X}}$, if m = n, the manipulator is nonredundant, ${\mathit{G}}^{\prime }(\mathit{y})$ is square and nonsingular, and there is a unique solution of Equation (6) for $\dot{\mathit{y}}$,

$$\dot{\mathit{y}}={{\mathit{G}}^{\prime }}^{(-1)}(\mathit{y})\dot{\mathit{z}}$$

(7)

In this special case, Equation (7) is an ordinary differential equation (ODE). For specified continuous $\dot{\mathit{z}}(\mathrm{t})$ and initial condition $\mathit{y}(0)={\mathit{y}}^0$, the initial-value problem with Equation (7) has a unique solution in a neighborhood of $t=0$.

For redundant manipulators with $\mathrm{n}>\mathrm{m}$, attractive properties of ODE for Equation (6) are no longer available. At a redundant manipulator configuration $\mathit{x}\in \tilde{\mathrm{X}}$, ${\mathit{G}}^{\prime }(\mathit{y})$ has full rank, but ${\mathit{G}}^{\prime }(\mathit{y})$ is not a square matrix, and Equation (6) with given $\dot{\mathbf{z}}(t)$ cannot be solved for a unique value of $\dot{\mathit{y}}$. In this case, Equation (6) is not an ODE in y. It is a Pfaffian differential equation [5] that behaves more like a partial differential equation than an ODE. While the ODE of Equation (7) with m = n has a general solution for y that depends on n arbitrary constants, a general solution of Equation (6) for y with n > m depends on arbitrary vector functions $\mathit{p}(\mathrm{t})$ [5]. As shown in [4], trading the nonlinear manipulator equations of Equations (2) and (3) for the Pfaffian differential equation of Equation (6) is a questionable decision.

A generalized-inverse-velocity-based redundancy resolution formulation for Equation (6) was introduced by Whitney [6] half a century ago in an attempt to create an ODE formulation for redundant-manipulator kinematics. At a configuration $\mathit{x}\in \tilde{\mathrm{X}}$ with n > m, ${\mathit{G}}^{\prime }(\mathit{y})$ has full rank and the Moore–Penrose generalized inverse [6,7,8] is defined as

$${{\mathit{G}}^{\prime }}^{†}(\mathit{y})={{\mathit{G}}^{\prime }}^{\mathrm{T}}(\mathit{y}){\left({\mathit{G}}^{\prime }(\mathit{y}){{\mathit{G}}^{\prime }}^{\mathrm{T}}(\mathit{y})\right)}^{-1}$$

(8)

Direct manipulation verifies that ${\mathit{G}}^{\prime }(\mathit{y}){{\mathit{G}}^{\prime }}^{†}(\mathit{y})=\mathit{I}$, and

$$\dot{\mathit{y}}={{\mathit{G}}^{\prime }}^{†}(\mathit{y})\dot{\mathit{z}}+\left(\mathit{I}-{{\mathit{G}}^{\prime }}^{†}(\mathit{y}){\mathit{G}}^{\prime }(\mathit{y})\right){\dot{\mathit{y}}}^0(\mathrm{t})$$

(9)

satisfies Equation (6) for arbitrary ${\dot{\mathit{y}}}^0(t)\in {\mathrm{R}}^{\mathrm{n}}$. While $\dot{\mathit{y}}$ of Equation (9) satisfies Equation (6), it does not provide a solution $\mathit{y}(t)$ of Equation (1), and there is no reason to believe Equations (6) and (9) are equivalent.

1.4. Problems with Generalized-Inverse-Velocity-Based Redundancy Resolution

In the redundant-manipulator literature, e.g., [6,7,8,9,10] and references cited therein, Equation (9) has been treated as a differential equation that relates output to input. It has been analytically and computationally shown, however, that use of Equation (9) leads to irregularities in predicted manipulator performance [4,11,12,13], including numerical drift, nonholonomic behavior, and divergence of computation. Analysis has shown that use of the generalized inverse of Equation (8) leads to nonholonomic equations of redundancy resolution that create systematic noncyclicity of the manipulator [12,13], i.e., a periodic output trajectory maps into a nonperiodic input trajectory. While some effort has been devoted to creating generalized inverses that overcome these problems, e.g., [14,15] and references cited therein, no comprehensive result has been reported. A treatment based on concepts similar to those employed herein is shown to partially resolve the noncyclicity problem [16]. Theoretical and numerical results presented in [4,16] complement the literature cited and show that the generalized inverse velocity approach to manipulator redundancy resolution is fundamentally flawed.

While the foregoing analyses of deficiencies of the generalized inverse velocity approach do not focus specifically on obstacle avoidance, the inaccuracies implied cast doubt on use of the approach for kinematic control with obstacle avoidance. In particular, no inverse kinematic mapping at the configuration level is defined with free parameters, which means that an indirect velocity approach to obstacle avoidance must be employed. Since obstacle avoidance occurs in configuration space, not in velocity space, an adaptation of generalized inverse velocity differential equations must be created to treat an application that is stated in terms of manipulator configuration.

1.5. Manipulator Trajectory Planning and Obstacle Avoidance

An extensive literature on kinematically redundant manipulator trajectory planning and kinematic control appeared in the last quarter of the 20th century, based primarily on the generalized inverse velocity representation of Equation (9), e.g., [6,7,8,9,10] and references cited therein. The most basic kinematic control formulation seeks to use manipulator redundancy to cause manipulator output z(t) to follow a desired output trajectory ${\mathit{z}}_d(t)$ and to satisfy constraints such as avoiding collision of manipulator links with obstacles. Objectives and constraints in these applications are most naturally stated in terms of manipulator input and output coordinates, rather than velocities. The apparent simplicity of the velocity equations of Section 1.3, with the redundancy-related arbitrary velocity ${\dot{\mathit{y}}}^0(t)$ of Equation (9), however, has led research and development engineers to adopt the generalized inverse velocity mapping of Equation (9) to address the kinematic control objective, e.g., [7,8] and references cited therein.

Quite recently, a configuration-based inverse kinematic mapping for kinematically redundant serial manipulators has been presented that avoids deficiencies associated with the generalized inverse velocity mapping approach [3,4]. This method is summarized in Section 3 and applied for manipulator path planning with obstacle avoidance.

2. Generalized-Inverse-Velocity-Based Kinematic Control and Obstacle Avoidance

The conventional approach to satisfying constraints such as obstacle avoidance using the generalized inverse velocity approach is the projected-gradient method, employing the second term of Equation (9), i.e., the null space term, to minimize a cost function $f(\mathit{y})$ that models the secondary criterion. This is performed by defining

$${\dot{\mathit{y}}}^0(t)=-k{\nabla }_y\mathit{f}(\mathit{y}(t))$$

(10)

with $k>0$, where ${\nabla }_{y}f(\mathit{y})={\left[\mathsf{\partial }f(\mathit{y})/\mathsf{\partial }{y}_1,\dots ,\mathsf{\partial }\mathit{f}(\mathit{y})/\mathsf{\partial }{y}_n\right]}^T$, and projecting it onto the null space of the Jacobian matrix by means of the projector $\left(\mathit{I}-{{\mathit{G}}^{\prime }}^{†}(\mathit{y}){\mathit{G}}^{\prime }(\mathit{y})\right)$ of Equation (9) [17]. As a result, y(t) will move in a direction that decreases f(y), without affecting the desired output trajectory ${\mathbf{z}}_d(t)$, i.e., self-motion. When the constraint is to avoid obstacles, $f(\mathit{y})$ can be chosen as any function that penalizes manipulator link proximity to obstacles [18]. For example, if f(y) is defined as

$$f(\mathit{y})={\displaystyle \sum _{i,j}1/d_{ij}(\mathit{y})}$$

(11)

where $d_{ij}(\mathit{y})$ is the minimum distance between link i and obstacle j, manipulator links will tend to move away from obstacles.

Another approach to obstacle avoidance using the generalized inverse velocity method consists in choosing ${\dot{\mathit{y}}}^0(\mathrm{t})$ as an input velocity that attempts to generate an escape velocity ${\dot{\mathit{z}}}_o(\mathrm{t})$ at the point ${\mathrm{P}}_0$ of the manipulator that is closest to the obstacle, where such escape velocity is directed away from the obstacle. This requires yet another generalized inversion of the Jacobian matrix ${\mathit{J}}_o$ that maps input velocities to the velocity of ${\mathrm{P}}_0$ [19]. Alternatively, Lee and Buss [18] use the transpose of ${\mathit{J}}_o$ that has lower computational burden than generalized matrix inversion and avoids problems with singularities, at the expense of accuracy in generation of the desired escape velocity. This is not crucial, as long as the link moves away from the obstacle.

When a hierarchy of tasks exists, i.e., there is a primary task and several secondary tasks with different levels of priority, where each task has an associate Jacobian, lower-priority tasks are optimized by projecting velocities on the null space of higher-priority tasks [20]. This hierarchy of projections is used in the method of Saturation in the Null Space (SNS) [21], which saturates input velocities that reach their limits and slows the task, i.e., scales time, to restore feasibility if such saturations render the desired task infeasible. The SNS method can achieve obstacle avoidance by adapting limits of joint velocities near obstacles [22] or by augmenting joint velocities with velocities of control points on links of the manipulator and limiting these velocities in the vicinity of obstacles [23]. The SNS method is usually formulated at the velocity level, but it has been extended in [24,25] to function also at acceleration and torque levels.

In any case, the foregoing methods are based on use of the generalized inverse in the velocity equation, which leads to deficiencies discussed in Section 1.4. Moreover, these methods operate at velocity or acceleration levels, whereas constraints such as obstacle avoidance are naturally defined at the configuration level. This requires recasting these constraints approximately to set bounds on velocities, e.g., joint range limits are typically reformulated as bounds on joint velocities by using a forward difference approximation of the derivative of configuration [22], which may lead to violation of these constraints.

Use of the generalized inverse in the velocity equation is not a robust way to handle obstacle avoidance, because it is based on a velocity equation that needs to be numerically integrated to obtain the configuration of the manipulator. For example, if ${\dot{\mathit{y}}}^0(t)$ of Equation (10) is inserted into Equation (9), which is integrated numerically with a given time step Δt, the resulting trajectory of the manipulator may not be feasible. This is because, as time progresses, Equation (9) is integrated, and the cost function $f(\mathit{y})$ may not be minimized at a sufficiently high rate to avoid obstacles. Indeed, if the minimization of $f(\mathit{y})$ is not performed sufficiently quickly as the manipulator approaches obstacles, penetration of an obstacle will occur that will produce a zero denominator in Equation (11), i.e., ${\mathrm{d}}_{ij}(\mathit{y})=0$ for some (i,j), and this will lead to failure of numerical integration. As is demonstrated in Section 4.1.2 and Section 4.2.3, it is necessary to carefully select the values of Δt (time step) and k (gain appearing in Equation (10)). In fact, as shown, it may be necessary to use extremely small values of Δt and k to obtain motion that is free of obstacle penetration. On the one hand, simulating the full trajectory using a small Δt means long computation times. On the other hand, when using Equation (9) for real-time kinematic control of the manipulator, a small Δt imposes extreme demands on the control hardware, which must be able to operate at such small sample times.

3. Operational Configuration Space Kinematic Control and Obstacle Avoidance

To replace the generalized inverse velocity formulation of Section 2 with a configuration space formulation, it is required that a set-valued inverse configuration kinematic mapping be constructed, i.e., that a solution $\mathit{y}=\mathit{g}(\mathit{z},\mathit{v})$ of Equation (1) be found with an arbitrary vector of parameters $\mathit{v}\in {\mathrm{R}}^{\mathrm{n}-\mathrm{m}}$. Such a mapping has been presented in [3] and extended in [4] on an operational space differentiable manifold, as summarized in this section.

3.1. Inverse Configuration Kinematics

The entire regular configuration space $\tilde{\mathrm{X}}$ cannot, in general, be characterized by a single continuously differentiable inverse kinematic mapping. The only practical global inverse kinematic representation is based on concepts of differential geometry [2] that employ local representations on open subsets ${\mathrm{N}}^{\mathrm{j}}\in \tilde{\mathrm{X}}$, whose union is the entire regular configuration space, i.e., ${\displaystyle {\cup }_{\mathrm{j}}{\mathrm{N}}^{\mathrm{j}}}=\tilde{\mathrm{X}}$. In each ${\mathrm{N}}^{\mathrm{j}}$, there is a base point ${\overline{\mathit{x}}}^{\mathrm{j}}$ about which the inverse kinematic mapping is constructed. The inverse kinematic mapping process that follows takes place on each ${\mathrm{N}}^{\mathrm{j}}$. For a given j and base point ${\overline{\mathit{x}}}^{\mathrm{j}}$ in ${\mathrm{N}}^{\mathrm{j}}$, define the $\mathrm{n}\times \mathrm{m}$ matrix ${\mathit{U}}^j$ and an $\mathrm{n}\times (\mathrm{n}-\mathrm{m})$ matrix ${\mathit{V}}^j$ such that

$${\mathit{U}}^j={{\mathit{G}}^{\prime }}^{\mathrm{T}}({\overline{\mathit{y}}}^j) {\mathit{G}}^{\prime }({\overline{\mathit{y}}}^j){\mathit{V}}^j=\mathbf{0} {\mathit{V}}^j{}^{\mathrm{T}}{\mathit{V}}^j=\mathit{I}$$

(12)

where ${\mathit{V}}^j$ is computed as a matrix whose columns form an orthonormal basis of the null space of ${\mathit{G}}^{\prime }({\overline{\mathit{y}}}^{\mathrm{j}})$, e.g., in MATLAB, using singular-value decomposition [26]. The matrices ${\mathit{U}}^{\mathrm{j}} \mathrm{and} {\mathit{V}}^{\mathrm{j}}$ are defined to be constant on ${\mathrm{N}}^{\mathrm{j}}$. Note from the second equation of Equation (12) that ${\mathit{U}}^j{}^T{\mathit{V}}^j=\mathbf{0}$ and ${\mathit{V}}^j{}^T{\mathit{U}}^j=\mathbf{0}$. Since ${\mathit{G}}^{\prime }({\overline{\mathit{y}}}^j)$ has full rank, so do ${\mathit{U}}^j$ and ${\mathit{V}}^j$. Further, since ${\mathit{V}}^j{}^{\mathrm{T}}{\mathit{U}}^j=\mathbf{0}$, the columns of ${\mathit{V}}^j$ are orthogonal to the columns of ${\mathit{U}}^j$ and vice-versa. The n linearly independent columns of ${\mathit{U}}^j$ (m columns) and ${\mathit{V}}^j$ ($\mathrm{n}-\mathrm{m}$ columns) therefore span ${\mathrm{R}}^{\mathrm{n}}$.

Using ${\mathit{V}}^j$ and ${\mathit{U}}^j$ of Equation (12), any solution of Equation (1) for y in a neighborhood of ${\overline{\mathit{y}}}^j$ can be written in the form

$$\mathit{y}={\overline{\mathit{y}}}^j+{\mathit{V}}^j(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^j(\mathit{u}-{\overline{\mathit{u}}}^{\mathrm{j}})$$

(13)

where ${\overline{\mathit{v}}}^{\mathrm{j}} \mathrm{and} {\overline{\mathit{u}}}^{\mathrm{j}}$ are values of v and u associated with ${\overline{\mathit{x}}}^{\mathrm{j}}$ on the trajectory that first enters ${\mathrm{N}}^{\mathrm{j}}$. They are introduced to assure continuity of y as a function of v and z. On ${\mathrm{N}}^1$, ${\overline{\mathit{v}}}^1=\mathbf{0}$ and ${\overline{\mathit{u}}}^1=\mathbf{0}$. Note that, at $\mathit{y}={\overline{\mathit{y}}}^j$ in Equation (13), $\mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}} \mathrm{and} \mathit{u}={\overline{\mathit{u}}}^{\mathrm{j}}$. To see that there is a unique solution of Equation (1) with y of Equation (13), i.e., a unique solution of

$$\mathit{G}({\overline{\mathit{y}}}^j+{\mathit{V}}^j(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^j(\mathit{u}-{\overline{\mathit{u}}}^{\mathrm{j}}))-\mathit{z}=0$$

(14)

for u as a function of z and v in a neighborhood of $\mathit{z}={\overline{\mathit{z}}}^{\mathrm{j}}$ and $\mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$, the derivative of the left side of Equation (14) with respect to u, evaluated at ${\overline{\mathit{x}}}^j$, i.e., at $\mathit{z}={\overline{\mathit{z}}}^{\mathrm{j}}$ and $\mathit{v}={\overline{\mathit{v}}}^{\mathrm{j}}$, using the chain rule of differentiation, is $-{\mathit{G}}^{\prime }({\overline{\mathit{y}}}^j){\mathit{U}}^j=-{\mathit{U}}^j{}^{\mathrm{T}}{\mathit{U}}^j$, which is nonsingular. Thus, the implicit-function theorem [27] implies existence of a unique, twice continuously differentiable solution,

$$\mathit{u}={\mathit{h}}^j(\mathit{z},\mathit{v})$$

(15)

of Equation (14) in a neighborhood of ${\overline{\mathit{x}}}^j$. From Equation (13),

$$\mathit{y}(\mathit{z},\mathit{v})={\overline{\mathit{y}}}^j+{\mathit{V}}^j(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^j({\mathit{h}}^j(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}})$$

(16)

This is the desired set-valued inverse configuration kinematic mapping on ${\mathrm{N}}^{\mathrm{j}}$.

If $\mathit{z}(t) \mathrm{and} \mathit{v}(t)$ are periodic of period $t_p$ on ${\mathrm{N}}^{\mathrm{j}}$, i.e., $\mathit{z}(t+t_p)=\mathit{z}(t)$ and $\mathit{v}(t+t_p)=\mathit{v}(t)$, since Equation (16) holds throughout ${\mathrm{N}}^{\mathrm{j}}$,

$$\mathit{y}(t+t_p)={\overline{\mathit{y}}}^{\mathrm{j}}+{\mathit{V}}^{\mathrm{j}}(\mathit{v}(t+t_p)-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^{\mathrm{j}}(\mathit{h}(\mathit{z}(t+t_p),\mathit{v}(t+t_p))-{\overline{\mathit{u}}}^{\mathrm{j}})=\mathit{y}(t)$$

(17)

Thus, $\mathit{y}(t)$ is periodic of period $t_p$, and the manipulator is cyclic on ${\mathrm{N}}^{\mathrm{j}}$. This shows that, with the differentiable manifold formulation, the manipulator is locally cyclic [4,16]. In kinematic control with obstacle avoidance, however, during a transient control time interval in which $\mathit{v}(t)$ is activated to resist penetration of an obstacle, it will generally not be periodic. During this time interval, the manipulator will not be locally cyclic. If, after transient action to avoid obstacles, the desired periodic output goes through cycles without control action required to avoid obstacles, i.e., $\mathit{v}(t)$ becomes constant, then during these cycles, the manipulator will be locally cyclic.

The role of the function ${\mathit{h}}^j(\mathit{z},\mathit{v})$ is important in assuring satisfaction of Equation (1). This is in contrast with the generalized inverse velocity approach presented in Section 2, in which Equation (1) is ignored. A computationally efficient iterative method for evaluation of ${\mathit{h}}^j(\mathit{z},\mathit{v})$ is presented in [3,4]. In this computation, when the number of iterations required for convergence exceeds a specified tolerance, the associated configuration $\mathit{x}={\left[\begin{array}{cc}{\mathit{y}}^{\mathrm{T}}& {\mathit{z}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ is designated ${\overline{\mathit{x}}}^{j+1}$, the associated y, v, and u are designated ${\overline{\mathit{y}}}^{\mathrm{j}+1}$, ${\overline{\mathit{v}}}^{\mathrm{j}+1}$, and ${\overline{\mathit{u}}}^{\mathrm{j}+1}$, a new neighborhood ${\mathrm{N}}^{\mathrm{j}+1}$ is entered, and the parameterization is redefined. As shown in [28] for dynamic system simulation, less than 0.1% of CPU time and no user interaction are required for reparameterization. For more detail on the process of selecting configurations ${\overline{\mathit{x}}}^j$ and reparameterization calculations, see [3,4,28].

For a given output $\mathit{z}$, with $\mathit{v}\in {\mathrm{R}}^{\mathrm{n}-\mathrm{m}}$ arbitrary in a neighborhood of v = ${\overline{\mathit{v}}}^{\mathrm{j}}$, Equation (16) defines a set of input coordinates,

$$\mathrm{SMM}(\mathit{z})=\{\mathit{y}={\overline{\mathit{y}}}^j+{\mathit{V}}^j(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^j({\mathit{h}}^j(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}}): \mathit{v} \mathrm{in} \mathrm{a} \mathrm{neighborhood} \mathrm{of} {\overline{\mathit{v}}}^{\mathrm{j}}\}$$

(18)

called the manipulator self-motion manifold in input space associated with output $\mathit{z}$. Since ${\mathit{h}}^j(\mathit{z},\mathit{v})$ is the solution of Equation (14), $\mathit{G}({\overline{\mathit{y}}}^j+{\mathit{V}}^j(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^j({\mathit{h}}^j(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}}))-\mathit{z}=\mathbf{0}$, for all v in a neighborhood of v = ${\overline{\mathit{v}}}^{\mathrm{j}}$, $y(\mathit{z},\mathit{v})$ of Equation (16) maps into $\mathit{z}$, i.e., $\mathit{z}=\mathit{G}(\mathit{y}(\mathit{z},\mathit{v}))$. Components of the vector $\mathit{v}\in {\mathrm{R}}^{\mathrm{n}-\mathrm{m}}$ are called self-motion coordinates. With arbitrary self-motion coordinates v in a neighborhood of ${\overline{\mathit{v}}}^{\mathrm{j}}$, Equation (16) defines $\mathrm{n}-\mathrm{m}$ redundant degrees of freedom $\mathit{v}\in {\mathrm{R}}^{n-m}$ of the manipulator that enables it to meet requirements that could not be met with a nonredundant manipulator. The restriction of v to a neighborhood of ${\overline{\mathit{v}}}^{\mathrm{j}}$ in the foregoing is to meet hypotheses of the implicit-function theorem. As shown in Section 4.1.1 and Section 4.2.2, Equations (16) and (18) may hold for large v in applications.

It is important to note that the self-motion manifold of Equation (18) is defined at the configuration level. Since configuration information is not defined in the generalized inverse velocity formulation, the self-motion manifold and self-motion coordinates cannot be explicitly defined and are not available for obstacle avoidance in that formulation.

3.2. Operational Space Differentiable Manifold

Defining manipulator operational coordinates $\mathit{w}={\left[\begin{array}{cc}{\mathit{z}}^{\mathrm{T}}& {\mathit{v}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in R^n$ and functional coordinates $\mathit{s}={\left[\begin{array}{cc}{\mathit{y}}^{\mathrm{T}}& {\mathit{w}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in {\mathrm{R}}^{2\mathrm{n}}$, the manipulator regular functional configuration space is

$$\tilde{\mathrm{S}}={\displaystyle \underset{{\mathrm{N}}^{\mathrm{j}}}{\cup }\left\{\mathit{s}\in {\mathrm{R}}^{2\mathrm{n}}:{\overline{\mathit{x}}}^j={\left[\begin{array}{cc}{\overline{\mathit{y}}}^j{}^{\mathrm{T}}& {\overline{\mathit{z}}}^j{}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}\in \tilde{\mathrm{X}},\mathit{y}\equiv {\mathit{\psi }}^{\mathrm{j}}(\mathit{w})={\overline{\mathit{y}}}^j+{\mathit{V}}^j(\mathit{v}-{\overline{\mathit{v}}}^{\mathrm{j}})-{\mathit{U}}^j({\mathit{h}}^j(\mathit{z},\mathit{v})-{\overline{\mathit{u}}}^{\mathrm{j}})\right\}}$$

(19)

Similar to the definition of the manipulator configuration space of Equation (2) as the product of input and output spaces, the serial manipulator regular functional configuration space is defined as the product of input and operational spaces. This is important in establishing $\tilde{\mathrm{S}}$ as a differentiable manifold, as presented in [4]. A family of charts (${\mathrm{N}}^{\mathrm{j}}$, ${\mathit{\psi }}^{\mathrm{j}}(\mathit{w})$) (an atlas) is defined to cover $\tilde{\mathrm{S}}$, such that the mappings ${\mathit{\psi }}^{\mathrm{j}}$ are compatible and $\tilde{\mathrm{S}}$ is a differentiable manifold that is parameterized by w [2,4]. The differentiable manifold can thus be parameterized by either y or w. It cannot, however, be parameterized by only output z.

Kinematics on $\tilde{\mathrm{S}}$ must be carried out on individual charts $({\mathrm{N}}^{\mathrm{j}},{\mathit{\psi }}^j(w))$ and transitioned to adjacent charts as manipulator configurations progress along a trajectory in $\tilde{\mathrm{S}}$, as shown schematically in Figure 2. Piecewise analysis on charts is unavoidable since, in general, there is no globally valid operational coordinate parameterization $\mathit{\psi }(\mathit{w})$ of $\tilde{\mathrm{S}}$ [2]. This attribute of differential geometry that transforms local to global properties of sets and mappings is one of its greatest contributions. The unavoidable reality, however, is that one must adopt local operational space parameterizations, since no global parameterization generally exists.

3.3. Differentiable-Manifold Output Trajectory Tracking and Obstacle Avoidance Algorithm

An output trajectory tracking and obstacle avoidance algorithm based on the inverse kinematic mapping of Equation (16) is presented to carry out kinematic control more rigorously and efficiently than with the generalized-inverse-velocity-based method of Section 2. In [3], the inverse mapping was employed to avoid collisions in redundant manipulators using an algorithm that marched along one-dimensional self-motion manifolds. That algorithm, however, is limited to manipulators with a degree of redundancy of one. First, a nominal trajectory ${\mathit{y}}_n(\mathit{t})$ for the inputs $\mathit{y}$ is selected that yields the desired output trajectory ${\mathit{z}}_d(\mathit{t})$, without exploiting kinematic redundancy. When the manipulator starts to move, it follows this nominal trajectory until a collision is detected at some $\mathit{z}*$ of the desired output trajectory, i.e., until the intersection of two bodies is not empty. When a collision is detected, the algorithm presented in [3] performs self-motions defined by Equation (18) that keeps $\mathit{z}=\mathit{z}*$, i.e., the self-motion coordinate $\mathit{v}$ is varied on a grid with a given step $\mathsf{\Delta }\mathit{v}$ along one direction (or the opposite) of the self-motion manifold until the interference disappears. This algorithm is feasible only when the self-motion manifold is one-dimensional, because one only needs to march along self-motion curves, exhaustively searching until a collision-free configuration is found.

In manipulators with higher degrees of redundancy, such as the manipulator of Section 1.2, the obstacle avoidance algorithm used in [3] is not feasible, because one would need to perform an exhaustive grid search in the higher-dimensional space of self-motion coordinates $\mathit{v}$, which would be computationally prohibitive, e.g., a four-dimensional grid for the manipulator of Section 1.2. The new algorithm employed herein efficiently treats obstacle avoidance using the inverse mapping of Equation (16) for higher degrees of redundancy, as the examples of Section 4.1.1 and Section 4.2.2 demonstrate.

The new algorithm is based on the fact that, in the vicinity of a contact between two bodies, it is possible to define a gap function $\mathit{g}$ [29,30] that is the signed distance between the closest points of the two bodies along their common normal. This works even when one of the bodies has a nonsmooth shape and does not have a unique normal at the point of contact [29,30]. Figure 3 illustrates this gap function $\mathit{g}$. Note that $\mathit{g}$ is a signed distance, which is positive when the bodies do not intersect, zero when their boundaries touch, and negative when one body has penetrated the other. The gap function $\mathit{g}$ depends on the relative pose of the contacting bodies, which is defined by input coordinates, i.e., $\mathit{g}=\mathit{g}(\mathit{y})$. Using the inverse mapping of Equation (16), one can write the gap function in terms of the operational coordinates, i.e., $\mathit{g}=\mathit{g}(\mathit{y}(\mathit{z},\mathit{v}))$.

Taking the foregoing into account, the new algorithm is as follows:

STEP 1: Initialize $\mathit{t}={\mathit{t}}_0$ and select a time step $\mathsf{\Delta }\mathit{t}$. The desired output trajectory is ${\mathit{z}}_d(\mathit{t})$ for ${\mathit{t}}_0<\mathit{t}<{\mathit{t}}_1$. The initial configuration ${\mathit{y}}_0$ yields the initial desired $\mathit{z}$, i.e., $G({\mathit{y}}_0)={\mathit{z}}_d({\mathit{t}}_0)$. Construct the inverse mapping of Equation (16) in a neighborhood of $\overline{\mathit{y}}={\mathit{y}}_0$, i.e., compute $\mathit{U}$ and $\mathit{V}$ and initialize $\mathit{v}$ = 0 and u = 0. Throughout execution of the algorithm, the self-motion vector v is updated only when necessary to avoid obstacles. If other secondary goals are to be met, the self-motion vector v can be used to optimize these secondary goals. In the present algorithm, the focus is on the use of redundancy only for obstacle avoidance.

STEP 2: Set $\mathit{z}={\mathit{z}}_d(\mathit{t})$, solve for $\mathit{u}=\mathit{h}(\mathit{z},\mathit{v})$ as in Section 3.1, and evaluate $\mathit{y}=\mathit{y}(\mathit{v},\mathit{u})$ of Equation (16). Whenever necessary, perform a reparameterization, i.e., a transition between charts illustrated in Figure 2, as outlined in Section 3.2.

STEP 3: For the computed configuration $\mathit{y}$, check if interferences occur, i.e., if any link of the manipulator intersects an obstacle. If no intersections occur, continue with step 4. Otherwise, go to step 5.

STEP 4: Send the collision-free configuration $\mathit{y}$ to the controller of the manipulator and proceed to the next time step, i.e., update $\mathit{t}=\mathit{t}+\mathsf{\Delta }\mathit{t}$. If $\mathit{t}\le {\mathit{t}}_1$, return to step 2. Otherwise, the desired trajectory has been completed and the algorithm ends.

STEP 5: If the algorithm has reached this step, there exists mechanical interference between the manipulator and at least one obstacle for the current value of $\mathit{y}=\mathit{y}*$, which is obtained from $\mathit{z}=\mathit{z}*$ and $\mathit{v}=\mathit{v}*$. Assume that $\mathit{c}>0$ contacts have occurred between links of the manipulator and obstacles. In that case, contacts define gap functions ${\mathit{g}}_i(\mathit{z},\mathit{v})$, $\mathit{i}=1,\dots \mathit{,}c$, which will be negative because obstacles are penetrated. The objective is to find a value of v that renders the gap functions non-negative. This can be formulated as the following nonlinear system:

$$\begin{array}{l}{\mathit{g}}_1(\mathit{z},\mathit{v})=k_1^2\\ \dots \\ {\mathit{g}}_c(\mathit{z},\mathit{v})=k_c^2\end{array}$$

(20)

where $\mathit{k}={[k_1,\dots ,k_c]}^T$ is a vector of auxiliary variables that are introduced to transform the desired inequalities ${\mathit{g}}_i(\mathit{z},\mathit{v})\ge 0$ into equivalent equalities ${\mathit{g}}_i(\mathit{z},\mathit{v})=k_i^2$. Next, Equation (20) is solved for $\mathit{r}={[{\mathit{v}}^T,{\mathit{k}}^T]}^T$, using the Newton–Raphson (N-R) method [26], where the starting values of v and k for N-R iteration are $\mathit{v}=\mathit{v}*$, i.e., the value that produced the interference that led to this step of the algorithm, and k = 0. Since Equation (20) is c equations in (c + n − m) unknowns, it comprises an underdetermined system. Therefore, when inverting the Jacobian used in the N-R method, the minimum-norm Moore–Penrose pseudoinverse is used, which updates the unknowns r with the vector Δr of minimum norm. The N-R method used to solve Equation (20) is described by Algorithm 1 shown below, where

$$\tilde{\mathit{g}}(\mathit{r})={\left[{\mathit{g}}_1(\mathit{z},\mathit{v})-k_1^2,\dots ,{\mathit{g}}_c(\mathit{z},\mathit{v})-k_c^2\right]}^T$$

(21)

and $\tilde{\mathit{g}}\prime (\mathit{r})$ is the $\mathit{c}\times (\mathit{c}+\mathit{n}-\mathit{m})$ Jacobian of $\tilde{\mathit{g}}(\mathit{r})$ with respect to r.

Algorithm 1. Newton–Raphson iterations to solve Equation (20)
1:	$\mathit{r}\leftarrow {[{\left(\mathit{v}*\right)}^T,0]}^T$
2:	$\mathrm{iterations}\leftarrow 0$
3:	do
4:	$\mathsf{\Delta }\mathit{r}\leftarrow -\tilde{\mathit{g}}{\prime }^{†}(\mathit{r})\tilde{\mathit{g}}(\mathit{r})$
5:	$\mathit{r}\leftarrow \mathit{r}+\mathsf{\Delta }r$
6:	$\mathrm{iterations} \leftarrow \mathrm{iterations}+1$
7:	$\mathbf{while} \Vert \tilde{\mathit{g}}(\mathit{r})\Vert >\mathit{\epsilon }$ AND iterations < max_iterations

Since the increment Δr with minimum norm is used in lines 4–5 of Algorithm 1 to update the unknowns, this favors continuity of the configuration of the manipulator. In fact, as the examples of Section 4 demonstrate, the proposed algorithm generates continuous trajectories for the manipulator, and execution of Algorithm 1 (whose objective is to find a new value of self-motion coordinates v that avoids collisions) takes only a few milliseconds on a modern computer. The N-R method stops when a solution $\mathit{r}=[{\mathit{v}}^T,{\mathit{k}}^T{]}^T$ that satisfies Equation (20) is found, or when a maximum of max_iterations is exceeded. In the first case, the algorithm yields a configuration y that is collision-free, and the algorithm jumps to step 4. In the second case, the desired trajectory ${\mathit{z}}_d(t)$ can be considered as infeasible, because no configuration (sufficiently near to the configuration at the previous time step) can be found to continue executing the trajectory while avoiding obstacles, and the algorithm ends.

4. Numerical Examples

The foregoing theory and computational algorithms are tested in this section using planar examples with four and twenty degrees of redundancy. The selection of planar examples is for ease of analysis and visualization of results. The theory and algorithm of Section 3.3 is equally applicable for spatial and planar manipulators. For an application of the differentiable manifold formulation with a seven degree of freedom spatial redundant serial manipulator, see [3].

4.1. Four-Degree-of-Redundancy Trajectory Tracking and Obstacle Avoidance

4.1.1. Differentiable-Manifold Output Tracking and Obstacle Avoidance

In this section, the method described in Section 3.3 is applied to plan the motion of the manipulator of Section 1.2 that has four degrees of redundancy. The manipulator starts with input ${\mathit{y}}_0={\left[0.5688, 0.4694, 0.0119, 0.3371, 0.1622, 0.7943\right]}^T$, and its end-effector is required to follow the periodic elliptic trajectory

$${\mathit{z}}_{\mathit{d}}(\mathit{t})={[\mathit{a}\cos (\mathit{\theta })\cos (\mathit{t})-\mathit{b}\sin (\mathit{\theta })\sin (\mathit{t})+{\mathit{x}}_0,\mathit{a}\sin (\mathit{\theta })\cos (\mathit{t})+\mathit{b}\cos (\mathit{\theta })\sin (\mathit{t})+{\mathit{y}}_0]}^T$$

(22)

while its links must avoid a circular obstacle centered at (1.5, 2) with radius 0.5. Equation (22) is the parametric equation of an ellipse centered at (x₀ = 1.7, y₀ = 2.9) with semiaxes a = 2 and b = 0.25, whose longer semiaxis forms an angle of θ = −0.15 rad with the x axis. The end-effector is required to follow three cycles on this ellipse, i.e., t runs from t = 0 to t = 6 π, with a time step of $\mathsf{\Delta }\mathit{t}=0.01$ s.

The algorithm described in Section 3.3 is carried out with the following parameters:

(1)

Iterations in the method presented in [4] to evaluate $\mathit{u}={\mathit{h}}^j(\mathit{z},\mathit{v})$ stop when $\Vert \mathsf{\Delta }\mathit{u}\Vert <0.0001$.

(2)

Iterations in Algorithm 1 stop when $\Vert \tilde{\mathit{g}}(\mathit{r})\Vert <0.001$ or when iterations > 10 = max_iterations.

With these parameters, the algorithm generates the time histories of y(t) and v(t) shown in Figure 4. As this figure shows, the time history of these variables is periodic after the first cycle, where vertical dashed lines separate cycles. In particular, v is constant after the first cycle. It can be seen in Figure 4 that the trajectory of y(t) becomes cyclic when v becomes constant, even before the output trajectory of Equation (22) has completed its first cycle. This locally cyclic behavior is consistent with the theory presented in Section 3.1.

Snapshots of the manipulator executing this trajectory are shown in Figure 5. An animation is shown in supplementary video S1 attached. Figure 5a–i show execution of the first cycle. In Figure 5a, the manipulator moves with v = 0 as it approaches the obstacle, until touching it. Then, the algorithm of Section 3.3 starts to actively update v(t) to avoid penetration of the obstacle. Figure 5b–g show how the manipulator continues tracking the desired elliptical trajectory, while some of its links touch the circular obstacle, until contact is lost in Figure 5h and v is no longer updated. After this, the manipulator continues executing the trajectory far from the obstacle until it completes the first cycle, as shown in Figure 5i.

Execution of the second cycle begins with the manipulator approaching the obstacle, as illustrated in Figure 5j. As shown in Figure 5k–n, during the second cycle, the manipulator is not in contact with the obstacle except during a very short period of time shown in Figure 5m, in which one of the links becomes tangent to the obstacle. This contact period is very short, as can be better observed in the supplementary animation S1 attached to the paper. After this short contact is lost (Figure 5n), the manipulator continues moving far from the obstacle until completing the second cycle (Figure 5o). The third and subsequent cycles are identical to the second cycle, as shown in Figure 4.

As Figure 4 and Figure 5 show, during the first cycle of the desired trajectory given by Equation (22), the algorithm of Section 3.3 actively updates v(t) to prevent penetration of the obstacle during contact. After contact is lost, v(t) does not need to be further updated and the input trajectory becomes cyclic, by virtue of Equation (17). Interestingly, the constant value that v(t) adopts after contact is lost in the first cycle allows the manipulator to continue repeating subsequent cycles during which contact without penetration occurs only during a very short fraction of the cycle, as illustrated in Figure 5m.

The average time required to run Algorithm 1 is 1.2 milliseconds (time measured on the following CPU running MATLAB R2022b: Intel Core i7-8750H at 2.20 GHz). This is the time required to find a new value of self-motion coordinates $\mathit{v}$ that avoids penetration of obstacles every time that such penetration is detected, which demonstrates a rapid correction of the trajectory to avoid obstacles, compatible with real time requirements.

As shown in Figure 5, to prevent obstacle penetration, the algorithm of Section 3.3 keeps the links in contact with the obstacle and moving smoothly around it until contact is lost, as required by the trajectory of the end-effector. If it were necessary to avoid the obstacle without contacting it, leaving some safety distance ${\mathit{d}}_s$, then one would only need to change the gap functions in Equation (20) to ${\mathit{g}}_i(\mathit{z},\mathit{v})-{\mathit{d}}_s={\mathit{k}}_i^2$, making it easier to accurately regulate distance to obstacles than with the generalized-inverse-velocity-based approach, which is illustrated next.

4.1.2. Generalized-Inverse-Velocity Output Tracking and Obstacle Avoidance

In this section, the approach described in Section 2 is used to solve the problem treated in Section 4.1.1, where the manipulator with four degrees of redundancy must track the periodic trajectory given in Equation (22). The manipulator starts at the same configuration ${\mathit{y}}_0$ as in Section 4.1.1. The same time step, $\mathsf{\Delta }\mathit{t}=0.01$ seconds, is used in the present section to numerically integrate Equation (9) using a fourth-order Runge–Kutta integrator [26]. In contrast to the generalized-inverse-velocity-based method, the algorithm of Section 3.3 does not need to perform any numerical integration. The time step in that case is used only to progress in time according to step 4 of the algorithm.

The method described In Section 2 is carried out by numerically integrating with a fixed time step of $\Delta t=0.01$ s. For a given time step $\Delta t$, the main parameter that can be tuned in the method of Section 2 is the coefficient k > 0, which adjusts the intensity of the penalty function $f(\mathit{y})$. A higher k will give greater weight to minimization of the cost function, which means that the manipulator links will tend to have larger distances from the obstacles.

Figure 6 presents a sequence of configurations adopted by the manipulator for three different values of k when executing the first half of the first cycle of the periodic trajectory given by Equation (22), i.e., when integrating Equation (9) from t = 0 to t = π seconds. According to Figure 6c, for higher values of k, the manipulator adopts configurations that are unnecessarily far from the obstacle. Thus, in the following, the value k = 0.01, which generates the result shown in Figure 6a, is used. Note, however, that if k is chosen too small, the rate of minimization of $f(\mathit{y})$ may be too slow for the motion of the robot, and one or more links may penetrate the obstacle before the term ${\dot{\mathit{y}}}^0(t)$ of Equation (10) leads to a trajectory that avoids the obstacle.

When the manipulator is required to transit the elliptic trajectory three times by integrating Equation (9) with k = 0.01 from t = 0 to t = 6π, the time evolution of inputs $y_{\mathrm{i}}(t)$ presented in Figure 7 is obtained. As this figure shows, the time history of $y_{\mathrm{i}}(t)$ is not cyclic, because the value of each $y_{\mathrm{i}}(t)$ at the end of each cycle of the output trajectory of Equation (22) does not coincide with its value at the beginning. This is an example of noncyclicity of the generalized inverse velocity approach to redundancy resolution, as discussed in Section 1.4. In contrast, as shown in Section 4.1.1, the differentiable manifold approach leads to a locally cyclic trajectory after nonzero control action is required to avoid the obstacle.

To better illustrate this noncyclic trajectory, Figure 8 presents a sequence of snapshots of the manipulator as its inputs $y_{\mathrm{i}}(t)$ traverse the trajectories graphed in Figure 7. Only the first two cycles are presented in Figure 8. Supplementary video S2 attached displays the animation of these two cycles, as well as a third cycle.

Figure 8a–i show execution of the first cycle. In Figure 8a, the manipulator is approaching the obstacle. Between Figure 8b,e, the gradient term of Equation (10) acts to avoid the obstacle. Between Figure 8f,i, the links of the manipulator are sufficiently far from the obstacle, and the cost f of Equation (11) is negligible (with negligible derivative), so the corrective term of Equation (10) does not have significant effect. Figure 8i shows the configuration of the manipulator at the end of the first cycle of the elliptic trajectory, which does not coincide with the initial configuration shown in Figure 8a.

Figure 8j–o show a sequence of snapshots of the configuration of the manipulator during execution of the second cycle, completing the second cycle in Figure 8o. When comparing these snapshots to those of the first cycle, one observes the noncyclicity of the input trajectory, where the manipulator is further away from the obstacle than in Figure 8a,i. A third cycle can be observed in attached video S2, which ends with the manipulator even further away from the obstacle.

As this example shows, generalized-inverse-velocity-based kinematic control can be used for obstacle avoidance, but its performance is not easy to tune or predict. For this example, a time step of $\Delta t=0.01$ seconds was sufficient to achieve obstacle avoidance for different values of k, and it was possible to avoid obstacles without excessively conservative distances between links and obstacles by choosing a small value of k = 0.01. However, this worked well only for the first cycle. After completing the first cycle, due to the nonholonomic nature of Equation (9), the manipulator drifts and executes subsequent cycles far from the obstacle, where ${\dot{\mathit{y}}}^0(t)$ of Equation (10) becomes negligible, yielding postures similar to those that would have been adopted if the first cycle had been executed using a larger value of k, as demonstrated in Figure 6. This suggests that the choice of parameters k and $\Delta t$ (which have been kept constant throughout this example) may actually be rather arbitrary, because nonholonomy may end up wasting or undoing a careful choice of such parameters. This is even more important in difficult scenarios such as the one treated in Section 4.2.3, where a highly redundant manipulator must avoid several obstacles, and choice of parameters k and $\Delta t$ is critical for performance of generalized-inverse-velocity-based kinematic control.

4.2. Twenty-Degree-of-Redundancy Trajectory Tracking and Obstacle Avoidance

4.2.1. A Twenty-Degree-of-Redundancy Manipulator

Consider a highly redundant variant of the manipulator of Figure 1 that is shown in Figure 9. Instead of having four links with relative angles $y_3$ … $y_6$, it has 21 links with relative angles $y_3$ … $y_{23}$, where all links have unit length.

The initial configuration of the manipulator is shown in Figure 10, given by the following input angles.

$${\mathit{y}}_0={\left[\begin{array}{llllllllll} & 0.7727& 0.1546& 0.1040& 0.4934& 0.9715& 0.3697& 0.6284& \dots & \\ \dots & 0.2710& 0.7981& 0.2884& 0.5227& 0.6964& 0.8729& 0.9409& \dots & \\ \dots & 0.5358& 0.1209& 0.1221& 0.2128& 0.7854& 0.2131& 0.7936& 0.2532& 0.9685\end{array}\right]}^T$$

(23)

This manipulator has n = 23 degrees of freedom. Outputs of the manipulator are the position coordinates of the outboard end of the last link and its orientation, i.e.,

$$\mathit{z}=\mathit{G}(\mathit{y})=\left[\begin{array}{c}{y}_1+{\displaystyle \sum _{i=3}^{23}\cos \left({\displaystyle \sum _{j=3}^i{y}_j}\right)}\\ y_2+{\displaystyle \sum _{i=3}^{23}\sin \left({\displaystyle \sum _{j=3}^i{y}_j}\right)}\\ {\displaystyle \sum _{i=3}^{23}{y}_i}\end{array}\right]$$

(24)

Since there are m = 3 outputs, the degree of redundancy is r = n $-$ m = 23 $-$ 3 = 20, so this can be considered a hyper-redundant manipulator.

The task of this manipulator is to follow the output trajectory

$${\mathit{z}}_{\mathit{d}}(t)={\left[-2.1164+2.5\left(\cos \right(4t)-1) , 4.0455+\sin (3t) , -1.6035 \mathrm{rad}\right]}^T$$

(25)

which means that the outboard endpoint of the manipulator must describe the desired path given by the first two coordinates of Equation (25), and the last link maintains a constant orientation given by the last coordinate of Equation (25). The desired path is displayed in Figure 10. The desired trajectory must be achieved from t = 0 to t = 1.2, while the manipulator avoids three circular obstacles that are shown in Figure 10. Two obstacles are stationary, labeled O1 and O2 in Figure 10, where O1 is a circle centered at (0, 2) with unit radius, and O2 is another circle of unit radius centered at (−2, 7). Note that, initially, the manipulator wraps around obstacle O1. There is a third circular obstacle labeled Om in Figure 10 with radius 0.75 that is mobile. Its center starts at (−4, 2) and translates with constant speed until reaching the point (−3, 5.5) at t = 1, remaining static at that point for t > 1.

4.2.2. Differentiable-Manifold Output Tracking and Obstacle Avoidance

The path-planning obstacle avoidance problem is first treated using the differentiable manifold algorithm of Section 3.3 with a time step of $\Delta t=0.001$ s, which results in the sequence of postures shown in Figure 11 (supplementary video S3 is attached that illustrates the continuous motion). As demonstrated by Figure 11, the algorithm is able to resolve redundancy and obtain a feasible motion that tracks the desired output trajectory of Equation (25), while avoiding penetration of all obstacles for all time. First, the manipulator contacts obstacle O1 (Figure 11b), and then it contacts obstacle O2 (Figure 11c–d), to lose contact later (Figure 11e–h). In Figure 11i, the manipulator contacts the mobile obstacle Om and subsequently adjusts its configuration continuously to avoid penetrating this mobile obstacle as it approaches stationary obstacle O2 (Figure 11i–l). A difficult situation occurs, as shown in Figure 11m, when the mobile obstacle Om stops close to obstacle O2, after which Om and O2 create a narrow corridor through which the manipulator must maneuver (see the magnified view of the corridor in the inset of Figure 11m). The differentiable manifold algorithm of Section 3.3 is able to handle this situation seamlessly. It completes the desired trajectory, as demonstrated in Figure 11m–o.

4.2.3. Generalized-Inverse-Velocity Output Tracking and Obstacle Avoidance

The problem treated in Section 4.2.2 is attempted using the generalized inverse velocity method, numerically integrating Equation (9) with the term of Equation (10) to avoid obstacles. For this method, it is necessary to select two parameters, the time step $\Delta t$ used to numerically integrate Equation (9) (using again the fourth-order Runge–Kutta method) and the coefficient k used in Equation (10) to tune the rate of minimization of the cost function that penalizes proximity to obstacles. For a less complex example such as the one studied in Section 4.1.2, these two parameters were successfully set. However, for this more complex example with three obstacles (one of them mobile) and, more importantly, a narrow corridor such as the one shown in Figure 11m, the choice of parameters becomes critical. A smaller time step $\Delta t$ allows accurate numerical integration and smooth motions, but the simulation takes more CPU time to complete the full trajectory (or, in case the method is used to update the configuration in a real-time controller, it requires a more demanding controller that is able to operate at smaller sample times $\Delta t$). A smaller k performs the minimization of the cost function more slowly, which yields smoother motions that come closer to obstacles. However, a small value of k requires using a very small time step $\Delta t$ to enable the algorithm to avoid penetrating obstacles.

Table 1. Results of the generalized inverse velocity method for different pairs $(k,\Delta t)$.

$\mathit{k}\backslash \mathrm{\Delta }\mathit{t}$(Seconds)	0.01	0.001	0.0001	0.00001 (Very Slow Simulation)
0.1	F	D	D	D
0.01	F	D	D	D
0.001	F	C	D	C
0.0001	F	F	D	C

shows 16 combinations of (k, $\Delta t$) and the result of obstacle avoidance for each combination. The result of each combination is represented by a grade F, D, or C, with the following meaning:

• Grade F means that the simulation fails when the first obstacles (O1 or O2) are contacted, long before the narrow corridor indicated in Figure 11m is formed. The failure consists in the manipulator suffering a discontinuity in its configuration upon contact.
• Grade D means that the simulation fails when the manipulator is trapped by the narrow corridor. The failure consists in the manipulator suffering a discontinuity in its configuration when the corridor is formed, without penetrating obstacles.
• Grade C means that, when the narrow corridor is formed, the manipulator starts to penetrate obstacles and suffers discontinuous jumps in its configuration.

The lower-right case of Table 1 (k = 0.0001 and $\mathrm{\Delta }t=0.00001$) is illustrated in Figure 12. This case has been chosen for illustration because, despite not being fully successful (in fact, no case of Table 1 is fully successful), it exhibits the most similar behavior to Figure 11. It should be noted that the result of Figure 12 required a very slow simulation, due to the small time step of 0.00001 s, in contrast to the simulation of Figure 11 that worked well with a time step of 0.001 s. As Figure 12 shows, the generalized-inverse-velocity-based method yields a solution that is not very different from the one shown in Figure 11 during the beginning of the trajectory. The links contact obstacles and wrap around them as the manipulator progresses, including the mobile obstacle Om. The problem occurs when the mobile obstacle forms the narrow corridor with static obstacle O2. In that case, as Figure 12m shows, the manipulator starts to penetrate obstacles. It also suffers discontinuities in its configuration that occur between Figure 12l and m. This behavior can be better observed in supplementary video S4 attached. In any case, the trajectory becomes infeasible. In fact, according to Table 1, no valid combination of (k, $\mathrm{\Delta }t$) was found that accomplishes this trajectory while avoiding obstacles with the generalized inverse velocity method. In contrast, the differentiable manifold method of Section 3.3 was able to easily solve this problem, as demonstrated in Figure 11 and Supplementary video S3.

5. Discussion

Kinematic control of redundant manipulators has traditionally been conducted at the velocity level, where a generalized inverse is used with the velocity equation of Equation (9) to obtain joint velocities $\dot{\mathit{y}}$ in terms of the task (or output) velocities $\dot{\mathit{z}}$ and arbitrary input velocities. Joint trajectories are then determined by numerical integration of $\dot{\mathit{y}}$. Obstacle avoidance is traditionally treated by performing self-motions that approximately minimize a cost function of Equation (11) that penalizes proximity to obstacles. As shown in Section 4.1.2, this approach leads to difficult-to-predict behavior, due to nonholonomy, as the manipulator performs quite different postures between cycles of specified output trajectories for the four-degree-of-redundancy manipulator, despite fine-tuning the parameters of the method (the rate of minimization of the cost function) to avoid obstacles during the first cycle. As shown in Section 4.2.3, regardless of the values of tunable parameters, the generalized inverse velocity approach encounters insurmountable difficulties for the twenty-degree-of-redundancy manipulator in a complicated situation such as a narrow corridor between obstacles, leading to infeasible trajectories that exhibit discontinuities or penetration of obstacles. These examples show that theoretical deficiencies of the generalized-inverse-velocity-based redundancy resolution method identified in Section 1.4 lead to severe problems in manipulator control applications.

An operational configuration space differentiable manifold formulation is presented for kinematic control of redundant manipulators during obstacle avoidance. In contrast with the traditional generalized-inverse-velocity-based approach, the differentiable manifold formulation resolves kinematic redundancy at the configuration level, constructing an inverse mapping that parameterizes input coordinates $\mathit{y}$ as functions of output coordinates $\mathit{z}$ and self-motion coordinates $\mathit{v}$. This mapping is holonomic, locally cyclic, and can be used to generate global obstacle-free motion plans, as described in Section 3.3. The algorithm presented defines a gap function $\mathit{g}$ in the vicinity of every collision that is a function of self-motion coordinates $\mathit{v}$. The algorithm actively adjusts self-motion coordinates $\mathit{v}$ to avoid penetration of obstacles, keeping $\mathit{g}$ non-negative by solving an under-constrained system of nonlinear equations. The example of Section 4.1.1 demonstrates the cyclicity and real-time capability of the algorithm with the four-degree-of-redundancy manipulator. The example of Section 4.2.2 demonstrates its ability to treat very restrictive obstacle situations with the twenty-degree-of-redundancy manipulator, negotiating a narrow corridor while accurately completing the desired task.

Although this paper has focused on redundant serial manipulators, the differentiable manifold formulation is presented for non-serial redundant manipulators in [3], which makes it possible to extend the algorithm presented to non-serial manipulators. Future applications of the formulation presented will not only consider control at the kinematics level but also control at the dynamics level [4], where accurate holonomic control of obstacle avoidance can be exploited for controlling contact forces between links and obstacles. Kinematic control applications involving constraints on velocities and accelerations can also be addressed.