MDSRC Algorithm for Efficient AMR Navigation

• Multi-Directional Safety Rectangular Corridor (MDSRC) is a geometric encoding technique that creates locally oriented, obstacle-free rectangles to map free space in complex environments.
• It systematically expands rectangles around seed waypoints at multiple orientations to maximize corridor area and reduce the number of necessary corridor constraints.
• Integration with sequential MPC leverages linear inequality constraints for enhanced real-time trajectory planning and collision avoidance in dynamic settings.

The Multi-Directional Safety Rectangular Corridor (MDSRC) algorithm is a geometric encoding technique central to efficient autonomous mobile robot (AMR) navigation in cluttered, semi-structured planar environments. By constructing convex, obstacle-free rectangles locally oriented to maximize free space around waypoints, MDSRC integrates with sequential Model Predictive Control (MPC) frameworks to reduce collision-checking and nonconvexity, thus accelerating real-time motion planning and solver convergence, particularly for nonholonomic vehicles in environments populated with heterogeneous static and dynamic obstacles (Qu et al., 15 Dec 2025).

1. Objectives and Motivation

The primary challenge addressed by MDSRC is the efficient encoding of the non-convex, fragmented free space ($\mathcal{C}_{free} \subset \mathbb{R}^2$) encountered by AMRs in environments with mixed, axis-aligned, and oblique obstacles. Standard axis-parallel rectangular corridors often produce narrow, discontinuous regions especially in diagonal or irregular passages, leading to numerous corridor switches and excessive computational burden. MDSRC is engineered to:

• Encode $\mathcal{C}_{free}$ as a minimal set of large, convex, obstacle-free rectangular regions.
• Adapt each rectangle's orientation to local workspace geometry, hence maximizing corridor area.
• Reduce the total number of corridor constraints in the MPC formulation, directly enhancing solver throughput and navigation smoothness.

These objectives enable coverage of wider regions per corridor, minimizing switching events and overhead, while maintaining strict collision avoidance (Qu et al., 15 Dec 2025).

2. Algorithmic Construction

MDSRC constructs a sequence of local corridors ("rectangles") by systematically expanding rectangles at each A* seed waypoint, searching for the largest obstacle-free region at multiple orientations. The key steps are:

1. For each seed waypoint $P_i$, candidate rectangles are generated at $N_c$ equally spaced orientations $\theta_{pi}^k = \frac{\pi}{2}\frac{k-1}{N_c}$.
2. For each orientation, the rectangle is iteratively "inflated" along its four faces in step $\Delta_L$ until hitting map boundaries, an obstacle, or reaching a maximum extent $L_{\max}$.
3. Collision checking at each expansion is performed only along the rectangle's edges by sampling $N_\ell$ points per face, significantly reducing computational overhead.
4. The final rectangle for $P_i$ is the orientation yielding the maximum area, described by its vertices and orientation $$(P_{ci,left}^{H_k}, P_{ci,right}^{H_k}, \theta_{pi}^k)$$.
5. For path sequences, waypoints covered by previous rectangles are skipped, and a new rectangle is forced at the final goal.

Batching and efficient data representations (occupancy map $\mathcal{M}$ as a binary grid, $4\times2$ vertex arrays) minimize overhead during rectangle construction (Qu et al., 15 Dec 2025).

3. Mathematical Formulation and Collision Constraints

For each selected corridor, the coordinate transformation and constraints are formulated as follows:

• Robot state: $\eta = [\bar\eta^T,\,\psi]^T$ with position $\bar\eta = [x, y]^T$ and heading $\psi$; control input $\nu = [u, v, r]^T$; kinematics $\dot\eta = f(\eta, \nu)$ discretized by $$\eta_{k+1} = g(\eta_k, \nu_k) = \eta_k + \Delta T\,f(\eta_k, \nu_k)$$.
• After rectangle expansion, corridor bounds in the local frame $H_k$ are $$P_{ci,left}^{H_k} = [x_{\min}^{H_k}, y_{\min}^{H_k}]^T$$, $$P_{ci,right}^{H_k} = [x_{\max}^{H_k}, y_{\max}^{H_k}]^T$$.
• The robot position is mapped into the corridor frame:

$$\eta^H = R^T(\theta_{pi}^k)\left(\bar\eta - P_i\right)$$

where $R^T(\theta_{pi}^k)$ is a rotation matrix.

• Corridor constraints are encoded as:

$A\,\eta^H - b_i < 0$

with

$$A = \begin{bmatrix} 1 & 0 \ -1 & 0 \ 0 & 1 \ 0 & -1 \end{bmatrix}, \; b_i = \begin{bmatrix} x_{\max}^{H_k} - \Delta_s \ -x_{\min}^{H_k} - \Delta_s \ y_{\max}^{H_k} - \Delta_s \ -y_{\min}^{H_k} - \Delta_s \end{bmatrix}$$

where $\Delta_s$ is a margin for safety. Thus, static obstacle avoidance reduces to four linear inequalities per corridor (Qu et al., 15 Dec 2025).

4. Integration with Sequential Model Predictive Control

MDSRC is foundational to the Improved Sequential Model Predictive Control (ISMPC) navigation paradigm. At each timestep, the MPC solves a finite-horizon optimal control problem (OCP) constrained within the current corridor:

• Current and next "center" targets are $\eta_{cg1}$ (corridor $i$) and $\eta_{cg2}$ (corridor $i+1$).
• The objective over horizon $N$ minimizes:

$$J = J_N(\eta_{N|t}) + \sum_{k=0}^{N-1} J_k(\eta_k, \nu_k)$$

with

$$J_N = \|\eta_{N|t} - \eta_{cg1}\|_{Q_1} + \|\eta_{N|t} - \eta_{cg2}\|_{Q_2}$$

$$J_k = \|\eta_k - \eta_{cg1}\|_{Q_1} + \|\eta_k - \eta_{cg2}\|_{Q_2} + \|\nu_k\|_{R} + \|\nu_{k+1} - \nu_k\|_{S}$$

• Subject to kinematics, input bounds, corridor linear constraints, dynamic obstacle constraints (via D-CBFs), and initial state constraints.

Corridor switches occur whenever the robot exits the current rectangle, triggering online re-solution of the OCP with updated constraints (Qu et al., 15 Dec 2025).

5. Computational Complexity and Performance

The computational profile of MDSRC is:

• Per-corridor generation: $O(N_c\,\frac{L_{\max}}{\Delta_L}\,4\,N_\ell)$ collision checks per seed. With typical parameters ($N_c=10$, $\Delta_L=0.1$, $L_{\max}=8.0$, $N_\ell=5$), runtime per rectangle remains around 2–4 ms on a standard laptop.
• Collision checking is limited to edge samples, not full interiors.
• Encoding static obstacle avoidance as four linear constraints per corridor permits the MPC to avoid mixed-integer or nonlinear checks, thus reducing nonconvexity and accelerating convergence (e.g., for IPOPT solvers).
• Real-world profile: occupancy maps stored as binary grids for $O(1)$ lookups; all vertex computations are in-place (Qu et al., 15 Dec 2025).

6. Quantitative Benchmarking and Comparative Outcomes

Empirical evaluation in dense, unstructured environments demonstrates:

Metric	MDSRC	SRC [13]	FSRC [14]
Avg. no. corridors (Env. 1)	30	52	(Baseline)
Avg. corridor area ($m^2$)	12.80	6.94	(Baseline)
Corridor gen. latency (ms)	$\approx$4	2	1

• MDSRC reduced the number of corridors by 35.86% and increased average corridor area by 41.05%.
• Generation latency, though higher, remained within real-time bounds ($\approx$4 ms).
• In end-to-end navigation trials, ISMPC with MDSRC produced smoother, shorter trajectories, operated at velocities and turn-rates within $[0.05, 1.0]$ m/s and $[-1.5, 1.5]$ rad/s, and retained 3 ms average MPC solve time.

These outcomes substantiate the approach's superiority in free-space utilization and throughput without compromising safety or responsiveness (Qu et al., 15 Dec 2025).

7. Significance and Synthesis

The MDSRC algorithm reformulates the encoding of obstacle-free workspace regions for AMR navigation, yielding large, locally oriented rectangles that are efficiently incorporated as linear inequality constraints within a sequential MPC. This methodology systematically increases per-step coverage, suppresses unnecessary corridor switching, and enables robust, real-time operation in highly cluttered environments. In synergy with dynamic obstacle avoidance via D-CBFs, the overall ISMPC+MDSRC framework delivers collision-free, high-throughput autonomous guidance, making it a pragmatic foundation for AMRs in industrial, warehouse, or similarly complex operational domains (Qu et al., 15 Dec 2025).