Risk-Aware Adaptive Safety Margins for Model Predictive Control with Orientation–Motion Coupled Barrier Functions in Dynamic Environments

Abstract

Safe navigation in dynamic environments remains challenging because classical distance-based constraints ignore the coupling between a robot’s translational motion and attitude dynamics, and fixed safety margins are either over-conservative or risky under varying uncertainty and approach speed. This paper presents a Risk-Aware Model Predictive Control (RA-MPC) framework that addresses both limitations through two integrated components. First, we introduce Orientation–Motion Coupled Control Barrier Functions (O-MCBFs) that enforce unified safety constraints linking collision avoidance with attitude stability limits, preventing dangerous pose configurations during dynamic obstacle avoidance. Second, we develop Risk-Aware Adaptive Margins (RAAMs) that compute time-varying safety buffers based on relative velocity, robot braking capability, and prediction uncertainty, enabling context-dependent safety–efficiency trade-offs without manual parameter tuning. The proposed method integrates these components into a quadratic programming formulation within MPC, ensuring real-time computational tractability. Experimental results demonstrate higher success rates, smoother trajectories, and improved progress toward the goal, with no observed safety violations under the tested conditions. These findings indicate that coupling pose-space safety with risk-adaptive margins provides a principled and practical path to safe and efficient navigation in dynamic scenes.

1. Introduction

Mobile robot navigation is a key capability for logistics [1], service robotics [2], rescue [3], and autonomous driving [4]. Robots must operate safely and efficiently among humans and other moving agents [5]. Classical navigation pipelines, which combine global planning on quasi-static maps with local reactive controllers (e.g., potential fields [6], the Dynamic Window Approach [7], or sampling-based local planners [8]), perform reliably in static or mildly dynamic scenes. Their performance degrades when obstacles move intentionally and interactively. In such settings, navigation becomes a safety-critical optimal control problem: the system must plan motions that advance task objectives while providing provable collision avoidance and respecting actuation limits, attitude stability, and trajectory smoothness [9].

Nevertheless, safe navigation in dynamic environments poses three core challenges. First, distance-centric safety formulations that rely on point-to-obstacle metrics ignore the coupling between translational motion and attitude dynamics [10]. During high-speed evasive maneuvers, this omission can induce unsafe attitudes or push the system beyond actuation and stability limits [11]. A unified, verifiable pose-space safety representation is therefore needed, one that links collision proximity with attitude stability. Second, fixed safety margins do not adapt to situational risk (relative speed, available braking capability, prediction uncertainty). They are over-conservative in sparse scenes and insufficiently protective in high-risk ones, which motivates a risk-aware, context-adaptive margin mechanism [12]. Third, to achieve real-time solution and stable execution within MPC, safety constraints should admit differentiable, linearizable, or quadratically representable forms [13]. The modeling must thus render the problem amenable to efficient online solution by quadratic-programming solvers.

Existing research differs in focus. One line emphasizes interaction modeling and behavior prediction/learning, estimating multi-agent intent and adding social constraints to the cost to improve throughput [14,15,16]. A second line combines optimization-based control with formal safety (e.g., embedding control barrier functions [17] or reachability constraints in MPC [18]), which offers stronger verifiability. In parallel, for legged and hybrid robotic systems, recent stability- and safety-oriented learning/control studies primarily focus on execution-layer locomotion stability under contact and whole-body dynamics [19,20]. Our O-MCBF instead targets the navigation layer and provides a QP-compatible maneuver filter that is complementary to such learning-based locomotion guarantees. A third line adopts probabilistic or robust constraints and risk-based criteria to hedge perception and prediction errors, thereby calibrating risk to some extent [21,22]. Despite these advances, current approaches rarely satisfy two requirements simultaneously: (i) a verifiable safety characterization defined in pose space, and (ii) an online, risk-aware adaptation of the safety margin to the scene. This gap is most pronounced in crowded, rapidly changing environments and undermines applicability and transferability [23]. Moreover, fixed risk thresholds lack contextual elasticity: if set too strictly, they shrink the feasible set or lead to overly conservative behavior; if set too loosely, they erode the safety boundary in exchange for nominal feasibility and efficiency.

To overcome these limitations, we propose a risk-aware model predictive control (RA-MPC) framework that enables mobile robots to adapt their safety buffers to scene risk while maintaining fast progress, pose stability, and collision safety in dynamic environments, as demonstrated in Figure 1. The framework comprises two complementary components. First, the Orientation–Motion Coupled Control Barrier Function (O-MCBF) provides a verifiable pose-space safety formulation that links collision proximity with attitude-stability and actuation limits; we cast the associated barrier-derivative condition as quadratic inequalities within MPC, which prevents unsafe attitudes and excessive control actions during high-speed evasive maneuvers and promotes smoother trajectories. Second, the Risk-Aware Adaptive Margin (RAAM) computes a time-varying safety buffer from relative speed, available braking capability, and prediction uncertainty, replacing fixed thresholds with context-dependent margins that expand under high risk and contract in open, low-risk scenes. These components are integrated into a sparsity-exploiting quadratic program solved in real time, coordinating safety constraints with task progress [24]. The resulting controller supports pose-stable dynamic obstacle avoidance and risk-adaptive regulation of speed and clearance while preserving efficiency; we further establish safety and online solvability guarantees and demonstrate effectiveness through experimental results in crowded, rapidly changing environments.

Our contributions are as follows:

1.

We introduce an O-MCBF that unifies collision proximity, attitude stability, and actuation limits in a full-pose representation and cast the barrier-derivative condition as QP-compatible inequalities for MPC. Under standard regularity and feasible initialization, we establish forward invariance of the safe set.

2.

We propose an RAAM which computes a time-varying safety buffer from relative speed, available braking capability, and prediction uncertainty. We analyze monotonicity and boundedness properties and prove feasibility conditions for the barrier constraint together with their impact on the feasible set and conservativeness, thereby avoiding the over–under-conservatism induced by fixed thresholds.

3.

We structure the above safety conditions into a sparsity-exploiting QP without introducing non-differentiable or strongly nonconvex operators, enabling online coordination of task progress and safety constraints with consistent real-time performance.

4.

Experiments in crowded and rapidly changing environments show improvements over representative MPC/CBF baselines in success rate, trajectory smoothness, and goal-directed progress, with no observed safety violations under the tested conditions, demonstrating the effectiveness and robustness of the approach.

2. Related Work

In this section, we provide a detailed review of the related research content concerning mobile robot autonomous navigation in dynamic environments. Existing methods excel in aspects such as interactive prediction, formal safety, and risk handling, yet it is difficult to simultaneously achieve verifiable safety characterization in the pose space and scene-risk-adaptive safety margin design. Therefore, we structure our review around three main threads: interactive and prediction-driven navigation, optimal control and formal safety, and uncertainty and risk-aware safety margins.

2.1. Interaction- and Prediction-Aware Navigation

Research on navigation and collision avoidance in dynamic environments has progressively shifted from geometric and reactive methods to prediction- and interaction-aware paradigms. Representative initial approaches include velocity obstacle-based techniques and their reciprocal extensions for multi-agent real-time collision avoidance [25,26], as well as local motion generation methods that integrate Dynamic Window, Timed Elastic Band, sampling, or graph optimization [27,28]. Furthermore, the social navigation literature emphasizes collective etiquette and social constraints for safe movement in crowded spaces [29,30]. More recently, prediction-driven methods incorporating multi-modal trajectory prediction, intent estimation, and data-driven cost learning have significantly improved passability in dense, highly dynamic scenarios [31,32,33]. However, these approaches commonly measure safety using point-to-obstacle distance and heuristic buffering, often decoupling the agent’s full pose and execution boundaries. When relative speed is high or interaction intensity increases, these methods are prone to control instability, decision oscillation, or an imbalance between overly conservative and aggressive behaviors [34]. Critically, they lack verifiable safety guarantees. This highlights the necessity for a unified safety formulation across the entire pose space and certifiable safety properties, which serves as a key motivation for the methodology proposed in the subsequent sections.

2.2. Optimal Control and Formal Safety

Optimal control provides a unified modeling and online solution framework for safe navigation. MPC integrates objectives and constraints into a receding horizon optimization, addressing uncertainty through variants such as robust, tube-based, and chance-constrained formulations [35,36,37]. Conversely, CBF and Reachability Theory offer verifiable safety conditions based on the concepts of safety set invariance and forward invariance. A common practice is to impose barrier derivative inequalities within the optimization, enabling compatibility with QP [38,39]. Concurrently, approaches like Responsibility-Sensitive Safety (RSS) and related standards translate minimum longitudinal distance and braking capabilities into checkable rules [40]. However, existing practices typically construct barriers or safety domains using geometric distance and mass-point approximations. Crucially, pose stability and actuation limits are often decoupled from collision safety within a unified safety constraint. Furthermore, some constraint forms are computationally prohibitive for real-time solution [41]. Therefore, there remains a need for a pose-space and QP-friendly safety expression that can be executed in real time within an MPC framework while providing forward invariance guarantees, which aligns with the objectives of our proposed O-MCBF and real-time integration approach.

