# Extending FABRIK with Obstacle Avoidance for Solving the Inverse Kinematics Problem

Authors: Songqiao Tao; Huajin Tao; Yumeng Yang
Journal: Journal of Robotics
 (2021)

---

## Abstract

Inverse kinematics (IK) has been extensively applied in the areas of robotics, computer animation, ergonomics, and gaming. Typically, IK determines the joint configurations of a robot model and achieves a desired end-effector position in robotics. Since forward and backward teaching inverse kinematics (FABRIK) is a forward and backward iterative method that finds updated joint positions by locating a point on a line instead of using angle rotations or matrices, it has the advantages of fast convergence, low computational cost, and visualizing realistic poses. However, the manipulators usually work in a complex environment. So, the kinematic chains are easy to produce the interference with their surrounding scenarios. To resolve the above mentioned problem, a two-step obstacle avoidance technology is proposed to extend the basic FABRIK in this study. The first step is a heuristic method that locates the updated linkage bar, the root joint, and the target position in a line, so that the interference can be eliminated in most cases. In the second step, multiple random rotation strategies are adopted to eliminate the interference that has not been eliminated in the first step. Experimental results have shown that the extending FABRIK has the obstacle avoidance ability.


---

## Body

## 1. Introduction

In recent years, IK has been extensively applied in the areas of robotics, computer animation, ergonomics, and gaming [1]. More generally, IK is the use of kinematic equations to determine the joint parameters of a manipulator, so that the end effector is moved to a desired position [2]. For a serial manipulator, for instance, IK is to find the angles of all joints in the manipulator desiring position of the end effector.Aristidou et al. [1] presented a forward and backward iterative technology called FABRIK to seek out updated joint positions by locating a point on a line instead of using angle rotations or matrices. The FABRIK has the advantages of converge in few iterations, low computational complexity, and visualizing realistic poses. Compared with the CCD algorithm, the FABRIK produces visually smooth movements without oscillations and discontinuities and handles multiple chains with multiple end effectors. However, the work environment of a manipulator is usually complex. So, the joint chain is easy to interfere with its surrounding scenarios. To resolve the above mentioned problem, we introduce a two-step obstacle avoidance technology to extend the basic FABRIK in this study.The remainder of this study is organized as follows. First, a brief review of the related work is presented in Section2. Second, the basic FABRIK is introduced in Section 3. In Section 4, an extending FABRIK with obstacle avoidance is presented in detail. Then, Section 5 presents some results of experiments that apply the proposed approach to solve IK problems. Finally, the study ends up with some conclusions in Section 6.


## 2. Related Work

Since, recently, IK problems have become an active research topic, a lot of works have been performed in disciplines such as use of robotics, computer animation, ergonomics, and gaming. Instead of giving a complete overview to IK problems, which can be found in [2–4], here we focus on the literature in analytical methods and numerical methods.


### 2.1. Analytical Methods

Analytical methods usually find all possible solutions as a function of the mechanism’s lengths, its initial position, and the rotation constraints. To just calculate a single solution, analytical solutions are usually based on some assumptions [2]. Typical analytical IK solutions for dealing with general 6R manipulators early appeared in [5, 6]. Diankov [7] has developed a tool called IKFast to solve robot IK equations in an analytic form and deployed motion planning algorithms in real-world robotics applications. More recently, Liu et al. [8] presented an analytical IK solver that realizes human-like configurations by means of selecting a redundancy resolution. Zhao et al. [9] proposed a new IK method to obtain anthropomorphic arm postures for a given end-effector position and orientation. The position and orientation of the end-effector determines the wrist position. Then, the elbow twist angle is adopted to solve the redundancy resolution of human arm. Tian et al. [10] proposed an analytical IK method for 7-DOF anthropomorphic manipulators. They realized the combined control of global arm configuration manifolds and local self-motion without offset by defining a new arm reference plane for 7-DOF anthropomorphic manipulators. Tong et al. [11] presented an analytical IK parameterized method for redundant sliding manipulators. They solved the IK analytically and used the Newton–Raphson algorithm for secondary adjustment. Analytical IK solutions are usually global, nonsingular, fast, simple, and reliable. However, the nonlinear characteristics of the kinematic equations make analytical IK methods unsuitable for redundant systems with more than 7-DoF.


### 2.2. Numerical Methods

