Terminal-Consistency-Constrained Informed RRT*-MPC Cooperative Navigation Method*

To address the weak terminal coordination in the hierarchical navigation framework combining Informed Rapidly-exploring Random Tree Star (Informed RRT*) and Model Predictive Control (MPC), this paper proposes a terminal-consistency-constrained Informed RRT*-MPC cooperative navigation method. A reference path generated by the global planner may be geometrically feasible but not sufficiently consistent with the terminal state required by the local controller, which can reduce trackability, increase unnecessary detours during dynamic obstacle avoidance, and weaken terminal convergence. In the global planning layer, a terminal deviation term is introduced into the node cost of the sampling-based planner and is further embedded in node extension, parent selection, and local rewiring. Thus, the generated reference path considers not only path length but also the convergence tendency toward the desired terminal pose. The discrete path is then smoothed and converted into a reference state sequence for local control. In the local control layer, a model predictive control optimization model is constructed with trajectory tracking, control effort, control increment, terminal consistency, and dynamic obstacle penalty terms. A goal-distance-based dynamic obstacle weight is also designed by combining the distance to the target and the predicted obstacle proximity, so that obstacle avoidance and terminal convergence can be balanced during receding-horizon optimization. Simulation experiments were conducted in a grid environment containing static obstacles and local dynamic obstacle regions. Compared with the baseline method using standard Informed RRT* and basic MPC, the proposed method reduced the path length by 5.4%, total travel time by 44.9%, curvature energy by 53.9%, and detour ratio by 5.7%. The results show that the proposed method improves path trackability, trajectory smoothness, terminal convergence, and overall navigation efficiency while maintaining obstacle-avoidance safety.