Vision-Based Shape Reconstruction of Soft Continuum Arms Using a Geometric Strain Parametrization

The shape of the SCA can be described by a position vector $\mathbf{p}(s)\in\mathbb{R}^{3}$ and a rotation frame $\mathbf{R}(s)\in SO(3)$ at each cross-section $s\in[0,L]$ along its length $L$. For convenience, the position and orientation are expressed in a compact way by joining them into a single matrix in the special Euclidean group $SE(3)$ [27]

The position and orientation of the center-points on the SCA’s cross-sections evolve, with respect to its length parameter $s$, according to

where $\boldsymbol{q}\in\mathbb{R}^{3}$ is a vector that contains the stretching/shearing strains, $\boldsymbol{\kappa}\in\mathbb{R}^{3}$ contains the bending/twisting strains, and $[\,\cdot\,]$ is the usual mapping of a vector in $\mathbb{R}^{3}$ to a skew-symmetric matrix in $\mathfrak{so}(3)$, the real vector space of $3$ by $3$ skew-symmetric matrices.

This can also be written compactly as

It is observed that knowing $\mathbf{\Omega}(s)$ and the initial pose at the base $\mathbf{X}_{0}$ is enough to reconstruct the full shape of the robot by integrating (3).

Given $\mathbf{\Omega}(s)$ and an initial base pose $\mathbf{X}_{0}$, we would like to obtain the projection of the SCA onto the camera’s image sphere. This is achieved in two steps: integrating equation (3) to obtain the shape $\mathbf{X}(s)$ in 3D space, then projecting the 3D positions to the camera’s image sphere.

Various methods could be applied to integrate (3), simplest of which is to discretize the strains $\mathbf{\Omega}(s)$ into a set of $K$ piecewise constant strains $\{\mathbf{\Omega}(s_{k})|s_{k}\in\mathcal{Z_{D}}\}$, where $\mathcal{Z_{D}}:=\{s_{1},\cdots,s_{K}\}$. After discretizing, it is possible to apply a simple first order integrator

This approach might suffer from drifting, especially if not enough discretization points are considered. More sophisticated approaches for integrating on Lie groups are the Crouch–Grossman method and Munthe–Kaas method [28]. Regardless of which method is used, the result of the integration is expressed as $\boldsymbol{\mathcal{X}}(s;\mathbf{X}_{0})$.

We consider the case where the camera is fixed in proximity to the base of the SCA. To capture the entire workspace of the SCA, a fisheye camera is used. We note here that the shape reconstruction method presented in this paper does not depend on the type of camera or model used and can be applied to any calibrated camera that is placed such that the SCA is in its field of view.

Without loss of generality, an image sphere is considered rather than an image plane since a wide-angle camera is considered [29]. The projection of a point $\mathbf{x}\in\mathbb{R}^{3}$ onto the image sphere centered at the origin is given by

Given a point on an image $[u,v]^{T}$, its corresponding image sphere projection can be obtained through the following equations

where $g(\cdot)$ is a function that depends on the distance of an image point from the image center $\rho=\sqrt{\bar{u}^{2}+\bar{v}^{2}}$,

and $\lambda=\sqrt{\bar{u}^{2}+\bar{v}^{2}+g(\bar{u},\bar{v})^{2}}$ is a normalizing scalar. The coefficients $\{\mathbf{A},\mathbf{c},a_{0},a_{2},a_{3},a_{4}\}$ are dependent on the fisheye lens that is used and can be estimated through a calibration process [29].

In this work, knowledge of the SCA’s geometry is utilized by considering the envelope of its projection onto the image (the green curves in Figure 3). The explicit equations for this envelope are dependent on the geometry of the SCA being used, thus explicit equations are not presented. However we generally denote for the right and left boundaries of the projected envelope as $\mathcal{P}_{r}(\mathbf{X}(s)),\mathcal{P}_{l}(\mathbf{X}(s))$, respectively.

Although $[\boldsymbol{\kappa}(s)]$ lives, without further assumptions, in the space of continuous function with values in $\mathfrak{so}(3)$, for a specific SCA we assume it lives in a finite dimensional function space and cannot have any arbitrary shape. In other words, the possible curvature profiles that the SCA adheres to can be expressed as a linear combination of a set of basis functions $\mathbf{\Phi}:=\{\Phi_{i}(s)\in\mathbb{R}^{3}|i=1,\cdots,N\}$

