Near-optimal Smooth Path Planning for Multisection Continuum Arms

TL;DR

This paper introduces a new path planning algorithm for multisection continuum arms that guarantees finding a path if one exists, outperforming inverse kinematics in success rate, especially in obstacle-rich environments.

Contribution

The paper presents a novel configuration graph-based path planning method for continuum arms, demonstrating completeness and efficiency through parallel computation.

Findings

The proposed algorithm guarantees path existence detection in continuum arms.

It outperforms inverse kinematics in success rate, especially with obstacles.

Extensive tests validate the method's completeness and efficiency.

Abstract

We study the path planning problem for continuum-arm robots, in which we are given a starting and an end point, and we need to compute a path for the tip of the continuum arm between the two points. We consider both cases where obstacles are present and where they are not. We demonstrate how to leverage the continuum arm features to introduce a new model that enables a path planning approach based on the configurations graph, for a continuum arm consisting of three sections, each consisting of three muscle actuators. The algorithm we apply to the configurations graph allows us to exploit parallelism in the computation to obtain efficient implementation. We conducted extensive tests, and the obtained results show the completeness of the proposed algorithm under the considered discretizations, in both cases where obstacles are present and where they are not. We compared our approach to the standard inverse kinematics approach. While the inverse kinematics approach is much faster when successful, our algorithm always succeeds in finding a path or reporting that no path exists, compared to a roughly 70% success rate of the inverse kinematics approach (when a path exists).

Tables

Table 1. Table I : Summary of the numerical results.

Scenario	# Cases	I. K. Rate	I. K. Time	Alg. Rate	Alg. Time
No obstacles	1000	74%	0.01	100%	75.43
Obstacles	1000	68.67%	0.45	100%	85.55

Equations

$${\sum }_k{l}_{ik}$$

$$\begin{array}{rl}{L}_i& ={\lambda }_i{\varphi }_i,\\ L_i+l_{i1}& =\left({\lambda }_i-r_i\sin {\theta }_i\right){\varphi }_i,\text{mbox}and\\ L_i+l_{i2}& =\left({\lambda }_i-r_i\sin \left(\frac{\pi }{3}+{\theta }_i\right)\right){\varphi }_i.\end{array}$$

$$\begin{array}{rl}{\theta }_i& =\arctan \left(l_{i2}\sqrt{3},2l_{i1}-l_{i2}\right),\\ {\varphi }_i& =\frac{2\sqrt{l_{i1}^2-l_{i1}{l}_{i2}+l_{i2}^2}}{r_i\sqrt{3}},\text{mbox}and\\ {\lambda }_i& =\frac{\sqrt{3}{L}_i{r}_i}{2\sqrt{l_{i1}^2-l_{i1}{l}_{i2}+l_{i2}^2}}.\end{array}$$

$${\mathbf{T}}_i$$

$$\displaystyle\qquad\qquad\mathbf{P}_{Z}\left(\sigma_{i}\right)\mathbf{R}_{Z}\left(\gamma_{i}\right)=\left[\begin{array}[]{cc}\mathbf{R}_{i}&\boldsymbol{p}_{i}\\ \boldsymbol{0}&1\end{array}\right],$$

$$g\left(l_{i1},l_{i2}\right)$$

$${\mathbf{T}}^i$$

$$\begin{array}{rl}{\mathbf{R}}^i& ={\mathbf{R}}^{i-1}{\mathbf{R}}_i,\\ {\mathbf{p}}^i& ={\mathbf{p}}^{i-1}+{\mathbf{R}}^{i-1}{\mathbf{p}}_i,\end{array}$$

Full text

We demonstrate how to leverage the continuum arm features to introduce a new model that enables a path planning approach based on the configurations graph, for a continuum arm consisting of three sections, each consisting of three muscle actuators. The algorithm we apply to the configurations graph allows us to exploit parallelism in the computation to obtain efficient implementation.

