# Numerical Shape Planning Algorithm for Hyper-Redundant Robots Based on Discrete Bézier Curve Fitting

^{*}

*Machines*—Feature Papers in Mechatronic and Intelligent Machines)

## Abstract

**:**

## 1. Introduction

## 2. Conceptual Description of Shape Planning Algorithm

_{i}), detailed in Chapter 3, which uses only universal joints to perform the motion. For a certain known architecture of a robot and a shape guidance curve, the problem of finding joint displacements which ensures the correspondence between robot and the imposed shape is called the shape inverse problem [38]. The solution existence for inverse shape problem is demonstrated and detailed in [38]. For a Python robot configuration, the existence of the solution for the inverse shape problem is conditioned only by the curvature value k of the shape guidance curve, which is limited by:

_{i}for each element as presented in Figure 1.

_{1}and P

_{n}(Figure 1) which defines the position of the first and final joint in the structure. In addition, the position of these points ${P}_{1}\left({x}_{1},{y}_{1},{z}_{1}\right)$ and ${P}_{n}\left({x}_{n},{y}_{n},{z}_{n}\right)$ are calculated from the input data.

_{1}and P

_{n}of the robot that describe the position of the first and last joints in the kinematic chain. The other two points ${B}_{1}$ and ${B}_{2}$, are placed on two support lines that pass through B

_{0}and B

_{1}and are colinear with the z axis of the ${O}_{0}{x}_{0}{y}_{0}{z}_{0}$ and ${O}_{n}{x}_{n}{y}_{n}{z}_{n}$ coordinate systems. By varying the position of B

_{1}and B

_{2}the length of the Bézier curve B(t) can be modified. The initial values for the two points are inherited from the solution found in the previous run of the S-GUIDE method. When the method is running for the first time, the position along the two-guidance lines for ${B}_{1}$ and ${B}_{2}$ are ${l}_{t}/2$ and $-{l}_{t}/2$, respectively.

_{R}(t) (Figure 3b). The ERM is further used, in S-GUIDE step 3, to calculate the SIP.

_{R}(t) is iteratively obtained by adjusting the candidate Bézier curve B(t). At each step in the iteration the ERM is reconstructed from P

_{1}to P

_{i−}

_{1}. The length d

_{n−1}of the final element in the model is obtained by calculating the distance between the points P

_{i−}

_{1}” (point belonging to the equivalent robot model) and point P

_{n}calculated in step 1 from the position of end-effector. The obtained distance d

_{n−}

_{1}is compared with the length of the n−1 element ${l}_{n-1}$, resulting in the error ${e}_{2}$ (Equation (7)).

_{R}(t) is recalculated, and the iterative process on A2 starts from the beginning. The iteration loop ends when the condition $\left|{e}_{2}\right|<{e}_{re}$ is fulfilled.

- -
- The position of point P
_{j}′ {j = 2…n−1} belonging to the backbone curve B_{R}(t) is determine by finding a segment (candidate element) on the B_{R}(t) curve that has the length ≈ l_{j−}_{1}. The length of the candidate element is measured between the point P_{i}′ ϵ B_{R}(t) and the point P_{j−}_{1}″ ϵ ERM that defines the end of the previous element. - -
- For each obtained P
_{j}′ the spherical coordinates are measured in respect to a coordinate system placed in P_{j−}_{1}″ that has the orientation of the global coordinate system.$$\begin{array}{c}a{z}_{j}=arctan2\left(P{x}_{j}-P{x}_{j-1},P{z}_{j}-P{z}_{j-1}\right)\\ e{l}_{j}=arctan2\left(P{z}_{j}-P{z}_{j-1},P{y}_{j}-P{y}_{j-1}\right)\end{array}$$ - -
- Using the angles $a{z}_{j}$ and $e{l}_{j}$ and the length l
_{j}, the elements j−1 from the ERM is constructed by finding the position of point ${P}_{j}^{\u2033}$ ϵ ERM using Equation (9).