Numerical methods are usually the iterative approaches that formulate IK problems using a cost function to be minimized. Typical numerical IK solutions have Jacobian, Newton, and Heuristic methods.Jacobian approaches usually use Jacobian matrix to express the linear approximation of nonlinear functions, which repeatedly change the configuration of a complete chain, such that it gradually makes the end-effector approach the target position at each step. Some different ways have been proposed for calculating or approximating the inverse of the Jacobian matrix, such as the Jacobian transpose [12, 13], singular value decomposition [14, 15], damped least squares [16, 17], and incorporating constraints [18, 19]. Jacobian methods usually have smooth posture. However, they have the disadvantages of high computational cost, complex matrix calculations, and singularity problems [20]. Recently, Libretto et al. [21] presented singularity-free solutions for inverse kinematics of degenerate mobile robots. They solved the IK problem using degenerate wheel structure that does not suffer from singularity problems.Newton approaches usually find target configurations that are posed as solutions to a minimization problem. Therefore, they return smooth motion without erratic discontinuities [20]. The most famous approaches are the Broyden method, Powell method, and the Broyden, Fletcher, Goldfarb, and Shanno method [22]. To resolve the inverse problems that the manipulator link lengths, joint angles, and end-effector uncertainty bounds are given, Kumar et al. [23] adopted an interval Newton method to solve the IK and redundancy resolution of a serial redundant manipulator. To resolve the problem that the 6-DOF joint robot with offset wrist cannot satisfy the Pieper rule, Lei et al. [24] presented a modified Newton iteration method to solve IK problem for 6-DOF joint robot with offset wrist. Duleba and Karcz-Duleba [25] presented accelerating Newton algorithms of inverse kinematics for robot manipulators. Erleben and Andrews [26] proposed a numerical approach for calculating the exact Hessian of an IK system with prismatic, revolute, and spherical joints. The advantage of Newton methods is that they have nonsingular problems. However, their disadvantages are complex, difficult to implement, and have high computational cost at each iteration.Heuristic approaches usually use simple ways for solving the IK problem without using complex equations and calculations. Typical heuristic methods for solving IK problems have cyclic coordinate descent (CCD) [27–30] and triangulation IK [31, 32]. CCD is an iterative heuristic technique that is suitable for interactive control of an articulated body, which is early proposed by Wang and Chen [27]. Kenwight has given a comprehensive review of CCD in [33]. CCD provides a numerically stable solution and has the advantage of low computational cost in an iteration. However, CCD may generate unrealistic postures and has difficulty in solving IK problems with multiple end effectors. As triangulation IK calculates the joint angle by using the cosine law, which usually finds the desired position in few iterations and has low computational cost. However, using triangulation IK to solve IK problems with multiple end effectors is an intractable task. FABRIK is another heuristic method for solving the IK problem, which finds the joint positions by locating a point on a line instead of using angle rotations [1]. Some researchers have developed extending FABRIK methods because of its efficiency and simplicity. Jing and Pelachaud [34] presented an efficient energy transfer IK solution using a variation of FABRIK. A mass-spring model with minimizing the force energy is adopted to adjust the joint positions. Agrawalet and Michiel [35] introduced a data-driven prior to warm start FABRIK for the production of high-quality motions. Tenneti and Sarkar [36] proposed a modified FABRIK method to solve the IK problem. They adopted Denavit and Hartenberg parameters to define the geometry of a serial robot manipulator. Santos et al. [37] presented a new IK method called M-FABRIK for mobile manipulator robots. They may use various criteria (such as decreasing convergence time, avoiding contact with obstacles, and avoiding joint angle limits) to locate the robot.


## 2.1. Analytical Methods

Analytical methods usually find all possible solutions as a function of the mechanism’s lengths, its initial position, and the rotation constraints. To just calculate a single solution, analytical solutions are usually based on some assumptions [2]. Typical analytical IK solutions for dealing with general 6R manipulators early appeared in [5, 6]. Diankov [7] has developed a tool called IKFast to solve robot IK equations in an analytic form and deployed motion planning algorithms in real-world robotics applications. More recently, Liu et al. [8] presented an analytical IK solver that realizes human-like configurations by means of selecting a redundancy resolution. Zhao et al. [9] proposed a new IK method to obtain anthropomorphic arm postures for a given end-effector position and orientation. The position and orientation of the end-effector determines the wrist position. Then, the elbow twist angle is adopted to solve the redundancy resolution of human arm. Tian et al. [10] proposed an analytical IK method for 7-DOF anthropomorphic manipulators. They realized the combined control of global arm configuration manifolds and local self-motion without offset by defining a new arm reference plane for 7-DOF anthropomorphic manipulators. Tong et al. [11] presented an analytical IK parameterized method for redundant sliding manipulators. They solved the IK analytically and used the Newton–Raphson algorithm for secondary adjustment. Analytical IK solutions are usually global, nonsingular, fast, simple, and reliable. However, the nonlinear characteristics of the kinematic equations make analytical IK methods unsuitable for redundant systems with more than 7-DoF.