We conducted extensive tests, and the obtained results show the completeness of the proposed algorithm under the considered discretizations, in both cases where obstacles are present and where they are not. We compared our approach to the standard inverse kinematics approach. While the inverse kinematics approach is much faster when successful, our algorithm always succeeds in finding a path or reporting that no path exists, compared to a roughly 70% success rate of the inverse kinematics approach (when a path exists).

I INTRODUCTION

In this paper, we study the path planning problem for continuum-arm robots. In the path planing problem, we are given two points, a starting point and a destination/end point, and we need to compute a path for the tip of the continuum arm from the starting point to the destination point. We consider both cases where obstacles are present and where they are not.

Continuum arms, such as trunk and tentacle robots, attempt to mimic the biological appendages—like elephant trunks—and have seen a surge of research in recent years [1, 2, 3, 4]. These robots lie between the two extremities of rigid and soft robots, and promise to capture the “best” of both worlds in terms of manipulability/elasticity, degrees of freedom (DoF), and compliance. Their need spans industrial, manufacturing, and healthcare applications, in which safe human-robot collaboration is essential [5, 6, 7, 8]. In spite of their demonstrated capabilities in manipulability, superior performance in constrained spaces, and compliant operation, their full potential has not been realized yet. This lag is largely due to their inherent physical features, which, while making continuum arms human-friendly, contribute to complexities in path planning and control.

A continuum arm consists usually of multiple sections, each consisting of several pneumatic muscle actuators [9, 10]. Figure 1 shows two pneumatically actuated multisection continuum arms prototypes assuming smooth bending deformation in each section to achieve complex poses. Due to the redundancy in the number of DoF they possess, the mapping between their configuration space ($\mathcal{C}$-Space) and their task space ($\mathcal{W}$-space) is nonlinear, which makes path planning for such robots a complex problem. (This redundancy is present since multiple $\mathcal{C}$-space configurations of the continuum arm correspond to the same point in its $\mathcal{W}$-space.) As a result, little work has been done on path planning for continuum arms [11, 12]. The prevailing mathematical tool used for path planning relies on inverse kinematics (IK). Due to the high redundancy alluded to above, IK-based methods are not reliable, and result in a high failure rate [13, 14]. These methods have the potential of getting stuck in local minima, especially if obstacles are present in the $\mathcal{W}$-space (such as in the application depicted in Fig. 1). Indeed, it has been shown in [14] that IK methods can be unreliable. Although they have not been applied yet to continuum manipulators, sampling-based path planning methods present another possibility for performing path planning (see [15]). However, we mention that path planning approaches that are based on sampling the $\mathcal{C}$-space (e.g., PRM and RRT) suffer from the lack of performance guarantee, and are not suitable for sensitive applications of continuum-arm robots (e.g., healthcare applications) [16].

A reliable approach, i.e., one that provides performance guarantee, for performing path planning is based on the $\mathcal{C}$-space graph, which captures the whole range of motion of the entire continuum arm, even in the presence of obstacles. In such approach, one chooses proper discretizations of the $\mathcal{W}$-space and the $\mathcal{C}$-Space, and constructs a mapping from the $\mathcal{C}$-space to the $\mathcal{W}$-space w.r.t. these discretizations. A graph is then constructed, whose vertices are the $\mathcal{C}$-space configurations and whose edges represent the adjacencies between configurations w.r.t. a 1-step variance in the discretization of the $\mathcal{C}$-space. Path planning is then performed on this graph, and mapped to the $\mathcal{W}$-space. In general, the issue with the above approach is that it becomes infeasible for systems with high DoF, as the number of configurations is exponential.

In this paper, we demonstrate how to leverage the continuum arm features to introduce a new model that enables a path planning approach based on the $\mathcal{C}$-space graph, for a continuum arm consisting of three sections, each consisting of three muscle actuators bundled together. This model allows discretizations of the $\mathcal{W}$-space and $\mathcal{C}$-space to a high-fidelity degree, resulting in a reliable and smooth path planning algorithm. The graph algorithm we apply to the $\mathcal{C}$-space graph allows us to exploit parallelism in the computation to obtain efficient implementation. We conducted extensive tests. The obtained results show the completeness of the proposed algorithm under the considered discretizations, in both cases where obstacles are present and where they are not: The algorithm provides a path between the initial and final point in the $\mathcal{W}$-space when such a path exists, and otherwise, reports that no path exists. Moreover, the algorithm does so within a reasonable amount of time. We compared our approach to the standard IK approach. While the IK approach is much faster when successful, our algorithm always succeeds in finding a path or reporting that no path exists, compared to a roughly 70% success rate of the IK approach (when a path exists).

