Two-Stage Path Following for Mobile Manipulators via Dimensionality-Reduced Graph Search and Numerical Optimization

Fuyu Guo; Yuting Mei; Yuyao Zhang; Qian Tang

cs.ROMar 7, 2026

Two-Stage Path Following for Mobile Manipulators via Dimensionality-Reduced Graph Search and Numerical Optimization

Authors: Fuyu Guo, Yuting Mei, Yuyao Zhang, Qian Tang

Organizations: College of Mechanical and Vehicle Engineering, ChongQing University, Chongqing, P.R.C.

Abstract

Efficient path following for mobile manipulators is often hindered by high-dimensional configuration spaces and kinematic constraints. This paper presents a robust two-stage configuration planning framework that decouples the 8-DoF planning problem into a tractable 2-DoF base optimization under a yaw-fixed base planning assumption. In the first stage, the proposed approach utilizes IRM to discretize the task-space path into a multi-layer graph, where an initial feasible path is extracted via a Dijkstra-based dynamic programming approach to ensure computational efficiency and global optimality within the discretized graph. In the second stage, to overcome discrete search quantization, feasible base regions are transformed into convex hulls, enabling subsequent continuous refinement via the L-BFGS algorithm to maximize trajectory smoothness while strictly enforcing reachability constraints. Simulation results demonstrate the theoretical precision of the proposed method by achieving sub-millimeter kinematic accuracy in simulation, and physical experiments on an omnidirectional mobile manipulator further validate the framework's robustness and practical applicability.

Figures & tables

Fig. 1 : Motion planning for path following of mobile manipulators

Fig. 2 : Reachability distribution map of the Z1 robotic arm, where red voxels indicate high pose redundancy and blue voxels signify limited reachability.

Fig. 3 : The results of the initial configuration planning method based on dynamic programming and Dijkstra’s algorithm

Panel of Figure 4 — Fig. 4 : Process of converting discrete point sets into geometric feasible regions

Fig. 5 : Schematic of secondary configuration optimization

Metric	Method	T1	T2	T3	T4	T5
$L_{b}$ [m]	HRC	$\mathbf{10.84}$	4.56	8.19	$\mathbf{6.40}$	11.50
	Two-Stage	11.23	$\mathbf{3.93}$	$\mathbf{7.97}$	6.70	$\mathbf{10.90}$
$S_{b}$ [m -1 ]	HRC	$5.65\times 10^{-2}$	$9.18\times 10^{-2}$	$4.78\times 10^{-2}$	$6.03\times 10^{-2}$	$5.49\times 10^{-2}$
	Two-Stage	$\mathbf{1.12\times 10^{-2}}$	$\mathbf{4.32\times 10^{-4}}$	$\mathbf{3.26\times 10^{-3}}$	$\mathbf{2.48\times 10^{-2}}$	$\mathbf{3.03\times 10^{-3}}$
$E_{ee}$ [mm]	HRC	0.643	6.193	7.785	6.780	14.734
	Two-Stage	$\mathbf{0.0014}$	$\mathbf{0.0017}$	$\mathbf{0.0012}$	$\mathbf{0.0014}$	$\mathbf{0.0019}$

TABLE I : Detailed Performance Metrics Across Five Test Trajectories

Panel of Figure 7 — Fig. 6 : Comparison of trajectory tracking performance between HRC and the proposed method.

Method	T1	T2	T3	T4	T5
CHOMP	0.9017	1.3804	8.6882	1.4459	0.6243
Two-Stage	$\mathbf{0.0014}$	$\mathbf{0.0012}$	$\mathbf{0.0001}$	$\mathbf{0.0016}$	$\mathbf{0.0008}$

TABLE II : End-Effector Position RMSE Comparison (mm)

Marker ID	1	2	3	4	Mean
$x$ (m)	0.62175	0.62460	0.62258	0.62225	0.62280
$y$ (m)	0.04963	0.03952	0.04194	0.04059	0.04292

TABLE III : Center Coordinates of the Fitted Circular Trajectories

