Risk-aware Pathfinding and Trajectory Planning

Simulation Environment and Grid Setup

The field was modelled as a 120 m by 90 m environment and divided into a grid using a 0.5 m cell resolution. This produced a 240 by 180 cell search space, balancing computational performance with enough spatial resolution for robot movement and obstacle interaction.

The robot started near the centreline of the left edge of the field at coordinates (0.25, 45.25). Four circular target zones were placed at known field locations, with each target represented as a small 2 by 2 cell goal region to make collision-free goal detection more reliable.

Obstacle and Occupancy Map Modelling

Fifteen rectangular obstacles were placed at fixed central positions throughout the field. Their widths were varied across simulation runs, with the longer edge defined as twice the obstacle width. This allowed the same navigation problem to be tested under increasingly constrained obstacle conditions.

An occupancy map was generated by checking each grid cell against the obstacle geometry. Cells overlapping an obstacle were marked as blocked and excluded from the valid search space used by the pathfinding algorithms.

Danger Map and Risk Penalty

In addition to the binary occupancy map, a Danger Map was created to represent hazard proximity around obstacles. Grid cells near obstacles were assigned increasing risk levels based on their Manhattan distance from the nearest obstacle, with risk extending outward within a 6-cell radius.

The A* algorithm incorporated this risk map by adding a danger penalty to the cost function. This encouraged the robot to avoid high-risk zones where possible, even when the safer route required a longer path.

f(n) = g(n) + h(n) + dangerPenalty

Pathfinding Implementation

Both algorithms operated on the same grid environment, with robot movement limited to four directions: up, down, left, and right. The pathfinding logic was structured into reusable segment-level functions, allowing the robot to move between successive target regions using the same planning process.

Before path planning, the target visitation order was optimised by testing permutations of the target sequence and selecting the route with the lowest Euclidean travel cost. This kept the target order consistent between A* and Greedy, allowing their behaviour to be compared more fairly.

A* Search Behaviour

A* combined the cost already travelled, the estimated cost to the target, and the additional danger penalty. This made it more suitable for risk-aware navigation because it could favour safer paths instead of simply selecting the most direct route.

In the simulation results, A* maintained fully safe paths for obstacle widths between 1 m and 8 m. As the obstacle width increased beyond this point, the environment became more constrained and the algorithm began accepting low, medium, and eventually high-risk cells where no fully safe route was available.

Greedy Best-First Search Behaviour

Greedy Best-First Search used only the heuristic distance to the target when selecting the next cell. This made the method more direct, but it did not consider cumulative path cost or the longer-term safety of the route.

This behaviour became clear as obstacle widths increased. Greedy often produced shorter routes than A*, but it entered high-risk areas much more frequently because it prioritised progress toward the target over risk avoidance.

Results and Performance Comparison

The results showed a clear trade-off between path safety and directness. At 12 m obstacle width, A* travelled 915 cells and included 26 high-risk cells. Greedy travelled 795 cells, but included 525 high-risk cells, meaning a large portion of its final path passed through unsafe regions.

This comparison demonstrated that Greedy could produce a shorter path, but at the cost of significantly greater risk exposure. A* produced a longer route, but remained much more suitable for safety-conscious navigation.

Development Challenges

A significant challenge involved managing MATLAB subplot visualisation. Combining obstacle overlays, occupancy maps, hazard maps, and animated robot paths introduced issues with overlapping axes, inconsistent scaling, and visual clutter.

Animation updates also required careful handling. Earlier versions produced ghosting effects or duplicate breadcrumb markers because previous plot elements were not always cleared or updated correctly. The final implementation used more controlled plot updates to show the robot movement while preserving the intended path trail.