II SYSTEM MODEL

Figure 2 shows the schematic of an $n$-section continuum arm, in which the sections are identical and are enumerated starting from the base section (index 1) attached to the task-space coordinate system, $\left\{O\right\}$. The $i^{th}$ continuum section, shown in Fig. 3, is actuated by three extending-mode PMAs that are affixed on rigid terminating plates at either end at distance $r\in\mathbb{R}^{+}$ from the neutral axis, and that are $\frac{2\pi}{3}$ radians apart. Each continuum section has an inextensible rigid chain of length $L\in\mathbb{R}^{+}$ in its neutral axis. The unactuated length of PMAs is $L\in\mathbb{R}^{+}$, and the maximum length change is $l_{max}\in\mathbb{R}^{+}$. The joint-space vector of the continuum section is $\boldsymbol{q}_{i}=\left[l_{i1},l_{i2},l_{i3}\right]^{T}$, where $l_{ij}\in\left[0,l_{max}\right]$ $\forall j\in\left\{1,2,3\right\}$. Individual continuum sections are jointed together using rigid joints that introduce $\sigma\in\mathbb{R}_{0}^{+}$ linear displacement along the +Z axis and $\gamma\in\mathbb{R}_{0}$ angular displacement about the +Z axis of $\left\{O_{i}\right\}$.

Along the length of the continuum section, PMAs are constrained to maintain $r_{i}$ clearance to the neutral axis. Consequently, any differential length changes of PMAs due to different pressure inputs cause the continuum section to bend in a circular arc or extend (when length changes are equal). The subsequent derivations rely on the assumption that the continuum sections bend in circular arc shapes. This is a reasonable assumption. As shown in [13] and [10], under operating conditions where the continuum arms are not subjected to large external forces, continuum sections satisfy this condition.

II-A Derivation of a Reduced DoF Mapping

The presence of an inextensible backbone introduces an over-constrained system. Unlike the pneumatically-actuated continuum arms without such a backbone (see [10, 9]), for any $i^{th}$ section, the length changes are governed by

[TABLE]

where physically this means that any extension of actuators results in the contraction of the others.

As a result, without loss of generality, the complete kinematic model for a continuum section has to be derived from the principles of force/moment balance. However, in this work, by utilizing the above constraint, we will introduce a reduced, two-DoF model. In this approach, we will disregard the third actuator, $l_{i3}$, and instead use $l_{i1},l_{i2}$, and the backbone length to derive a complete mapping. The objective of our approach is to reduce the overall DoF of the continuum arm, without betraying the physical behavior, in order to explore the graph-theoretic path planning approach proposed herein.

By definition, the bending of a continuum section can be described by three curve parameters, the radius $\lambda_{i}$ of the circular arc, the angle $\phi_{i}$ subtended by the arc, and the angle $\theta_{i}$ between the +X axis and the bending plane [13]. Figure 3 shows the schematic of the $i^{th}$ section bending in a circular arc with the arc parameters $\lambda_{i}$, $\phi_{i}$, and $\theta_{i}$.

Utilizing the arc geometry, we can derive the following relations between the actuator length changes and the curve parameters. A similar and detailed exposition of the methodology, developed for continuum arms without constraining backbones, is given in [13].

[TABLE]

By manipulating the relations given in (2), we can derive the curve parameters as functions of the length changes as

[TABLE]

Using (3), the homogeneous transformation matrix (HTM) for the $i^{th}$ section can be derived as

[TABLE]