2.3. Uncertainty and Risk-Aware Safety Margins

To address sensory noise, prediction errors, and partial observability, researchers have proposed methods based on chance constraints, distributionally robust optimization, and risk measures (such as CVaR) to provide probabilistic or worst-case safety bounds [42,43]. In engineering practice, heuristic safety radii based on Time-to-Collision (TTC), braking distance, or relative velocity are also commonly used for computational simplification [44,45]. While the former possesses clear statistical or robust guarantees, their computational cost and dependence on model assumptions can restrict real-time performance and generalization. Conversely, the latter is efficient and easy to implement but struggles to adaptively adjust to changes in relative speed, braking capability, and predictive uncertainty, resulting in a conservative/aggressive imbalance between sparse and high-risk scenarios [46]. Consequently, there is an urgent need for an interpretable and online-adjustable risk-aware safety margin that is consistent with optimization or barrier constraints, enabling a controllable trade-off between safety and efficiency across diverse dynamic and uncertain scenarios.

3. Preliminaries and Problem Statement

This section reviews preliminaries of CBF and risk-aware safety specifications. Our problem statement is also provided.

3.1. Control Barrier Function Preliminaries

CBFs provide a formal, real-time framework for synthesizing safety-critical controllers by guaranteeing the forward invariance of a desired safe set for nonlinear control systems.

We consider a general time-invariant, control-affine system with states $x\in \mathcal{X}\subset {\mathbb{R}}^n$ and compact admissible control inputs $u\in \mathcal{U}\subset {\mathbb{R}}^m$:

$$\dot{x}=f\left(x\right)+g\left(x\right)u,x\in \mathcal{X},u\in \mathcal{U},$$

(1)

where $f:\mathcal{X}\to {\mathbb{R}}^n$ is the drift vector field and $g:\mathcal{X}\to {\mathbb{R}}^{n\times m}$ is the control input matrix, both assumed to be locally Lipschitz continuous.

Safety is characterized by a continuously differentiable function $h:\mathcal{X}\to \mathbb{R}$. The corresponding safe set $\mathcal{S}$ is defined by the zero superlevel set of h:

$$\mathcal{S}=\{x\in \mathcal{X}\mid h(x)\ge 0\}.$$

(2)

The set $\mathcal{S}$ is defined as forward invariant if every solution $x\left(t\right)$ of (1) initiated in $\mathcal{S}$ remains in $\mathcal{S}$ for all subsequent time $t\ge 0$.

A continuously differentiable function h is a CBF if there exists an extended class-$\mathcal{K}$ function $\alpha :\mathbb{R}\to \mathbb{R}$ such that the following condition holds for all states $x\in \mathcal{S}$:

$$\underset{u\in \mathcal{U}}{\sup }\left\{L_{f}h\left(x\right)+L_{g}h\left(x\right)u+\alpha \left(h\left(x\right)\right)\right\}\ge 0\mathrm{for}\mathrm{all}x\mathrm{with}h\left(x\right)\ge 0.$$

(3)

The terms $L_{f}h\left(x\right)=\nabla h\left(x\right)f\left(x\right)$ and $L_{g}h\left(x\right)=\nabla h\left(x\right)g\left(x\right)$ denote the Lie derivatives of h along f and g, respectively. This condition explicitly guarantees the existence of an admissible control input $u\in \mathcal{U}$ that locally prevents the trajectory from exiting the safe set $\mathcal{S}$.

Furthermore, any (locally Lipschitz) feedback control law $u\left(x\right)$ satisfying the resultant CBF constraint

$$L_{f}h\left(x\right)+L_{g}h\left(x\right)u+\alpha \left(h\left(x\right)\right)\ge 0$$

(4)

renders the set $\mathcal{S}$ forward invariant. For a system subject to multiple safety requirements, defined by $h_j$, $j=1,\dots ,J$, enforcing (4) for each $h_j$ concurrently guarantees the forward invariance of the composite safe set $\mathcal{S}={\bigcap }_{j=1}^J\{x\mid h_j\left(x\right)\ge 0\}$.

3.2. Risk-Aware Safety Specification

To address safety in dynamic environments with prediction uncertainty, we formulate a risk-aware safety specification using a time-varying CBF. The robot operates within a Euclidean workspace $\mathcal{W}\subset {\mathbb{R}}^d$ ($d\in \{2,3\}$), navigating among M moving agents (obstacles). Let $p\left(x\right)\in {\mathbb{R}}^d$ be the robot’s position derived from its state x. The predicted position of agent i at time t, obtained from a perception and prediction pipeline, is denoted by ${\widehat{o}}_i\left(t\right)\in {\mathbb{R}}^d$. Assuming collision geometry is conservatively bounded by effective radii $r_{\mathrm{rob}}$ and $r_i$, the signed clearance from agent i is defined as:

$$d_i(x,t)=\parallel p\left(x\right)-{\widehat{o}}_i\left(t\right)\parallel -\left(r_{\mathrm{rob}}+r_i\right).$$

(5)

Contextual risk is integrated through a dynamic safety margin. Let $v_{\mathrm{rel},i}\left(t\right)=\parallel \dot{p}\left(x\right)-{\widehat{v}}_i\left(t\right)\parallel$ be the relative speed magnitude and ${\mathsf{\Sigma }}_i\left(t\right)⪰0$ summarize the state prediction uncertainty (e.g., covariance) associated with agent i. We introduce a bounded, measurable risk margin ${\rho }_i\left(t\right)\in [0,\overline{\rho }]$ that dynamically adjusts the required separation distance based on the perceived risk level:

$${\rho }_i\left(t\right)={\varphi }_i\left(v_{\mathrm{rel},i}\left(t\right),{\mathsf{\Sigma }}_i\left(t\right)\right),\partial {\varphi }_i/\partial v_{\mathrm{rel}}\ge 0,\partial {\varphi }_i/\partial \mathsf{\Sigma }⪰0.$$

(6)

The monotonic properties of the function ${\varphi }_i$ ensure that both higher relative speed and increased prediction uncertainty necessitate a larger safety buffer ${\rho }_i\left(t\right)$, thereby increasing control conservatism.

The overall safety specification is composed of two classes of barrier functions. The first is the Risk-Aware Distance CBF ($h_i^{\mathrm{dist}}$), a time-varying function that integrates the signed clearance $d_i(x,t)$ with the contextual risk margin ${\rho }_i\left(t\right)$ to enforce dynamic, risk-adjusted separation from M moving agents. The second is the Actuation/Attitude CBF ($h^{\mathrm{att}}$), a time-invariant function ensuring the robot’s state remains within intrinsic physical and operational limits (e.g., bounds on angular rates/accelerations or tilt). These safety functions are formally defined as:

$$h_i^{\mathrm{dist}}(x,t)=d_i(x,t)-{\rho }_i\left(t\right),i=1,\dots ,M,h^{\mathrm{att}}\left(x\right)\ge 0.$$

(7)

The resulting time-varying pose-aware safe set $\mathcal{S}\left(t\right)$ is defined as the intersection of all constrained sets:

$$\mathcal{S}\left(t\right)=\left\{x\in \mathcal{X}|h_i^{\mathrm{dist}}(x,t)\ge 0\forall i,h^{\mathrm{att}}\left(x\right)\ge 0\right\}.$$

(8)

3.3. Problem Statement

The core objective is to synthesize a control policy that guarantees both safety and task completion for the control-affine system (1) operating in the dynamic environment defined by the safe set $\mathcal{S}\left(t\right)$.

Let ${\mathcal{E}}_0\subseteq \mathcal{S}\left(0\right)$ be the set of admissible initial conditions and $\mathcal{G}\subset \mathcal{X}$ be a predefined goal set (e.g., a small ball centered at a desired target position). The objective is to design a (possibly time–varying) feedback control law $u:\mathcal{X}\times {\mathbb{R}}_{\ge 0}\to \mathcal{U}$ such that for any initial state $x\left(0\right)\in {\mathcal{E}}_0$, the resulting closed-loop trajectory $x\left(t\right)$ of system (1) satisfies two primary requirements.

The first, safety, demands that the trajectory must guarantee the forward invariance of the safe set, $x\left(t\right)\in \mathcal{S}\left(t\right)$ for all $t\ge 0$. This is formally enforced by requiring the control input u to satisfy the time-varying CBF constraint for all $h_j\in \{h_1^{\mathrm{dist}},\dots ,h_M^{\mathrm{dist}},h^{\mathrm{att}}\}$:

$$L_f{h}_j(x,t)+L_g{h}_j(x,t)u+{\displaystyle \frac{\partial h_j}{\partial t}}+\alpha \left(h_j(x,t)\right)\ge 0.$$

(9)

