Inverse Kinematics with a Geometrical Approximation for Multi-Segment Flexible Curvilinear Robots

Abstract

Despite research related to flexible or continuum curvilinear robots, there lacks a common simulation tool for continuum robots, which are unlike rigid robots. Thus, in this paper, a robotics toolbox is utilized to model a wire-driven flexible manipulator as one of the continuum robots. Constant curvature property can enable the robotics toolbox to represent the flexible manipulator and validate its kinematics. Moreover, because the closed-form inverse kinematics methods developed previously for real-time control conceded limitations in modeling some continuum robots, we hereby develop an inverse kinematics method for the wire-driven flexible manipulator which can provide fast and reliable inverse results. Experimental results showed that geometrical information offered a stable starting point for the proposed inverse kinematics algorithm. Moreover, the first and second derivatives of a fitness function further contributed to a fast-converging solution within a few microseconds. Lastly, for the potential feasibility of an active compliance controller without physical force/torque sensors, a reaction torque observer was investigated for a flexible manipulator with direct drive mechanisms.

1. Introduction

To improve efficiency and curvilinear accessibility in medical, service, and industry fields, new curvilinear robotic technologies called flexible or continuum robots have emerged. Unlike rigid robots, continuum robots [1] have inherent compliance, curvilinear accessibility, are relatively lightweight, and have high dexterity, which can be suitable for unstructured or confined environments such as the human body [2,3]. Moreover, these continuum robots are typically operated by shape memory alloy (SMA) [4], electroactive polymer (EAP) [5], pneumatic artificial muscle (PAM) [6], piezoelectric ceramic (PZT) [7], electric motors with wires or tendon transmissions [8], combinations of concentric tubes [9], etc. Then, kinematics and dynamic modeling for these kinds of new hardware designs and actuation methods are developed for operation in various applications.

Historically, the work [10] suggested a modal approach for hyper-redundant robots. Moreover, an Air-OCTOR robot inspired by an element trunk suggested the conventional Denavit–Hartenberg (D-H) method for kinematic analysis of the continuum robot [11]. Closed-form inverse kinematics (IK) provided a geometrical analysis with constant curvature property [12].

On the other hand, numerical optimization techniques are generally applied to solve inverse kinematics for the continuum robot [13,14,15,16,17,18]. As an example, Iqbal et al. [13] suggested the use of interval analysis to deal with uncertainties in the IK. Recently, to deal with nonlinearities of a continuum arm’s hardware, machine learning methods were also applied to learn the IK of a tendon-driven manipulator [19]. IK studies for continuum robots are listed in

Table 1. Inverse kinematics analysis for continuum robots.

Literature	IK Method		Note
	Classification	Computation Time
A. Jones et al. [11]	Analytic	N/A	Only for manipulator curvature to cable lengths
S. Iqbal et al. [13]	Numeric	Average 0.072 s	Interval analysis was used
S. Neppalli et al. [12]	Analytic	N/A	---
I. S. Godage et al. [15,16]	Numeric	The order of tens of milliseconds	---
Z. Zhang et al. [17]	Numeric	The order of tens of milliseconds	---
J. Chen and H. Y. K. Lau [19]	Data-driven	N/A	Restricted by a used hardware set-up

and well-organized summaries of the kinematics of the continuum robot were reported in [6,20].

This paper describes a new mathematical formulation for a wire-driven flexible manipulator (WDM) as one of the continuum robots for forward and especially inverse kinematics, as well as utilization of the MATLAB robotics toolbox [21]. For real-time control of the continuum robot, fast and accurate solution of the IK is generally required. Even though the previous closed-form IK method [12] is quite suitable for real-time control, it could not be applied to some types of continuum robots, as will be explained later, since the solution may be unreliable near singularities [21]. Moreover, some literature reported that the order of tens of milliseconds, as shown in Table 1, was required to solve an IK problem by iterative numerical methods [13,15,17]. Because the computation time might be an average value and it might be affected by the complexity of the continuum robot’s design, such as multi-section, the worst-case might jeopardize control performance. In this paper, to reduce the time required to solve the IK problem, a geometrical approximation is used for an initial guess of numerical optimization. The closed-form IK method did not provide an accurate solution but gave an approximate bending angle value in the WDM. Thus, this proper starting point enables the reduction of convergent time and even avoids local optima. The first and second derivatives of a fitness function are also derived for fast and accurate computation. As a result, the convergent rate of the proposed IK algorithm is approximately 100 times faster than the previous ones, as will be shown in the experiment section.