where $\mathbf{P}_{X}\in\mathbb{SE}^{3},$ $\mathbf{R}_{Z}\in\mathbb{SO}^{3}$, and $\mathbf{R}_{Y}\in\mathbb{SO}^{3}$ are HTM that denote translation along the +X axis, rotation about the +Z and +Y axes, respectively. $\mathbf{R}_{i}:\left(\boldsymbol{q}_{i},\xi_{i}\right)\mapsto\mathbb{SO}^{3}$ is the resultant rotation matrix and $\boldsymbol{p}_{i}:\left(\boldsymbol{q}_{i},\xi_{i}\right)\mapsto\mathbb{R}^{3}$ is the position vector. The scalar $\xi_{i}$ denotes any point along the neutral axis, where $\xi_{i}=0$ is the base where $\left\{O_{i}^{\prime}\right\}\equiv\left\{O_{i}\right\}$ and $\xi_{i}=1$ is the tip of the continuum section. We then apply the $15^{th}$ order multivariate Taylor series expansion on the terms of (6) to obtain numerically efficient and stable modal form of the HTM (see [13]).

II-B Actuator Range Derivation

In order to preserve the entire $\mathcal{W}$-space of the continuum section, the two actuator ranges have

to be modified. For instance, the actual PMAs only extend during operation, but as we have omitted $l_{i3}$, to describe the motion contributions of $l_{i3}$, as per (1), we will modify the ranges of $l_{i1}$ and $l_{i2}$. Under the premise that PMA extension is proportional to the differential pressure input, using the resulting curve parameters given by (3) with the length constraint (1), one can easily derive the correct ratio of pressures given to PMAs.

Noting the maximum bending, $\phi_{i,max}=\pi$, of the continuum sections, we can find the valid actuator combinations that result in $\phi_{i}\leq\pi$, as shown in Fig. 4. We identified this valid length change range by comparing the actuator combinations in the range $\left[-0.04,0.04\right]$ for $L_{i}=0.15\,m$ and $r_{i}=0.0125\,m$. The resulting valid actuator range is give by the rotated ellipse given by

[TABLE]

where $a=-0.5766$, $b=0.5789$, $c=-0.5766$, $d=0,e=0$, and $f=0.0007$. This means that if $g(l_{i1},l_{i2})\geq 0$ in (7), then the value pair $\left(l_{i1},l_{i2}\right)$ is a valid combination, and results in $\phi_{i}\leq\pi$.

II-C Kinematic Model

Employing the continuum section HTM given in (6) and the kinematics principles of serial robot chains, the HTM of the $i^{th}$ section with respect to the $\mathcal{W}$-space coordinate system $\left\{O\right\}$, $\mathbf{T}^{i}:\left(\boldsymbol{q}^{i},\xi_{i}\right)\mapsto\mathbb{SE}^{3}$, is given by

[TABLE]

The HTM in (10) can be expanded to obtain the recursive form of the kinematics as

[TABLE]

where $\mathbf{R}^{i-1}:\left(\boldsymbol{q}^{i-1}\right)\mapsto\mathbb{SO}^{3}$ and $\boldsymbol{p}_{i}:\left(\boldsymbol{q}^{i-1}\right)\mapsto\mathbb{R}^{3}$ is the section tip rotation matrix and position vector of the preceding continuum section, respectively. Notice the absence of $\xi_{i}$, as $\xi_{k}=1,\forall k<i,$ by the definition of $\xi_{i}$ (see [13]).

III The Algorithm

We assume familiarity with basic graph algorithms and terminologies, and refer the reader to the textbook [17].

The approach we employ starts by constructing a mapping between the $\mathcal{C}$-space and the $\mathcal{W}$-space of the continuum arm. This is done by discretizing the pneumatic pressures of the three arm-sections to an appropriate granularity level that allows a feasible enumeration of the corresponding $\mathcal{C}$-space configurations, without trading off (by much) the quality of the path planning achieved. After enumerating the $\mathcal{C}$-space configurations, we construct an auxiliary graph of the $\mathcal{C}$-space. Path planning is then performed on this auxiliary graph and mapped back to the $\mathcal{W}$-space. We proceed to the details.