## 2.2. Numerical Methods

Numerical methods are usually the iterative approaches that formulate IK problems using a cost function to be minimized. Typical numerical IK solutions have Jacobian, Newton, and Heuristic methods.Jacobian approaches usually use Jacobian matrix to express the linear approximation of nonlinear functions, which repeatedly change the configuration of a complete chain, such that it gradually makes the end-effector approach the target position at each step. Some different ways have been proposed for calculating or approximating the inverse of the Jacobian matrix, such as the Jacobian transpose [12, 13], singular value decomposition [14, 15], damped least squares [16, 17], and incorporating constraints [18, 19]. Jacobian methods usually have smooth posture. However, they have the disadvantages of high computational cost, complex matrix calculations, and singularity problems [20]. Recently, Libretto et al. [21] presented singularity-free solutions for inverse kinematics of degenerate mobile robots. They solved the IK problem using degenerate wheel structure that does not suffer from singularity problems.Newton approaches usually find target configurations that are posed as solutions to a minimization problem. Therefore, they return smooth motion without erratic discontinuities [20]. The most famous approaches are the Broyden method, Powell method, and the Broyden, Fletcher, Goldfarb, and Shanno method [22]. To resolve the inverse problems that the manipulator link lengths, joint angles, and end-effector uncertainty bounds are given, Kumar et al. [23] adopted an interval Newton method to solve the IK and redundancy resolution of a serial redundant manipulator. To resolve the problem that the 6-DOF joint robot with offset wrist cannot satisfy the Pieper rule, Lei et al. [24] presented a modified Newton iteration method to solve IK problem for 6-DOF joint robot with offset wrist. Duleba and Karcz-Duleba [25] presented accelerating Newton algorithms of inverse kinematics for robot manipulators. Erleben and Andrews [26] proposed a numerical approach for calculating the exact Hessian of an IK system with prismatic, revolute, and spherical joints. The advantage of Newton methods is that they have nonsingular problems. However, their disadvantages are complex, difficult to implement, and have high computational cost at each iteration.Heuristic approaches usually use simple ways for solving the IK problem without using complex equations and calculations. Typical heuristic methods for solving IK problems have cyclic coordinate descent (CCD) [27–30] and triangulation IK [31, 32]. CCD is an iterative heuristic technique that is suitable for interactive control of an articulated body, which is early proposed by Wang and Chen [27]. Kenwight has given a comprehensive review of CCD in [33]. CCD provides a numerically stable solution and has the advantage of low computational cost in an iteration. However, CCD may generate unrealistic postures and has difficulty in solving IK problems with multiple end effectors. As triangulation IK calculates the joint angle by using the cosine law, which usually finds the desired position in few iterations and has low computational cost. However, using triangulation IK to solve IK problems with multiple end effectors is an intractable task. FABRIK is another heuristic method for solving the IK problem, which finds the joint positions by locating a point on a line instead of using angle rotations [1]. Some researchers have developed extending FABRIK methods because of its efficiency and simplicity. Jing and Pelachaud [34] presented an efficient energy transfer IK solution using a variation of FABRIK. A mass-spring model with minimizing the force energy is adopted to adjust the joint positions. Agrawalet and Michiel [35] introduced a data-driven prior to warm start FABRIK for the production of high-quality motions. Tenneti and Sarkar [36] proposed a modified FABRIK method to solve the IK problem. They adopted Denavit and Hartenberg parameters to define the geometry of a serial robot manipulator. Santos et al. [37] presented a new IK method called M-FABRIK for mobile manipulator robots. They may use various criteria (such as decreasing convergence time, avoiding contact with obstacles, and avoiding joint angle limits) to locate the robot.


## 3. The Basic FABRIK