_{i}from the ERM model is represented in a local coordinate system attached to the i−1 element, that have the z axis colinear with the element. These values are obtained by multiplying the inverse rotation matrix ${}_{i-1}{}^{0}R$ with the position vector of point P

_{i}.

## 3. Numerical Results

#### 3.1. Performance Assessment of the S-GUIDE Method

#### 3.2. S-GUIDE Method Testing for the Python Robot

- -
- remain in a home position until a harvest command is received
- -
- determine the needed trajectory from the home position to the object to grasp
- -
- perform grasping
- -
- place the object in designated containers and return to the home position.

_{x}= 0 mm G

_{y}= 390 mm and G

_{z}= 435 mm with the orientation G

_{α}= −110 deg, G

_{β}= 0 deg and G

_{γ}= 0 deg (Figure 9b). From this position the end-effector moved to G

_{x}= 0 mm, G

_{y}= 578 mm and G

_{z}= 345 with the final orientation G

_{α}= −90 deg, G

_{β}= 0 deg, and G

_{γ}= 0 deg (Figure 9c). The transition between the start and stop points was implemented using a spline with three intermediary points, the orientation G

_{α}was changed linearly between the initial value to the final value.

_{12}(reference value) and the position resulting at the robot’s 12th universal joint (the associated point P

_{12}on the robot) are presented in Figure 11a. The obtained absolute errors between the two parameters along the Ox, Oy and Oz axis are displayed in Figure 11b.

_{x}

_{12}position, 0.051 mm for P

_{y}

_{12}position and 0.0151 mm for P

_{z}

_{12}position. These values were in the same range as the imposed e

_{re}, and were characterized by slight random fluctuation around e

_{re}.

#### 3.3. Discussions and Results Comparison

- -
- a comparative study between different algorithms for solving the IKP problem for a 10-DoF structure was presented in [36]. The authors used an exhaustive method and error optimization algorithms for this purpose. It was observed that the exhaustive methods gave good results on positioning errors, but the processing time was fairly high and not applicable for real-time applications (ex.:18 s for a 4 DoF structure). Using error optimization algorithms (Patternsearch, Genetic algorithms, Multistart and Simulannelbnd), the computation time for a 10-DoF robot varied from 0.5 s to 14 s with errors that ranged from 4 to 10 mm. Using S-GUIDE for a 24-DoF robot, the computational time was 0.001 s with an average positioning error of 0.0537 mm.
- -
- Another method proposed for calculating the inverse kinematics of HRRs called PASO is based on a particle swarm optimization algorithm and was presented in [35]. Using this method for a 30-DoF robot (ReMod3D) a processing time of 1.57 s with an average positioning error of 0.46 mm was obtained. Using S-GUIDE for a robot with 30 DoF the processing time was 0.001731 s with a similar positioning precision.
- -
- A good performance in relation to the computational time was obtained using the natural CCD algorithm presented in [39]. For a 20-DoF robot (integrates joints with 2 DoF) the processing time was around 0.05 s with a precision of 0.1. Using S-GUIDE for a 24-DoF robot the computational time was 0.001 s with a precision of 0.006%.

## 4. Conclusions

## Author Contributions

## Funding

## Acknowledgments

## Conflicts of Interest

## References