III-A The Auxiliary Graph

We construct an auxiliary graph, $G_{C}$, whose vertices correspond to the enumerated configurations, and whose edges correspond to adjacent configurations (i.e., configurations that are obtained from one another via a one-step variation in a single DoF). Since one of our goals is achieving smooth path planning, we associate weights with the edges of the auxiliary graph that reflect the change in the orientation between the two configurations that are endpoints of the same edge. This is quantified by measuring the distance between two vectors whose coordinates are formed by the six angles $\theta_{i}$ and $\phi_{i}$ (see Fig. 3), corresponding to the three arm sections (i.e., two angles per section).

III-B Path Planning

In the path planning problems under consideration, we are given two points, a starting point $s$ and a destination $t$, in a 3-dimensional $\mathcal{W}$-space, and we wish to compute a smooth $s$-$t$ path for the continuum arm. We considered both cases: where the $\mathcal{W}$-space is obstacle-free and where it contains obstacles.

To perform path planning, we start by discretizing the $\mathcal{W}$-space by creating a cubic-grid, in which each cube has the same dimension (1 unit). Given two points $s$ and $t$ in the 3-dimensional $\mathcal{W}$-space that correspond to two $\mathcal{C}$-space configurations, between which we wish to compute a (smooth) path, we first determine the two cubes $Q_{s}$ and $Q_{t}$ containing $s$ and $t$, respectively. We then find a shortest—w.r.t. the Euclidean distance—cube path between $Q_{s}$ and $Q_{t}$, which is a sequence of adjacent cubes in the cubic-grid that starts at $Q_{s}$ and ends at $Q_{t}$. This step is done by constructing a graph whose vertices are the cubes in the grid, and whose edges correspond to adjacent cubes; we refer to this graph as the cubes-graph, and denote it by $G_{Q}$. An edge in $G_{Q}$ has weight equal to the Euclidean distance between the centers of the two cubes corresponding to the endpoints of the edge. We then find a shortest path $P_{st}$ in $G_{Q}$ between $Q_{s}$ and $Q_{t}$; this can be done by applying Dijkstra’s algorithm. Let $P_{st}=(Q_{0}=Q_{s},Q_{1},Q_{2},\ldots,Q_{r}=Q_{t})$. Based on the enumeration of the $C$-space, we can find the set of configurations, $C_{i}$, whose corresponding points in the $\mathcal{W}$-space fall within cube $Q_{i}$, for $i=1,\ldots,r$; we let $C_{0}$ be the singleton containing the configuration corresponding to the initial point $s$. In the case where obstacles are present, we purge from each $C_{i}$ the configurations that are invalid because they correspond to a position in which the continuum arm intersects an obstacle in the $\mathcal{W}$-space. In order to test that, for each configuration, we keep track of intermediate points (in the $\mathcal{W}$-pace) along the three sections of the continuum arm, and use those points to detect intersection with obstacles.