where $\mathcal{A}:=\{a_{i}\in\mathbb{R}|i=1,\cdots,N\}=\mathbb{R}^{N}$ is the set of coefficients corresponding to the basis functions $\Phi_{i}(s)$. Applying this to represent functions valued in the Lie algebra $\mathfrak{se}(3)$ of $SE(3)$ results in

Choosing specific bases, we can recover using this viewpoint some of the representations used in the literature, for example:

• •

  Constant Curvature

  $$\mathbf{\Phi}(s):=\left\{\begin{bmatrix}1\\ 0\\ 0\end{bmatrix},\begin{bmatrix}0\\ 1\\ 0\end{bmatrix},\begin{bmatrix}0\\ 0\\ 1\end{bmatrix}\right\},$$

• •

  $N$ Piecewise Constant Curvatures

  $$\mathbf{\Phi}(s):=\left\{\begin{bmatrix}h_{j}\\ 0\\ 0\end{bmatrix},\begin{bmatrix}0\\ h_{j}\\ 0\end{bmatrix},\begin{bmatrix}0\\ 0\\ h_{j}\end{bmatrix}\Big{|}j=1,\cdots,N\right\},$$

  $$h_{j}(s)=\begin{cases}1&s_{j}\leq s<s_{j+1}\\ 0&\text{otherwise}\end{cases}$$

• •

  Linear Curvature

  $$\mathbf{\Phi}(s):=\left\{\begin{bmatrix}1\\ 0\\ 0\end{bmatrix},\begin{bmatrix}0\\ 1\\ 0\end{bmatrix},\begin{bmatrix}0\\ 0\\ 1\end{bmatrix},\begin{bmatrix}s\\ 0\\ 0\end{bmatrix},\begin{bmatrix}0\\ s\\ 0\end{bmatrix},\begin{bmatrix}0\\ 0\\ s\end{bmatrix}\right\}.$$

Other basis functions may also be used, such as higher order polynomials or trigonometric functions. With this formulation, it is possible to analyze the performance of various basis functions in capturing the space of shapes that a specific SCA can perform, or perhaps learn a basis that best describes the shapes of a SCA. However this is out of the scope of this study.

To reflect this parametrization, the integral of equation (3) is written as $\boldsymbol{\mathcal{X}}(s;\mathcal{A},\mathbf{X}_{0})$. This formulation will be used in Section III-B to find the coefficients that best optimize for a specified cost function.

Given an image $\mathcal{I}$ and a basis $\mathbf{\Phi}$ that can accurately express the possible curvature profiles of a SCA, the full shape of the SCA can be reconstructed through finding the set of coefficients $\mathcal{A}$ that minimize a suitable cost function $f(\cdot)$,

From the image $\mathcal{I}$, we assume it is possible to extract image coordinates of the right and left boundary edges of the SCA’s projection, $\mathbf{y}_{r}(s_{m};\mathcal{I})$ & $\mathbf{y}_{l}(s_{m};\mathcal{I})$, for a set of sample points $\mathcal{Z_{I}}:=\{s_{1},\cdots,s_{M}\}$ on the SCA. Such task is possible using computer vision methods such as Mask RCNN [30], applying such methods is out of the scope of this study.

To quantify the fit between the estimated shape and observed shape, we take the square root error between $\mathbf{y}_{r}(s_{m};\mathcal{I})$ & $\mathbf{y}_{l}(s_{m};\mathcal{I})$ and the boundary lines $\mathcal{P}_{r}$ & $\mathcal{P}_{l}$ of the estimated shape for a given set of coefficients $\mathcal{A}$,

where $\|\cdot\|^{2}_{\mathbf{w}}$ is a weighted sum of squares with $\mathbf{w}$ being a vector of (strictly positive) weights. Equation (12) can be solved using a nonlinear least squares solver such as the Trust Region Reflective Non-linear Algorithm [31].

A series of experiments were conducted to evaluate the accuracy of the proposed shape sensing method. In these experiments, various configurations were tested by applying different pressures for the bending and the twisting actuators. The bending and twisting actuator were given $10$ pressures each ranging from $0$ to $25$ psi and $0$ to $20$ psi, respectively. From the combinations of these pressures, 100 sample configurations were obtained for half of the robot’s workspace. At each sample, an image was captured and readings from the magnetic sensor were collected. These measurements were preprocessed to find unknown parameters such as the pose of the SCA’s base and the relative transformation between the camera frame and the magnetic source frame.

