Integrated safe trajectory planning and control for high relative degree nonlinear systems: a first-order cbf approach

This paper develops an integrated framework for safe trajectory planning and control of disturbed high relative degree nonlinear systems. The approach combines a composite disturbance-rejection controller with a safety-aware trajectory generator, which performs online re-planning using a safety filter. A disturbance observer estimates unknown external disturbances, and the resulting estimates are incorporated into the tracking control layer to enhance robustness against external disturbances. Implemented as a first-order dynamical system in the feedback loop, the trajectory generator enables enforcement of safety constraints through standard first-order control barrier functions (CBFs), avoiding the complexity and conservativeness of high-order CBF (HoCBF) designs. A CBF-based performance regulation mechanism adjusts the generator's output in response to tracking errors, coordinating safety and performance objectives. The resulting strategy ensures satisfaction of safety specifications for disturbed nonlinear systems with high relative degree while reducing conservativeness and enhancing disturbance rejection. Moreover, the framework can be readily extended to satisfy signal temporal logic (STL) specifications with minor modifications. Numerical simulations demonstrate the effectiveness of the proposed method.