The basic FABRIK can be found in section 3 of [1]. Here, we use a manipulator with a single target T and four joints P1,P2,P3,P4 to explain a full iteration. In Figure 1, P1 and P4, respectively, represent the root joint and the end effector, as well as P2 and P3, respectively, express the middle joints, and dj=Pj+1−Pjj=1,2,3 is the distance between two adjacent joints Pj+1 and Pj.Figure 1
An example of a full iteration for a manipulator with a single target and four joints. (a) The initial state of the manipulator and the target position. (b) The end effectorP4 moved to the target position (T). (c) The middle joint P3’s updated position P30. (d) The updated positions P20 and P10. (e) P10 moved to the initial position P1 (P11 is the updated position of P10). (f) Updated positions P21, P31, and P41.
(a)(b)(c)(d)(e)(f)First, calculate the distancesdt=T−P1 and dj=Pj+1−Pjj=1,2,3. Then, check the target position T whether it is reachable or not. If dt<d1+d2+d3, the target position T is reachable, and a full iteration will be implemented in two stages; else, T is unreachable, and the FABRIK is terminated. In the first stage, Figures 1(b)–1(d) represent the forward search. Figure 1(b) shows that the end effector P4 is moved to the target position T. In Figure 1(c), the updated position P30 of the middle joint P3 is found in the line l3, which has the distance P40−P30=P4−P3=d3. Similarly, the updated positions P20 and P10 for the middle joint P2 and the root joint P1 are, respectively, shown in Figure 1(d). In the second stage, Figures 1(e)–1(f) express the backward search. Figure 1(e) shows that P10 is moved to the initial position P1 (P11 is the updated position of P10). Similar to Figures 1(b) and 1(c), the updated positions P21, P31, and P41 are found in Figure 1(f). After a full iteration is performed, the updated position P41 of the end effector P4 is very close to the target point T. Repeat above iteration procedures until the distance between the end effector P4 and the target position T is less than the convergence precision β.The limitation of the FABRIK (a singularity problem) is explained and resolved in section 3.2 of [20]. Besides, the proof of FABRIK converges can be found in section 3.3 of [20].


## 4. Extending FABRIK with Obstacle Avoidance

Since a manipulator usually works in a complex environment, the joint chain may have the interference with its surroundings. Thus, the surrounding scenarios between the desired positionT and the initial position P1 should be considered for extending the basic FABRIK. So, a two-step obstacle avoidance technique is adopted to ensure that the extending FABRIK has the obstacle avoidance ability.


### 4.1. The Two-Step Obstacle Avoidance Technique

The first step is a heuristic way that locates the updated linkage bar, the root joint, and the target position in a line, so that the interference can be eliminated in most cases. If a linkage barPj−1vPj0j=3,4,…,n (Pj−10Pjvj=2,3,…,n−1) has the interference with its surroundings in the forward (backward) search, it will rotate along a local optimization direction to an updated position Pj−1lPj0(Pj−10Pjl) (Figures 2(b) and 2(e)). The local optimization direction is that the root joint P1, the updated linkage bar Pj−1lPj0(Pj−10Pjl), and the target position T are located in a line, and the updated position Pj−1l(Pjl) is close to the root joint P1 (the target position T). The reasons are that if P1, Pj−1lPj0(Pj−10Pjl) and T are collinear, the interference between Pj−1lPj0(Pj−10Pjl) and the obstacles can be eliminated in most cases, and when Pj−1l(Pjl) is as close to the root joint P1 (the target position T) as possible, the joint chains may have slight changes.Figure 2
Extending FABRIK with obstacle avoidance. (a), (d) The initial state of the manipulator, the target position, and the obstacle. (b), (e)P4 moved to the target position (T) and the optimization P30 found. (c), (f) The basic FABRIK used to find the updated positions P20 and P10.
(a)(b)(c)(d)(e)(f)In the second step, multiple random rotation strategies are adopted to eliminate the interference that has not been eliminated in the first step (Figure2(e)). Since there is the interference between the linkage bar Pj−1lPj0(Pj−10Pjl) and the obstacle in Figure 2(e), Pj−1lPj0(Pj−10Pjl) randomly rotates many times around Pj0(Pj−10) in the forward (backward) search so that the interference is eliminated. And the optimal solution Pj−10Pj0 is selected from k updated positions of Pj−1kPj0(Pj−10Pjk), which corresponds to the position with the minimum rotation angle (Figure 2(f)). As only the linkage bar Pj−1vPj0(Pj−10Pjv) is optimized in an iteration, the rotation technique is just a local optimization. However, the FABRIK is a heuristic iterative approach, subsequent iterations will gradually improve previous results, and the final result is very close to the optimum solution.


