Collision-free kinematics for hyper-redundant manipulators in dynamic scenes using optimal velocity obstacles

Abstract

Hyper-redundant manipulators have been widely used in the complex and cluttered environment for achieving various kinds of tasks. In this article, we present two contributions. First, we provide a novel algorithm of relating forward and backward reaching inverse kinematic algorithm to velocity obstacles. Our optimization-based algorithm simultaneously handles the task space constraints, the joint limit constraints, and the collision-free constraints for hyper-redundant manipulators based on the generalized framework. Second, we present an extension of our inverse kinematic algorithm to collision avoidance for the hyper-redundant manipulators, where the workspaces may have different types of obstacles. We highlight the performance of our algorithm on hyper-redundant manipulators with various degrees of freedom. The results show that our algorithm has made full use of dexterity of hyper-redundant manipulators in complex environments, enhancing the performance and increasing the flexibility.

Keywords

Inverse kinematics hyper-redundant manipulators collision avoidance dynamic scenario

Introduction

Hyper-redundant manipulators consist of more degrees of freedom (DOF) than those of traditional manipulators. Due to the richness in DOF, hyper-redundant manipulators have been used in many complex and cluttered environments to achieve the tasks that traditional manipulators cannot do, such as medical applications, search, rescue, and reactor repair.^1

–4 Moreover, many applications use hyper-redundant manipulators that work in an environment with static and/or moving obstacles. Therefore, collision-free inverse kinematics is a fundamental problem in the application of hyper-redundant manipulators. The problem can be defined with respect to the sets of constraints, including task space constraints, joint limit constraints, and collision-free constraints. During each control sampling period, the manipulator must compute an inverse kinematic solution based on the desired pose of its end effector and local observations of the workspace, such that it stays free of collisions with the static and/or moving obstacles.

Many kinds of research have been focused on effective methods to resolve the inverse kinematics and collision avoidance problem in robotics. The popular way is to use Jacobian inverse methods, where the collision-free constraints can be regarded as a subtask.^5

–8 Those methods map the collision-free joint space vector into task space velocities based on the projection operator $I - J^{†} J$ (i.e. the null space of the transformation). However, the high computational cost of the $J^{†}$ computation makes the Jacobian inverse methods unsuitable for the manipulators with a large number of DOF.^9,10

This limit leads to the development of shape trajectory control approaches, which use differential geometry to formulate the closed-form modal inverse kinematic solutions and adjust the manipulator configurations to avoid the obstacles.^11

–15 Those approaches to solve the inverse kinematic problem are only for limited kinematic models, which constrain the performance (dexterity in cluttered environments) of the hyper-redundant manipulators and restrict their potential applicability.

Therefore, some research in this field have been focused on using the data-driven methods, such as learned methods and deep learning methods that use prelearned configurations to match the end effector’s position to generate a feasible posture from the databases.^{16

–23} A major limitation of those methods is that the robots need large datasets to obtain more accurate inverse kinematic solutions. However, the training data are difficult to generate when the robots work in cluttered environments. Furthermore, the discontinuity in configurations is considered as one of the most important disadvantages of data-driven inverse kinematic solvers.

Moreover, there are many studies on collision-avoidance problem for motion planning of robots in dynamic workspaces.^24

–27 The limitations of these studies though are that most of the approaches assume the robots to simple circle models and only take two-dimensional (2D) workspace into account, which limits their applicability to hyper-redundant manipulators.

In this article, we address those shortcomings by introducing a new collision-free inverse kinematic algorithm for a hyper-redundant manipulator, which operates in a dynamic environment with static and/or dynamic obstacles. First, we provide a novel combination of forward and backward reaching inverse kinematics (FABRIK)^28,29 and velocity obstacles.³⁰ The basic idea is that we compute inverse kinematic solutions that satisfy the task space and joint limit constraints while accounting for collision-avoidance constraints. Then, we present an extension of collision-free inverse kinematic algorithm to special cases of workspace for hyper-redundant manipulators⁵ with a different number of joints. Finally, we implement our algorithm in simulation for a variety of hyper-redundant manipulators working with different types of obstacles. The results show that our algorithm is capable of solving the inverse kinematic problem of hyper-redundant manipulators in dynamic working environments.

The main contributions of this article are provided as follows:

We provide a novel inverse kinematic algorithm of relating FABRIK to velocity obstacles. We will demonstrate that our algorithm can be applied directly to any general planar and spatial manipulators with high DOF, such as handling the task space constraints, the joint limit constraints, and the collision-free constraints.

We present an extension of our approach to collision avoidance for the hyper-redundant manipulators, where the workspaces may have different types of obstacles. We will show specifically the obstacles such as moving obstacles, pole, and wall.

The rest of the article is organized as follows. In the second section, we briefly discuss related works in inverse kinematics of hyper-redundant manipulators. In the third section, we define the problem of inverse kinematics and extend the notion of velocity obstacles to hyper-redundant manipulators. The fourth section presents our approach for special cases with different types of hyper-redundant manipulators and moving or static obstacles. The fifth section highlights the performance of our method and the sixth section summarizes the conclusion.

Previous work

In this section, we give a brief overview of prior work on inverse kinematics of hyper-redundant manipulators.

Some of the popularly used approaches to generate inverse kinematic solutions are Jacobian inverse methods. Maciejewski and KleinObstacle⁵ and Nakamura et al.⁶ present an algorithm to handle the end-effector trajectory and obstacle avoidance (OA) constraints by the null space of the transformation. Stilman⁷ presents a global randomized motion planning method for joint space based on tangent-space sampling and first-order retraction. Lee et al.⁸ solve the inverse kinematic problem by the robust task priority (RTP)-closed-loop inverse kinematic algorithm with task-priority strategy.

