Diagnosis-Driven Stage-Wise Trajectory Planning for a Mobile Dual-Arm Robot in Confined Derrick Pin Disassembly

Abstract

Derrick pin disassembly in drilling structures is a typical confined-space maintenance task that requires dual-arm cooperation, multi-stage motion transition, and strict safety control. Although a fixed workstation may satisfy pose-level reachability and collision-free requirements, executable full-process motion generation remains difficult because local safety-margin degradation, inter-arm interference, and blocked stage transitions may still occur during continuous planning. To address this problem, this paper proposes a diagnosis-driven stage-wise trajectory planning method for a mobile dual-arm robotic system operating under a fixed workstation input. The method first formulates the task as an ordered sequence of stage targets under unified safety constraints. A trajectory-risk diagnosis mechanism is then introduced to identify the minimum safety margin, bottleneck location, and dominant constraint relation of each stage trajectory. Based on the diagnosis result, a cascade repair framework is constructed by integrating local segment repair, endpoint-neighborhood repair, and global recovery with adaptive step size. After the geometric trajectory has been repaired and verified, piecewise quintic polynomial time parameterization is applied to generate smoother executable output. The proposed method is intended to bridge the gap between pose-level feasibility and full-process executability in confined dual-arm derrick operations. It provides a practical planning framework for converting fixed-workstation feasible results into continuous and task-consistent dual-arm motions under multiple geometric safety constraints.