### 4.2. An Example for Extending FABRIK with Obstacle Avoidance

In the following, we use Figure2 to explain the extending FABRIK with obstacle avoidance. The circles with shadow express the obstacles in Figure 2. Two cases are that the obstacles with various positions are listed in Figures 2(a)–2(c) and Figures 2(d)–2(f). Figures 2(a)–2(c) represent the case that P1, P3lP40, and T are collinear, and there is no interference between P3lP40 and the obstacle. Thus, P3lP40 does not need to rotate. Conversely, Figures 2(d)–2(f) express the case that there is the interference between P3lP40 and the obstacle.First, the initial state of the manipulator, the target position, and the obstacle are listed in Figures2(a) and 2(d). Then, Figures 2(b) and 2(e) are that the end effector P4 is moved to the target position T (P40 is the updated position of P4), and the basic FABRIK is adopted to find the updated position P3v of P3. However, there is the interference between the updated linkage bar P3vP40 and the obstacle. So, P3vP40 took a rotation along the local optimization direction to its updated position P3lP40, which has P1, P3vP40, and T in a line. Since there is no interference between the updated linkage bar P3lP40 and the obstacle in Figure 2(b), the local optimization rotation is terminated. However, P3lP40 is still interferential with the obstacle in Figure 2(e). To resolve above mentioned problem, multiple random rotations are used for finding k updated positions of P3kP40. And the optimization position P30P40 is selected from P3kP40, which has the smallest rotation angle. Finally, the basic FABRIK is adopted to find the updated positions P20 and P10 in Figures 2(c) and 2(f).


### 4.3. The Procedure of Extending FABRIK with Obstacle Avoidance

In the following, we give the procedures of extending FABRIK with obstacle avoidance in detail.Input. The target positionT, the root joint P1, the end effector Pn, middle joint Pj+1j=1,2,…,n−2, the convergent precision β, and the maximum number of iterations nmaxOutput. Updated joint positionsPjj=1,2,…,nStep 1. Calculate the distancesdj=Pj+1−Pjj=1,2,…,n−1 and dt=T−P1Step 2. Checks whether the target positionT is within reachIfdt<∑j=1n−1dj, the target position T is reachable, go to the next step; else, go to Step 6.Step 3. Forward search with obstacle avoidanceStep 3.1. Move the end effectorPn to the target position T (Pn0 is the updated position of Pn), and find the updated position Pn−1v of the middle joint Pn−1 in the line Pn−1Pn0, which has the distance Pn0−Pn−1v=Pn−Pn−1=dn−1;Step 3.2. Check whether there is the interference between the linkage barPn−1vPn0 and the obstacle. If the interference exists, go to the next step; else, go to Step 3.6;Step 3.3. The linkage barPn−1vPn0 takes a rotation around Pn0, until P1,Pn−1lPn0 are located in a line; then, Pn−1v has its updated position Pn−1l, and Pn−1l is close to the root joint P1;Step 3.4. Check whether there is the interference between the linkage barPn−1lPn0 and the obstacle. If the interference exists, go to the next step; else, go to Step 3.6;Step 3.5. The linkage barPn−1lPn0 rotates many times around Pn0 at random, until there is no interference between Pn−1lPn0 and the obstacle; then, select the optimization position Pn−10 from k updated positions of Pn−1k;Step 3.6. Repeat Step 3.1–Step 3.5, and find updated positionsPn−20,…,P20,P10.Step 4. Backward search with obstacle avoidanceStep 4.1. MoveP10 to the root joint P1 (P11 is the updated position of P10), and find the updated position P2v of the middle joint P2 in the line P2P11, which has the distance P2v−P11=P2−P1=d1;Step 4.2. Check whether there is the interference between the linkage barP11P2v and the obstacle. If the interference exists, go to the next step; else, go to Step 4.6;Step 4.3. The linkage barP11P2v takes a rotation around P11, until T,P11P2l are located in a line; then, P2v has its updated position P2l, and P2l is close to the target position T;Step 4.4. Check whether there is the interference between the linkage barP11P2l and the obstacle. If the interference exists, go to the next step; else, go to Step 4.6;Step 4.5. The linkage barP11P2l rotates many times around P11 at random, until there is no interference between P11P2l and the obstacle; then, select the optimization position P21 from k updated positions of P2k;Step 4.6. Repeat Step 4.1–Step 4.5, and updated positionsP31,P41,…,Pn1.Step 5. Check whether the updated linkage barsP11P21,P21P31,…,Pn−11Pn1 have interference with the obstacles and if the distance d=T−Pn1<β:If the two constraint conditions are true, the target position is reachable, and the updated joint positionsPjj=1,2,…,n are output; otherwise, go to Step 2. When the number of iterations n≥nmax, two constraint conditions are false, and the target position is unreachable.Step 6. End.