Another contribution of this paper is to suggest the utilization of the robotics toolbox [21] as a common tool for continuum robots’ kinematic analysis. Although there is a lot of work on continuum robots, there is no common continuum robot analysis tool to compare with rigid robots, which may make it difficult for beginners to enter this field. Thanks to the constant curvature property, the kinematic behavior of a continuum robot can be easily visualized by the robotics toolbox, which can help to understand it deeply, save time to simulate it, and validate derived kinematic equations.

Furthermore, so-called intrinsic force sensing [22] and active compliance controls such as force control [23] and impedance control [24] have been recently studied in continuum robots. As a preliminary step for active compliance controls, a reaction torque observer (ROB) at a proximal side of the WDM with direct drive [25] is briefly introduced without physical force/torque sensors. Its potential feasibility for compliance control is also investigated.

The remaining parts of the paper are organized as follows: Section 2 briefly reviews a wire-driven flexible manipulator—D-H parameters of the WDM are derived and the MATLAB robotics toolbox [21] is used to represent the WDM; Section 3 analyzes the closed-form IK method and proposes an IK algorithm which gives fast and accurate results; Section 4 introduces the ROB for the WDM; Section 5 shows the results of the proposed IK algorithm, comparing with the closed IK method and responses of the ROB in the WDM; and Section 6 concludes the paper.

2. WDM’s Inverse Kinematics with the Robotics Toolbox

2.1. Review of the WDM

Figure 1 shows a wire-driven serpentine manipulator, a type of WDM inspired by a snake and an octopus’ arm [26]. The mechanism design of this system can be referred to in [25]. In this section, the kinematics of the WDM are not only briefly reviewed but also revised to utilize the robotics toolbox [21].

At first, the kinematics of the WDM can be defined as two stages [8]: from actuator space to configuration space $\left(f_1\right)$, and from configuration space to task space $\left(f_2\right)$. Although the flexible WDM can be considered as the continuum robot with its constant curvature property, it can be modeled by joint and link compositions for simplicity of the kinematic analysis. The definition of design parameters and the illustration of the configuration (individual bending angle $\theta$ and rotation angle $\Phi$) can be referred to in Figure 2 and Figure 3. By referring to [26] in Figure 2, each wire length $L_i,i=1,2,3,4$ can be obtained as

$$\begin{array}{l}\begin{array}{c}{L}_1=L_0-2N\left(a\sin \frac{\theta }{2}+h_0{\sin }^2\frac{\theta }{4}\right)\\ L_2=L_0-2N\left(b\sin \frac{\theta }{2}+h_0{\sin }^2\frac{\theta }{4}\right)\end{array}\\ \begin{array}{c}{L}_3=L_0+2N\left(a\sin \frac{\theta }{2}-h_0{\sin }^2\frac{\theta }{4}\right)\\ L_4=L_0+2N\left(b\sin \frac{\theta }{2}-h_0{\sin }^2\frac{\theta }{4}\right)\end{array}\end{array}\}$$

(1)

where $\mathrm{a}=\frac{d}{2}\cos \Phi$, $\mathrm{b}=\frac{d}{2}\sin \Phi$, $L_0$ is the initial wire lengths at rest state, $N$ is the number of joints of the manipulator and $h_0$ is the spacing of the two adjacent links at rest state.

Now, based on the above equations, $f_1$’s kinematic relationships can be derived as follows:

Rotation angle $\Phi$

$$\Phi =atan2\left(\Delta L_2,\Delta L_1\right)$$

(2)

Bending angle $\Theta$

$$\Theta =\left(N+1\right)\theta =2\left(N+1\right){sin}^{-1}\left(\frac{\sqrt{\Delta {L_1}^2+\Delta {L_2}^2}}{2Nd}\right)$$

(3)

where $\Delta L_1=L_3-L_1$ and $\Delta L_2=L_4-L_2$.

Then, the position of the WDM can be easily calculated from Figure 4. Suppose that an origin $o_0$ is placed on the first joint and an origin $o_{N+1}$ is located on the center of the WDM’s tip. The 3D tip position can be easily calculated as

$$\begin{array}{c}{X}_{N+1}=\left\{l\left({\displaystyle \sum _{i=1}^{N+1}}\sin \left(i\theta \right)\right)\right\}\cos \Phi \\ Y_{N+1}=\left\{l\left({\displaystyle \sum _{i=1}^{N+1}}\sin \left(i\theta \right)\right)\right\}\sin \Phi \\ Z_{N+1}=l\left({\displaystyle \sum _{i=1}^{N+1}}\cos \left(i\theta \right)\right)\end{array}\}$$

