Abstract

This paper addresses the path planning problem for mobile robots operating in complex environments. We investigate classical algorithms including global planning approaches (A* algorithm, Dijkstra's algorithm) and local planning methods (Dynamic Window Approach, Artificial Potential Field). All algorithms are implemented and validated using the MATLAB simulation platform. Through the construction of grid maps and dynamic obstacle environments, we analyze performance metrics such as path length, computational time, and obstacle avoidance capability. A hybrid path planning strategy (global A* combined with local DWA) is proposed, effectively enhancing path safety and real-time performance for robots operating in complex scenarios. Experimental results demonstrate that the hybrid approach outperforms individual algorithms in terms of path length and obstacle avoidance success rate, providing a viable solution for robot navigation in challenging environments.

Keywords: Mobile Robot; Path Planning; MATLAB Simulation; A* Algorithm; Dynamic Window Approach; Hybrid Strategy

1. Introduction

Mobile robot path planning involves finding an optimal (or feasible) path from a start point to a goal within a given environmental model, while satisfying constraints such as obstacle avoidance, energy consumption, and time requirements. Complex environments—including indoor dynamic obstacles and unstructured outdoor terrain—demand higher real-time performance and robustness from path planning algorithms.

Current path planning algorithms can be divided into two primary categories:

• Global Planning: Based on known environmental models (such as grid maps or topological maps), global paths are computed offline (e.g., A*, Dijkstra's algorithm).
• Local Planning: Based on real-time sensor data of the local environment, paths are adjusted online to avoid dynamic obstacles (e.g., DWA, APF algorithms).

This paper systematically implements the aforementioned algorithms using MATLAB and compares their performance through simulation, proposing a hybrid planing strategy suitable for complex environments.

2. Environment Modeling and Problem Formulation

2.1 Environment Representation

The grid map approach is employed to model complex environments: the environment is divided into an M×N two-dimensional grid, where each cell is labeled as free space, obstacle, or unknown region. The robot is simplified to a point mass occupying a single grid cell.

2.2 Problem Formulation

Given a start point S=(x_s, y_s), a goal point G=(x_g, y_g), and a grid map, the objective is to plan a path P={p_1, p_2, ..., p_k} where p_i=(x_i, y_i), satisfying the following constraints:

1. Connectivity: Consecutive path points p_i and p_{i+1} must be adjacent in the grid (horizontal, vertical, or diagonal neighbors).

2. Obstacle Avoidance: All path points p_i must lie within free space.

3. Optimality: The path length should be minimized (or time/energy consumption minimized).

4. Global Path Planning Algorithm Implementation

3.1 A* Algorithm

3.1.1 Algorithm Principle

The A* algorithm is a heuristic search method that selects optimal nodes through an evaluation function f(n)=g(n)+h(n):

• g(n): The actual cost from the start node to node n (path length).
• h(n): The heuristic estimated cost from node n to the goal (commonly Euclidean distance h_E(n)=√((x_n−x_g)^2+(y_n−y_g)^2) or Manhattan distance h_M(n)=|x_n−x_g|+|y_n−y_g|).

3.1.2 MATLAB Implementation

function route = heuristic_astar(grid_map, start_pos, target_pos)
    % Input: grid_map (binary grid where 1=obstacle, 0=free)
    %        start_pos (2-element vector [row, col])
    %        target_pos (2-element vector [row, col])
    % Output: route (Nx2 matrix of waypoint coordinates)
    
    [grid_rows, grid_cols] = size(grid_map);
    
    % Initialize data structures
    frontier = priorityQueue();
    explored = false(grid_rows, grid_cols);
    predecessor = zeros(grid_rows, grid_cols, 2);
    cost_sofar = inf(grid_rows, grid_cols);
    est_total = inf(grid_rows, grid_cols);
    
    % Setup initial node
    cost_sofar(start_pos(1), start_pos(2)) = 0;
    est_total(start_pos(1), start_pos(2)) = manhattan_distance(start_pos, target_pos);
    frontier.insert(start_pos, est_total(start_pos(1), start_pos(2)));
    
    % Define 8-connected neighborhood
    neighbor_offsets = [-1, 0; 1, 0; 0, -1; 0, 1; -1, -1; -1, 1; 1, -1; 1, 1];
    
    while ~frontier.isEmpty()
        current = frontier.extractMin();
        
        % Check if goal reached
        if current(1) == target_pos(1) && current(2) == target_pos(2)
            break;
        end
        
        explored(current(1), current(2)) = true;
        
        % Process all neighboring cells
        for idx = 1:size(neighbor_offsets, 1)
            neighbor_pos = current + neighbor_offsets(idx, :);
            [nbr_row, nbr_col] = deal(neighbor_pos(1), neighbor_pos(2));
            
            % Validate neighbor position
            if nbr_row < 1 || nbr_row > grid_rows || nbr_col < 1 || nbr_col > grid_cols
                continue;
            end
            if grid_map(nbr_row, nbr_col) == 1 || explored(nbr_row, nbr_col)
                continue;
            end
            
            % Calculate tentative cost
            movement_cost = sqrt(sum((neighbor_pos - current).^2));
            tentative_cost = cost_sofar(current(1), current(2)) + movement_cost;
            
            if tentative_cost < cost_sofar(nbr_row, nbr_col)
                predecessor(nbr_row, nbr_col, :) = current;
                cost_sofar(nbr_row, nbr_col) = tentative_cost;
                est_total(nbr_row, nbr_col) = tentative_cost + manhattan_distance(neighbor_pos, target_pos);
                
                if ~frontier.contains(neighbor_pos)
                    frontier.insert(neighbor_pos, est_total(nbr_row, nbr_col));
                end
            end
        end
    end
    
    % Reconstruct path from goal to start
    route = [];
    cursor = target_pos;
    while cursor(1) ~= start_pos(1) || cursor(2) ~= start_pos(2)
        route = [cursor; route];
        cursor = squeeze(predecessor(cursor(1), cursor(2), :))';
    end
    route = [start_pos; route];
end

function dist = manhattan_distance(node_a, node_b)
    dist = abs(node_a(1) - node_b(1)) + abs(node_a(2) - node_b(2));
end

3.2 Dijkstra's Algorithm

Dijkstra's algorithm represents a special case of A* where the heuristic function h(n)=0. While this breadth-first search approach guarantees finding the shortest path, it exhibits lower computational efficiency. The MATLAB implementation follows the A* structure with the heuristic function replaced by zero.

4. Local Path Planning Algorithm Implementation

4.1 Dynamic Window Approach (DWA)

4.1.1 Algorithm Principle

The DWA algorithm operates based on a robot kinematic model. During each control cycle:

1. Velocity Sampling: Sample candidate velocities within the allowable velocity space (v, ω).
2. Trajectory Prediction: For each candidate velocity, predict the trajectory over a future time horizon T.
3. Evaluation Function: Evaluate trajectory quality based on distance to target, distance to obstacles, and velocity magnitude, selecting the optimal velocity.

4.1.2 MATLAB Implementation

function [linear_vel, angular_vel] = dynamic_window_controller(local_map, current_state, current_velocity, target_state, parameters)
    % Input: local_map (obstacle grid), current_state ([x, y, theta])
    %        current_velocity ([v, omega]), target_state ([x, y])
    %        parameters (struct with sampling resolution and constraints)
    % Output: linear_vel (optimal linear velocity), angular_vel (optimal angular velocity)
    
    % Generate velocity samples within constraints
    velocity_samples = parameters.v_min:parameters.v_res:parameters.v_max;
    omega_samples = parameters.omega_min:parameters.omega_res:parameters.omega_max;
    
    best_rating = -inf;
    optimal_v = 0;
    optimal_w = 0;
    
    % Evaluate each velocity combination
    for v = velocity_samples
        for w = omega_samples
            % Predict trajectory under constant velocity assumption
            trajectory = simulate_motion(current_state, [v, w], parameters.horizon_time, parameters.time_step);
            
            % Calculate component scores
            goal_score = 1 / (1 + euclidean_distance(trajectory(end, 1:2), target_state(1:2)));
            obstacle_score = clearance_from_obstacles(trajectory, local_map, parameters.safety_margin);
            smoothness_score = (v + abs(w)) / (parameters.v_max + abs(parameters.omega_max));
            
            % Combined evaluation score
            total_rating = parameters.alpha * goal_score + parameters.beta * obstacle_score + parameters.gamma * smoothness_score;
            
            if total_rating > best_rating
                best_rating = total_rating;
                optimal_v = v;
                optimal_w = w;
            end
        end
    end
    
    linear_vel = optimal_v;
    angular_vel = optimal_w;
end

function path = simulate_motion(initial_state, velocity, total_time, time_step)
    steps = round(total_time / time_step);
    path = zeros(steps, 2);
    
    x = initial_state(1);
    y = initial_state(2);
    heading = initial_state(3);
    v = velocity(1);
    omega = velocity(2);
    
    for t = 1:steps
        x = x + v * time_step * cos(heading);
        y = y + v * time_step * sin(heading);
        heading = heading + omega * time_step;
        path(t, :) = [x, y];
    end
end

4.2 Artificial Potential Field (APF)

The APF algorithm models the robot as a charged particle, with the goal point generating an attractive field and obstacles producing repulsive fields. The direction of the resultant force determines the robot's motion direction. MATLAB implementation requires careful consideration of local minima issues, which can be addressed through random perturbation strategies.

5. Simulation Experiments and Results Analysis

5.1 Experimental Setup

• Map: 20×20 grid map containing static obstacles (walls, furniture) and dynamic obstacles (moving pedestrians).
• Robot Specifications: Dimensions 0.5m×0.5m, maximum linear velocity 1m/s, maximum angular velocity 1rad/s.
• Evaluation Metrics: Path length L, computation time t, obstacle avoidance success rate R.

5.2 Experimental Results Comparison

5.3 Result Analysis

1. Global Algorithms (A*, Dijkstra): Achieve the shortest path lengths but cannot handle dynamic obstacles effectively (lower success rates).

2. Local Algorithms (DWA, APF): Exhibit excellent real-time performance but tend to converge to local minima (longer path lengths).

3. Hybrid Strategy (Global A* with local DWA adjustment): Balances global optimality with real-time obstacle avoidance, demonstrating superior overall performance.

4. Conclusions and Future Work

This paper presents a comprehensive implementation of global and local path planning algorithms for mobile robots in complex environments using MATLAB, with algorithm effectiveness validated through simulation. The hybrid path planning strategy (A* combined with DWA) demonstrates excellent performance across path length and obstacle avoidance success rate metrics, making it suitable for dynamic and complex environments.

Future research directions include:

1. Incorporating machine learning (such as reinforcement learning) to optimize local planning strategies.
2. Extending the approach to three-dimensional environments (such as UAV path planning).
3. Integrating multi-sensor fusion (LiDAR, vision systems) to enhance environmental perception accuracy.

References

[1] Hart P E, Nilsson N J, Raphael B. A Formal Basis for the Heuristic Determination of Minimum Cost Paths. IEEE Transactions on Systems Science and Cybernetics, 1968.

[2] Fox D, Burgard W, Thrun S. The Dynamic Window Approach to Collision Avoidance. IEEE Robotics & Automation Magazine, 1997.

[3] Wang Y. Research on Path Planning Algorithms for Mobile Robots. Harbin Institute of Technology, 2020.