Extending FABRIK with Obstacle Avoidance for Solving the Inverse Kinematics Problem

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.(e 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.


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. e 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. e remainder of this study is organized as follows. First, a brief review of the related work is presented in Section 2. Second, the basic FABRIK is introduced in Section 3. In Section 4, an extending FABRIK with obstacle avoidance is presented in detail. en, 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.
problems, which can be found in [2][3][4], here we focus on the literature in analytical methods and numerical methods.

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.
e position and orientation of the end-effector determines the wrist position. en, 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. ey realized the combined control of global arm configuration manifolds and local selfmotion 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. ey 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.

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. ey 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. erefore, they return smooth motion without erratic discontinuities [20]. e 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. e 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][28][29][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. ey 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. ey may use various criteria (such as decreasing convergence time, avoiding contact with obstacles, and avoiding joint angle limits) to locate the robot.

The Basic FABRIK
e basic FABRIK can be found in section 3 of [1]. Here, we use a manipulator with a single target T and four joints P 1 , P 2 , P 3 , P 4 to explain a full iteration. In Figure 1, P 1 and P 4 , respectively, represent the root joint and the end effector, 2 Journal of Robotics as well as P 2 and P 3 , respectively, express the middle joints, and d j � |P j+1 − P j |(j � 1, 2, 3) is the distance between two adjacent joints P j+1 and P j . First, calculate the distances d t � |T − P 1 | and d j � |P j+1 − P j |(j � 1, 2, 3). en, check the target position T whether it is reachable or not. If d t < |d 1 + d 2 + d 3 |, 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 Figure 1(b) shows that the end effector P 4 is moved to the target position T. In Figure 1(c), the updated position P 0 3 of the middle joint P 3 is found in the line l 3 , which has the distance |P 0 Similarly, the updated positions P 0 2 and P 0 1 for the middle joint P 2 and the root joint P 1 are, respectively, shown in Figure 1 Figure 1(e) shows that P 0 1 is moved to the initial position P 1 (P 1 1 is the updated position of P 0 1 ). Similar to Figures 1(b) and 1(c), the updated positions P 1 2 , P 1 3 , and P 1 4 are found in Figure 1(f).
After a full iteration is performed, the updated position P 1 4 of the end effector P 4 is very close to the target point T. Repeat above iteration procedures until the distance between the end effector P 4 and the target position T is less than the convergence precision β. e 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].

Extending FABRIK with Obstacle Avoidance
Since a manipulator usually works in a complex environment, the joint chain may have the interference with its surroundings. us, the surrounding scenarios between the desired position T and the initial position P 1 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. ). (f ) Updated positions P 2 1 , P 3 1 , and P 4 1 .