Panel of Figure 10 — Fig. 7 : Kinematic calibration of the mobile manipulator system.

Panel of Figure 11 — Fig. 8 : Tracking performance for the lemniscate path.

Panel of Figure 12 — Fig. 9 : Tracking performance for the capsule path.

Panel of Figure 13 — Fig. 10 : Tracking performance for the polygonal path.

Trajectory	Dimensions (m)	Max Error (mm)	Mean Error (mm)	Error Location
Lemniscate	(1.2, 3.0, 0.2)	91.82	23.26	High curvature
Capsule-shaped	(2.5, 1.0, 0)	83.72	21.17	Curve transitions
Polygonal	(3.0, 1.5, 0.2)	80.65	20.03	Corners

TABLE IV : Tracking Accuracy for Different Trajectories

Panel of Figure 15 — Fig. 11 : Time-lapse snapshots of the lemniscate trajectory-tracking experiment

Robot Motion Planning

Robot motion planning finds safe, feasible paths and trajectories for robots moving through constrained, changing environments.

See all 271 papers

Smooth Feedback Motion Planning with Reduced Curvature
Apr 2, 2026Aref Amiri, Steven M. LaValleMotion Planning Safe Motion Planning
The authors are with the Faculty of Information Technology and Electrical Engineering, University of Oulu, Oulu, Finland
DRAFTO: Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization for Robotic Manipulators
Mar 10, 2026Yichang Feng, Xiao Liang, Minghui ZhengTrajectory Optimization Robotic Optimization
Yichang Feng and Minghui Zheng are with the J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA · Xiao Liang is with the Zachry Department of Civil and Environmental Engineering, Texas A&M University, College Station, TX 77843 USA
Conflict-Based Lazy Search for Fast Multi-Manipulator Planning
Jul 5, 2026Dongliang Zheng, Zhipeng Wang, Siqi Wang +4Multi-Robot Motion Planning Conflict-Based Search
Shanghai Research Institute for Intelligent Autonomous Systems, Shanghai Institute of Intelligent Science and Technology, the State Key Laboratory of Autonomous Intelligent Unmanned Systems, and Frontiers Science Center for Intelligent Autonomous Systems of Ministry of Education, Tongji University, Shanghai 201203, China · Department of Automation, Shanghai Jiao Tong University, Shanghai 200240, China · School of Aerospace Engineering and Institute for Robotics and Intelligent Machines, Georgia Institute of Technology, Atlanta, GA 30332, USA

Apr 2, 2026J/K move · Enter open · S save

Smooth Feedback Motion Planning with Reduced Curvature

Aref Amiri, Steven M. LaValle

The authors are with the Faculty of Information Technology and Electrical Engineering, University of Oulu, Oulu, Finland

Motion Planning Safe Motion Planning

Open paper AskPDF arXiv

Feedback motion planning over cell decompositions provides a robust method for generating collision-free robot motion with formal guarantees. However, existing algorithms often produce paths with unnecessary bending, leading to slower motion and higher control effort. This paper presents a computationally efficient method to mitigate this issue for a given simplicial decomposition. A heuristic is introduced that systematically aligns and assigns local vector fields to produce more direct trajectories, complemented by a novel geometric algorithm that constructs a maximal star-shaped chain of simplexes around the goal. This creates a large ``funnel'' in which an optimal, direct-to-goal control law can be safely applied. Simulations demonstrate that our method generates measurably more direct paths, reducing total bending by an average of 91.40% and LQR control effort by an average of 45.47%. Furthermore, comparative analysis against sampling-based and optimization-based planners confirms the time efficacy and robustness of our approach. While the proposed algorithms work over any finite-dimensional simplicial complex embedded in the collision-free subset of the configuration space, the practical application focuses on low-dimensional (

d\le3

) configuration spaces, where simplicial decomposition is computationally tractable.

Abstract

Robot Motion Planning

Explore similar work

Smooth Feedback Motion Planning with Reduced Curvature

DRAFTO: Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization for Robotic Manipulators

Conflict-Based Lazy Search for Fast Multi-Manipulator Planning