Riemannian Optimization-based Whole-body Obstacle Avoidance for Mobile Continuum Robots in Cluttered Environments
Authors: Duo Tang, Ping Deng, Sophie Armanini, Yajing Shen, Toshio Fukuda, Cosimo Della Santina, Peng Lu
Organizations: Department of Mechanical Engineering, University of Hong Kong, Hong Kong, China · Department of Aeronautics, Imperial College London, UK · Department of Electronic and Computer Engineering, Hong Kong University of Science and Technology, Hong Kong, China · Nagoya University, Meijo University, Japan and Beijing Institute of Technology, China · Department of Cognitive Robotics, Delft University of Technology, Delft, The Netherlands
Inspired by the coordination of global motion and local shape adaptation in snakes, elephant trunks, and octopus arms, we propose a unified planning and control framework for mobile continuum robots in cluttered and confined environments. Existing approaches often address high-level path planning and local motion control separately, limiting coordinated base-arm motion, kinematic consistency, and collision avoidance. Our framework integrates long-horizon trajectory planning with real-time geometric refinement under kinematic and environmental constraints. The global layer combines A* search with minimum-jerk planning to generate smooth reference trajectories. The local layer uses a geometry-consistent inverse kinematics solver and Riemannian optimization to coordinate mobile-base motion and continuum deformation while avoiding obstacles. We evaluate the framework through simulated pipe-traversal tasks with multiple baseline planners and real-world experiments involving navigation among dense obstacles and manipulation in confined spaces. Results demonstrate improved motion consistency, feasibility, and adaptability, supporting coordinated whole-body motion of mobile continuum robots in constrained environments.
Figures & tables
Fig. 1: Overall framework of the proposed motion planning and execution pipeline for a mobile-base continuum robot in cluttered environments. (a) Whole-body motion planning module: Generates globally feasible whole-body trajectories toward the task goal. (b) Collision detection module: Continuously detects potential collisions between the robot and the environment. (c) Local collision avoidance module: Searches the orientation space and modifies the motion to avoid collisions with obstacles. (d) Low-level control and execution module: Executes control commands to track the planned trajectories.
Fig. 2: Illustration of the analytical collision checking scheme for continuum robots, showing continuous arc modeling, obstacle projection, and safety boundary evaluation. The method overcomes limitations of discrete sampling approaches through geometric parameterization.
Fig. 3: Illustration of the Riemannian optimization process with guided descent.
Fig. 4: Structural overview and kinematic representation of the tendon-driven continuum robot. (a) Physical prototype of the mobile-base continuum robot. (b) Simplified tendon-driven backbone model showing segment-wise bending characterized by arc angle φi and bending plane orientation βi , with tendon routing and coordinate frames Fi . (c) Tendon routing and structural layout for each segment, illustrating tendon paths through spacer disks and inter-segment connections.
Fig. 5: (a) Control architecture of the mobile continuum robot. The planner outputs manipulator and base trajectories, which are mapped to tendon length commands and base velocity references. Pose and cable-length errors drive the manipulator motors, while the position feedback regulates the mobile base. (b) Experimental Setup.
Fig. 6: Iterative inverse kinematics process illustrating how the end-effector converges to the target. The initial upright pose (iteration = 1) starts with a large error. After the first iteration (iteration = 2), orientation is aligned while position error remains. Subsequent iterations refine the solution until convergence at iteration 6 with position error below 0.001 (unit link length).
Fig. 7: Convergence of the inverse kinematics solution. Top: position error (unit link length) versus iteration, showing a rapid drop from 1.5 to 0.165 in the first iteration and convergence below 0.001 thereafter. Bottom: evolution of configuration angles, indicating that a near-optimal solution is already reached at iteration 2.
Fig. 8: Pipe traversal simulation of the mobile continuum robot: (a) Snapshots of the robot navigating through the pipe, (b) Evolution of the end-effector orientation vector over time, (c) Tendon length variations during motion, and (d) Schematic of the three-section continuum robot’s tendon configuration.
Method
Avg Time (ms)
Max Time (ms)
Avg Iter
Max Iter
Success Rate (%)
Ours
2.20
21.84
4.16
8
100
FABRIKc [ 35 ]
22.2
82.6
44.55
112
–
FABRIKx [ 37 ]
3.3
–
17
–
93.8
Jacobian [ 35 ]
192.6
217.7
1604.8
1722
–
TABLE I: Performance Comparison of Inverse Kinematics Solvers
Fig. 9: Quantitative comparison of our method and baseline planning algorithms. The upper plot presents the position error comparison (normalized by the continuum link length of 1.5 units). The lower plot presents the distribution of the absolute values of angular velocity changes, which serves as the primary metric for trajectory smoothness.
TABLE II: Quantitative Comparison of Planning Methods
Fig. 10: End-Effector Control with Obstacle Avoidance. (a) Motion snapshots: simulation (top), physical (bottom); (b) End-effector trajectories (reference via RViz marker, simulated, experimental); (c) Bending angle tracking; (d) Arm configuration evolution; (e) Computation time box-plot; (f) End-effector pitch/roll angles measurement.
Fig. 11: Evolution of penetration depth and computation time over iterations. The curve (left y-axis) shows the penetration depth decreasing until a collision-free state is achieved. The bar graph (right y-axis) represents the computation time per iteration.
Fig. 12: Experiment 2: Circular trajectory tracking experiment with two spherical obstacles. (a) Snapshots of the fixed-base continuum robot tracking a circular path while avoiding obstacles, (b) Arm configuration evolution during motion, (c) 3D end-effector trajectory (desired vs. estimated), (d) Position coordinates (Px,Py,Pz) over time, (e) Euler angle variations.
Fig. 13: Experiment 3: Trajectory tracking through barrier plates. Top: Snapshots of (a) four-leaf clover, (b) pentagram, and (c) helical trajectories. Bottom: 3×4 subplots showing quantitative performance; first two rows correspond to single barrier plate trajectories, third row corresponds to the helical trajectory with dual-layer barrier plates.
Fig. 14: Experiment 4: Hurdle crossing experiment. Snapshots of the continuum robot navigating over obstacles at three different heights: (a) 65 cm, (b) 60 cm, (c) 55 cm.
Fig. 15: Experiment 5: Collaborative tracking of the letter-shaped trajectory in a cluttered environment. Top: Experimental snapshots and simulated trajectory. Bottom: Planar trajectory view with performance metrics including position errors, bending angles, and computation time.
Fig. 16: Experiment 6: Box traversal with mobile continuum robot. (a) Motion snapshots, (b) End-effector trajectory inside the box (green), (c) Base and tip position evolution, (d) Four distinct traversal configurations, (e) Configuration angle profiles over time, (f) Position error distribution.
Fig. 17: Experiment 7: Pipe traversal with the mobile continuum robot. Sequential snapshots demonstrate coordinated base–arm motion and continuous shape adaptation, enabling smooth and collision-free passage through the constrained pipeline.
This paper addresses the motion control problem for mobile robots in obstacle-cluttered environments. The mobile robot has partial environment information only, and aims to move from an initial position to a target position without collisions. For this purpose, a reactive planning based control strategy (RPCS) is proposed. First, the initial and target positions are connected as a reference trajectory. Then, a reactive planning strategy (RPS) is developed to ensure the collision avoidance by modifying the reference trajectory locally based on the partial environment information. Next, an adaptive tracking control strategy (ATCS) is proposed to track the reference trajectory with potentially local modifications via the discretization techniques. Finally, the RPS and ATCS are combined to establish the RPCS, whose efficacy and advantages are illustrated by numerical examples.
Li Tan, Junlin Xiong, Yan Wang +1
Department of Automation, University of Science and Technology of China, Hefei, 230031, China. · School of Intelligence Science and Engineering, Harbin Institute of Technology Shenzhen, Shenzhen 515100, China · School of Control Science and Engineering, Dalian University of Technology, Dalian 116024, China.
Humanoid locomotion in highly confined environments requires navigating dense environmental obstacles and complex self-collision bounds while maintaining multi-contact dynamic feasibility. Traditional trajectory optimizers frequently struggle in these restricted spaces, as navigating the large collision space with splines on particle abstractions is insufficient and leads to poor local minima. To address this, we propose a three-stage whole-body planning framework that formulates kinematic path planning directly over kinematically reachable rigid-body volumes. By integrating differentiable collision avoidance into a reachability-constrained formulation, our framework synthesizes volume-informed guides that reliably guide a full-order trajectory optimizer over long horizons. We show that these optimized plans serve as high-quality references to train a residual reinforcement learning policy for robust online execution. We validate our approach on the Unitree G1 humanoid across three benchmark testbeds exceeding NIST emergency response standards, achieving restricted confinement ratios (Cr<1.5). Our framework generates feasible trajectories across 12-to-18-second tasks with complex foot and hand contacts where standard baselines fail, while the learned policy successfully tracks these plans under extensive domain randomization in physics simulation.
Carlos Gonzalez, Luis Sentis
Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712, USA.
We present the PccDiffuser, a conditional diffusion framework for continuum robots that learns a multimodal distribution over complete configuration-space paths and samples multiple candidate solutions in parallel, which are subsequently converted into an executable trajectory by time allocation considering actuator constraints. Under the piecewise constant-curvature model, we use exponential co-ordinates to describe the robot kinematics, and use graph neural network to encode a variable number of environment obstacles. Analytical differential kinematics is incorporated in the denoising process to improve terminal accuracy and whole-body clearance. On a mixed test set comprising workspace with zero to four obstacles, PccDiffuser achieved a success rate of 91%. Compared with existing sampling- and optimisation-based benchmarks, it delivered both a higher success rate and greater computational efficiency, with the latter advantage becoming more substantial when sampling more candidate solutions. Experiments on a three-section tendon-driven continuum robot further demonstrate consecutive planning, multi-solution planning, and whole-body obstacle avoidance.
Ke Qiu, Sifan Chen, Si Wang +3
Department of Control Science and Engineering, Zhejiang University, Hangzhou, China