## 4.1. The Two-Step Obstacle Avoidance Technique

The first step is a heuristic way that locates the updated linkage bar, the root joint, and the target position in a line, so that the interference can be eliminated in most cases. If a linkage barPj−1vPj0j=3,4,…,n (Pj−10Pjvj=2,3,…,n−1) has the interference with its surroundings in the forward (backward) search, it will rotate along a local optimization direction to an updated position Pj−1lPj0(Pj−10Pjl) (Figures 2(b) and 2(e)). The local optimization direction is that the root joint P1, the updated linkage bar Pj−1lPj0(Pj−10Pjl), and the target position T are located in a line, and the updated position Pj−1l(Pjl) is close to the root joint P1 (the target position T). The reasons are that if P1, Pj−1lPj0(Pj−10Pjl) and T are collinear, the interference between Pj−1lPj0(Pj−10Pjl) and the obstacles can be eliminated in most cases, and when Pj−1l(Pjl) is as close to the root joint P1 (the target position T) as possible, the joint chains may have slight changes.Figure 2
Extending FABRIK with obstacle avoidance. (a), (d) The initial state of the manipulator, the target position, and the obstacle. (b), (e)P4 moved to the target position (T) and the optimization P30 found. (c), (f) The basic FABRIK used to find the updated positions P20 and P10.
(a)(b)(c)(d)(e)(f)In the second step, multiple random rotation strategies are adopted to eliminate the interference that has not been eliminated in the first step (Figure2(e)). Since there is the interference between the linkage bar Pj−1lPj0(Pj−10Pjl) and the obstacle in Figure 2(e), Pj−1lPj0(Pj−10Pjl) randomly rotates many times around Pj0(Pj−10) in the forward (backward) search so that the interference is eliminated. And the optimal solution Pj−10Pj0 is selected from k updated positions of Pj−1kPj0(Pj−10Pjk), which corresponds to the position with the minimum rotation angle (Figure 2(f)). As only the linkage bar Pj−1vPj0(Pj−10Pjv) is optimized in an iteration, the rotation technique is just a local optimization. However, the FABRIK is a heuristic iterative approach, subsequent iterations will gradually improve previous results, and the final result is very close to the optimum solution.


## 4.2. An Example for Extending FABRIK with Obstacle Avoidance

In the following, we use Figure2 to explain the extending FABRIK with obstacle avoidance. The circles with shadow express the obstacles in Figure 2. Two cases are that the obstacles with various positions are listed in Figures 2(a)–2(c) and Figures 2(d)–2(f). Figures 2(a)–2(c) represent the case that P1, P3lP40, and T are collinear, and there is no interference between P3lP40 and the obstacle. Thus, P3lP40 does not need to rotate. Conversely, Figures 2(d)–2(f) express the case that there is the interference between P3lP40 and the obstacle.First, the initial state of the manipulator, the target position, and the obstacle are listed in Figures2(a) and 2(d). Then, Figures 2(b) and 2(e) are that the end effector P4 is moved to the target position T (P40 is the updated position of P4), and the basic FABRIK is adopted to find the updated position P3v of P3. However, there is the interference between the updated linkage bar P3vP40 and the obstacle. So, P3vP40 took a rotation along the local optimization direction to its updated position P3lP40, which has P1, P3vP40, and T in a line. Since there is no interference between the updated linkage bar P3lP40 and the obstacle in Figure 2(b), the local optimization rotation is terminated. However, P3lP40 is still interferential with the obstacle in Figure 2(e). To resolve above mentioned problem, multiple random rotations are used for finding k updated positions of P3kP40. And the optimization position P30P40 is selected from P3kP40, which has the smallest rotation angle. Finally, the basic FABRIK is adopted to find the updated positions P20 and P10 in Figures 2(c) and 2(f).


## 4.3. The Procedure of Extending FABRIK with Obstacle Avoidance