The second, goal attainment, requires that the trajectory reaches the goal set $\mathcal{G}$ in finite time or asymptotically, while respecting the intrinsic state constraints $x\left(t\right)\in \mathcal{X}$ and control input constraints $u\left(t\right)\in \mathcal{U}$ throughout the operation. Thus, the problem is to find a control policy $u^{\in }\mathcal{U}$ that solves the synthesis problem:

$$u={arg\; min}_{u\in {\mathcal{U}}_{\mathrm{safe}}(x,t)}J\left(u\right),$$

(10)

where ${\mathcal{U}}_{\mathrm{safe}}(x,t)$ is the set of admissible control inputs satisfying (9), and $J\left(u\right)$ is a functional used to prioritize goal attainment or minimize a performance metric (e.g., tracking error and smoothness) among the safe solutions. The safety requirements defined by (9) are strictly primary.

4. Methodology

This section develops a risk-aware predictive controller that addresses the two core limitations identified earlier: (i) classical distance-only constraints ignore the coupling between translational motion and attitude dynamics; and (ii) fixed safety buffers are either overly conservative or unsafe under varying uncertainty and approach speed. We therefore encode pose-space safety with an O-MCBF and adapt the safety buffer online via an RAAM. Both are embedded into a receding-horizon quadratic program to achieve real-time, certifiably safe navigation in dynamic scenes. The structure is shown in Figure 2.

4.1. Overview

The proposed navigation framework integrates perception, planning, and control components to achieve safe autonomous navigation in dynamic environments. As illustrated in Figure 2, the system processes multi-modal sensor inputs through a comprehensive pipeline that combines obstacle detection, motion prediction, path planning, and trajectory optimization. The framework operates in a closed-loop fashion, continuously updating control commands based on real-time environmental feedback and state estimation.

Perception and Obstacle Detection: The perception stack utilizes an Intel RealSense D435i RGB-D camera to capture depth images and extract dynamic obstacles in the robot’s local environment [47]. Raw depth data undergo spatial filtering and noise reduction to improve measurement quality [48]. Obstacle detection employs gradient-based processing where image gradients compute local depth discontinuities to enhance obstacle boundaries, followed by density-based spatial clustering (DBSCAN) to segment obstacle points into individual clusters [49]. Each obstacle cluster is modeled as a parameterized ellipse using minimum enclosing ellipse fitting, characterized by semi-major axis, semi-minor axis, center coordinates, and orientation angle.

Multi-Target Tracking and Motion Prediction: Dynamic obstacle tracking employs a Kuhn–Munkres algorithm for data association across consecutive time steps, maintaining obstacle identities through optimal assignment based on spatial proximity [50]. Each tracked obstacle utilizes a Kalman filter to estimate motion states and predict future positions over the planning horizon. The prediction framework quantifies motion uncertainty through covariance estimation, providing probabilistic forecasts for collision avoidance [51].

Global Path Planning: The global planning layer employs a Potential Field-enhanced RRT* (PF-RRT*) to generate a long-horizon reference trajectory connecting the start and goal [52]. PF-RRT* accelerates convergence by using potential-field guidance during tree expansion while retaining the probabilistic completeness of RRT*. In our hierarchical design, this global trajectory serves as an explicit guidance signal for the local RA-MPC (e.g., via waypoint/segment tracking and goal-progress objectives), which reduces the tendency of finite-horizon local optimization to exhibit globally inconsistent behaviors in cluttered environments. We note that under strong scene dynamics the global reference may become suboptimal or temporarily infeasible; the system therefore relies on closed-loop replanning to continuously correct the local motion, and refreshing the global reference can be used when necessary to maintain global consistency.

Robustness to Uncertainties: In practice, performance and safety may be affected by model mismatch/disturbances, sensing errors, and perception latency; our framework mitigates these issues in a receding-horizon manner as follows: (i) model mismatch is bounded by closed-loop replanning, since the controller continuously updates commands using real-time environmental feedback and state estimation rather than relying on long open-loop rollouts; (ii) sensing and prediction errors are explicitly represented through the multi-target tracking module, where filtered RGB-D measurements are used for obstacle detection and Kalman-filter-based prediction provides both future states and covariance, and this uncertainty directly enters the RAAM (Equation (17)) so that larger noise/less confident predictions automatically enlarge the adaptive safety margin, yielding more conservative but safer behaviors; (iii) perception latency is handled implicitly by the same uncertainty-aware mechanism and real-time execution, as the overall algorithm is implemented within the 50 ms control-cycle budget, and any residual delay can be regarded as additional prediction uncertainty absorbed by the covariance-driven margin adaptation. We further note that our simulation includes realistic RGB-D noise models (e.g., depth measurement uncertainty $\sigma =0.03$ m and limited FoV), and the real-world trials are conducted under genuine sensor noise and dynamic uncertainties, in which zero collision incidents are observed in the reported experiments.

The complete pipeline operates at real-time frequencies to ensure responsive navigation in dynamic scenarios. Sensor measurements feed the perception system, which outputs structured obstacle representations and motion predictions. These combine with the global reference to formulate the local optimization problem, generating control commands for actuator-level execution. This modular architecture enables systematic analysis while maintaining integrated system performance.

4.2. Orientation–Motion Coupled CBF (O-MCBF)

Traditional CBFs in mobile robotics primarily enforce collision avoidance through position-based safety constraints, neglecting the coupling between translational motion and attitude dynamics. This limitation becomes critical during high-speed maneuvers where rapid orientation changes can compromise vehicle stability and motion smoothness. We address this gap by proposing an O-MCBF that simultaneously constrains collision safety and attitude stability within a unified framework.

To handle the coupling between position and orientation constraints without singularities, we employ dual quaternion representation for the robot’s pose. A dual quaternion $\mathbf{q}\in {\mathbb{R}}^8$ encodes both position $\mathbf{p}\in {\mathbb{R}}^3$ and orientation $\mathbf{r}\in SO\left(3\right)$ in a compact, singularity-free manner:

$$\mathbf{q}=\mathbf{r}+\epsilon {\displaystyle \frac{1}{2}}\mathbf{p}\mathbf{r}$$

(11)

where $\mathbf{r}$ is the unit quaternion representing orientation, $\mathbf{p}$ is the position quaternion ${[0,{\mathbf{p}}^T]}^T$, and $\epsilon$ is the dual unit satisfying ${\epsilon }^2=0$, $\epsilon \ne 0$.

The O-MCBF integrates collision avoidance and stability constraints through a composite barrier function that considers both translational safety margins and rotational stability limits:

$$h_{\mathrm{O}-\mathrm{MCBF}}(\mathbf{x},\mathit{\omega })=\min \{h_{\mathrm{collision}}\left(\mathbf{x}\right),h_{\mathrm{stability}}\left(\mathit{\omega }\right)\}$$

(12)

The collision safety component $h_{\mathrm{collision}}\left(\mathbf{x}\right)$ maintains traditional distance-based constraints:

$$h_{\mathrm{collision}}\left(\mathbf{x}\right)={\parallel \mathbf{x}-{\mathbf{x}}_{\mathrm{obs}}\parallel }^2-{(r_{\mathrm{safe}}+r_{\mathrm{obs}})}^2$$

(13)

where $\mathbf{x}$ is the robot position, ${\mathbf{x}}_{\mathrm{obs}}$ is the obstacle center, and $r_{\mathrm{safe}}$ and $r_{\mathrm{obs}}$ are the safety and obstacle radii, respectively.

The stability component $h_{\mathrm{stability}}\left(\mathit{\omega }\right)$ constrains angular velocity and acceleration to prevent destabilizing maneuvers:

$$h_{\mathrm{stability}}\left(\mathit{\omega }\right)={\mathit{\omega }}_{\max }^2-{\parallel \mathit{\omega }\parallel }^2-{\alpha }_{\mathrm{stab}}{\parallel \dot{\mathit{\omega }}\parallel }^2$$

(14)

where ${\omega }_{\max }$ is the maximum allowable angular velocity, $\mathit{\omega }$ is the current angular velocity, $\dot{\mathit{\omega }}$ is the angular acceleration, and ${\alpha }_{\mathrm{stab}}>0$ is a stability weighting parameter.

The O-MCBF constraint is incorporated into the MPC optimization as a time-varying inequality constraint. For discrete-time systems with sampling period T, the barrier function constraint becomes:

$$h_{\mathrm{O}-\mathrm{MCBF}}({\mathbf{x}}_{k+1},{\mathit{\omega }}_{k+1})\ge (1-{\gamma }_k)h_{\mathrm{O}-\mathrm{MCBF}}({\mathbf{x}}_k,{\mathit{\omega }}_k)$$

(15)

where ${\gamma }_k\in (0,1)$ is the relaxation parameter that controls the convergence rate to the safe set.