- Chirikjian, G.S.; Joel, W.B. An obstacle avoidance algorithm for hyper-redundant manipulators. In Proceedings of the IEEE International Conference on Robotics and Automation, Cincinnati, OH, USA, 13–18 May 1990; pp. 625–631. [Google Scholar] [CrossRef] [Green Version]
- Chirikjian, G.S. Theory and Applications of Hyperredundant Robotic Mechanisms. Ph.D. Thesis, Department of Applied Mechanics, California Institute of Technology, Pasadena, CA, USA, 22 May 1992. Available online: https://thesis.library.caltech.edu/4458/1/Chirikjian_gs_1992.pdf (accessed on 21 July 2022).
- Rad, C.; Hancu, O.; Lapusan, C. Aspects regarding “soft” grasping in smart agricultural harvesting tasks. Acta Tech. Napoc. Ser. Appl. Math. Mech. Eng.
**2020**, 63, 389–394. [Google Scholar] - Martín-Barrio, A. Design, Modelling, Control and Teleoperation of Hyper-Redundant Robots. Ph.D. Thesis, Universidad Politécnica de Madrid, Madrid, Spain, 2 November 2020. Available online: https://oa.upm.es/65161/1/ANDRES_MARTIN_BARRIO.pdf (accessed on 21 July 2022).
- Singh, I. Curve Based Approach for Shape Reconstruction of Continuum Manipulators. Ph.D. Thesis, Universite de Lille, Lille, France, 2018. Available online: https://hal.archives-ouvertes.fr/tel-01967054/document (accessed on 21 July 2022).
- Liu, J.; Tong, Y.; Liu, J. Review of snake robots in constrained environments. Robot. Auton. Syst.
**2021**, 101, 103785. [Google Scholar] [CrossRef] - Lapusan, C.; Hancu, O.; Rad, C. Shape Sensing of Hyper-Redundant Robots Using an AHRS IMU Sensor Network. Sensors
**2022**, 22, 373. [Google Scholar] [CrossRef] [PubMed] - Walker, I.D. Continuous backbone “continuum” robot manipulators. ISRN Robot.
**2013**, 2013, 726506. [Google Scholar] [CrossRef] [Green Version] - Kolachalama, S.; Lakshmanan, S. Continuum robots for manipulation applications: A survey. J. Robot.
**2020**, 2020, 4187048. [Google Scholar] [CrossRef] - Rad, C.; Hancu, O.; Lapusan, C. Data-Driven Kinematic Model of PneuNets Bending Actuators for Soft Grasping Tasks. Actuators
**2022**, 11, 58. [Google Scholar] [CrossRef] - Yeshmukhametov, A.; Koganezawa, K.; Yamamoto, Y.; Buribayev, Z.; Mukhtar, Z.; Amirgaliyev, Y. Development of Continuum Robot Arm and Gripper for Harvesting Cherry Tomatoes. Appl. Sci.
**2022**, 12, 6922. [Google Scholar] [CrossRef] - Canali, C.; Pistone, A.; Ludovico, D.; Guardiani, P.; Gagliardi, R.; De Mari Casareto Dal Verme, L.; Sofia, G.; Caldwell, D.G. Design of a Novel Long-Reach Cable-Driven Hyper-Redundant Snake-like Manipulator for Inspection and Maintenance. Appl. Sci.
**2022**, 12, 3348. [Google Scholar] [CrossRef] - Tang, J.; Zhang, Y.; Huang, F.; Li, J.; Chen, Z.; Song, W.; Zhu, S.; Gu, J. Design and Kinematic Control of the Cable-Driven Hyper-Redundant Manipulator for Potential Underwater Applications. Appl. Sci.
**2019**, 9, 1142. [Google Scholar] [CrossRef] [Green Version] - Lapusan, C.; Rad, C.; Hancu, O. Kinematic analysis of a hyper-redundant robot with application in vertical farming. IOP Conf. Ser. Mater. Sci. Eng.
**2021**, 1190, 012014. [Google Scholar] [CrossRef] - Martin, A.; Terrile, S.; Barrientos, A.; del Cerro, J. Hyper-redundant robots: Classification, state-of-the-art and issues. Rev. Iberoam. De Automática E Inf. Ind.
**2018**, 15, 351–362. [Google Scholar] [CrossRef] - Lee, C.; An, D. AI-Based Posture Control Algorithm for a 7-DOF Robot Manipulator. Machines
**2022**, 10, 651. [Google Scholar] [CrossRef] - Lapusan, C.; Hancu, O.; Rad, C. Quaternion-Based Approach for Solving the Direct Kinematics of a Modular Hyper Redundant Robot. Acta Tech. Napoc. Ser. Appl. Math. Mech. Eng.
**2020**, 63, 363–366. [Google Scholar] - Zhao, Y.; Jin, L.; Zhang, P.; Li, J. Inverse Displacement Analysis of a Hyper-redundant Elephant’s Trunk Robot. J. Bionic Eng.
**2018**, 15, 397–407. [Google Scholar] [CrossRef] - Chibani, A.; Mahfoudi, C.; Chettibi, T.; Merzouk, R.; Zaatri, A. Generating optimal reference kinematic configurations for hyper-redundant parallel robots. Proc. Inst. Mech. Eng. Part I J. Syst. Control Eng.
**2015**, 229, 867–882. [Google Scholar] [CrossRef] - Bieze, T.M. Contribution to the Kinematic Modeling and Control of Soft Manipulators Using Computational Mechanics. Ph.D. Thesis, Universite de Lille, Lille, France, 24 October 2017. Available online: https://hal.archives-ouvertes.fr/tel-03516545/document (accessed on 21 July 2022).
- Qi, P.; Liu, C.; Zhang, L.; Wang, S.; Lam, H.-K.; Althoefer, K. Fuzzy logic control of a continuum manipulator for surgical applications. In Proceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO), Bali, Indonesia, 5–10 December 2014; pp. 413–418. [Google Scholar] [CrossRef]
- Runge, G.; Peters, J.; Raatz, A. Design optimization of soft pneumatic actuators using genetic algorithms. In Proceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO), Macau, Macao, 26 March 2018; pp. 393–400. [Google Scholar] [CrossRef]
- Melingui, A.; Lakhal, O.; Daachi, B.; Mbede, J.B.; Merzouki, R. Adaptive Neural Network Control of a Compact Bionic Handling Arm. IEEE/ASME Trans. Mechatron.
**2015**, 20, 2862–2875. [Google Scholar] [CrossRef] - Hannan, M.W.; Walker, I.D. Novel kinematics for continuum robots. In Advances in Robot Kinematics; Lenarčič, J., Stanišić, M.M., Eds.; Springer: Dordrecht, Netherlands, 2000; pp. 227–238. [Google Scholar]
- Chirikjian, G.S. Conformational Modeling of Continuum Structures in Robotics and Structural Biology: A Review. Adv. Robot.
**2015**, 29, 817–829. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Trivedi, D.; Lotfi, A.; Rahn, C.D. Geometrically Exact Models for Soft Robotic Manipulators. IEEE Trans. Robot.
**2008**, 24, 773–780. [Google Scholar] [CrossRef] - Trivedi, D.; Rahn, C.D.; Kier, W.M.; Walker, I.D. Soft robotics: Biological inspiration, state of the art, and future research. Appl. Bionics Biomech.
**2008**, 5, 99–117. [Google Scholar] [CrossRef] - Wang, T.; Lin, B.; Chong, B.; Whitman, J.; Travers, M.; Goldman, D.I.; Blekherman, G.; Choset, H. Reconstruction of backbone curves for snake robots. IEEE Robot. Autom. Lett.
**2021**, 6, 3264–3270. [Google Scholar] [CrossRef] - Zeid, I. Mastering CAD/CAM, 1st ed.; McGraw-Hill Science/Engineering/Math: New York, NY, USA, 2004. [Google Scholar]
- Sarcar, M.M. Computer Aided Design and Manufacturing, 1st ed.; Prentice-Hall of India Pvt. Ltd: Delhi, India, 2008. [Google Scholar]
- Pérez, L.H.; Aguilar, M.C.M.; Montés Sánchez, N.; Montesinos, A.F. Path Planning Based on Parametric Curves. In Advanced Path Planning for Mobile Entities; Róka, R., Ed.; IntechOpen: London, UK, 2017; pp. 125–143. [Google Scholar]
- Chirikjian, G.S.; Burdick, J.W. A modal approach to hyper-redundant manipulator kinematics. IEEE Trans. Robot. Autom.
**1994**, 10, 343–354. [Google Scholar] [CrossRef] [Green Version] - Zanganeh, K.E.; Angeles, J. The inverse kinematics of hyper-redundant manipulators using splines. In Proceedings of the IEEE International Conference on Robotics and Automation, Nagoya, Japan, 21–27 May 1995; pp. 2797–2802. [Google Scholar] [CrossRef]
- Song, S.; Li, Z.; Yu, H.; Ren, H. Shape reconstruction for wire-driven flexible robots based on Bézier curve and electromagnetic positioning. Mechatronics
**2015**, 29, 28–35. [Google Scholar] [CrossRef] - Collins, T.; Shen, W.M. PASO: An Integrated, Scalable PSO-Based Optimization Framework for Hyper-Redundant Manipulator Path Planning and Inverse Kinematics; Technical Report No. ISI-TR-697; Information Sciences Institute, University of Southern California (USC) Viterbi School of Engineering: Los Angeles, CA, USA, 2016. [Google Scholar]
- Espinoza, M.S.; Gonçalves, J.; Leitao, P.; Sánchez, J.L.G.; Herreros, A. Inverse kinematics of a 10 DoF modular hyper-redundant robot resorting to exhaustive and error-optimization methods: A comparative study. In Proceedings of the 2012 Brazilian Robotics Symposium and Latin American Robotics Symposium, Fortaleza, Brazil, 16–19 October 2012; pp. 125–130. [Google Scholar] [CrossRef] [Green Version]
- Gravagne, I.A.; Walker, I.D. Manipulability, force, and compliance analysis for planar continuum manipulators. IEEE Trans. Robot. Autom.
**2002**, 18, 263–273. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Mochiyama, H.; Shimemura, E.; Kobayashi, H. Shape correspondence between a spatial curve and a manipulator with hyper degrees of freedom, In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Innovations in Theory, Practice and Applications, Victoria, BC, Canada, 17 October 1998; Volume 1, pp. 161–166. [Google Scholar] [CrossRef]
- Martin, A.; Barrientos, A.; Del Cerro, J. The natural-CCD algorithm, a novel method to solve the inverse kinematics of hyper-redundant and soft robots. Soft Robot.
**2018**, 5, 242–257. [Google Scholar] [CrossRef] [PubMed]