The SCA’s base with respect to the camera’s frame, $\mathbf{X}_{0}$, is unknown. Here we present an approach that utilizes the same optimization framework and provides an accurate estimate of the transformation. The relative transformation between the base of the SCA and the camera was obtained by finding the pose that minimizes the error between the observed projection of the base, $\mathbf{y}_{r}(0;\mathcal{I})$ & $\mathbf{y}_{l}(0;\mathcal{I})$, and the projection of the estimated base throughout the whole set of images $\{\mathcal{I}_{i}|i=1,\dots,N\}$

Once the initial pose $\hat{\mathbf{X}}_{0}$ was found, it was fixed and used to solve (12).

The pose obtained from the magnetic sensor, $\mathbf{X}_{sen}^{mag}\in SE(3)$, is with reference to a magnetic source inside the lab. However the estimate obtained from equation (12) is with respect to the camera frame. To be able to cross validate the estimated poses with the magnetic sensor readings, we need to find the relative transformation between the camera frame and the magnetic source frame, $\mathbf{X}_{mag}^{cam}$, in order to represent the data with respect to the camera frame. Furthermore, the magnetic sensor does not align exactly with the SCA’s tip, therefore we need to obtain the relative transformation between the magnetic sensor and the SCA’s tip, $\mathbf{X}_{tip}^{sen}$.

An estimate of these relative transformations were obtained by minimizing the error between the observed projection of the tip, $\mathbf{y}_{r}(L;\mathcal{I})$ & $\mathbf{y}_{l}(L;\mathcal{I})$, and the projection of the transformed sensor readings throughout the whole set of images

The pose $\mathbf{X}^{s}_{t}$ was constrained to a subset of $SE(3)$, translations along the x and z-axis and rotation around the x-axis. We numerically verified that equation (15) always converges locally to the same values. Once the transformations were estimated, the magnetic sensor readings were transformed to the corresponding tip position with respect to the camera frame. This was then used to measure the error in the estimated tip position and direction.

The proposed method has been tested with five different basis functions for the curvature: constant, piecewise constant, linear, quadratic, and cubic functions. The weights, in equation (13), have been chosen to increase linearly with the length of the arm (i.e. more weight is given to the end tip). The mean, standard deviation, and maximum of the errors in the estimated tip position and direction are summarized in Table I. For the specific SCA being used in the experiment, the basis with minimum error is a constant piecewise function with two segments with an error of $13.0$ $mm$ and $12.2$ degrees in the position and angle, respectively. Relative to the diameter of the SCA, the estimated tip position is, on average, within approximately half of the diameter. It is worthy to note that, for the BR², increasing the order of the polynomial to the third order or more does not improve the results, as seen in the last line of Table I.

From the spatial distribution of the errors (see Figure 5) it can be observed that the region with maximum error is when the SCA is close to being fully extended. This could be due to the difficulty of getting accurate marker coordinates when the SCA is in this configuration. This region is also more sensitive to errors in the image coordinates of the detected markers, since small changes in the marker coordinates contribute to large displacements in the tip position. Another way to look at this is to observe that there is a certain configuration (outside of the robots workspace) where all the markers will get projected to the same point on the image, and thus shape reconstruction becomes difficult when this configuration is approached.

For some applications, the upper half of the work space might be more important (i.e. when the SCA is bending more). When considering only this region, an improvement in the estimation accuracy is observed. A two-segment piecewise basis achieves an accuracy of $6.1$ $mm$ and $9.9$ degrees.

One source of error includes inaccuracies in camera calibration, especially since the fisheye lenses have high distortions on the edges compared to narrow angle lenses. This could be seen when the SCA’s tip is close to the edges of the cameras view, as in Figure 3 (e). These regions have higher errors due to the lens distortions. Fine tuning the camera calibration parameters could resolve this issue. Another source of error could be in the detection of the marker positions in the image. Since the number of markers is relatively low, small errors in their image coordinates will lead to errors in the overall shape estimation. This can be resolved by considering the entire projected curve rather than only on the markers, thus improving the estimation accuracy.