CRRIK: A Fast Heuristic Algorithm for the Inverse Kinematics of Continuum Robot

Abstract

The inverse kinematics (IK) of continuum robot is a challenging work due to the hyper-redundant DOFs and the narrow and complex operating environment. However, many of the currently available algorithms are with the disadvantage of complex solving processes, low computational efficiency, and even singular problems or low convergence rates. Referred to the Forward and Backward Reaching Inverse Kinematics (FABRIK) algorithm, a novel heuristic algorithm is proposed in this paper, which is called Continuum Robot Reaching Inverse Kinematics (CRRIK). The CRRIK algorithm is with the advantages of high convergence rate and low computational cost, which are suitable for real-time applications. The forward iteration of CRRIK is inspired by the physical process of pulling a rope with a fixed end, which is straightforward and obvious, avoiding complicated nonlinear operations. Furthermore, the joint angle boundary condition and obstacle avoidance can also be implemented in the backward iteration, broadening the application scenarios of the CRRIK algorithm. The effectiveness of the CRRIK algorithm is demonstrated through a five-segment continuum robot’s trajectory tracking and obstacle avoidance simulation. And a comparison between the CRRIK algorithm and some of the most popular IK methods is also presented to validate the performance of CRRIK.

Appendix: Brief Introduction of Other Algorithms

1.1 Jacobian

The continuum robot’s Jacobian matrix represents the mapping between configuration space and task space. Where x is the task space parameter and q is the configuration space parameter

$$\dot{x}=\frac{\partial k(q)}{\partial q}\dot{q}=J(q)\dot{q}$$

(26)

Where:

$$J(q)=\left[\begin{array}{cccc}\frac{\partial {x}_1}{\partial {q}_1}& \frac{\partial {x}_1}{\partial {q}_2}& \cdots & \frac{\partial {x}_1}{\partial {q}_j}\\ {}\frac{\partial {x}_2}{\partial {q}_1}& \frac{\partial {x}_2}{\partial {q}_2}& \cdots & \frac{\partial {x}_2}{\partial {q}_j}\\ {}\vdots & \vdots & \ddots & \vdots \\ {}\frac{\partial {x}_i}{\partial {q}_1}& \frac{\partial {x}_i}{\partial {q}_2}& \dots & \frac{\partial {x}_i}{\partial {q}_j}\end{array}\right]$$

(27)

The configuration parameters of the i + 1th iteration can be obtained from the data of i^th iteration. Where ξ_i is a given positive coefficient and J^#(q_i) is the pseudo-inverse of the Jacobian matrix. To retrieve invertibility in close vicinity of singular configurations, a diagonal matrix A scaled with a small positive coefficient λ is usually added to the manipulability matrix M.

$${\displaystyle \begin{array}{c}{q}_{i+1}={q}_i+{\xi}_i\cdot {J}^T\left({q}_i\right){\left(M\left({q}_i\right)+\lambda A\right)}^{-1}\left({x}_f-k\left({q}_i\right)\right)\\ {}={q}_i+{\xi}_i\cdot {J}^{\#}\left({q}_i\right)\left({x}_f-k\left({q}_i\right)\right)\end{array}}$$

(28)

Where:

$$M\left({q}_i\right)=J\left({q}_i\right){J}^T\left({q}_i\right)$$

(29)

The feasible solution for the robot can be obtained through iteration.

1.2 FABRIK

FABRIK divides the IK into two phases, forward and backward iterative approaches. Each joint location is determined by placing an anchor point on the line.

$${\mathbf{p}}_{\mathbf{i}}=\left(1-{\lambda}_i\right){\mathbf{p}}_{\mathbf{i}+\mathbf{1}}+{\lambda}_i{\mathbf{p}}_{\mathbf{i}}$$

(30)

Where:

$${\lambda}_i=\frac{\left|{\mathbf{p}}_{\mathbf{i}+\mathbf{1}}-{\mathbf{p}}_{\mathbf{i}}\right|}{\left|\mathbf{t}-{\mathbf{p}}_{\mathbf{i}}\right|}$$

(31)

A graphical representation of a full iteration with 4 joints is illustrated in Fig. 19.

Fig. 19

An example of a full iteration of FABRIK. (a-c) Backward iteration (d-f) Forward iteration [29]

As illustrated in Fig. 20, the iteration strategy can also be applied to the continuum robot, and the configuration parameters of the continuum robot can be further obtained.

$$\theta =2\operatorname{arccos}\left(\frac{{\overrightarrow{\mathtt{Z}}}_{\mathtt{0}}\cdot {\overrightarrow{\mathtt{H}}}_{\mathtt{0}}}{\left\Vert {\overrightarrow{\mathtt{Z}}}_{\mathtt{0}}\right\Vert \cdot \left\Vert {\overrightarrow{\mathtt{H}}}_{\mathtt{0}}\right\Vert}\right)$$

(32)

$$\alpha =\arctan 2\left({\mathtt{X}}^{\prime },{\mathtt{Y}}^{\prime}\right)$$

(33)

Fig. 20

An example of a single iteration of the FABRIK algorithm for a two-segment continuum robot. [33]

1.3 FTL

The FTL algorithm references the movement of the snake. Given the collision-free tip trajectory, the other joints of the robot follow the end. As shown in Fig. 21, the iteration progress starts at the end of the robot. The current joint position is located on the previous posture of the robot. Then the joint position is updated from the base to the end to guarantee that the end position of the fixed base coincides.

Fig. 21

Schematic diagram of FTL algorithm for the robot with three joints. (a) Backward iteration (b) Forward iteration [39]

1.4 CCD

Cyclic coordinate descent (CCD) is an iterative numerical algorithm. From the end to the base of the robot, only one configuration parameter is changed during a single iteration. And the gradient descent is used to solve the maximum value of the cost function during a single iteration (Figs. 22 and 23).

$$\left\{\begin{array}{c}\min\ \left\Vert P-{P}_{\mathrm{target}}\right\Vert \\ {}s.t.{\alpha}_i={\alpha}_0\left(\theta ={\theta}_0\right)\\ {}\begin{array}{c}\ {\theta}_{\mathrm{low}}\le {\theta}_i\le {\theta}_{\mathrm{high}}\\ {}\ \left({\alpha}_{\mathrm{low}}\le {\alpha}_i\le {\alpha}_{\mathrm{high}}\right)\end{array}\end{array}\right.$$

(34)

Fig. 22

CCD algorithm of a single two-link chain. [21]

Fig. 23

CCD algorithm of a three-segment continuum robot