**Figure 2.**S-GUIDE method steps (

**a**) Task specification initial points (

**b**) Attached robot shape on the Bézier curve (

**c**) Robot shape based on the algorithm results.

**Figure 3.**Robot shape planning (

**a**) calculating the candidate Bézier curve (algorithm A1) (

**b**) reconstruction of the equivalent robot model (algorithm A2).

**Figure 5.**Calculation of the BC for a 3D imposed trajectory (

**a**) constant orientation for the end-effector (

**b**) variable orientation for the end-effector.

**Figure 6.**Variation of the absolute positioning error ${e}_{Gi}$ (

**a**) for constant orientation of the end−effector (

**b**) for variable orientation of the end−effector.

**Figure 9.**Python virtual model in Simscape (

**a**) backbone curve for the imposed trajectory (

**b**) initial position (

**c**) final position.

**Figure 11.**Simulation results (

**a**) variation of the end-effector position along the Ox, Oy and Oz axis (

**b**) variation of the absolute error.

**Table 1.**S-GUIDE method. Short description and symbolic graphical representation of each method steps.

I. Task Specifications | II. Robot Shape Planning | III. Shape Inverse Problem |
---|---|---|

Specifying the robot’s geometric characteristics, end-effector pose (position and orientation) and algorithm specific parameters. (See Figure 2a) | Based on the operational specifications, a candidate 3D Bézier parametric curve for modeling the shape of the robot is generated (A1). Further, the candidate curve is iteratively adjusted (BC of HRR) simultaneously with the reconstruction of an equivalent model of the robot (A2). (See Figure 2b) | For a particular robotic structure (here, a Python robot) and a planned 3D shape the joint displacements are determined (based on step 3 results). (See Figure 2c) |