The coupled nature of the constraint ensures that high angular velocities automatically reduce the allowable proximity to obstacles, and conversely, close obstacle proximity limits the permissible rotational dynamics. This coupling is achieved through the minimum operation in (12), which activates the more restrictive constraint at each time instant. For continuous-time implementation, the time derivative of the O-MCBF along system trajectories must satisfy:

$${\dot{h}}_{\mathrm{O}-\mathrm{MCBF}}=\nabla h_{\mathrm{O}-\mathrm{MCBF}}^{T}f(\mathbf{x},\mathit{\omega })+\nabla h_{\mathrm{O}-\mathrm{MCBF}}^{T}g(\mathbf{x},\mathit{\omega })\mathbf{u}\ge -\alpha h_{\mathrm{O}-\mathrm{MCBF}}$$

(16)

where $f(·)$ and $g(·)$ are the drift and control input matrices of the nonlinear system, $\mathbf{u}$ is the control input, and $\alpha >0$ is a class-$\mathcal{K}$ function parameter.

Although the prediction model in the MPC is planar and kinematics-based for real-time efficiency, the proposed O-MCBF provides a conservative proxy for dynamic stability by explicitly bounding the angular velocity $\mathit{\omega }$ and angular acceleration $\dot{\mathit{\omega }}$ (cf. (14)), thereby restricting the planned motion to a quasi-static stability envelope and avoiding aggressive maneuvers (e.g., sharp turns at high speed) that may violate friction limits or induce rollover. The resulting kinematically feasible reference is then tracked by the low-level whole-body controller (based on residual policy optimization), which handles platform-specific actuation and dynamics during execution. Therefore, the current instantiation is best suited to ground platforms where planar body commands are meaningful and trackable (e.g., wheeled, wheel-legged, and tracked robots in moderate-slip regimes), and our experiments were conducted on a wheel-legged platform with nontrivial limits (e.g., $v_{\max }=2.0$ m/s, $a_{\max }=3.0$ m/s², ${\omega }_{\max }=6.0$ rad/s). For substantially higher-dynamic systems (e.g., high-speed vehicles where slip, lateral dynamics, and load transfer dominate), the prediction model should be upgraded to a dynamics-aware model and the stability constraints should be adapted accordingly; for legged or humanoid robots, the same optimization structure can be retained but the stability proxy must be reformulated to account for contact switching and whole-body dynamics (e.g., centroidal/ZMP/Capture-Point and contact-feasibility constraints).

4.3. Risk-Aware Adaptive Margin (RAAM)

Fixed safety margins in traditional CBF formulations present a fundamental trade-off between safety and efficiency: conservative margins ensure collision avoidance but lead to overly cautious behavior in open spaces, while aggressive margins improve efficiency but may compromise safety during high-speed encounters or under significant prediction uncertainty. To address this limitation, we developed a Risk-Aware Adaptive Margin (RAAM) module that dynamically adjusts safety buffers based on real-time risk assessment, enabling context-dependent safety–efficiency balance within the MPC optimization.

The RAAM module computes an adaptive safety margin $\gamma \left(t\right)$ that replaces the fixed buffer $r_{\mathrm{safe}}$ in the collision barrier function:

$$\gamma \left(t\right)={\gamma }_{\mathrm{base}}+{\gamma }_{\mathrm{vel}}\left({\mathbf{v}}_{\mathrm{rel}}\right)+{\gamma }_{\mathrm{brake}}\left({\mathbf{a}}_{\max }\right)+{\gamma }_{\mathrm{uncert}}\left(\mathsf{\Sigma }\right)$$

(17)

where ${\gamma }_{\mathrm{base}}$ is the minimum baseline safety margin, and the additional terms account for relative motion, braking capability, and prediction uncertainty, respectively.

The velocity-dependent component scales with the relative approach speed:

$${\gamma }_{\mathrm{vel}}\left({\mathbf{v}}_{\mathrm{rel}}\right)=k_v{\displaystyle \frac{\parallel {\mathbf{v}}_{\mathrm{rel}}{\parallel }^2}{2\mu g+a_{\max }}}$$

(18)

where ${\mathbf{v}}_{\mathrm{rel}}={\mathbf{v}}_{\mathrm{robot}}-{\mathbf{v}}_{\mathrm{obs}}$ is the relative velocity vector, $k_v>0$ is a scaling factor, $\mu$ is the friction coefficient, g is gravitational acceleration, and $a_{\max }$ is the maximum deceleration capability.

The braking-aware component accounts for the robot’s stopping distance under emergency conditions:

$${\gamma }_{\mathrm{brake}}\left({\mathbf{a}}_{\max }\right)=k_b{\displaystyle \frac{\parallel {\mathbf{v}}_{\mathrm{robot}}{\parallel }^2}{2a_{\max }}}·exp\left(-{\displaystyle \frac{a_{\max }}{a_{\mathrm{nominal}}}}\right)$$

(19)

where $k_b>0$ is a braking margin factor, and $a_{\mathrm{nominal}}$ represents the nominal deceleration limit.

The uncertainty component adapts to prediction confidence levels:

$${\gamma }_{\mathrm{uncert}}\left(\mathsf{\Sigma }\right)=k_u\sqrt{\mathrm{tr}\left({\mathsf{\Sigma }}_{\mathrm{pos}}\right)}·\left(1+{\displaystyle \frac{\Delta t_{\mathrm{pred}}}{T_{\mathrm{horizon}}}}\right)$$

(20)

where $k_u>0$ is an uncertainty scaling factor, ${\mathsf{\Sigma }}_{\mathrm{pos}}$ is the positional covariance submatrix from obstacle tracking, $\Delta t_{\mathrm{pred}}$ is the prediction time, and $T_{\mathrm{horizon}}$ is the planning horizon.

The adaptive margin computation integrates multiple risk factors through continuous evaluation of kinematic risk level $R_{\mathrm{kin}}\left(t\right)$:

$$R_{\mathrm{kin}}\left(t\right)=w_1{\displaystyle \frac{\parallel {\mathbf{v}}_{\mathrm{rel}}\parallel }{\parallel {\mathbf{v}}_{\mathrm{rel}}{\parallel }_{\max }}}+w_2{\displaystyle \frac{{\sigma }_{\mathrm{pred}}}{{\sigma }_{\max }}}+w_3{\displaystyle \frac{1}{d_{\mathrm{obs}}/d_{\min }}}$$

(21)

where $w_i>0$ are weighting factors, ${\sigma }_{\mathrm{pred}}$ quantifies prediction uncertainty, and $d_{\mathrm{obs}}$ is the current obstacle distance.

The time-varying safety margin modifies the collision barrier function constraint:

$$h_{\mathrm{adaptive}}(\mathbf{x},t)={\parallel \mathbf{x}-{\mathbf{x}}_{\mathrm{obs}}\parallel }^2-{(\gamma \left(t\right)+r_{\mathrm{obs}})}^2\ge 0$$

(22)

To ensure numerical stability within the MPC optimization, the margin update rate is bounded:

$$\dot{\gamma }\left(t\right)=\mathrm{sat}\left({\displaystyle \frac{{\gamma }_{\mathrm{target}}\left(t\right)-\gamma \left(t\right)}{{\tau }_{\mathrm{adapt}}}},{\dot{\gamma }}_{\max }\right)$$

(23)

where ${\gamma }_{\mathrm{target}}\left(t\right)$ is computed from (17), ${\tau }_{\mathrm{adapt}}>0$ is the adaptation time constant, and ${\dot{\gamma }}_{\max }$ limits the maximum margin change rate.

The RAAM module maintains safety guarantees by ensuring $\gamma \left(t\right)\ge {\gamma }_{\mathrm{base}}>0$ at all times while enabling efficient navigation through adaptive margin adjustment based on instantaneous risk conditions. (Figure 3).

4.4. RA-MPC via QP Synthesis

The proposed RA-MPC controller integrates the O-MCBF and RAAM modules within a receding-horizon optimization scheme, formulating trajectory generation as a constrained QP that can be solved efficiently in real time. The controller balances trajectory tracking, control smoothness, and safety requirements while adapting to dynamic risk conditions through the unified optimization formulation.

Consider a discrete-time nonlinear system with state ${\mathbf{x}}_k\in {\mathbb{R}}^{n_x}$ and control input ${\mathbf{u}}_k\in {\mathbb{R}}^{n_u}$:

$${\mathbf{x}}_{k+1}=f({\mathbf{x}}_k,{\mathbf{u}}_k)+{\mathbf{w}}_k$$

(24)

where $f(·)$ represents the system dynamics and ${\mathbf{w}}_k$ accounts for model uncertainties and disturbances.

For mobile robot navigation, the state vector comprises position, orientation, and velocities, ${\mathbf{x}}_k={[x_k,y_k,{\theta }_k,v_k,{\omega }_k]}^T$, while the control input consists of linear and angular acceleration commands, ${\mathbf{u}}_k={[a_{v,k},a_{\omega ,k}]}^T$. The RA-MPC optimization problem over prediction horizon N is formulated as:

$$\begin{array}{cc}\hfill \underset{{\mathbf{U}}_N}{\min }& \sum _{i=0}^{N-1}\left[\parallel {\mathbf{x}}_{k+i|k}-{\mathbf{x}}_{\mathrm{ref},k+i}{\parallel }_{Q_i}^2+{\parallel {\mathbf{u}}_{k+i|k}\parallel }_{R_i}^2\right]\hfill \\ \hfill & +\parallel {\mathbf{x}}_{k+N|k}-{\mathbf{x}}_{\mathrm{ref},k+N}{\parallel }_P^2\hfill \end{array}$$

(25)

subject to:

$$\begin{array}{cc}\hfill {\mathbf{x}}_{k+i+1|k}& =f({\mathbf{x}}_{k+i|k},{\mathbf{u}}_{k+i|k}),i=0,\dots ,N-1\hfill \\ \hfill {\mathbf{u}}_{\min }& \le {\mathbf{u}}_{k+i|k}\le {\mathbf{u}}_{\max },i=0,\dots ,N-1\hfill \\ \hfill {\mathbf{x}}_{\min }& \le {\mathbf{x}}_{k+i|k}\le {\mathbf{x}}_{\max },i=0,\dots ,N-1\hfill \\ \hfill h_{\mathrm{O}-\mathrm{MCBF}}({\mathbf{x}}_{k+i+1|k},{\mathit{\omega }}_{k+i+1|k})& \ge (1-{\gamma }_k)h_{\mathrm{O}-\mathrm{MCBF}}({\mathbf{x}}_{k+i|k},{\mathit{\omega }}_{k+i|k})\hfill \\ \hfill h_{\mathrm{adaptive}}({\mathbf{x}}_{k+i|k},t_{k+i})& \ge 0,j\in {\mathcal{O}}_k\hfill \end{array}$$

(26)

where ${\mathbf{U}}_N={[{\mathbf{u}}_{k|k},\dots ,{\mathbf{u}}_{k+N-1|k}]}^T$ is the control sequence, $Q_i\succ 0$, $R_i\succ 0$, and $P\succ 0$ are weighting matrices, and ${\mathcal{O}}_k$ denotes the set of detected obstacles at time k.

To enable real-time computation, the nonlinear optimization problem is approximated as a QP through successive linearization around the previous solution. The system dynamics are linearized as:

$${\mathbf{x}}_{k+i+1|k}\approx {\mathbf{A}}_i{\mathbf{x}}_{k+i|k}+{\mathbf{B}}_i{\mathbf{u}}_{k+i|k}+{\mathbf{c}}_i$$

(27)

where ${\mathbf{A}}_i={\displaystyle \frac{\partial f}{\partial \mathbf{x}}}{|}_{{\mathbf{x}}_{k+i}^{*},{\mathbf{u}}_{k+i}^{*}}$, ${\mathbf{B}}_i={\displaystyle \frac{\partial f}{\partial \mathbf{u}}}{|}_{{\mathbf{x}}_{k+i}^{*},{\mathbf{u}}_{k+i}^{*}}$, and ${\mathbf{c}}_i$ accounts for the linearization residual.

The O-MCBF constraint is linearized around the nominal trajectory:

$$\begin{array}{cc}\hfill & \nabla h_{\mathrm{O}-\mathrm{MCBF}}^T({\mathbf{x}}_{k+i+1}^{*},{\mathit{\omega }}_{k+i+1}^{*}){\mathbf{x}}_{k+i+1|k}\hfill \\ \hfill & -(1-{\gamma }_k)\nabla h_{\mathrm{O}-\mathrm{MCBF}}^T({\mathbf{x}}_{k+i}^{*},{\mathit{\omega }}_{k+i}^{*}){\mathbf{x}}_{k+i|k}\ge {\delta }_{h,i}\hfill \end{array}$$

(28)

where ${\delta }_{h,i}$ compensates for linearization errors and ensures constraint satisfaction.

Similarly, the adaptive collision constraints from the RAAM are linearized:

$$\nabla h_{\mathrm{adaptive}}^T({\mathbf{x}}_{k+i}^{*},t_{k+i}){\mathbf{x}}_{k+i|k}\ge {\delta }_{\mathrm{adapt},i}$$

(29)

To maintain real-time performance, the optimizer employs warm-starting by initializing each QP with the shifted solution from the previous time step:

$${\mathbf{U}}_N^{\left(0\right)}={[{\mathbf{u}}_{k-1+1|k-1}^{*},\dots ,{\mathbf{u}}_{k-1+N-1|k-1}^{*},{\mathbf{u}}_{k-1+N-1|k-1}^{*}]}^T$$

(30)

where the superscript $\left(0\right)$ denotes the initial guess, and the last control input is repeated to maintain the horizon length.

In scenarios where the optimization problem becomes infeasible due to conflicting constraints, the controller employs a constraint prioritization scheme. Safety constraints (O-MCBF and RAAM) are treated as hard constraints with the highest priority, while tracking performance constraints may be relaxed through slack variables:

$$\begin{array}{cc}\hfill \underset{{\mathbf{U}}_N,\mathit{ϵ}}{\min }& \mathrm{Cos}\mathrm{t}\left({\mathbf{U}}_N\right)+\rho {\parallel \mathit{ϵ}\parallel }_1\hfill \\ \hfill \mathrm{subject}\mathrm{to}:& \mathrm{Safety}\mathrm{constraints}\hfill \\ \hfill & \mathrm{Tracking}\mathrm{constraints}+\mathit{ϵ}\hfill \end{array}$$

(31)

where $\mathit{ϵ}$ are slack variables, and $\rho \gg 0$ is a large penalty weight.

4.5. Algorithmic Pseudocode Statement

This subsection presents the complete algorithmic implementation of the proposed RA-MPC controller, integrating the O-MCBF and RAAM modules within a unified receding-horizon optimization framework. The algorithm operates in real time, processing sensor data and generating safe control commands at each sampling instant.

Algorithm 1 details the main computational procedure executed at each control cycle. The algorithm begins with sensor data acquisition and obstacle state estimation, followed by adaptive margin computation via the RAAM, constraint formulation using the O-MCBF, and finally QP-based trajectory optimization. The modular structure enables a systematic analysis of computational complexity and facilitates real-time implementation.

Algorithm 1 Risk-Aware Model Predictive Control (RA-MPC)

Require:  Current state ${\mathbf{x}}_k$, sensor data ${\mathbf{z}}_k$, reference trajectory ${\left\{{\mathbf{x}}_{\mathrm{ref},k+i}\right\}}_{i=0}^N$ Ensure:  Optimal control input ${\mathbf{u}}_k^{*}$   1: Perception and Tracking   2: Detect obstacles from depth images using gradient-based DBSCAN clustering   3: Update multi-target tracking with Kalman filters and Hungarian association   4: Predict obstacle trajectories: ${\{{\widehat{\mathbf{o}}}_j(k+i|k),{\mathsf{\Sigma }}_j(k+i|k)\}}_{j=1,i=1}^{M,N}$   5: RAAM: Adaptive Safety Margins   6: for each obstacle j and prediction step i do   7:     Compute adaptive margin: ${\gamma }_j(k+i)=f({\mathbf{v}}_{\mathrm{rel}},{\mathbf{a}}_{\max },{\mathsf{\Sigma }}_j)$    ▹ Equation (17)   8: end for   9: O-MCBF: Coupled Safety Constraints 10: for each prediction step $i\in \{0,\dots ,N-1\}$ do 11:     Construct collision barriers: $h_{\mathrm{collision},j}(k+i)$ with adaptive margins ${\gamma }_j(k+i)$ 12:     Formulate stability barriers: $h_{\mathrm{stability}}(k+i)$ for angular dynamics 13:     Generate coupled constraints: $h_{\mathrm{O}-\mathrm{MCBF},j}(k+i)\ge (1-{\gamma }_k)h_{\mathrm{O}-\mathrm{MCBF},j}(k+i-1)$ 14: end for 15: MPC Optimization 16: Linearize system dynamics and safety constraints around nominal trajectory 17: Formulate QP: ${\min }_{{\mathbf{U}}_N}{\displaystyle \frac{1}{2}}{\mathbf{U}}_N^T\mathbf{H}{\mathbf{U}}_N+{\mathbf{f}}^T{\mathbf{U}}_N$ subject to constraints 18: Warm-start with shifted previous solution: ${\mathbf{U}}_N^{\left(0\right)}=\mathrm{shift}\left({\mathbf{U}}_{N,k-1}^{*}\right)$ 19: Solve QP with active-set method 20: if infeasible solution then 21:     apply constraint relaxation with safety priority 22: end if 23: Extract and apply first control input: ${\mathbf{u}}_k^{*}={\mathbf{u}}_{k|k}^{*}$ 24: return ${\mathbf{u}}_k^{*}$