For shape trajectory control approach, Chirikjian and Burdick^11,12 introduce a novel method to solve the inverse kinematic problem of hyper-redundant manipulators based on a “backbone curve.” Mochiyama and Kobayashi¹³ adopt Lyapunov design to estimate the desired curve parameter, where a time-varying curve is given to track the manipulator shape. Fahimi et al.¹⁴ present a mode shape technique-based approach to resolve the OA problem for hyper-redundant manipulators. Mu et al.¹⁵ propose a new strategy named hybrid OA method, which can be used in OA motion planning for hyper-redundant manipulators.

For the intelligence algorithms, Rose et al.¹⁶ introduce an inverse kinematic algorithm based on the radial basis function (RBF) interpolation. Rolf et al.¹⁷ introduce a method to learn inverse kinematics of redundant systems based on a path-based sampling approach and without prior or expert knowledge. Toshani and Farrokhi¹⁸ apply RBF neural networks in redundant manipulators to obtain the real-time joint values by the Cartesian coordinate of the end effector. Collins and Shen¹⁹ propose an integrated approach, named path planning with swarm optimization, to solve the inverse kinematics and path planning problems for hyper-redundant manipulators in three-dimensional (3D) workspaces. Peng et al.²⁰ introduce a hierarchical learning-based method to control the motion style of a 3D bipedal. Starke et al.²¹ use a hybrid evolutionary approach to solve inverse kinematics for highly articulated and humanoid robots. Huang et al.²² propose a parallelizable constrained inverse kinematic technique by modeling and using dynamic joint motion parameters, which can be automatically learned from input mocap data. Xidias²³ presents a method for time optimal trajectory planning for a hyper-redundant manipulator, which transforms the trajectory planning problem to a global optimization problem and solve it by a genetic algorithm.

There have been some developments for collision avoidance for redundant manipulators in dynamic environments. Duindam et al.³¹ use the inverse kinematic solutions to compute feasible paths for steerable needles to avoid obstacles. Park et al.³² present a novel method to compute a collision-free trajectory for the robot by an incremental trajectory optimization framework. Tao and Yang³³ propose a method for motion planning of a redundant manipulator based on the OA-FABRIK algorithm. Menon et al.³⁴ introduce an optimization algorithm that all links of a hyper-redundant manipulator avoid the obstacles using calculus of variation. Other recent optimization methods studied are based on curve-constrained collision-free trajectory control,³⁵ vector field inequalities,³⁶ and Newton’s method.³⁷

Materials and methods

In this section, we present our algorithm for collision avoidance, which extends velocity obstacles to hyper-redundant manipulators based on FABRIK. We derive the formulation for calculating the velocity obstacle constraints while they are induced by other moving obstacles in a time window. We also present an efficient technique to compute the possible safe position for each joint of a hyper-redundant manipulator that simultaneously satisfies collision-free constraints, task space constraints, and joint limit constraints.

Preliminaries

Hyper-redundant manipulators have many more DOF than the number of a particular task required. As the number of DOF increases, the widely used methods for solving the inverse kinematic problems, such as analytical methods³⁸ and generic kinematics and dynamic library algorithm,³⁹ are considered unsuitable for hyper-redundant manipulators. The position and orientation of the end effector cannot be fully defined by link lengths and joint angles because there exists an infinite number of inverse kinematic solutions. In addition, more restrictions will impose on inverse kinematic solutions when the manipulator encounters static and moving obstacles. The challenge of inverse kinematics for hyper-redundant manipulators is that the algorithm must compute changes in joint values to avoid collision. The functional form of the inverse kinematic problem of the hyper-redundant manipulators is given by

θ_{1, \dots, n} = f^{- 1} (ξ_{E})

where $ξ_{E}$ is the desired pose of the end effector and $θ_{1, ..., n}$ are required joint coordinates. Because of redundancy, it is necessary to consider a numerical method that computes a feasible solution. The inverse kinematic solver FABRIK is a heuristic method that combines forward and backward iterative methods, as given by Aristidou and Lasenby.²⁸ Instead of using joint vector $θ$ , FABRIK performs a joint position search using these methods and terminates when either of these methods converges to an inverse kinematic solution.

In Figure 1(a), the manipulator is a concatenation of four identical joints, where the first joint is fixed at its base and the last joint is its end effector. Let $p_{i}$ (i = 1,…,4) represent currently the joint position of the manipulator and let $p_{t}$ be a target position. We assume that the distances between each joint d_i can be derived from the joint position $p_{i}$ by the function: $d_{i} = ‖p_{i + 1} - p_{i}‖$ .

Figure 1.

An example of a forward search and a backward search of FABRIK for a manipulator with four joints. (a) The positions of the manipulator and the target at the beginning of the movement. (b) Move the end effector $p_{4}$ to the target, and then find the joint positions ${p^{'}}_{3}$ , ${p^{'}}_{2}$ , and ${p^{'}}_{1}$ . (c) Move the base joint ${p^{'}}_{1}$ to its initial position, and then find the joint positions ${p^{″}}_{2}$ , ${p^{″}}_{3}$ , and ${p^{″}}_{4}$ . FABRIK: forward and backward reaching inverse kinematics.