Experiment | $\mathbf{MAE}\text{}(\mathbf{\Delta}\mathit{t}=0.0001)$ (mm) | $\mathbf{MAE}\text{}(\mathbf{\Delta}\mathit{t}=0.00005)$ (mm) |
---|---|---|

Constant orientation for end −effector | 0.0012 | 0.00077 |

Variable orientation for end −effector | 0.0019 | 0.00076 |

$\mathbf{Method}\text{}\mathbf{Imposed}\text{}\mathbf{Error}\text{}{\mathit{e}}_{\mathit{r}\mathit{e}}$ (mm) | Step 2 A1 Time (s) | Step 2 A2 Time (s) | Step 3.1 Time (s) | Step 3.2 Time (s) | Cycle Time (s) |
---|---|---|---|---|---|

${e}_{re}=0.5$ | 0.000384 | 0.000318 | 0.000054 | 0.000103 | 0.000859 |

${e}_{re}=0.1$ | 0.000403 | 0.000334 | 0.000055 | 0.000105 | 0.000897 |

${e}_{re}=0.05$ | 0.000391 | 0.000507 | 0.000055 | 0.000106 | 0.001059 |

${e}_{re}=0.01$ | 0.000441 | 0.0013 | 0.000060 | 0.000108 | 0.001909 |