The goal now becomes to compute a shortest path in $G_{C}$ that starts at $C_{0}$, proceeds to visit a vertex in each $C_{i}$, for $i=1,\ldots,r-1$ (in the listed order), and ends up at a vertex in $C_{r}$. To compute such a path, and in order to reduce memory storage (as storing the whole configurations graph $G_{C}$ turns out to be quite inefficient) and exploit parallelism, we employ a shortest-path algorithm that takes advantage of the structure of the layered graph, $G_{L}$, whose vertex-set consists of the configurations in $\bigcup_{i\in\{0,\ldots,r\}}C_{i}$, and edge-set consists of those edges in $G_{C}$ that join configurations in consecutive layers $C_{i},C_{i+1}$, for $i\in\{0,\ldots,r-1\}$. For this purpose, we apply a variant of the Bellman-Ford’s algorithm (see [17] for Bellman-Ford’s algorithm) for computing a shortest path between the single configuration in $C_{0}$ and every vertex in $C_{r}$. Here, We do not insist on reaching $t$, and settle for any configuration corresponding to a point in $t$’s cube, and hence is close enough to $t$, i.e., within distance $\sqrt{3}\cdot d$ from $t$, where $d$ is the cube dimension. The algorithm starts by initializing the cost for $C_{0}$ to [math], and for each other configuration in $G_{L}$ to $\infty$, which is a sentinel value that is assumed to be larger than any concrete number. The algorithm employs the subroutine Relax($u$, $v$), which operates on edge $uv$ and relaxes $uv$ by updating $cost(v)$ to become $cost(u)+wt(u,v)$, in case the current value of $cost(v)$ exceeds $cost(u)+wt(u,v)$. The algorithm then performs $r$ iterations, where in iteration $i$, $i=0,\ldots,r-1$, the algorithm relaxes all edges between layer $C_{i}$ and layer $C_{i+1}$ in $G_{L}$. (This computation is parallelized by running it separately on the set of all edges incident to the same node in $C_{i+1}$. Moreover, after iteration $i$, the vertices in $C_{i}$ are no longer useful during the rest of the algorithm, and hence, can be removed from memory and replaced with those in $C_{i+2}$.) At the end of step $r-1$, we have computed the shortest path between the single configuration in $C_{0}$ and every vertex in $C_{r}$. The shortest path among among all those (shortest paths) is then mapped to an $s$-$t$ path in the $\mathcal{W}$-space.

IV IMPLEMENTATION AND RESULTS

We implemented and tested our algorithm on a Windows-10 computer, with 64-bit CPU, 6 core 3.20 GHz processor, and 64 GB RAM. Several considerations were made over the course of the project to reduce the CPU and memory load. First of all, most of the calculations were performed using the “NumPy” Python library [18]. The array and matrix operations in “NumPy” are more efficient than the standard library, as they are optimized to use less memory and CPU time. We encoded the cube indices and the coordinates of the points as decimal numbers to save memory. In order to determine the points in the $\mathcal{W}$-space, given their corresponding configurations, we relied on large matrix multiplication. To speed up this process, two features were used. First of all, the matrix is modified so that it can be vectorized; this has the effect of parallelization without any explicit threads, and improves performance. The other feature we used is the “JIT” compiler of the Numba Library [19] to improve performance. (For instance, the function that calculates the shortest distance between points on the path is aggressively vectorized.) These techniques brought the time taken to generate the $\mathcal{C}$-space graph down from 16 hours to 10 minutes. To avoid recalculating the graph each time, we used the NumPy save function and the “pickle” python library111https://docs.python.org/3/library/pickle.html to store the graph, cube list, and point list on disk for easy reuse. Lastly, in order to perform graph operations, we used the python library “igraph”[20]. This is a high-performance library that handles all of the graph generation and simple path planning for the cubes.

To test our algorithm, we discretized the three sections of the continuum arm; each section has 2 DoF, and each DoF was uniformly discretized into (roughly) 26 steps. This resulted in sampling 688 configurations for the tip of each section, and $688^{3}=325,660,672$ configurations for the tip of the whole arm. Thus, the number of vertices in the $\mathcal{C}$-space graph is $325,660,672$. Based on the points in the $\mathcal{W}$-space corresponding to the sampled configurations, we placed a bounding box of $X$-$Y$-$Z$ dimensions $82\,\text{cm}\times 82\,\text{cm}\times 74\,\text{cm}$ that contains all these points. This bounding box was discretized into cubes, each of dimension 1 cm.

We ran 2,000 tests, 1000 tests in which the $\mathcal{W}$-space is obstacle-free, and 1000 tests in which the $\mathcal{W}$-space contains either 2 or 3 spherical-shape obstacles. The starting and ending points of the path were generated randomly from the $\mathcal{W}$-space (by randomly choosing two configurations), and so was the center of the spherical-shape obstacles; the radius of the obstacles was randomly chosen from the interval $[3\,\text{cm},25\,\text{cm}]$ . We then located the two cubes containing the starting and ending points; computed a shortest cube-path between these two cubes in the cubes graph; and then ran our algorithm on the corresponding layered graph resulting from the cubes on this cube-path, as explained in the previous section.