In the forward search, the end effector position of the manipulator $p_{4}$ is moved to the target position $p_{t}$ , as shown in Figure 1(b). There exists a line, which passes through the joint positions ${p^{'}}_{4}$ and $p_{3}$ . The new joint position ${p^{'}}_{3}$ is then lies on that line with distance d₃ from ${p^{'}}_{4}$ . All other joints use the same algorithm to select their change of position, including the position of the first joint ${p^{'}}_{1}$ . In the backward search, as shown in Figure 1(c), the first joint position ${p^{'}}_{1}$ is moved to its initial position $p_{1}$ , that is, ${p^{″}}_{1} = p_{1}$ . Then, use the same procedure as the forward search to select the new joint positions for all the remaining joints. To keep the joint values within the joint limits in each search, it is recommended to add a second adjustment to the joint positions. The search continues until when all the elements fall below a stopping criterion, and the current position and orientation of each joint are returned as an inverse kinematic solution.

Relating FABRIK algorithm to velocity obstacles

Given the method above, we would like to extend the notion of velocity obstacles to OA based on FABRIK. As shown in Figure 2, the ellipsoid A is represented by a center point $p_{A}$ , an orthonormal set of axis-direction vectors $\{U_{0}, U_{1}, U_{2}\}$ , and associated extents e_i with $e_{0} \geq e_{1} \geq e_{2}$ . The sphere B is centered at $p_{B}$ with radius r_B. For ellipsoid A and sphere B in the Cartesian space, the Minkowski sums $A \oplus B$ can be defined as

A \oplus B = {a + b | a \in A, b \in B}

Figure 2.

In the XYZ-dimensional, an ellipsoid and a sphere move toward one another with velocity $v_{A}$ and $v_{B}$ , respectively.

Let $λ^{τ} (p, v) = {p + t v | t \in [0, τ]}$ denotes a ray for time window $τ$ , which starts at position p with direction v . As can be seen in Figure 3, the velocity obstacle $V O_{A | B}^{τ}$ is given by

V O_{A | B}^{τ} = {v | λ^{τ} (p_{A}, v - v_{A}) \cap - A \oplus B \neq \emptyset}

where $v_{A}$ is the velocity vector of the ellipsoid A.

Figure 3.

The gray shaded area is the velocity obstacle for ellipsoid A induced by sphere B in time window $τ$ . The green ellipsoid is the Minkowski sum of −A and B scaled by $τ$ with a center point $(p_{B} - p_{A}) / τ$ .

As shown in Figure 4, if the sphere B selects a velocity $v_{B}$ , we observe that sphere B may collide with moving ellipsoid A during the time interval [0, $τ$ ] if the relative velocity vector $v_{A} - v_{B}$ is inside the region $V O_{A | B}^{τ}$ . The problem of collision avoidance is now defined as the relative velocity vector that must be outside $V O_{A | B}^{τ}$ before time $τ$ . Having identified a velocity $v_{B}$ for sphere B, the vector $ω$ from $v_{A} - v_{B}$ to the closest point of the $V O_{A | B}^{τ}$ ’s boundary, as shown in Figure 4, is given as

ω = \{\begin{array}{l} argmin | | v - (v_{A} - v_{B}) | |_{2}) - (v_{A} - v_{B}) \\ v \in \partial V O_{A | B}^{τ} \end{array}

Figure 4.

The vector $ω$ represents the amount of load the ellipsoid A takes to avoid sphere B with respect to $v_{A}$ and $v_{B}$ .

Given the defined generalized method for collision avoidance using velocity obstacles, we present the extension of this method for collision-free inverse kinematics for hyper-redundant manipulators. In FABRIK algorithm, we define a task error

e = ‖p_{n} - p_{t}‖

To compute the joint position $p_{i}$ for given $p_{t}$ without any collisions, we would like to solve the optimization problem with equality constraints as follows

\begin{matrix} argmin & e \\ s.t. & \{\begin{array}{l} v_{I} - v_{O} \in \partial V O_{I | O}^{τ} \\ ‖p_{i + 1} - p_{i}‖ \in D_{i} \\ θ_{i} \in J L_{i} \end{array} \end{matrix}

where $v_{I}$ is the velocity of link I (I = 1,…, $n - 1$ ), $v_{O}$ is the velocity of obstacle O, D_i is the distance constraint between adjacent joints (i = 1,…, n), and $J L_{i}$ is the joint limit constraint.

An example for solving equation (6) is shown in Figure 5. We consider a hyper-redundant manipulator and a moving obstacle sharing a common working environment, where the obstacle moves with a known linear trajectory. The start positions of link I and moving obstacle are shown in Figure 5(a). Let $p_{i}$ be the position of joint i. Let us now define $d_{i} = ‖p_{i} - p_{i - 1}‖$ as the distance between joint i and joint i − 1. The moving obstacle O is centered at $p_{O}$ with velocity $v_{O}$ . To apply velocity obstacles in the process of inverse kinematics, each movable link of the hyper-redundant manipulator is described by an ellipsoid. These ellipsoids move around other dynamic obstacles. Each dynamic obstacle is assumed to be a sphere or is bounded by a sphere. The position $p_{I}$ of link (ellipsoid) I is

Figure 5.

(a) In the XYZ-dimensional, the link I of a hyper-redundant manipulator and a dynamic obstacle are bounded by an ellipsoid and a sphere, respectively. (b) The gray-shaded area is the velocity obstacle $V O_{I | O}^{τ}$ and the vector $ω$ is the minimal velocity change of link I to avoid the moving obstacle O. The half-space below the green plane $C A_{I | O}^{τ}$ is the set of permitted velocities specified by a collision avoidance constraint. (c) The half-space below the orange plane $C A_{i | O}^{τ}$ is the set of permitted velocities by a collision avoidance constraint. The blue composite ellipsoidal shape $J L_{i}$ denotes the joint limit constraint. The sphere D_i denotes the distance constraint between adjacent joints.

p_{I} = \frac{p_{i} + p_{i - 1}}{2}

where $i = I + 1$ . The extents of ellipsoid I are defined as $e_{0}^{I} = d_{i} / 2$ , and $e_{1}^{I}$ and $e_{2}^{I}$ are dynamically determined by the size of the links to make sure that there are no collisions.

Figure 5(a) also shows the first step of the forward search in the FABRIK algorithm. Joint i updates its position to ${p^{'}}_{i}$ based on its initial position $p_{i}$ and the previous joint position ${p^{'}}_{i + 1}$ . Then, we assume that the velocity $v_{p_{i}}$ of joint i is determined only by its new position ${p^{'}}_{i}$ and the initial position $p_{i}$

v_{p_{i}} = \frac{{p^{'}}_{i} - p_{i}}{Δ t}

where $Δ t$ is the time duration of the end effector moving between the two positions $p_{n}$ and $p_{t}$ at one movement step. The velocity $v_{I}$ can be derived from the velocity of joint i and joint i − 1 by

v_{I} = \frac{v_{p_{i}} + v_{p_{i - 1}}}{2}

Then, the velocity obstacles $V O_{I | O}^{τ}$ induced by obstacle O are defined, as shown in Figure 5(b). We can find that the velocities $v_{I}$ and $v_{O}$ will lead link I and obstacle O to a collision course within $τ$ time, that is, $v_{I} - v_{O} \in V O_{I | O}^{τ}$ . Let vector $ω$ be the closest point to $v_{I} - v_{O}$ on the boundary of $V O_{I | O}^{τ}$ , and let vector u be the outward normal of $V O_{I | O}^{τ}$ at the point $v_{I} - v_{O} + ω$ . We can then compute set $C A_{I | O}^{τ}$ of possible safe velocity for link I before time $τ$

C A_{I | O}^{τ} = \{v | (v - (v_{I} + ω)) \cdot u \geq 0\}

Since for link velocity, it holds that $v_{I} = (v_{p_{i}} + v_{p_{i - 1}}) / 2$ , the set $C A_{I | O}^{τ}$ can be transformed to determine sets of permitted velocities for both joint i and joint i − 1 as

C A_{i | O}^{τ} = \{v | (v - (v_{p_{i}} + ω)) \cdot u \geq 0\}

and

C A_{i - 1 | O}^{τ} = \{v | (v - (v_{p_{i - 1}} + ω)) \cdot u \geq 0\}

where the two joints will share the responsibility for collision avoidance between the link I and the moving obstacle O equally. As shown in Figure 5(c), $C A_{i | O}^{τ}$ is the half-space pointing in the direction of u containing the point $v_{p_{i}} + ω$ . To avoid a collision, joint i needs to select a new velocity $v_{p_{i}}^{n e w}$ that lies inside the region of permitted velocities $C A_{i | O}^{τ}$ and closes to its velocity $v_{p_{i}}$ . While adhering to a distance constraint and a joint limit constraint

v_{p_{i}}^{n e w} = argmin ‖v - v_{p_{i}}‖, v \in C A_{i | O}^{τ} \cap D_{i} \cap J L_{i}

Finally, joint i updates its position

{p^{'}}_{i} = v_{p_{i}}^{n e w} Δ t

and all other joints use the same approach to avoid collisions with the obstacles. For the backward search, apply the same method to the joints, and it would compute the set of feasible changes in velocity and update joint positions to avoid collisions with moving obstacles.

The detailed procedures of our approach for finding the inverse kinematic solutions based on FABRIK and velocity obstacles can be given in Algorithm 1. Moreover, Algorithm 1 terminates after $e \leq ε$ or a maximum computation time $t_{max}$ , where $ε$ is a stopping criterion. The set $C A_{I | O}^{τ}$ can be computed using equation (10) within function Safe_Velocity_SET(), see lines 13 and 25. We can compute a collision-free position of joint i using equation (14) within function Collisioin_Free_Set(), see lines 15 and 27. The joint limits can be applied within function Joint_Limit_Set(), see lines 10 and 22 (see Aristidou et al.^28,29 for work specifically including the joint limits in FABRIK algorithm).

Algorithm 1:

Collision-free inverse kinematic solution generator (our novel algorithm that accounts for task space, joint limit, and collision avoidance constraints)

If $C A_{i | O}^{τ} \cap D_{i} \cap J L_{i} = \emptyset$ (i.e. the position of joint i is not able to find when adhering to these constraints), a smaller value of time window $τ$ will be selected to increase the region of the set $C A_{i | O}^{τ}$ . This approach will make the link I move toward the obstacle which is easy to avoid and then adhering to the constraints, which are difficult to handle. However, it does not hold for the situation that the position of joint i must be changed more to avoid a collision, as shown in Figure 6. In this case, we can rewrite equation (13) as

v_{p_{i}}^{n e w} = v_{p_{i}} + ω

that not only including distance constraint and/or joint limit constraint of joint i but also of the collision-free constraint in this search (forward or backward). Joint i updates its position using equation (14). The next link (i.e. link $I + 1$ for forward search or $I - 1$ for backward search) can then choose the change in position with respect to this updated position of link I. In other words, link $I + 1$ or link $I - 1$ will share the responsibility for avoiding collisions of link I. Note that this approach will imply more iterations to reach the stopping criteria.

Figure 6.

The new velocity $v_{p_{i}}^{n e w}$ as in Figure 5 for different obstacle velocity $v_{O}$ .

The proposed algorithm has the consequence that if the moving obstacle potentially results in a collision with the hyper-redundant manipulator, then the changes in joint values will be computed to avoid this collision. This algorithm can be applied to general planar and spatial hyper-redundant manipulators with a single end effector for real-time control. It is able to compute collision-free inverse kinematic solutions that satisfy the complex dynamic constraints in the workspace and kinematic constraints (i.e. task space and joint limit constraints).

Implementation on special cases

We have introduced the above algorithm for collision avoidance for a hyper-redundant manipulator in an uncertain dynamic environment. We did so through a new algorithm of relating FABRIK algorithm to velocity obstacles. As shown in Algorithm 1, we can find a collision-free inverse kinematic solution for a hyper-redundant manipulator if given the position and velocity of the obstacle. We will now show how the previous algorithm of collision avoidance can be implemented in different working environments.

Avoiding collisions with multiple dynamic obstacles

Algorithm 1 is presented for collision avoidance with one moving obstacle that can easily be extended to more than two obstacles. Let O be a set of moving obstacles sharing a common workspace with a hyper-redundant manipulator. The position $p_{O_{j}}$ , the velocity $v_{O_{j}}$ , and the radius $r_{O_{j}}$ for each obstacle $O_{j} \in O$ are a priori known, as shown in Figure 7.

Figure 7.

A case where a hyper-redundant manipulator and two moving obstacles O₁ and O₂ in the workspace.

Then, the velocity obstacles of the link I with respect to every obstacle can be defined using equation (3), as shown in Figure 8(a). The function Safe_Velocity_Set() in Algorithm 1 can now be defined as having the link computes the set of feasible changes in velocity based on the velocity obstacles $V O_{I | O_{1}}^{τ}$ and $V O_{I | O_{2}}^{τ}$ , as can be seen in Figure 8(b), which gives

C A_{I}^{τ} = \underset{O_{j} \in O}{\cap} C A_{I | O_{j}}^{τ}

Figure 8.

(a) The velocity obstacles and (b) the set $C A_{I}^{τ}$ for two moving obstacles in Figure 7.

The set of safe changes in velocity for joint i can then be determined by $C A_{I}^{τ}$ . Similar to equation (13), the new joint velocity that is safe from all moving collisions is given as

v_{p_{i}}^{n e w} = argmin ‖v - v_{p_{i}}‖, v \in \underset{O_{j} \in O}{\cap} C A_{i | O_{j}}^{τ} \cap D_{i} \cap J L_{i} .

Since the only modification to Algorithm 1 is in the Collisioin_Free_Set() function, the set $C A_{i | O}^{τ}$ is extended to the set $\underset{O_{j} \in O}{\cap} C A_{i | O_{j}}^{τ}$ , which includes other moving obstacles. Note that the problem of collision avoidance between the moving obstacles is not considered in this algorithm.

Avoiding collisions with static obstacles

Algorithm 1 gives an inverse kinematic algorithm for the hyper-redundant manipulators and the moving obstacles to avoid collisions. However, the typical robot workspace also contains static obstacles, such as wall and pole. We will apply the same algorithm as above to this situation, it would compute the set $C A_{I}^{τ}$ for the link I to avoid collisions with static obstacles.

We assume that the geometric shape of the static obstacle is a regularly shaped body. More specifically, we would like to apply the basic space geometric shapes of cuboids and cylinders to model the static obstacles of walls and poles, respectively. This is shown in Figure 9. We can assume that the wall obstacle is modeled as a cuboid, and its geometric shape is determined by the position, orientation, and dimension of the wall obstacle. Let O be the cuboid, and let link I be an ellipsoid centered at $p_{I}$ . Then, the velocity obstacles $V O_{I | O}^{τ}$ is defined by equation (3), as shown in Figure 10(a) and (b). If the velocity $v_{I}$ is inside $V O_{I | O}^{τ}$ , link I will collide with the cuboid O within $τ$ time. To avoid a collision, joint i needs to select a new velocity $v_{p_{i}}^{n e w}$ that satisfies the conditions of equation (13).

Figure 9.

A case where a hyper-redundant manipulator and a static obstacle O in the workspace.

Figure 10.

(a) The velocity obstacles $V O_{I | O}^{τ}$ and (b) the set $C A_{I}^{τ}$ for a static obstacle in Figure 9.

Implementation and performance

In this section, we highlight the performance of our algorithm in simulated environments that contain a hyper-redundant manipulator and many moving or static obstacles in the 2D and 3D workspaces. We present the kinematic parameters for hyper-redundant manipulators used in the simulations and the results from those simulations.

Implementation details

The simulations are developed in a C++ environment. The code runs on a single notebook running Ubuntu 16.04 64-bit with CPU i7-7700HQ at 2.81 GHz and 8GB RAM. To verify the adaptability of our approach, we run simulations that include the hyper-redundant manipulators with a different number of joints. The main structure of OA inverse kinematic scheme for hyper-redundant manipulators is shown in Figure 11. The simulations are initialized by decomposing the manipulator model into a series of ellipsoids (ellipses for planar manipulators). For simplicity, when the distance between the link and the obstacle is sufficiently large, we do not take this link into account while computing the collision-free constraint. The task space constraints are used to compute positioning error, and the velocity obstacles are used to define the safe velocity sets for each joint of the manipulator. A collision-free joint position $p_{i}^{n e w}$ is computed by the iterations in Algorithm 1. The loop terminates as soon as all constraints are satisfied.

Figure 11.

The obstacle avoidance inverse kinematic scheme for hyper-redundant manipulators.

Unless otherwise noted, we set a value of $τ = 2 s$ in these simulations. The stopping criteria $ε$ is $10^{- 7} %$ of the manipulator length and the timeout $t_{max} = 5 ms$ . The frequency that is used to control a manipulator is 100 Hz (corresponding to a control sampling period of 10 ms, i.e. $Δ t = 10 ms$ ). Moreover, all joints are within their specified limits.

Simulation results

Simulation A: Planar manipulator with a static obstacle

To illustrate our algorithm, we have provided a variety of simulations. In simulation A, the algorithm applied to a 7-DOF planar manipulator is compared with the shape trajectory control approach based on backbone curve¹² and the natural-cyclic coordinate descent (CCD) algorithm.⁴⁰ The simulation setup is shown in Figure 12. A 7-DOF planar manipulator (each joint has one DOF) and a circle-shaped obstacle are involved in this simulation. The black lines are the desired path of the end effector (i.e. joint position $p_{8}$ ), which moves at a constant velocity norm. The motion command for the manipulator is generated based on the shape trajectory control approach, natural-CCD algorithm, and our OA-FABRIK algorithm, respectively. In addition, the end effector must maintain the task constraint during the execution.

Figure 12.

Setup for simulation A.

The link length of the manipulator is $d = [d_{1}, \dots, d_{7}] = [10, 9, 8, 7, 6, 5, 4] (mm)$ . The associated extents of ellipses are $e_{0, i} = d_{i} / 2$ and $e_{1, i} = d_{i} / 6$ . The joint limit $J L_{i} = (- 3.14, + 3.14) (rad)$ . The desired velocity norm of the end effector during the simulations is $‖v_{E}‖ = 5.7$ . The circle-shaped obstacle with $r_{O} = 2 mm$ and $p_{O} = [15, 15]$ (mm) in the workspace.

The selected screenshots of simulation A are shown in Figures 13 to 15. As can be seen, when the manipulators use the shape trajectory control approach (i.e. Figure 13) and the natural-CCD algorithm (i.e. Figure 14), it would lead to a collision with the obstacle during the operation. The configurations in Figure 15, with manipulator solving the inverse kinematics using our algorithm (i.e. OA-FABRIK), show no collisions over its entire operation. The joint trajectory computed by our algorithm in the manipulation phase in Figure 16 shows no sudden activation and no oscillation over its entire length.

Figure 13.

(a–d) Shape trajectory control approach.

Figure 14.

(a–d) Natural-CCD algorithm.

Figure 15.

(a–d) Our collision-free inverse kinematic algorithm.

Figure 16.

Angle variation for each joint for simulation A.

Simulation B: Spatial manipulator with dynamics obstacles

One set of simulations consists of a spatial hyper-redundant manipulator and groups of moving obstacles, where the manipulator and the obstacles were simulated separately. The setup of simulation B is shown in Figure 17. The workspace includes a 30-DOF manipulator with 10 joints (each joint has three DOF) and two moving obstacles, in which the velocity and position can be obtained. In this simulation, the manipulator tries to keep its end effector (i.e. $p_{11} = [0, 0, 1.3] m$ ) in a fixed position and the moving obstacle may collide with the manipulator if the joints remain in the initial positions, as shown in Figure 17.

Figure 17.

Setup for simulation B.

The parameters for the manipulator and the obstacles in this simulation are chosen as follows: the link length of the manipulator is $d_{i} = 0.2 m$ . The associated extents of ellipsoid are $e_{0, i} = 0.1 m$ , $e_{1, i} = 0.075 m$ , and $e_{2, i} = 0.075 m$ . The joint limit $J L_{i} = (- 3.14, + 3.14) (rad)$ . The sphere-shaped obstacle with $r_{O_{j}} = 0.1 mm$ in the workspace.

Based on the simulation setup, we performed three simulations, where one, two, and four moving obstacles cross the workspace with different velocity and direction, respectively. The manipulator can update its control input by the positions and velocities of those moving obstacles to avoid collisions. Selected screenshots of those simulations are shown in Figures 18 to 20.

Figure 18.

(a–d) A simulation that contains a dynamic obstacle, which is a red sphere with velocity $v_{O} = [0, - 0.08, 0] (m/s)$ .

Figure 19.

(a–d) A simulation that contains two dynamic obstacles with varying velocities. In the first screenshot, the velocity of left red sphere is $v_{O_{1}} = [- 0.06, 0, 0]$ (m/s), and the velocity of right red sphere is $v_{O_{2}} = [0, - 0.08, 0] (m/s)$ .

Figure 20.

(a–d) A simulation that contains four dynamic obstacles. In the first screenshot, the velocities of two red spheres on the left are $v_{O_{1}} = v_{O_{2}} = [- 0.065, 0, 0] (m/s)$ , and the velocities of two red spheres on the right are $v_{O_{3}} = v_{O_{4}} = [- 0.065, 0, 0] (m/s)$ .

Simulation C: Spatial manipulator with static obstacles

In simulation C, we test the performance of our algorithm on spatial hyper-redundant manipulator with the static obstacles. In this simulation setup, the typical static obstacles are used in the workspace. As shown in Figure 21(a) and (b), two challenging scenarios are presented, respectively, that is, scenario 1: a manipulator with 10 joints (each joint has three DOF) and a wall and a hole; and scenario 2: a manipulator with 19 joints (each joint has three DOF) and a cylindrical obstacle. In the simulations, a path planner of the end effector is needed, which has been given by the user. The joint motion command for the manipulator is generated based on our collision-free inverse kinematic algorithm.

Figure 21.

(a, b) Setup for simulation C.

These two manipulators have the same kinematic parameters, which have been presented in simulation B. In addition, the desired velocity norm of the end effector during the simulations is $‖v_{E}‖ = 0.25$ . Figures 22 and 23 show simulation results of these two scenarios, respectively. As can be seen, the end effector can maintain the task constraint, and there are no collisions during task execution.

Figure 22.

(a–d) A simulation in the scenario with a wall and a hole.

Figure 23.

(a–d) A simulation in the scenario with a cylinder obstacle.

Performance analysis

Computational time

To quantify the computational time of our algorithm, we calculate the computation time of the inverse kinematic problem resolution, that is, the computation time it takes for one hyper-redundant manipulator to determine its joint positions of safe changes with respect to the moving or static obstacles in a single control cycle. As shown in Figure 24, the computation time grows with an increase in the number of moving obstacles and joints, as expected. In the simulation, we found that the safe velocity sets are computed without significantly increasing the computational time when the manipulator needs to avoid obstacles. As can be seen in Figure 24(b), the computational time also shows that for a single obstacle, the static obstacle could require a further increase in the computational time (at least in the workspace we implemented).

Figure 24.

(a, b) The computational time of the inverse kinematic problem resolution is made for several number of simulations shown above.

Comparison

To further highlight the performance and efficiency of the proposed algorithm, the comparisons with some existing inverse kinematic approaches are listed in Table 1. It can be observed that the algorithms in the literature^17,39 do not consider the obstacles in the workspaces. In the literature,^{7,15,18,33,37} although the proposed approaches can avoid collisions, they cannot be applied to the dynamic working environments. Moreover, the algorithms in Rolf et al.¹⁷ and Safeea et al.³⁷ only apply to 2D workspaces. Note that the proposed algorithm can compute the collision-free inverse kinematic solutions in dynamic workspaces and can handle hyper-redundant manipulators. Furthermore, this algorithm is faster than other inverse kinematic algorithms, does not need any training, and can be applied in real-time control.

Table 1.

Comparisons among different inverse kinematic algorithms.

	Number of DOF	Computational time (ms)	Application scenario	Geometric model	Database
This article	57	0.26	Dynamic and static obstacle	Sphere and ellipsoid	N
Stilman⁷	8	≥5	Static obstacle	—	N
Mu et al.¹⁵	18	—^b	Static obstacle	Superquadric function	N
Rolf et al.¹⁷	50^a	—^b	2D no obstacle	—	Y
Toshani and Farrokhi¹⁸	7	85.8	Static obstacle	Sphere	Y
Tao and Yang³³	7	29.9	Static obstacle	Circle	N
Safeea et al.³⁷	15^a	≤5	2D static obstacle	Point	N
Inverse kinematics with the kinematics³⁹	7	0.61	No obstacle	—	N

DOF: degrees of freedom.

^a The algorithm is applied on a planar hyper-redundant manipulator.

^b The algorithm is suitable for real-time control.

Conclusions

In this article, we present a novel method toward solving the inverse kinematic problem of hyper-redundant manipulators in complex working environments. To do so, we extend the notion of velocity obstacles to OA based on FABRIK. We then show how to implement the OA inverse kinematic algorithm in special cases. In our simulations, we see that the hyper-redundant manipulators can perform better OA using our algorithm.

The proposed approach has shown the expected performance of the collision-free kinematics for hyper-redundant manipulators in dynamic scenes. Due to the structural characteristic of soft manipulators,^41,42 a future line of research will be focused on the integration of our algorithm with the soft robotic arm by converting the discrete inverse kinematic solutions to continuous curves. Moreover, we would like to evaluate the performance in dynamic scenes with human or other complex obstacles.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported by the National Natural Science Foundation of China [Project Number: 91848101 and 51805107] and the Foundation for Innovative Research Groups of the National Natural Science Foundation of China [Grant Number: 51521003].

ORCID iD

Jingdong Zhao

Supplemental material

Supplemental material for this article is available online.

References

Chirikjian

Burdick

JW.

Hyper-redundant robot mechanisms and their applications. In: 1991 IEEE international workshop on intelligent robots and systems (ed Harashima

), Osaka, Japan, 3–5 November 1991, pp. 185–190. IEEE.

Chirikjian

Burdick

. A hyper-redundant manipulator. IEEE Robot Autom Mag 1994; 1(4): 22–29.

Fahimi

Hyper-redundant manipulators. In: 2009 Autonomous robots: modeling, path planning, and control (ed Loynachan

), Boston, MA, USA 2009, pp. 1–30.

Ivanescu

Cojocaru

Hyper redundant manipulators. In: Advanced strategies for robot manipulators (ed Shafiei

), Rijeka: IntechOpen, 2010, pp. 27–60.

Maciejewski

KleinObstacle

. Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments. Int J Robot Res 1985; 4(3): 109–116.

Nakamura

Hanafusa

Yoshikawa

. Task-priority based redundancy control of robot manipulators. Int J Robot Res 1987; 6(2): 3–15.

Stilman

. Global manipulation planning in robot joint space with task constraints. IEEE Trans Robot 2010; 26(3): 576–584.

Lee

Park

, et al. A whole-body rescue motion control with task-priority strategy for a rescue robot. Autonom Robot 2017; 41: 243–258.

Pasquale

Stefano

Lorenzo

, et al. Closed-loop inverse kinematics schemes for constrained redundant manipulators with task space augmentation and task priority strategy. Int J Robot Res 1991; 10(4): 410–425.

10.

Colom’e

Torras

. Closed-loop inverse kinematics for redundant robots: comparative assessment and two enhancements. IEEE/ASME Trans Mechatron 2015; 20(2): 944–955.

11.

Chirikjian

Burdick

. An obstacle avoidance algorithm for hyper-redundant manipulators. In: 1990 IEEE international conference on robotics and automation (ed Volz

), Cincinnati, OH, USA, 13–18 May 1990, pp. 625–631. IEEE.

12.

Chirikjian

Burdick

. A modal approach to hyper-redundant manipulator kinematics. IEEE Trans Robot Autom 1994; 10(3): 343–354.

13.

Mochiyama

Kobayashi

. The shape Jacobian of a manipulator with hyper degrees of freedom. In: 1999 IEEE international conference on robotics and automation, Detroit, Michigan, USA, 10–15 May 1999 (ed Akeel

), pp. 2837–2842. IEEE.

14.

Fahimi

Ashrafiuon

Nataraj

. Obstacle avoidance for spatial hyper-redundant manipulators using harmonic potential functions and the mode shape technique. J Robot Syst 2003; 20(1): 23–33.

15.

Liu

, et al. A hybrid obstacle-avoidance method of spatial hyper-redundant manipulators for servicing in confined space. Robotica 2019; 37(6): 998–1019.

16.

Rose

III CF

Sloan

P-PJ

Cohen

. Artist-directed inverse-kinematics using radial basis function interpolation. Comput Graph Forum 2001; 20(3): 239–250.

17.

Rolf

Steil

Gienger

. Goal babbling permits direct learning of inverse kinematics. IEEE Trans Autonom Ment Dev 2010; 2(3): 216–229.

18.

Toshani

Farrokhi

. Real-time inverse kinematics of redundant manipulators using neural networks and quadratic programming: a Lyapunov-based approach. Robot Autonom Syst 2014; 62(6): 766–781.

19.

Collins

Shen

. PASO: an integrated, scalable PSO-based optimization framework for hyper-redundant manipulator path planning and inverse kinematics. 2016 ISI Tech Report, University of Southern California, Los Angeles, USA, 2016, pp. 1–7.

20.

Peng

Berseth

Yin

, et al. DeepLoco: dynamic locomotion skills using hierarchical deep reinforcement learning. ACM Trans Graph 2017; 36(4): 4101–4113.

21.

Starke

Hendrich

Krupke

, et al. Evolutionary multi-objective inverse kinematics on highly articulated and humanoid robots. In: 2017 IEEE/RSJ international conference on intelligent robots and systems (IROS) (ed Bicchi

), Vancouver, BC, Canada, 24–28 September 2017, pp. 6959–6966. IEEE.

22.

Huang

Fratarcangeli

Ding

, et al. Inverse kinematics using dynamic joint parameters: inverse kinematics animation synthesis learnt from sub-divided motion micro-segments. Visual Comput 2017; 33(12): 1541–1553.

23.

Xidias

. Time-optimal trajectory planning for hyper-redundant manipulators in 3D workspaces. Robot Comput Integ Manuf 2018; 50: 286–298.

24.

Shiller

Large

Sekhavat

Motion planning in dynamic environments: obstacles moving along arbitrary trajectories. In: 2001 IEEE international conference on robotics and automation (ed Chung

), Seoul, South Korea, 21–26 May 2001, pp. 3716–3721. IEEE.

25.

Van Den Berg

Snape

Guy

, et al. Reciprocal collision avoidance with acceleration velocity obstacles. In: 2011 IEEE international conference on robotics and automation (ed Ming

), Shanghai, China, 9–13 May 2011, pp. 3475–3482. IEEE.

26.

Rufli

Alonso-Mora

Siegwart

. Reciprocal collision avoidance with motion continuity constraints. IEEE Trans Robot 2013; 29(4): 899–912.

27.

Bareiss

van den Berg

. Generalized reciprocal collision avoidance. Int J Robot Res 2015; 34(12): 1501–1514.

28.

Aristidou

Lasenby

. Fabrik: a fast, iterative solver for the inverse kinematics problem. Graph Models 2011; 73(5): 243–260.

29.

Aristidou

Chrysanthou

Lasenby

. Extending FABRIK with model constraints. Comput Animat Virt World 2016; 27(1): 35–57.

30.

Fiorini

Shiller

. Motion planning in dynamic environments using velocity obstacles. Int J Robot Res 1998; 17(7): 760–772.

31.

Duindam

Alterovitz

, et al. Three-dimensional motion planning algorithms for steerable needles using inverse kinematics. Int J Robot Res 2010; 29(7): 789–800.

32.

Park

Pan

Manocha

ITOMP: incremental trajectory optimization for real-time replanning in dynamic environments. In: 22nd international conference on automated planning and scheduling (ICAPS) (ed McCluskey

), Atibaia, Sao Paulo, Brazil, June 2012, pp. 207–215. AAAI Press.

33.

Tao

Yang

. Collision-free motion planning of a virtual arm based on the FABRIK algorithm. Robotica 2017; 35(6): 1431–1450.

34.

Menon

Ravi

Ghosal

. Trajectory planning and obstacle avoidance for hyper-redundant serial robots. J Mech Robot 2017; 9(4): 041010 (9 pages).

35.

Dalla

Pathak

. Curve-constrained collision-free trajectory control of hyper-redundant planar space robot. Proc Instit Mech Eng Part I 2017; 231(4): 282–298.

36.

Marinho

Adorno

Harada

, et al. Dynamic active constraints for surgical robots using vector-field inequalities. IEEE Trans Robot 2019; 35(5): 1166–1185.

37.

Safeea

Bearee

Neto

. Collision avoidance of redundant robotic manipulators using Newton’s method. J Intell Robot Syst 2020; 99(3): 673–681.

38.

Shimizu

Kakuya

Yoon

, et al. Analytical inverse kinematic computation for 7-dof redundant manipulators with joint limits and its application to redundancy resolution. IEEE Trans Robot 2008; 24(5): 1131–1142.

39.

Ruben

Erwin

. Inverse kinematics with the kinematics and dynamics library (KDL). Orocos Developers. http://www.orocos.org/forum/orocos/orocos-users/inverse-kinematics-kdl (2010, accessed July 2020).

40.

Andres

Antonio

Jaime

. The natural-CCD algorithm, a novel method to solve the inverse kinematics of hyper-redundant and soft robots. Soft Robot 2018; 5(3): 242–257.

41.

Trivedi

Rahn

Kier

, et al. Soft robotics: biological inspiration, state of the art, and future research. Appl Bionics Biomech 2008; 5(3): 99–117.

42.

Kim

Laschi

Trimmer

. Soft robotics: a bioinspired evolution in robotics. Trends Biotechnol 2013; 31(5): 287–294.

Supplementary Material

Please find the following supplemental material available below.

For Open Access articles published under a Creative Commons License, all supplemental material carries the same license as the article it is associated with.

For non-Open Access articles published, all supplemental material carries a non-exclusive license, and permission requests for re-use of supplemental material or any part of supplemental material shall be sent directly to the copyright owner as specified in the copyright notice associated with the article.

0.00 MB