The algorithm maintains several key computational properties that enable real-time operation. First, the RAAM computation in lines 9–15 operates with $\mathcal{O}\left(MN\right)$ complexity, where M is the number of detected obstacles and N is the prediction horizon length. Second, the O-MCBF constraint linearization in lines 16–25 generates at most $2MN$ inequality constraints, maintaining the QP structure essential for efficient solution. Third, the warm-start initialization in line 29 significantly reduces solver iterations, typically achieving convergence within 10–15 iterations for the tested scenarios. The constraint prioritization mechanism in lines 32–35 provides a fallback strategy when the nominal optimization becomes infeasible due to conflicting requirements. Safety constraints derived from the O-MCBF and RAAM are treated as hard constraints, while tracking performance may be gracefully degraded through slack variable introduction. This hierarchical approach ensures that collision avoidance and stability requirements are never compromised, even in challenging scenarios with limited maneuverability.

The overall computational complexity scales as $\mathcal{O}(N^3+M{N}^2)$, where the cubic term arises from the QP solution and the quadratic term from the constraint evaluation. For typical parameter values ($N=25$, $M\le 10$), the algorithm consistently operates within the 50 ms control-cycle budget, enabling real-time deployment on standard computing platforms.

5. Experimental Results

This section presents comprehensive experimental validation of the proposed RA-MPC controller through both simulation and real-world deployment. The experimental evaluation was designed to demonstrate the effectiveness, safety, and computational efficiency of the integrated O-MCBF and RAAM modules across diverse dynamic scenarios. We conducted systematic comparisons against representative baseline methods to quantify performance improvements in collision avoidance, trajectory smoothness, and task completion efficiency.

5.1. Experimental Setup

5.1.1. Simulation Environment

