Introduction to Motion Planning for Continuum Robots  Part 2
Today, we continue our introduction to motion planning for continuum robots. If you have missed Part 1 of the intro, look at last week’s blog post before you continue here!
What makes motion planning particularly challenging for continuum robots?
First, let’s discuss one of the primary differences between continuum robots and more “traditional” robots (for lack of a better term)—modeling.
As you’ve seen in prior (and future, I promise) blogs on this site, modeling the mechanics of continuum robots is fundamentally different from more traditional seriallink manipulator arms.
As a subset of mechanics, consider just the forward kinematics (FK) problem for continuum robots, i.e., mapping the robot’s joint state to the robot’s resultant shape in space. For concentric tube robots this may be mapping the robot’s tubes’ rotation/translation values to the robot’s shape and for tendondriven robots this may be mapping the robot’s tendon tensions/displacements to the robot’s shape.
What’s actually happening in the physical world is complicated here. There are elastic elements interacting with (potentially) other elastic elements in complex ways. There is difficulttomodel friction between components of the robot, hysteresis, potentially difficulttopredict material properties, etc.
The continuum robotics community has taken a variety of approaches to modeling, but a fundamental trend exists: a tradeoff between accuracy and speedofcomputation. That is, models which may be fast to compute are typically fast because they neglect to account for some of the physical phenomenon present in the actual robot. On the flip side, models that account for those phenomenon are slow to compute. In reality, there’s a spectrum here.
Constant curvature models, for instance, may take a purely geometric approach to FK, which is fast to compute, but tend to be less accurate. On the other end of the spectrum, you could consider leveraging something like finite element analysis to simulate the physics of a continuum robot to compute its shape. This would improve accuracy but would be much, much slower to compute. Somewhere in the middle of these you may find things like the Cosserat rod/string models, which are a popular way to model continuum robots and will be discussed in other blog posts here (this is the approach we take in my lab, usually, when we aren’t using machine learningbased methods).
Let’s contrast this with seriallink manipulator arms. For these, FK typically require only the multiplication of a handful of matrices, which is comparatively VERY fast, and comparatively VERY accurate, as these robots are rigid.
Okay, so why is this a problem when performing motion planning for continuum robots?
Well, one way to think about motion planning is that we are reasoning in advance of actually moving about the best way to move the robot to achieve our goals. This means that we need our ability to reason about motion to be accurate, otherwise how we predict the robot will move doesn’t mean much. For this reason, model inaccuracy plays havoc with naïve motion planning methods (such as we’ve discussed in last week’s post), and continuum robots have inaccurate models.
Further, the ways in which we are reasoning in advance about moving require us to consider a whole bunch of potential motions for the robot. Every vertex in the grid/latticebased and samplingbased methods represents an FK computation. Every edge between them requires multiple (often many) FK computations. In the optimizationbased methods, the computation of objective and constraint gradients frequently require many FK computations. So the time it takes to compute FK has HUGE implications in motion planning.
This modeling tension, fast but inaccurate or (somewhat) accurate but slow, is one of the fundamental challenges of applying motion planning to continuum robots, and the one I’ll focus on for the rest of this post.
How has the community overcome these challenges?
Modeling accuracy (or lack thereof)
The continuum robotics community (myself included) has largely taken an extremely unsatisfying approach to overcoming modeling inaccuracy. This approach, also used by much of the noncontinuum robotics planning world, boils down to a simple idea: don’t let the robot get close to things. That way, if the model is inaccurate, hopefully the robot is far enough away from its environment that it won’t collide with it in unexpected ways.
As an example, in one of my papers^{1}, we build into the objective function a notion of distance from obstacles. Specifically, We define the cost of a configuration $\boldsymbol{q}$ as:
\[\begin{split} &\texttt{cost}(\boldsymbol{q})=\left\{ \begin{array}{rcl} \cfrac{1}{\texttt{clear}(\boldsymbol{q})},&&\texttt{clear}(\boldsymbol{q}) > 0\\ \infty,&&\texttt{clear}(\boldsymbol{q}) \le 0, \end{array}\right. \end{split}\]where $\texttt{clear}$ is, informally defined here, the signed distance from the robot’s body at that configuration to the nearest obstacle.
The cost of a path $\boldsymbol{\xi}$ then becomes \(\texttt{Cost}(\boldsymbol{\xi}) = \int_0^1 \texttt{cost}(\boldsymbol{\xi}(s))ds .\)
where $s$ is a normalized path arclength parameter. Note here that I’ve used little ‘c’ cost for cost over configuration and big ‘C’ Cost for cost over a path. Creative, I know.
You should really read the paper if you want formality here, but this should give you the gist. Also note that in the paper the notation is slightly different, but I’ve changed it here to moreclosely match what I’ve used in the last post.
In that method, we leverage both samplingbased motion planning and optimizationbased motion planning in parallel (building on [13]), where the method attempts to minimize the $\texttt{cost}$ of a path. This results in paths that travel as far from obstacles as possible, while still being constrained at the start and end configurations.
Computation speed (or lack thereof)
The community has taken a variety of approaches to overcome the slow computation speed of continuum robot mechanical models. Here are a few, with some examples.
The first, and most obvious, is the use of a fastbutinaccurate mechanical model: For instance, in Lyons et al. ^{2}, the authors simplify the mechanical model of concentric tube robots in two key ways. First, they assume that the tubes are torsionally rigid. Second, they assume that the stiffness of each tube dominates all of the tubes nested inside of it. This means that the shape of a tube is assumed to be independent of the motions of the tubes internal to it and that each tube deploys in a constantcurvature circular shape. The mechanical model that leverages these assumptions is sufficiently fast for planning, but again as discussed above ad nauseam, comes at the significant cost of accuracy.
Another approach is to lean on advances in software engineering and programming languages to speed up the computation: Leibrandt et. al.^{3} leverage template metaprogramming in C++, which (I’m oversimplifying this explanation) moves a lot of expensive runtime computation to compiletime computation for concentric tube robots. This enabled very fast kinematics computation. The method leverages this in a PRMstyle motion planner. In our paper^{1} mentioned above, we use this templatebased kinematic model, wrapping our parallel sampling and optimization method around it. Speaking of, our^{1} use of parallelism is another example of leveraging software engineering concepts to speed up computation in planning for continuum robots.
We can also consider leveraging precomputation: In the paper ^{4} (its extension ^{5}), the authors propose leveraging precomputation to build a roadmap for planning after the environment is known but in advance of when it is needed. This is a pretty specific (and restrictive) scenario, but makes sense in the application put forth by the papers, namely in surgery. These works present the case that preoperative medical imaging provides the method with a 3D environment a day (or more) prior to when the surgery will happen. The method can then leverage that time to compute a dense, collisionfree roadmap before it’s needed at surgery time. The proposed method utilizes the samplingbased method Rapidlyexploring Random Graph (RRG) (which is also presented in ^{6}, in addition to PRM* and RRT*). The method also utilizes one of, if not the most precise (at the time, at least) mechanical models available for concentric tube robots. This dense, collisionfree roadmap is built over the course of many hours using this accurate model and the anatomical environment segmented from the preoperative medical imaging. Then, at the time of the surgery, the method proposes an interactiverate supervisory controlstyle scheme in which a haptic device is used by the surgeon to input a desired tip position for the robot in the patient. Leveraging a nearestneighbor data structure, the vertex in the roadmap that corresponds to the configuration with the robot’s tip closest to the desired position is then identified. Graph search is performed to identify a path on the roadmap from the current configuration to that configuration, and then the method “steps off” of the roadmap and leveraging damped leastsquares iterative inverse kinematics ^{7}^{8}, which is conceptually very similar to resolvedrates control, drives the tip of the robot as close to the desired tip position as it can. This then repeats in a loop.
In our paper^{9}, my group has built upon this concept in a few ways. Among other contributions, we adapt the concepts to work for tendondriven continuum robots (^{4} and ^{5} are specific to concentric tube robots), introduce a modification to mechanical modeling and collision detection to improve speed, and remove the need to have the environment in advance of precomputing the dense roadmap. The method still heavily leverages precomputation, but only requires the environment immediately prior to when the supervisory control loop is started.
So what’s next?
What are the open problems or areas to be explored in motion planning for continuum robots? Here are a few that I’m personally excited about.
Leveraging learned models in planning
There has recently been a large interest in machine learningbased mechanical models of continuum robots, both for their potential to learn unmodeled (or unknowable/unmodelable) effects and their potential for fast computation (e.g., frequently the shape computation of a learned model is faster than the physicsbased equivalent). I’ll leave the details of these methods to another post, written by myself or someone else. However germane to THIS post, how to leverage these types of models for motion planning of continuum robots is asyet underexplored.
Principled planning under uncertainty
Motion planning in a way that handles uncertainty in a principled way has been leveraged to great effect in other areas of robotics. Given the uncertainty associated with continuum robots, the application/exploration of using these types of methods/formulations in planning for continuum robots has great potential.
Multifidelity planning
The fact that we have a variety of models for continuum robots that trade computation speed for accuracy implies exciting potential for the use of multifidelity planning. One can imagine, as maybe the most obvious example, using a fast but inaccurate model to prioritize edge evaluation in a lazy planning paradigm, look up Lazy PRM if that doesn’t make sense to you, where full edge evaluation is then done with a highfidelity model.
Closing remarks
Hopefully the above alongside last week’s Part 1 were a relatively gentle introduction to motion planning and how it relates conceptually to continuum robots. This wasn’t intended to be a comprehensive survey of planning for continuum robots. Rather, my intention was to give you the vocabulary and an understanding of the rough concepts sufficient for you to investigate the rest of the literature yourself, and to innovate on the stateoftheart as you need for your specific problems.
References

A. Kuntz, M. Fu, R. Alterovitz: Planning HighQuality Motions for Concentric Tube Robots in Point Clouds via Parallel Sampling and optimization. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 22052212, 2019. doi: 10.1109/IROS40897.2019.8968172 ↩ ↩^{2} ↩^{3}

L. A. Lyons, R. J. Webster, R. Alterovitz: Motion planning for active cannulas. IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 801806, 2009. doi: 10.1109/IROS.2009.5354249 ↩

K. Leibrandt, C. Bergeles, G.Z. Yang: Concentric Tube Robots: Rapid, Stable PathPlanning and Guidance for Surgical Use. IEEE Robotics & Automation Magazine, vol. 24, no. 2, pp. 4253, 2017. doi: 10.1109/MRA.2017.2680546 ↩

L. G. Torres, C. Baykal, R. Alterovitz: Interactiverate motion planning for concentric tube robots. IEEE International Conference on Robotics and Automation (ICRA), pp. 19151921, 2014. doi: 10.1109/ICRA.2014.6907112 ↩ ↩^{2}

L. G. Torres et al.: A motion planning approach to automatic obstacle avoidance during concentric tube robot teleoperation. IEEE International Conference on Robotics and Automation (ICRA), pp. 23612367, 2015. doi: 10.1109/ICRA.2015.7139513 ↩ ↩^{2}

S. Karaman and E. Frazzoli: Samplingbased algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846894, 2011. doi: 10.1177/0278364911406761 ↩

Y. Nakamura and H. Hanafusa: Inverse Kinematic Solutions With Singularity Robustness for Robot Manipulator Control. ASME. J. Dyn. Sys., Meas., Control., 108(3):163–171, 1986. doi: 10.1115/1.3143764 ↩

C. W. Wampler: Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped LeastSquares Methods. IEEE Transactions on Systems, Man, and Cybernetics, 16(1):93101, 1986. doi: 10.1109/TSMC.1986.289285 ↩

M. Bentley, C. Rucker, A. Kuntz: InteractiveRate Supervisory Control for ArbitrarilyRouted Multitendon Robots via Motion Planning. IEEE Access, vol. 10, pp. 8099981019, 2022. doi: 10.1109/ACCESS.2022.3194515 ↩