The IK approach was run on the same generated test cases. Table I below summarizes the results obtained for both approaches. The third column of the table shows the success rate of the IK approach and the fourth column shows the average time (in seconds) taken by the IK approach (over all cases tested). Columns 5 and 6 show the corresponding data for our algorithm (Alg), given in the previous section.

Figure 5 shows a test case in an obstacle-free space where both the proposed and the IK approaches succeeded in finding a solution. Given the (continuous) nature of the IK approach, the resulting path generated by the IK approach is shorter and smoother than the one computed by the proposed approach. (Our approach is limited by the rough discretizations of the $\mathcal{C}$-Space and $\mathcal{W}$-Space in order to ensure a feasible computation.) The reliability of our approach stems from the fact that it has apriori access to the entire (discretized) $\mathcal{C}$-space, which it could use to avoid “knotting” or running into local minima. As shown in Fig. 6, in this example our planner was able to find a path , whereas the IK approach did not even converge to a solution. It can be seen that the base and the mid sections of the continuum arm form a Knot [21], as the IK approach tries to minimize the distance to the target point but the mid and base sections are already at the maximum bending. Consequently, the IK approach failed to yield a solution. This is the common reason behind the failure of IK-based continuum arm path planners. Since the $\mathcal{C}$-space graph—constructed according to a proper discretization—allows us to capture a wide range of motion for the continuum arm covering the entire task-space, our algorithm has the ability to adjust itself, when started from certain initial configurations, to reach the destination point, in situations where the IK approach, starting from the same initial configurations, is incapable of doing so, and diverges to points that are far away from the destination points.

Figure 7 shows a test case with obstacles. Due to the highly-constrained nature of the problem, unlike the obstacle-free scenario, in this case it is not possible to generate a meaningful shortest path between the starting and end point. Therefore, in favor of a fair comparison with the IK approach, we removed the shortest path requirement so that the IK planner is only required to find a path. In the IK planner, we formulated the obstacle avoidance as a constrained function; we considered ten points along a continuum section (30 points total), and the constraint function must maintain a positive distance between all the points and obstacles. In contrast, the proposed planner solves for the shortest path. As shown in Fig. 7, in this test, the IK failed to converge. Similarly to the obstacle-free test case shown in Fig. 6, the IK planner was stuck in a local minima (knot configuration), whereas, due to the apriori knowledge of the $\mathcal{W}$-Space, our planner was able to find poses that circumvent configurations associated with local minima.

Since we generate the obstacles randomly, there is the possibility of having cases where there is no solution; for instance, this is the case if the obstacles form a separator in the $\mathcal{W}$-Space, disconnecting the starting point from the end point. We discarded such cases when computing the success rate in Table I, and computed the success rate of IK as a percentage of the successful cases, i.e., in which a path exists. (Note that, given how our planner works, it is able to find a solution if there is a path.) Consequently, the proposed approach is near-optimal and reliable for planning path of multisection continuum arms in spaces with or without obstacles. In addition, it facilitates the introduction of multiple optimization criteria—such as smooth trajectories, in contrast to the IK approaches, which will increasingly fail as the number of constraints grow. In future work, we will explore the possibility of using other optimality criteria, such as energy efficiency and dynamic stability, for path planning.

V CONCLUSIONS

Continuum arms have seen a surge of research in recent years. However, due to the complex and nonlinear kinematics associated with continuum structures, limited research has been conducted on path planning. Most of such planners have been based on inverse kinematics, and therefore resulted in poor reliability due to their potential of running into local minima. This paper presented a near-optimal smooth path planning approach for multisection continuum arms using a graph-theoretic approach, which is based on a high-fidelity mapping between the configuration space and the work space of the arm. The proposed approach was then tested against the classical IK solvers in work spaces with and without obstacles. The proposed approach was able to solve the path planning problems (100% without obstacles) where the IK consistently reported poor reliability (70% without obstacles). For tests with randomly placed obstacles in the work space, the proposed planner reported 100% success rate where there exists a solution, while the IK yielded 69% success rate.