e Two-Step Obstacle Avoidance Technique.
e 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 bar P v j−1 P 0 j (j � 3, 4, . . . , n) (P 0 j−1 P v j (j � 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 P l j−1 P 0 j (P 0 j−1 P l j ) (Figures 2(b)  and 2(e)). e local optimization direction is that the root joint P 1 , the updated linkage bar P l j−1 P 0 j (P 0 j−1 P l j ), and the target position T are located in a line, and the updated position P l j−1 (P l j ) is close to the root joint P 1 (the target position T) . e reasons are that if P 1 , P l j−1 P 0 j (P 0 j−1 P l j ) and T are collinear, the interference between P l j−1 P 0 j (P 0 j−1 P l j ) and the obstacles can be eliminated in most cases, and when P l j−1 (P l j ) is as close to the root joint P 1 (the target position T) as possible, the joint chains may have slight changes.
In the second step, multiple random rotation strategies are adopted to eliminate the interference that has not been eliminated in the first step (Figure 2(e)). Since there is the interference between the linkage bar P l j−1 P 0 j (P 0 j−1 P l j ) and the obstacle in Figure 2(e), P l j−1 P 0 j (P 0 j−1 P l j ) randomly rotates many times around P 0 j (P 0 j−1 ) in the forward (backward) search so that the interference is eliminated. And the optimal solution P 0 j−1 P 0 j is selected from k updated positions of P k j−1 P 0 j (P 0 j−1 P k j ), which corresponds to the position with the minimum rotation angle (Figure 2(f)). As only the linkage bar P v j−1 P 0 j (P 0 j−1 P v j ) 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.

An Example for Extending FABRIK with Obstacle
Avoidance. In the following, we use , and the basic FABRIK is adopted to find the updated position P v 3 of P 3 . However, there is the interference between the updated linkage bar P v 3 P 0 4 and the obstacle. So, P v 3 P 0 4 took a rotation along the local optimization direction to its updated position P l 3 P 0 4 , which has P 1 , P v 3 P 0 4 , and T in a line. Since there is no interference between the updated linkage bar P l 3 P 0 4 and the obstacle in Figure 2(b), the local optimization rotation is terminated. However, P l 3 P 0 4 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 P k 3 P 0 4 . And the optimization position P 0 3 P 0 4 is selected from P k 3 P 0 4 , which has the smallest rotation angle. Finally, the basic FABRIK is adopted to find the updated positions P 0 2 and P 0 1 in Figures 2(c) and 2(f).

4.3.
e Procedure of Extending FABRIK with Obstacle Avoidance. In the following, we give the procedures of extending FABRIK with obstacle avoidance in detail.
Input. e target position T, the root joint P 1 , the end effector P n , middle joint P j+1 (j � 1, 2, . . . , n − 2), the convergent precision β, and the maximum number of iterations n max Output. Updated joint positions P j (j � 1, 2, . . . , n) Step 1. Calculate the distances d j � |P j+1 − P j | (j � 1, 2, . . . , n − 1) and d t � |T − P 1 | Step 2. Checks whether the target position T is within reach If d t < n−1 j�1 d j , the target position T is reachable, go to the next step; else, go to Step 6.
Step 3. Forward search with obstacle avoidance Step 3.1. Move the end effector P n to the target position T (P 0 n is the updated position of P n ), and find the updated position P v n−1 of the middle joint P n−1 in the line P n−1 P 0 n , which has the distance Step 3.2. Check whether there is the interference between the linkage bar P v n−1 P 0 n and the obstacle. If the interference exists, go to the next step; else, go to Step 3.6; Step 3.3. e linkage bar P v n−1 P 0 n takes a rotation around P 0 n , until P 1 , P l n−1 P 0 n are located in a line; then, P v n−1 has its updated position P l n−1 , and P l n−1 is close to the root joint P 1 ; Step 3.4. Check whether there is the interference between the linkage bar P l n−1 P 0 n and the obstacle. If the interference exists, go to the next step; else, go to Step 3.6; Step 3.5. e linkage bar P l n−1 P 0 n rotates many times around P 0 n at random, until there is no interference between P l n−1 P 0 n and the obstacle; then, select the optimization position P 0 n−1 from k updated positions of P k n−1 ; Step 3.6. Repeat Step 3.1-Step 3.5, and find updated positions P 0 n−2 , . . . , P 0 2 , P 0 1 .

Step 4. Backward search with obstacle avoidance
Step 4.1. Move P 0 1 to the root joint P 1 (P 1 1 is the updated position of P 0 1 ), and find the updated position P v 2 of the middle joint P 2 in the line P 2 P 1 1 , which has the distance |P v 2 − P 1 1 | � |P 2 − P 1 | � d 1 ; Step 4.2. Check whether there is the interference between the linkage bar P 1 1 P v 2 and the obstacle. If the interference exists, go to the next step; else, go to Step 4.6; Step 4.3. e linkage bar P 1 1 P v 2 takes a rotation around P 1 1 , until T, P 1 1 P l 2 are located in a line; then, P v 2 has its 4 Journal of Robotics updated position P l 2 , and P l 2 is close to the target position T; Step 4.4. Check whether there is the interference between the linkage bar P 1 1 P l 2 and the obstacle. If the interference exists, go to the next step; else, go to Step 4.6; Step 4.5. e linkage bar P 1 1 P l 2 rotates many times around P 1 1 at random, until there is no interference between P 1 1 P l 2 and the obstacle; then, select the optimization position P 1 2 from k updated positions of P k 2 ; Step 4.6. Repeat Step 4.1-Step 4.5, and updated positions P 1 3 , P 1 4 , . . . , P 1 n .
Step 5. Check whether the updated linkage bars P 1 1 P 1 2 , P 1 2 P 1 3 , . . . , P 1 n−1 P 1 n have interference with the obstacles and if the distance d � |T − P 1 n | < β: If the two constraint conditions are true, the target position is reachable, and the updated joint positions P j (j � 1, 2, . . . , n) are output; otherwise, go to Step 2. When the number of iterations n ≥ n max , two constraint conditions are false, and the target position is unreachable.

Experimental Results
To validate the proposed method, we use Matlab program to test if the extending FABRIK has the obstacle avoidance ability. e 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  1.7, 1.7)), and their radius are randomly generated between 0.1 and 0.3. e random rotating number k, the maximum number of iterations n max , and the convergence precision β are, respectively, set at 10, 100, and 0.001. e 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.
e statistical results in test 1-test 3 are, respectively, given in Tables 1-3, and  In Figures 3-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       e reason is that the distance between the target position T and the initial position P 1 is closer in Figures 4 and 5.

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. e reason is that a random rotation is adopted to avoid the obstacle (Step 3.5 in Section 4.3). e next step is to improve the stability and optimize the test results for the proposed method in the future.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest
e authors declare that there are no conflicts of interest.