(4)

where $l$ is the link length of the rigid manipulator. Applying the Dirichlet kernel and the Lagrange’s trigonometric identities, (4) can be rewritten as

$$\begin{array}{c}{X}_{N+1}=\left[l\left\{\frac{1}{2}\cot \frac{\theta }{2}-\frac{\cos \left(\left(N+\frac{3}{2}\right)\theta \right)}{2\sin \frac{\theta }{2}}\right\}\right]\cos \Phi \\ Y_{N+1}=\left[l\left\{\frac{1}{2}\cot \frac{\theta }{2}-\frac{\cos \left(\left(N+\frac{3}{2}\right)\theta \right)}{2\sin \frac{\theta }{2}}\right\}\right]\sin \Phi \\ Z_{N+1}=\frac{l}{2}\left(-1+\frac{\sin \left(\left(N+\frac{3}{2}\right)\theta \right)}{\sin \frac{\theta }{2}}\right)\end{array}\}.$$

(5)

2.2. Denavit–Hartenberg Parameters of the WDM

Based on this assumption and the constant curvature in the bending phase, the WDM can be easily represented by the MATLAB robotics toolbox, a well-known tool for analyzing robots, particularly rigid robots. Figure 3 illustrates the shape of the WDM in the bending phase on the x–z plane and D–H coordinates for the WDM can be set, as shown in Figure 4. In addition, the entire Denavit–Hartenberg (D–H) parameters are listed in

Table 2. D-H parameters for the WDM.

Link	${\mathit{a}}_{\mathit{i}}$	${\mathit{\alpha }}_{\mathit{i}}$	${\mathit{d}}_{\mathit{i}}$	${\mathit{\theta }}_{\mathit{i}}$
1 ($\Phi$)	0	$-\frac{\pi }{2}$	0	${\theta }_1$
2 (Node: 1)	l	0	0	${\theta }_2-\frac{\pi }{2}$
3 (Node: 2)	l	0	0	${\theta }_3$
$⋮$	$⋮$	$⋮$	$⋮$	$⋮$
N + 1 (Node: N)	l	0	0	${\theta }_{N+1}$
N + 2 (End node)	l	0	0	${\theta }_{N+2}$

. Note that the curved shape of the WDM between two nodes can be approximately represented by a chord equivalent to a link $\left(l=H+h_0\right)$ [26] of a rigid manipulator, as shown in Figure 3. Further note that a parameter ${\theta }_1$ is assigned for the rotation ($\Phi$) of the WDM, and an offset $-\frac{\pi }{2}$ is necessary for the straight shape of the WDM (zero rotation ($\Phi$) and bending ($\Theta$) angles), as shown in Figure 4.

2.3. Robotics Toolbox for the WDM

With the D-H parameters in Table 2, the WDM can be modeled by the robotics toolbox. Because the WDM has the same parameters from link 2 to link N + 2, D-H parameters can be easily assigned by a link command of the robotics toolbox and a for–loop command even if there are many nodes in the WDM. Note that all ${\theta }_i$ parameters in single section arm have the same value except for ${\theta }_1$ because of the constant curvature property. Further note that an offset angle $-\frac{\pi }{2}$ must be applied to the link 2 object using an offset member function of the link class in the robotics toolbox. Finally, using the plot member function, the shape of the WDM can be easily visualized. Figure 5 shows different configurations of both one section and two section WDM.

Utilization of the robotics toolbox has two main advantages. The robotics toolbox helps beginners visualize their continuum robot’s configurations and the derived forward kinematic equations can be validated by the forward kinematic member function of the robotics toolbox as a common validation tool. Furthermore, the suggested approach can be easily extended to multi-section robots with proper D–H coordinates.

3. Inverse Kinematics with a Geometrical Approximation for the WDM

A fast and reliable IK is very important for real-time control of the WDM with the task space pose command. Even though the closed-form IK [12] for continuum robots was proposed, this method could not be applied to every type of continuum robot because of violation of assumptions. In other words, the closed-form IK method was based on two assumptions: the constant curvature, and the center of the circular arc, which must be located on the x-axis of the base reference after the bending plane is rotated to coincide with the x–z plane of the base reference. However, these assumptions are not guaranteed in all cases. To explain these, we chose a regular octagon with a circumcircle, as properties of a regular polygon can help analyze a circular arc’s geometry.

Figure 6a illustrates a WDM with N = 2, showing that the assumption of the center of the circular arc is valid [12]. However, the constant curvature property is not guaranteed because the first bending angle $\left(\theta \right)$ is not the same as the second $\left(2\theta \right)$. On the contrary, if the assumption of the constant curvature is ensured, as shown in Figure 6b, then the other becomes invalid.