In the following, we give the procedures of extending FABRIK with obstacle avoidance in detail.Input. The target positionT, the root joint P1, the end effector Pn, middle joint Pj+1j=1,2,…,n−2, the convergent precision β, and the maximum number of iterations nmaxOutput. Updated joint positionsPjj=1,2,…,nStep 1. Calculate the distancesdj=Pj+1−Pjj=1,2,…,n−1 and dt=T−P1Step 2. Checks whether the target positionT is within reachIfdt<∑j=1n−1dj, the target position T is reachable, go to the next step; else, go to Step 6.Step 3. Forward search with obstacle avoidanceStep 3.1. Move the end effectorPn to the target position T (Pn0 is the updated position of Pn), and find the updated position Pn−1v of the middle joint Pn−1 in the line Pn−1Pn0, which has the distance Pn0−Pn−1v=Pn−Pn−1=dn−1;Step 3.2. Check whether there is the interference between the linkage barPn−1vPn0 and the obstacle. If the interference exists, go to the next step; else, go to Step 3.6;Step 3.3. The linkage barPn−1vPn0 takes a rotation around Pn0, until P1,Pn−1lPn0 are located in a line; then, Pn−1v has its updated position Pn−1l, and Pn−1l is close to the root joint P1;Step 3.4. Check whether there is the interference between the linkage barPn−1lPn0 and the obstacle. If the interference exists, go to the next step; else, go to Step 3.6;Step 3.5. The linkage barPn−1lPn0 rotates many times around Pn0 at random, until there is no interference between Pn−1lPn0 and the obstacle; then, select the optimization position Pn−10 from k updated positions of Pn−1k;Step 3.6. Repeat Step 3.1–Step 3.5, and find updated positionsPn−20,…,P20,P10.Step 4. Backward search with obstacle avoidanceStep 4.1. MoveP10 to the root joint P1 (P11 is the updated position of P10), and find the updated position P2v of the middle joint P2 in the line P2P11, which has the distance P2v−P11=P2−P1=d1;Step 4.2. Check whether there is the interference between the linkage barP11P2v and the obstacle. If the interference exists, go to the next step; else, go to Step 4.6;Step 4.3. The linkage barP11P2v takes a rotation around P11, until T,P11P2l are located in a line; then, P2v has its updated position P2l, and P2l is close to the target position T;Step 4.4. Check whether there is the interference between the linkage barP11P2l and the obstacle. If the interference exists, go to the next step; else, go to Step 4.6;Step 4.5. The linkage barP11P2l rotates many times around P11 at random, until there is no interference between P11P2l and the obstacle; then, select the optimization position P21 from k updated positions of P2k;Step 4.6. Repeat Step 4.1–Step 4.5, and updated positionsP31,P41,…,Pn1.Step 5. Check whether the updated linkage barsP11P21,P21P31,…,Pn−11Pn1 have interference with the obstacles and if the distance d=T−Pn1<β:If the two constraint conditions are true, the target position is reachable, and the updated joint positionsPjj=1,2,…,n are output; otherwise, go to Step 2. When the number of iterations n≥nmax, two constraint conditions are false, and the target position is unreachable.Step 6. End.


## 5. Experimental Results