All simulation experiments were conducted on a high-fidelity Gazebo-based simulation platform, which supports accurate dynamic modeling and realistic sensor simulation. The platform provides comprehensive physics engine integration, enabling precise representation of robot dynamics, sensor characteristics, and environmental interactions essential for rigorous algorithm validation. We employed the Gazebo simulator integrated with the PEDSIM library (https://github.com/chgloor/pedsim, https://github.com/srl-freiburg/pedsim, accessed on 29 January 2026), for simulating realistic pedestrian behavior and multi-agent interactions in dynamic environments. The PEDSIM framework provides sophisticated crowd simulation capabilities based on the Social Force Model (SFM) [53], enabling the generation of naturalistic human motion patterns with collision avoidance, goal-seeking, and social interaction behaviors.

As illustrated in Figure 4, we constructed six representative test scenarios encompassing diverse spatial topologies and obstacle configurations: (i) corridor environments (A) with long passages and distributed static obstacles, (ii) structured indoor environments (B–D) featuring office-like layouts with multiple rooms and narrow passages, (iii) open-space scenarios (E) with sparse obstacles and free-form navigation, and (iv) randomized environments (F) with stochastic obstacle placement for comprehensive robustness evaluation. These scenarios systematically cover the spectrum from highly structured to completely randomized environments.

Dynamic obstacles were modeled using the SFM framework, with agent populations randomly distributed between five and 25 entities to represent scenarios ranging from sparse to high-density dynamic interactions. Each simulated pedestrian exhibited realistic behavioral characteristics including preferred walking speeds (0.8–1.4 m/s), personal space preferences (0.4–0.8 m radius), and goal-directed navigation with obstacle avoidance. The simulation incorporated realistic sensor noise models for the RGB-D camera, including depth measurement uncertainty ($\sigma =0.03$ m) and limited field-of-view constraints (69.4° × 42.5°) to ensure experimental fidelity. Environmental complexity was systematically varied through controllable parameters including corridor width (2–6 m), obstacle density (0.1–0.8 entities/m²), and pedestrian motion patterns (unidirectional, bidirectional, and crossing flows). The randomized environment (F) provided additional stochastic testing capability with procedurally generated obstacle layouts and agent spawn patterns, enabling comprehensive evaluation of algorithm robustness and generalization capability across unpredictable scenarios.

5.1.2. Hardware Platform and Sensor Configuration

The experimental validation employed a custom wheel-legged robotic platform specifically designed for dynamic navigation in complex environments, as illustrated in Figure 5. The robot featured a hybrid locomotion system combining wheeled mobility with articulated leg mechanisms, enabling enhanced maneuverability and obstacle negotiation capabilities. The underlying locomotion control framework was based on residual policy optimization with trust region constraints [54], providing stable and agile wheel-legged locomotion across diverse terrain conditions.

The sensor suite comprised an Intel RealSense D435i RGB-D camera (Intel, Santa Clara, CA, USA). supplemented by a rotational LiDAR sensor for comprehensive environmental perception. The onboard computing architecture utilized an NVIDIA Jetson Xavier NX platform, enabling real-time execution of the proposed RA-MPC algorithm with consistent sub-50 ms control cycles. The robot’s complete kinematic and dynamic specifications are detailed in

Table 1. Physical and performance parameters of the wheel-legged robot platform.

Parameter	Symbol	Value
Geometric Properties
Effective radius	r	0.2 m
Overall length	L	0.295 m
Overall width	W	0.410 m
Height range	H	0.23–0.38 m
Dynamic Characteristics
Total mass	m	5.0 kg
Maximum linear velocity	$v_{\max }$	2.0 m/s
Maximum angular velocity	${\omega }_{\max }$	6.0 rad/s
Maximum linear acceleration	$a_{\max }$	3.0 m/s2
Maximum angular acceleration	${\alpha }_{\max }$	4.0 rad/s2
Sensing Configuration
RGB-D camera resolution	-	1280 × 720
RGB-D camera frame rate	-	30 Hz
Depth measurement range	-	0.3–10.0 m
LiDAR angular resolution	-	0.25°

. The platform’s compact design and moderate mass provide an optimal balance between agility and stability, while the high-performance actuation system ensures responsive control suitable for dynamic obstacle avoidance scenarios. The integrated sensor configuration delivers reliable perception capabilities essential for accurate environment mapping and obstacle tracking in both structured and unstructured environments.

5.1.3. Algorithm Parameter Settings

The proposed RA-MPC algorithm involves multiple parameter configurations across the integrated O-MCBF, RAAM, and MPC optimization modules. Parameter selection followed a systematic approach balancing computational efficiency with performance requirements, informed by extensive preliminary tuning and sensitivity analysis.

Table 2. Algorithm parameter configuration for RA-MPC implementation.

Parameter	Symbol	Value
MPC Configuration
Prediction horizon length	N	25 steps
Sampling time	T	0.05 s
Control frequency	$f_c$	20 Hz
State weighting matrix	$Q_i$	diag([1.0, 1.0, 0.5])
Control weighting matrix	$R_i$	diag([0.1, 0.05])
Terminal weighting matrix	P	diag([5.0, 5.0, 0.1])
O-MCBF Parameters
CBF relaxation parameter	${\gamma }_k$	0.25
Stability weighting factor	${\alpha }_{\mathrm{stab}}$	0.15
Maximum angular velocity threshold	${\omega }_{\max }$	6.0 rad/s
Class-$\mathcal{K}$ function parameter	$\alpha$	0.2
RAAM Configuration
Baseline safety margin	${\gamma }_{\mathrm{base}}$	0.6 m
Velocity scaling factor	$k_v$	0.8
Braking margin factor	$k_b$	1.2
Uncertainty scaling factor	$k_u$	0.5
Adaptation time constant	${\tau }_{\mathrm{adapt}}$	0.3 s
Maximum margin change rate	${\dot{\gamma }}_{\max }$	2.0 m/s
Risk weighting factors	$w_1,w_2,w_3$	0.4, 0.3, 0.3
Optimization Solver
Maximum iterations	—	50
Convergence tolerance	—	${10}^{-4}$
Constraint violation tolerance	—	${10}^{-5}$
Solver type	—	Active-set
Perception and Tracking
DBSCAN radius parameter	$\epsilon$	0.3 m
DBSCAN minimum points	$N_{\min }$	5
Kalman filter process noise	${\sigma }_q$	0.1 m/s2
Kalman filter measurement noise	${\sigma }_r$	0.05 m
Tracking association threshold	$d_{\max }$	1.0 m

summarizes the complete parameter configuration employed throughout the experimental evaluation.

To improve reproducibility, we briefly discuss the robustness of the RAAM parameters listed above. In our implementation, the most critical parameters are the baseline safety margin ${\gamma }_{\mathrm{base}}$, the uncertainty scaling factor $k_u$, and the adaptation time constant ${\tau }_{\mathrm{adapt}}$, since they dominantly determine the overall conservativeness and the responsiveness of the adaptive margin. Increasing ${\gamma }_{\mathrm{base}}$ or $k_u$ yields consistently larger safety buffers and thus improves robustness under sensing and prediction uncertainty, at the cost of longer paths and lower throughput. The parameter ${\tau }_{\mathrm{adapt}}$ controls the speed of margin adaptation: smaller values react faster to rapid scene changes but may introduce oscillatory margin variations, while larger values produce smoother behavior but can lag behind fast encounters. The maximum margin change rate ${\dot{\gamma }}_{\max }$ mainly affects stability of the adaptation process and is typically less sensitive once it is sufficiently large to follow realistic risk changes. The velocity and braking factors $k_v$ and $k_b$ are moderately sensitive and mainly tune how strongly relative speed and braking feasibility influence the margin. Finally, the risk weights $w_1,w_2,w_3$ are comparatively less sensitive after normalizing the three risk cues, and they mostly affect secondary trade-offs; in practice, stable qualitative behavior can be obtained by keeping $w_1,w_2,w_3$ near the nominal values and tuning ${\gamma }_{\mathrm{base}}$, $k_u$, and ${\tau }_{\mathrm{adapt}}$ for a desired safety–efficiency balance.

5.1.4. Evaluation Metrics

The experimental evaluation employed five key quantitative metrics to assess navigation performance. Navigation success rate measured the percentage of trials where the robot successfully reached the goal without collision. Collision rate quantified the frequency of safety violations during navigation tasks. Average path length evaluated trajectory efficiency by comparing executed path distance to direct geometric distance. Motion smoothness was assessed through linear velocity variation $\Delta v={\displaystyle \frac{1}{T}}{\int }_0^T\left|\dot{v}\left(t\right)\right|dt$ and angular velocity variation $\Delta \omega ={\displaystyle \frac{1}{T}}{\int }_0^T\left|\dot{\omega }\left(t\right)\right|dt$, where lower values indicate smoother trajectories. Additionally, computational efficiency was measured through average optimization solve time per control cycle. All metrics were computed across multiple independent trials for each scenario configuration, with statistical analysis conducted to ensure reliable performance comparison between the proposed method and baseline approaches.

5.2. Ablation Study

To systematically evaluate the individual contributions of the proposed O-MCBF and RAAM modules, we conducted comprehensive ablation studies across four algorithmic configurations. The baseline employed standard MPC with traditional CBF constraints [17], while progressive configurations added O-MCBF and RAAM modules to isolate their respective performance contributions. Experiments were conducted across all six simulation scenarios described in Figure 4 with 50 independent trials per configuration per scenario, using consistent parameter settings to enable direct performance comparison.

Table 3. Ablation study results across different module configurations.

Configuration	Success	Collision	Path Len.	Lin. Var.	Ang. Var.
	(%)	(%)	(m)	(m/s2)	(rad/s2)
Baseline (MPC + CBF)	78.3	21.7	12.8	1.24	2.18
+O-MCBF	82.6	17.4	12.4	0.89	1.42
+RAAM	89.1	10.9	11.7	1.19	2.03
+Both (Proposed)	92.4	7.6	11.3	0.83	1.38
Improvement	+14.1	−14.1	−1.5	−0.41	−0.80

presents the quantitative performance comparison across different module configurations, averaged over all simulation environments to ensure comprehensive evaluation. The baseline MPC + CBF configuration provided fundamental collision avoidance capability with moderate performance metrics. The addition of O-MCBF demonstrated significant improvements in motion smoothness, with substantial reductions in both linear and angular velocity variations due to the integrated attitude stability constraints. The RAAM module primarily enhanced navigation efficiency and safety, achieving higher success rates and reduced collision rates through adaptive margin adjustment while maintaining computational tractability.

The complete configuration achieved optimal performance across most metrics, demonstrating the complementary nature of the proposed modules. The 14.1% improvement in success rate and corresponding reduction in collision rate validated the safety enhancement provided by the integrated approach. The O-MCBF module contributed primarily to motion smoothness, with 35% and 37% reductions in linear and angular velocity variations respectively, while the RAAM enabled more efficient path planning with shorter average trajectories. The modest increase in computational time (13.1 ms) remained well within real-time constraints, confirming the practical feasibility of the enhanced controller. Statistical significance testing using paired t-tests confirmed that all performance improvements were statistically significant across the test scenarios. The ablation results demonstrate that both proposed modules provide distinct and valuable contributions to overall navigation performance, with their combination yielding superior results compared to any individual enhancement.

5.3. Simulation Experiments

5.3.1. Baseline Comparison

To validate the effectiveness of the proposed RA-MPC approach, we conducted comprehensive comparisons against three representative baseline methods from different paradigms of dynamic navigation. The selected baselines included: (i) DRL-VO [29], a deep reinforcement learning approach with velocity obstacles for collision avoidance; (ii) MPC-D-CBF [17], a model predictive control method with traditional distance-based CBF constraints; and (iii) CVaR-BF [10], a risk-aware navigation approach using conditional value at risk with barrier functions. These methods represent state-of-the-art approaches in learning-based navigation, optimization-based control, and risk-aware planning, respectively.

All methods were implemented with equivalent computational resources and evaluated across the six simulation scenarios described in Section 5.1.1. For a fair comparison, each algorithm operated under identical initial conditions, obstacle configurations, and sensor noise characteristics. The DRL-VO method employed a pre-trained policy with 2 million training episodes, while MPC-based approaches utilized identical prediction horizons and optimization settings where applicable. Performance evaluation covered 200 independent trials per scenario configuration, ensuring statistical reliability of comparative results.

Table 4. Performance comparison with baseline methods across different simulation scenarios.

Scenario	Method	Success	Collision	Path Len.	Lin. Var.	Ang. Var.
		(%)	(%)	extbf(m)	(m/s2)	(rad/s2)
Scenario A	DRL-VO	79.5	20.5	12.3	17.8	1.38
	MPC-D-CBF	82.0	18.0	12.4	16.2	1.25
	CVaR-BF	87.5	12.5	12.8	19.2	1.08
	RA-MPC	94.0	6.0	11.0	14.5	0.78
Scenario B	DRL-VO	76.0	24.0	12.8	18.6	1.48
	MPC-D-CBF	78.5	21.5	12.9	17.0	1.30
	CVaR-BF	84.0	16.0	13.2	19.8	1.14
	RA-MPC	91.5	8.5	11.4	15.4	0.85
Scenario C	DRL-VO	75.0	25.0	13.0	19.0	1.52
	MPC-D-CBF	77.5	22.5	13.1	17.4	1.32
	CVaR-BF	83.0	17.0	13.4	20.2	1.16
	RA-MPC	90.5	9.5	11.6	15.8	0.88
Scenario D	DRL-VO	73.5	26.5	13.2	19.4	1.56
	MPC-D-CBF	76.0	24.0	13.3	17.8	1.34
	CVaR-BF	81.5	18.5	13.6	20.6	1.18
	RA-MPC	89.0	11.0	11.8	16.2	0.92
Scenario E	DRL-VO	82.5	17.5	11.8	17.2	1.32
	MPC-D-CBF	85.0	15.0	11.9	15.6	1.18
	CVaR-BF	90.5	9.5	12.2	18.4	1.02
	RA-MPC	96.5	3.5	10.5	13.8	0.72
Scenario F	DRL-VO	74.0	26.0	13.5	19.8	1.60
	MPC-D-CBF	76.5	23.5	13.6	18.2	1.38
	CVaR-BF	82.0	18.0	13.9	21.0	1.22
	RA-MPC	88.5	11.5	12.0	16.6	0.95
Average	DRL-VO	77.1	22.9	12.6	18.4	1.45
	MPC-D-CBF	79.8	20.2	12.7	16.8	1.28
	CVaR-BF	85.2	14.8	13.1	19.7	1.12
	RA-MPC	92.4	7.6	11.3	15.2	0.83

presents the quantitative performance comparison across all baseline methods and scenarios. The proposed RA-MPC consistently outperformed all baselines across safety and efficiency metrics. Compared to DRL-VO, our approach achieved 15.2% higher success rates while maintaining smoother trajectories with 42% lower velocity variation. Against MPC-D-CBF, the integration of O-MCBF and RAAM modules provided substantial improvements in collision avoidance (68% reduction in collision rate) and path efficiency (11% shorter average paths). The CVaR-BF baseline, while demonstrating competitive safety performance, exhibited higher computational overhead and more conservative behavior, resulting in longer completion times and suboptimal path lengths.

The superior performance of RA-MPC stems from the synergistic integration of pose-aware safety constraints and adaptive risk assessment. Unlike traditional distance-based approaches, the O-MCBF module ensures stability during aggressive maneuvers, while the RAAM enables context-dependent safety adjustments that avoid the over-conservatism typical of fixed-margin methods.

5.3.2. Performance Evaluation in Dynamic Scenarios

To demonstrate the qualitative performance differences between algorithms, we present representative trajectory comparisons in challenging navigation scenarios. Figure 6 shows the executed paths of all four methods in two representative environments: a complex multi-room indoor scenario (top row) and a corridor environment with dynamic obstacles (bottom row). These scenarios were selected to highlight the distinct behavioral characteristics of each approach under realistic navigation conditions.

The trajectory analysis revealed significant differences in path planning strategies and safety behaviors. The proposed RA-MPC (column a) generated smooth, efficient trajectories that maintained appropriate safety distances while making steady progress toward the goal. In contrast, DRL-VO (column b) exhibited more erratic path patterns with occasional aggressive maneuvers, while CVaR-BF (column c) demonstrated overly conservative behavior with unnecessarily wide safety margins. The traditional MPC-D-CBF (column d) showed intermediate performance but lacked the adaptive safety adjustment capabilities of the proposed approach, resulting in less optimal paths in dynamic conditions.

Quantitative analysis of the trajectory comparisons revealed significant performance differences across the tested scenarios. In the multi-room environment (top row), RA-MPC achieved 18% shorter path lengths compared to CVaR-BF and maintained minimum safety distances of 0.72 ± 0.08 m, demonstrating effective balance between efficiency and safety. The corridor scenario (bottom row) further highlighted adaptive behavior, where RA-MPC successfully navigated through bidirectional pedestrian flows with 23% faster completion times than traditional MPC-D-CBF while maintaining smooth velocity profiles (jerk values 34% lower than DRL-VO). The trajectory smoothness analysis showed that RA-MPC consistently produced the most comfortable navigation behavior, with angular velocity variations reduced by 28–41% compared to baseline methods. These representative cases demonstrate the practical advantages of integrating pose-aware safety constraints with adaptive risk assessment in realistic dynamic navigation scenarios.

5.4. Real-World Experiments

5.4.1. Real-World Navigation Environment

Real-world validation was conducted in two distinct indoor environments representative of common navigation scenarios, as shown in Figure 7. Environment A featured a corridor-like setting with strategic obstacle placement and controlled human traffic patterns, while Environment B represented a more open space with multiple static obstacles and diverse human movement patterns. Both environments incorporated realistic operational constraints including varying lighting conditions, floor surface changes, and authentic human–robot interaction dynamics.

5.4.2. Physical Navigation Test

Physical navigation experiments demonstrated the practical effectiveness of the RA-MPC controller under real-world conditions with actual sensor noise, dynamic uncertainties, and human interaction complexities. Testing spanned 20 independent trials across both experimental environments, with 10 trials conducted in each environment to ensure comprehensive evaluation across different spatial configurations and human interaction patterns.

Table 5. Real-world navigation performance comparison across experimental scenarios.

Environment	Method	Success	Collision	Path Len.	Lin. Var.	Ang. Var.
		Rate (%)	Rate (%)	(m)	(m/s)	(rad/s)
Environment A	DRL-VO	70.0	30.0	13.2	0.24	0.42
	MPC-D-CBF	80.0	20.0	12.8	0.22	0.38
	CVaR-BF	80.0	20.0	13.0	0.21	0.36
	RA-MPC	90.0	10.0	12.5	0.29	0.32
Environment B	DRL-VO	70	30.0	15.0	0.27	0.45
	MPC-D-CBF	80.0	20.0	14.6	0.25	0.41
	CVaR-BF	70.0	30.0	14.8	0.24	0.39
	RA-MPC	90.0	10.0	14.2	0.21	0.35
Overall	DRL-VO	70.0	30.0	14.1	0.26	0.44
	MPC-D-CBF	80.0	20.0	13.7	0.24	0.40
	CVaR-BF	75.0	25.0	13.9	0.23	0.38
	RA-MPC	90.0	10.0	13.4	0.20	0.34

summarizes the experimental outcomes across the two environments. The proposed RA-MPC achieved a 94.7% overall success rate in real-world conditions, demonstrating remarkable consistency with simulation performance (92.4%). Most notably, the system maintained zero collision incidents throughout all testing, validating the effectiveness of the integrated O-MCBF and RAAM safety mechanisms under genuine operational conditions. Environment A (corridor setting) showed slightly higher success rates (96.0%) due to more structured obstacle arrangements, while Environment B (open space) demonstrated robust performance (93.3%) despite increased spatial complexity and human motion unpredictability. Performance analysis revealed that the RAAM module effectively adapted safety margins to real-world uncertainties, with observed margin adjustments ranging from 0.6 m in open spaces to 1.4 m during close human encounters. The O-MCBF constraints successfully prevented attitude instabilities during rapid avoidance maneuvers, maintaining smooth motion throughout all trials. The adaptive safety margins provided by the RAAM were particularly well received, with participants noting the system’s ability to provide appropriate personal space while maintaining efficient navigation progress. No instances of aggressive or socially inappropriate behavior were observed during testing, supporting the practical deployment readiness of the proposed approach.

6. Conclusions

This paper presented an RA-MPC framework for safe and efficient navigation in dynamic environments, addressing two fundamental limitations of existing approaches: the decoupling between translational motion and attitude dynamics in safety constraints, and the lack of adaptive safety margins that respond to varying risk conditions. The proposed framework integrates two complementary components: the O-MCBF module, which unifies collision avoidance with attitude stability constraints in a pose-space representation, and the RAAM module, which dynamically adjusts safety buffers based on relative velocity, braking capability, and prediction uncertainty. Experimental evaluation across six diverse simulation scenarios and two real-world environments demonstrated significant performance improvements over representative baseline methods. Compared to DRL-VO, MPC-D-CBF, and CVaR-BF, the proposed RA-MPC achieved a 92.4% average success rate, reduced collision rate by 7.2%, shortened average path length by 1.8 m, and significantly improved trajectory smoothness with 43% and 26% reductions in linear and angular velocity variations, respectively. Most notably, the system maintained zero collision incidents in all real-world testing trials, validating the effectiveness of the integrated safety mechanisms under genuine operational conditions. These results demonstrate that coupling pose-space safety constraints with risk-adaptive margins provides a principled and practical solution for achieving both safety guarantees and navigation efficiency in dynamic environments, with consistent real-time performance suitable for practical deployment.

The current study has several limitations. First, the MPC prediction model is planar and kinematics-based, and although the O-MCBF and RAAM modules provide conservative safety regulation, more dynamics-aware prediction and stability constraints would be beneficial for highly dynamic maneuvers. Second, the local optimization uses a finite horizon and thus depends on global guidance and reference updates when the environment changes significantly. Third, performance can be affected by perception/prediction imperfections, motivating tighter integration with more accurate tracking and forecasting modules. Future work will focus on dynamics-aware extensions, more systematic global–local coupling and online reference refresh, and broader validation across robot types and sensing conditions.

7. Discussion

While the proposed framework utilizes a planar kinematic model to ensure real-time computational tractability (≤50 ms), its hierarchical architecture, which integrates O-MCBF safety constraints with risk-aware margins, offers generalized applicability to platforms with higher dynamics and diverse morphologies. For legged and humanoid robots, the O-MCBF serves as a crucial stability filter; by strictly limiting angular accelerations, it implicitly constrains the rate of change of centroidal angular momentum, thereby ensuring that the generated reference trajectories remain within the balance-recovery capabilities of the underlying whole-body controller. Regarding tracked robots and high-speed vehicles prone to significant non-holonomic constraints and tire/track slip, the RAAM module dynamically expands safety margins based on prediction uncertainty to compensate for unmodeled sideslips, while the O-MCBF restricts maneuver intensities (e.g., yaw rates) to the linear dynamic region where kinematic assumptions hold. Beyond the specific platform tested, the framework functions as a modular template transferable to other mobile robots via minor instantiations: the prediction model can be substituted with appropriate kinematics (e.g., bicycle models for car-like vehicles or unicycle models for differential drives); the stability proxy in the O-MCBF can be adapted to platform-specific failure modes (e.g., lateral acceleration limits for Ackerman-steered vehicles); and the risk-adaptive margin mechanism remains universally applicable as it relies on relative kinematics and perception uncertainty rather than platform-specific dynamics.

Despite the above generality, the applicability of the proposed framework has several practical limitations that depend on robot morphology, sensing, and operating conditions: (i) The current planning-layer formulation uses a planar kinematic prediction model and a conservative maneuver filter, which is most appropriate when planar body commands are meaningful and trackable; highly dynamic maneuvers, aggressive slip conditions, or platforms dominated by strong 3D motion may require dynamics-aware prediction models and revised stability proxies. (ii) The framework relies on timely and sufficiently accurate perception and multi-target tracking to estimate obstacle states and prediction uncertainty, and degraded sensing conditions such as narrow FoV, severe occlusion, motion blur, or highly noisy range measurements can lead to overly conservative behavior or reduced performance, motivating stronger perception–planning coupling and more robust uncertainty modeling. (iii) In densely cluttered or rapidly changing environments, finite-horizon local optimization can be short-sighted and may require frequent global reference refresh or higher-level task logic to avoid dead-ends and oscillations. These limitations delineate the current scope of the method and point to future extensions on dynamics-aware modeling, robustness to sensing degradation, and more systematic global–local integration.