From the above analysis, we found that the previous closed-form IK method could not be applied to derive an accurate IK solution from the WDM. Nevertheless, this geometrical information can be used to obtain fast and accurate IK if it is combined with a numerical optimization method because in the numerical analysis, a proper initial guess is generally very important to obtain an accurate solution with fast convergence. Interestingly, the geometrical analysis in Figure 6b can be used to provide a proper initial guess of the bending angle, $\Theta$, as shown in Figure 6, which does not provide a true $\Theta$, but a ${\Theta }_p$. In Figure 7, R_p, C_p, and ${\Theta }_p$ represent a pseudo radius, a pseudo center, and a pseudo $\Theta$, respectively. Nevertheless, because the range of the bending angle $\Theta$ is generally limited by mechanical constraints and a pseudo ${\Theta }_p$ is near a true bending angle, the ${\Theta }_p$ can be used as a good initial point to find true $\Theta$ in the numerical optimization. Further note that the smaller the bending angle, the more the pseudo ${\Theta }_p$ is converged to the true $\Theta$.

In the numerical optimization, to find the accurate bending angle $\Theta$ and the corresponding $\theta$ with respect to the desired end position (x, y, z), a cost function, which quantifies the tip position error projected on the bending plane, is defined as

$$f\left(\theta \right)=\frac{1}{2}\left(e_x^2+e_z^2\right)$$

(6)

where $e_x=x^{\prime }-X_{est}$ and $e_z=z^{\prime }-Z_{est}$. $x^{\prime }$ and $z^{\prime }$ are the projected desired position on the x-axis and z-axis, respectively. $X_{est}=\sqrt{{X_{N+1}}^2+{Y_{N+1}}^2}$ and $Z_{est}=Z_{N+1}$ are the estimated tip position computed by forward kinematics derived in (5). The optimal bending angle can be found by solving the following constrained optimization problem:

$$\underset{\theta }{\min }f\left(\theta \right)$$

(7)

$$s.t.{\theta }_L\le \theta \le {\theta }_U$$

where $\theta$ is constrained by the inherent design of the manipulator, with ${\theta }_L=0$ as the lower bound and ${\theta }_U$ as the biggest individual bending angle allowed and ${\theta }_U=2atan2\left(h_0,D\right)$.

Moreover, a solution was computed by sequential quadratic programming, which is often used to solve nonlinear optimization problems.

Finally, the displacement of each wire pair of the WDM can be derived as follows

$$\begin{array}{c}\Delta L1=L_3-L_1=2Nd\sin \left(\frac{\theta }{2}\right)\cos \varphi \\ \Delta L2=L_4-L_2=2Nd\sin \left(\frac{\theta }{2}\right)\sin \varphi \end{array}\}$$

(8)

4. Reaction Torque Observer for the WDM

Although there have been a few studies on active compliance control recently, physical force/torque sensors are generally used to perform these controls. In this section, a ROB in the WDM’s actuation space is briefly introduced to estimate external torque at the load side of the WDM’s direct drive. The motion equation of the direct drive can be described as [25]:

$$T_e=J\frac{d\omega }{dt}+f_{Fric}(·)\omega +T_{ext}$$

(9)

where $T_e$ is the actuator torque, which is composed of the controller torque $T_C$ and the pretension torque $T_P$, $J$ is the equivalent inertia of the direct drive, $\omega$ represents the angular velocity $\dot{q}$, $f_{Fric}(·)$ is the friction torque of the direct drive, and $T_{ext}$. is the external torque. Then, using the disturbance observer technique [27], the external torque $T_{ext}$ can be estimated as

$${\widehat{T}}_{ext}=LPF\left(T_C+T_P+g\widehat{J}\dot{q}\right)-g\widehat{J}\dot{q}$$

(10)

where $\mathrm{LPF}(·)=\frac{g}{s+g}$ is a low pass filter, $g$ is the coefficient of the low pass filter, $\widehat{J}$ is the estimated inertia of the direct drive and $T_e=T_c+T_P$. Note that the friction term in (9) was compensated by a friction compensator [25].

5. Experiments

To validate the proposed IK method, simulation and actual tests were performed by prepared trajectories in task space; these paths are equivalent to the rotation angle $\Phi$ with a constant speed after the change of the bending angle $\Theta$ from zero to $\frac{\pi }{4}$. These tests were implemented on the WDM, which is composed of a wire-driven flexible arm, the direct drive mechanisms, and the embedded control systems (developed in [25]). The WDM’s mechanical parameters are listed as follows: $H=0.005$ m, $h_0=0.001$ m, and $N=14$. Moreover, the dynamics of the direct drive were estimated by system identification experiments [25].