To validate the proposed method, we use Matlab program to test if the extending FABRIK has the obstacle avoidance ability. The test subject is a four-joint manipulator with the coordinates (0, 0), (0, 1), (0, 2), (0, 3), and (0, 4). Here, three groups of target positions with different distances are used to explain the influence of distance on the experimental results. The coordinates of target positions in test 1–test 3 are, respectively, set at (2.8, 2.8), (2.4, 2.4), and (2.0, 2.0). Besides, five round obstacles with various centers and radius are selected to test the obstacle avoidance adaptability of the proposed method. The centers of five round obstacles in test 1–test 3 are, respectively, located in three rectangles with diagonal vertices ((0.3, 0.3), (2.5, 2.5)), ((0.3, 0.3), (2.1, 2.1)), and ((0.3, 0.3), (1.7, 1.7)), and their radius are randomly generated between 0.1 and 0.3. The random rotating numberk, the maximum number of iterations nmax, and the convergence precision β are, respectively, set at 10, 100, and 0.001. The tests are carried out on a computer with an AMD 3.6 GHz CPU and 8.0 GB RAM.In three groups of the experiments, the extending FABRIK is run nine times in succession to test if it has the ability to avoid various obstacles and reach different target positions. The statistical results in test 1–test 3 are, respectively, given in Tables1–3, and visualization experiment results are, respectively, shown in Figures 3–5 and. Tables 1–3 present the number of iterations needed to reach the target, run time, and the distance between the target position and the end effector. It has shown that the extending FABRIK has lower computational cost and faster convergence speed since it treats finding the joint locations as a problem of finding a point on a line. And the distance between the target position and the root joint is closer, and the number of iterations and run time are less.Table 1
The experiment results of extending FABRIK with obstacle avoidance in test 1.
Whether the target is within reach or not?The number of iterationsThe distance between the target position and the end effector (mm)Run time (s)The results1Yes779.8638e-040.2659Figure3(a)2Yes559.8044e-040.2206Figure3(b)3Yes98.5390e-040.1097Figure3(c)4Yes308.7228e-040.2102Figure3(d)5Yes109.2472e-040.1272Figure3(e)6Yes238.5583e-040.1408Figure3(f)7No1000.11950.8193Figure3(g)8No1000.31360.9215Figure3(h)9No1000.29381.1800Figure3(i)Table 2
The experiment results of extending FABRIK with obstacle avoidance in test 2.
Whether the target is within reach or not?The number of iterationsThe distance between the target position and the end effector (mm)Run time (s)The results1Yes78.6819e-040.0880Figure4(a)2Yes46.1996e-040.0898Figure4(b)3Yes34.2464e-040.0853Figure4(c)4Yes25.1837e-040.1160Figure4(d)5Yes53.0560e-040.1012Figure4(e)6Yes52.5677e-060.0984Figure4(f)7Yes34.9393e-060.1119Figure4(g)8Yes36.0699e-040.1326Figure4(h)9Yes114.6694e-040.2120Figure4(i)Table 3
The experiment results of extending FABRIK with obstacle avoidance in test 3.
Whether the target is within reach or not?The number of iterationsThe distance between the target position and the end effector (mm)Run time (s)The results1Yes34.9565e-040.0748Figure5(a)2Yes23.7932e-050.0826Figure5(b)3Yes21.0127e-050.0917Figure5(c)4Yes15.0544e-040.0884Figure5(d)5Yes23.1292e-050.0918Figure5(e)6Yes22.7737e-040.0922Figure5(f)7Yes45.5242e-040.1394Figure5(g)8Yes62.7428e-050.1448Figure5(h)9Yes33.7580e-050.1350Figure5(i)Figure 3
Visualization experiment results of extending FABRIK with obstacle avoidance in test 1.
(a)(b)(c)(d)(e)(f)(g)(h)(i)Figure 4
Visualization experiment results of extending FABRIK with obstacle avoidance in test 2.
(a)(b)(c)(d)(e)(f)(g)(h)(i)Figure 5
Visualization experiment results of extending FABRIK with obstacle avoidance in test 3.
(a)(b)(c)(d)(e)(f)(g)(h)(i)In Figures3–5, the joint chains are labeled as blue, as well as the target positions and obstacles are, respectively, represented by red circles and pink circles. Meanwhile, vertical joint chains express the initial state, and other joint chains are the final postures that the end effectors reach the desired positions. It can be seen from Figure 3 that the extending FABRIK successfully avoids obstacles six times to reach their desired positions (Figures 3(a)–3(f)), and the target position is without reach thrice (Figures 3(g)–3(i)). In Figures 4 and 5, the extending FABRIK successfully avoids obstacles nine times to make the end effectors reach the desired positions. The experimental results have shown that the extending FABRIK has a stronger adaptability for avoiding various obstacles with different sizes and positions. Even if the desired positions are unreachable in Figures 3(g)–3(i), the extending FABRIK ensures that the end effector is close to the target point. Clearly, the experimental results of test 2 and test 3 are better than test 1. The reason is that the distance between the target position T and the initial position P1 is closer in Figures 4 and 5.


## 6. Conclusions

Since the manipulator usually works in a complex environment, the joint chain may produce an interference with its surrounding scenarios. So, a two-step obstacle avoidance technique is proposed to extend the basic FABRIK in this study. In the first step, a heuristic method is adopted to ensure the updated linkage bar, the root joint, and the target position in a line, so that the interference can be eliminated in most cases. In the second step, multiple random rotation strategies are applied for eliminating the interference that has not been eliminated in the first step. Experimental results have shown that the proposed method has the obstacle avoidance ability. However, the extending FABRIK still confronts with the problem that the experimental results may vary for different tests. The reason is that a random rotation is adopted to avoid the obstacle (Step 3.5 inSection 4.3). The next step is to improve the stability and optimize the test results for the proposed method in the future.



---

*Source: 5568702-2021-04-27.xml*