${e}_{re}=0.005$ | 0.000490 | 0.0022 | 0.000062 | 0.000106 | 0.002858 |

${e}_{re}=0.001$ | 0.0011 | 0.0114 | 0.000061 | 0.000103 | 0.012664 |

${e}_{re}=0.0005$ | 0.0022 | 0.0251 | 0.000062 | 0.000107 | 0.027469 |

$\mathbf{Method}\text{}\mathbf{Imposed}\text{}\mathbf{Error}\text{}{\mathit{e}}_{\mathit{r}\mathit{e}}$ (mm) | Cycle Time for HRR 18 DoF (s) | Cycle Time for HRR 24 DoF (s) | Cycle Time for HRR 30 DoF (s) | Cycle Time for HRR 36 DoF (s) |
---|---|---|---|---|

${e}_{re}=0.5$ | 0.000846 | 0.000859 | 0.001414 | 0.001414 |

${e}_{re}=0.1$ | 0.000867 | 0.000897 | 0.001649 | 0.001726 |

${e}_{re}=0.05$ | 0.001037 | 0.001059 | 0.001731 | 0.002053 |

${e}_{re}=0.01$ | 0.001665 | 0.001909 | 0.004247 | 0.004725 |

${e}_{re}=0.005$ | 0.002714 | 0.002858 | 0.006968 | 0.008511 |

${e}_{re}=0.001$ | 0.01012 | 0.012664 | 0.03328 | 0.041637 |

${e}_{re}=0.0005$ | 0.02274 | 0.027469 | 0.064486 | 0.064486 |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Lapusan, C.; Hancu, O.; Rad, C.
Numerical Shape Planning Algorithm for Hyper-Redundant Robots Based on Discrete Bézier Curve Fitting. *Machines* **2022**, *10*, 894.
https://doi.org/10.3390/machines10100894

**AMA Style**

Lapusan C, Hancu O, Rad C.
Numerical Shape Planning Algorithm for Hyper-Redundant Robots Based on Discrete Bézier Curve Fitting. *Machines*. 2022; 10(10):894.
https://doi.org/10.3390/machines10100894

**Chicago/Turabian Style**

Lapusan, Ciprian, Olimpiu Hancu, and Ciprian Rad.
2022. "Numerical Shape Planning Algorithm for Hyper-Redundant Robots Based on Discrete Bézier Curve Fitting" *Machines* 10, no. 10: 894.
https://doi.org/10.3390/machines10100894