At first, to see the errors of the closed-form IK method in the WDM and verify that this geometrical method can be a good initial point for the optimization, simulations using MATLAB were performed and the results are shown in Figure 8a. As mentioned in section 3, an error $\left(=truevalue-estimatedvalue\right)$ is increased as the bending angle $\Theta$ increases. Nevertheless, because errors have small ranges and the range of $\Theta$ is generally limited by the mechanical constraint, these estimated ${\Theta }_p$ can be utilized as a starting point for the numerical optimization. Thus, using MATLAB’s fmincon optimization function, the proposed IK method was simulated, and accurate results were obtained, as shown in Figure 8b. Moreover, Figure 9 shows a comparison of the closed-form and the proposed IK methods in the task space motion control and the kinematic effect may become more critical in multi-section robots.

Since a fast solution to inverse kinematics is even more critical for real-time control of multi-section WDM, we extended the proposed method to a two-section WDM. The two sections are identical and have the same mechanical parameters as the ones used in the previous section. Configuration space parameters for section one and section two are $\left[{\Theta }_1,{\Phi }_1\right]$ and $\left[{\Theta }_2,{\Phi }_2\right]$ respectively. In the simulation, we consider a specific scenario:${\Theta }_1={\Theta }_2$; in other words, the two sections bend in the same plane. Though this configuration is one of many, it demonstrates the superiority of the proposed algorithm. Simulation results for the absolute error of ${\Theta }_1$ and ${\Theta }_2$ are shown in Figure 10a,b, respectively. We can see that the solutions are quite accurate, with absolute errors within 6 × 10⁻³ mm, though the errors at the boundary of ${\Theta }_1$ and ${\Theta }_2$ are slightly bigger. Compared to the results of a one-section WDM, the errors for $\Theta$ are larger, which is consistent with our expectation.

Secondly, the proposed IK method was implemented on a real WDM testing bed previously designed by us [25] and by C++ in Windows (CPU: i5-4690 3.5 GHz) to compare its convergent rate with other numerical IK methods. For these tests, optimization functions of a dlib library [28] were used. Figure 11 shows the computation times and errors of the proposed IK approach. Although it is not easy to compare with the previous approaches [13,14,15,16,17,18] due to its different kinematic model, the proposed IK method provides accurate and fast (almost 100 times faster) solutions [13,15,17]. Simulation and actual tests of the proposed IK method in the WDM can be found in the supplementary video.

Finally, the designed ROB was applied to the WDM with the same trajectory to investigate the feasibility of the use of the ROB in the WDM. Figure 12 shows the results of the estimated external torques of the four motors of the direct drive system when ROB is applied. Note that motor 1 and 3 and motor 2 and 4 are coupled to each other because of their antagonistic actions. Therefore, when 40 mNm pretensions were applied to prevent a slack problem, ROB outputs for motor 1 and 4 had −40 mNm in the initial equilibrium status. The outputs of the ROB were changed based on the WDM motions and estimated external torques were uniformly repeated in periodic rotation motions of the WDM with constant $\Theta$ and $\Phi \in \left[-\pi ,\pi \right]$ despite high nonlinearities of the flexible arm part. Note that deviated ROB paths, especially in Figure 12a,d, were originated from the WDM’s bending motion (from zero to $\frac{\pi }{4}$). Small variations of the ROB in the periodic trajectory show that the ROB data can be used to train machine learning algorithms for identifying noncontact/contact status without physical sensors. It can be extended to sensor-less active compliance controls [29], which will be studied in the future.

6. Conclusions

In this paper, the robotic toolbox was introduced to model the WDM. The proposed kinematic analysis and D–H coordinates could enable the visualization of the WDM by the robotics toolbox. Moreover, for real-time control of the WDM, the IK method was proposed. With geometrical approximation and derivatives of the fitness function, the accurate and fast IK algorithm was implemented by C++. Thanks to the very fast convergent rate, the proposed method can even be applied to WDM with multiple bending sections.

In the future, the proposed IK method will be applied to a multi-section WDM and tip position sensing will be utilized to compensate for inaccuracy brought about by improper design and prototyping of the flexible arm will improve position accuracy. Moreover, through the proposed ROB and the flexible arm design, learning algorithms for identifying contact status and sensor-less active compliance controls will be studied.