Shape Estimation of Soft Manipulators using Piecewise Continuous Pythagorean-Hodograph Curves

In recent years, there has been significant interest in use of soft and continuum manipulators in diverse fields including surgical and agricultural robotics. Consequently, researchers have designed open-loop and feedback control algorithms for such systems. Here, the knowledge of the manipulator shape is critical for effective control. The estimation of the manipulator shape is challenging due to their highly deformable and non-linear nature. Researchers have explored inductive, magnetic and optical sensing techniques to deduce the manipulator shape. However, they are intrusive and economically expensive. Alternate non-contact sensing approaches may involve use of vision or inertial measurement units (IMUs) that are placed at known intervals along the manipulator. Here, the camera provides position of the marker, while the slope (rotation matrix or direction cosines) can be determined using IMUs. In this paper, we mathematically model the manipulator shape using multiple piecewise continuous quintic Pythogorean-Hodograph (PH) curves. A PH-curve has continuous slope and is a convenient parametric model for curves with constant length. We investigate the use of multiple piecewise continuous-curvature PH curves to estimate the shape of a soft continuum manipulator. The curves model manipulator segments of constant lengths while the slopes at the knots are assumed to be known. For N curve segments with (4N + 8) unknowns, the shape estimation is formulated as a constrained optimization problem that minimizes the curve bending energy. The algorithm imposes (4N + 3) nonlinear constraints corresponding to continuity, slope and segment length. Unlike traditional cubic splines, the optimization problem is nonlinear and sensitive to initial guesses and has potential to provide incorrect estimates. We investigate the robustness of the algorithm by adding variation to the direction cosines, and compare the output shapes. The simulation results on a five-segment manipulator illustrate the robustness of the algorithm. While the experimental results on a soft tensegrity-spine manipulator validate the effectiveness of the approach. Here estimation error of the end-effector position normalized to the manipulator length are 6.53% and 6.2% for the two experimental poses.