Autonomous Navigation of Mobile Robots: A Hierarchical Planning–Control Framework with Integrated DWA and MPC

. 2025 Mar 23;25(7):2014. doi: 10.3390/s25072014

Algorithm 1 DWA-MPC Hierarchical Navigation Framework

Require:
Global path G, Robot state X
Ensure:
Control command $u$
1:
loop
2:
Local map M, $Lidar_cloud \leftarrow S ENSOR U PDATE ()$
3:
$Collision_flag \leftarrow E VALUATE C OLLISION R ISK (Lidar_cloud, X)$
4:
if $Collision_flag$ then ▹ Local Replanning via DWA
5:
$Candidate_goals \leftarrow G ENERATE S UBGOALS (G, X, M, radius = 2.0 m)$
6:
$Optimal_subgoal \leftarrow arg max S CORE S UBGOALS (Candidate_goals, X, M)$
7:
$τ \leftarrow DWAP LANNER (X, Optimal_subgoal, M)$
8:
$MPC . ReferenceTrajectory \leftarrow τ$
9:
$R ESET MPCH ORIZONS (N_{p}, N_{c})$
10:
else
11:
$MPC . ReferenceTrajectory \leftarrow G$
12:
end if

▹ Adaptive MPC Tracking
13:
if $MPC_execution_cycle$ then
14:
$Current_ref_point \leftarrow F INDNEAREST P OINT (MPC . ReferenceTrajectory, X)$
15:
$κ \leftarrow C OMPUTE O PTIMAL R EFERENCE (MPC . ReferenceTrajectory, Current_ref_point)$
16:
$(N_{P_{adapt}}, N_{C_{adapt}}) \leftarrow A DJUST H ORIZONS (κ)$
17:
$u_{opt} \leftarrow S OLVE M P C (X, κ)$
18:
$A PPLY C ONTROL (u_{opt} [0])$
19:
end if
20:
end loop