Generalized Voronoi Diagram-Guided and Contact-Optimized Motion Planning for Snake Robots

Abstract

In robot motion planning in a space with obstacles, the goal is to find a collision-free path for robots from the start to the target position. Numerous fundamentally different approaches, and their many variants, address this problem depending on the types of obstacles, the dimensionality of the space and the restrictions on robot movements. We present a hierarchical motion planning framework for snake-like robots navigating cluttered environments. At the global level, a bounded Generalized Voronoi Diagram (GVD) generates a maximal-clearance path through complex terrain. To overcome the limitations of pure avoidance strategies, we incorporate a local trajectory optimization layer that enables Obstacle-Aided Locomotion (OAL). This is realized through a simulation-in-the-loop system in CoppeliaSim, where gait parameters are optimized using Particle Swarm Optimization (PSO) based on contact forces and energy efficiency. By coupling high-level deliberative planning with low-level contact-aware control, our approach enhances both adaptability and locomotion efficiency. Experimental results demonstrate improved motion performance compared to conventional planners that neglect environmental contact.

1. Introduction

Planning trajectories for mobile robots in environments with obstacles is a well-studied problem in robotics [1,2,3,4]. Motion planning algorithms encompass a variety of approaches designed to find collision-free paths in complex environments [5,6,7,8].

Typically, robots are modelled as single points or rigid bodies, which allows significant simplifications in motion planning. Under such simplification, following a safe corridor generated by a Voronoi diagram (VD) is a reasonable and widely adopted strategy for global path planning.

Generalized Voronoi diagrams (GVDs) provide an efficient geometric method to generate paths that maximize clearance from obstacles by creating a topological skeleton of the free space. GVDs reduce the complexity of path planning by representing a safe corridor around obstacles and are widely used for global path planning in robotics [9]. However, snake robots differ significantly due to their elongated, highly articulated bodies and complex dynamics [10]. These robots exploit their environment through obstacle-assisted locomotion, actively pushing against obstacles to improve stability and propulsion [11,12]. This interaction offers a promising strategy for navigating cluttered and unstructured terrains, but is not addressed by conventional roadmap planners that treat obstacles solely as entities to avoid.

Existing GVD-based planning methods typically treat obstacles as entities to avoid and do not incorporate strategies where robots actively use obstacles for locomotion, as exploited in Obstacle-Aided Locomotion (OAL). This highlights a notable gap in research combining global path planning that maximizes clearance with local obstacle exploitation for propulsion.

1.1. Main Contribution

In this paper, we propose to bridge the above mentioned gap by integrating global path planning based on GVD with local OAL control for snake robots [13,14]. This approach aims to enable efficient and robust navigation in complex environments by combining safe, clearance-maximizing paths with adaptive, obstacle-utilizing locomotion.

1.2. Paper Structure

The rest of this paper is structured as follows. Section 2 presents the principles of motion planning algorithms based on cell decomposition and roadmap methods. This section is partially based on the structure outlined in the conference overview article [15] and, to a lesser extent, in [16]. However, it has been significantly expanded to address the gradual scanning of unknown environments using sensors. This expansion encompasses both the decomposition strategy and generalized Voronoi diagrams, which can be further applied to rapidly exploring random trees. Section 3 introduces the concept of hierarchical motion planning. Section 4 covers the global path planning in a planar workspace using snake robots. Finally, Section 5 provides concluding remarks.

2. Motion Planning Approaches

Motion planning algorithms are diverse and can be grouped into several categories [5]. Among these are potential-field methods [17,18], which use artificial forces to guide motion but are prone to local minima. Cell decomposition techniques divide the space into manageable regions to systematically search for a path, yet they can suffer from combinatorial explosion and resolution limitations [19]. Roadmap methods, such as visibility graphs and Voronoi diagrams [20,21] capture connectivity information from the free space and enable efficient global planning, although the resulting paths may not be smooth [22] and are not suitable for robots with significant kinematic or dynamic constraints.

Modern planning techniques often rely on probabilistic sampling of the configuration space [23] to build roadmaps or search trees. Representative examples include the Probabilistic Roadmap Method (PRM) and Rapidly exploring Random Trees (RRT) [24,25,26].

These sampling-based approaches scale well to high-dimensional spaces and offer probabilistic completeness [1,27].

Optimization-based planning formulates the motion planning problem as one minimizing a cost function that typically balances path length, smoothness, and dynamic feasibility. Methods such as CHOMP and TrajOpt have demonstrated a strong performance in generating feasible, smooth trajectories for robots with complex dynamics and constraints [28].

Recently, learning-based approaches, including reinforcement learning and imitation learning, have emerged to enable adaptive and data-driven planning strategies. By leveraging experience and environment interaction, these methods aim to improve planning efficiency and robustness in uncertain or changing environments [29].

From the wide range of motion planning methodologies, this paper focuses on approaches based on Voronoi diagrams. We emphasize tuning the paths generated along the Voronoi edges to enable a snake-like robot to safely and efficiently navigate near obstacles, exploiting this proximity to improve navigation performance and energy efficiency [30,31].

2.1. Decomposition into Cells

To begin, we examine the problem of robot motion planning in a simplified setting where it is equivalent to guiding a point within the free space F. Under this formulation, the cell decomposition approach may be expressed in the following manner [2]:

1.

Partition the free space F into connected subregions, referred to as cells.

2.

Identify pairs of cells that share a common boundary, and represent these relationships using an adjacency graph. In this graph, vertices correspond to individual cells, while edges connect cells that are directly adjacent.

3.

Determine the specific cells that contain the start and goal positions, and then search within the adjacency graph for a path linking these two cells.

4.

Based on the ordered sequence of cells obtained in the previous step, construct a continuous path by joining the designated points within each cell—such as their centroids—through the midpoints of their shared boundaries.

When the environment is partitioned into square cells, the navigation task can be represented as a succession of movements along any of eight possible directions. Despite its simplicity, this approach exhibits several inherent limitations. Given a scene composed of $m\times n$ cells, and noting that the generated trajectories do not ensure a minimum number of elements in the sequence to ensure connectivity between the start and target positions, it is reasonable to prescribe the sequence length as no less than $2\times max(m,n)$. The size of the search space is then $8^{2\times max(m,n)}$. Let us consider $m=n=30$. Then the size of the search space is $8^{2\times 30}={\left(2^3\right)}^{60}=2^{180}={\left(2^{10}\right)}^{18}={\left(1024\right)}^{18}>{\left({10}^3\right)}^{18}={10}^{54}$. In such an extensive search space, determining the shortest possible trajectory becomes computationally infeasible within a reasonable timeframe, necessitating the application of a suitable heuristic method. Evidently, the selected approach is subject to the phenomenon of combinatorial explosion.

On the other hand, refining the grid is limited by the requirement that the robot must not exceed cell boundaries with its projection onto the plane.

However, in these approaches, we most often assume that the robot is represented as a point, because its size can be added to the size of the obstacles using Minkowski sums. Even in such scenarios, movements between free cells may be infeasible in certain configurations, particularly when a diagonal move between two free cells would intersect the vertex of a polygonal obstacle, thereby causing a collision.

A further major limitation of employing heuristic methods for generating movement sequences lies in the proliferation of infeasible solutions, arising from collisions with obstacles or from trajectories that extend beyond the workspace boundaries.

When a database of previously solved instances is available and the newly specified start–target pair closely matches one of these cases, the case-based reasoning approach [19] can be employed. In this approach, a search is conducted from the new start point to the corresponding start point of the stored case and, likewise, from the new target point to the stored one. These newly computed segments are then concatenated with the pre-existing solution path. Nevertheless, this strategy does not ensure optimality, as a direct search performed independently of the database may produce a shorter trajectory.

In a more sophisticated implementation of case-based reasoning, neighbourhoods are constructed around the new start and target positions. The nearest intersections between these neighbourhoods and the paths stored in the database are then identified, with these intersection points being connected to the start and target to produce the complete solution. However, the principal difficulty lies in the generation and ongoing maintenance of a sufficiently comprehensive database of known solutions—a task rendered particularly challenging by the complexity outlined above.

2.2. Progressive Detection of Obstacles Within a Partitioned Environment

In the preceding subsection, we presumed a scene with a fully mapped layout of static obstacles.

In contrast, real-world environments are often partially known—or entirely unknown [32]. In such settings, obstacle positions may change, and detection capabilities are constrained by the limitations inherent in the employed sensors.

These scenarios may be solved using the D* Lite algorithm, originally developed by Koenig and Likhachev [33] and implemented by Jukic [34]. The subsequent paragraphs present the experiments employing these tools.

For the sake of clarity, we assume that diagonal transitions between adjacent free cells are permissible even when obstacles occupy both neighbouring sides. A square centered on the robot indicates the detection range of its sensors, while a red line denotes the shortest available path to the target, determined using only the information within this perceptual boundary. A red point denotes the robot’s current position, while a green point designates the target location. Layouts beyond the sensor’s range remain undetermined; only the start and target positions are predefined.

Cells that the robot has previously occupied are shaded in gray to indicate its traversed path. In the right panel of Figure 1, the depicted shortest path intersects an obstacle—this occurs because the robot has not yet sensed that obstacle within its detection range.

Figure 2 presents trajectories for two nearly identical environments. In the right-hand scene, the square obstacle includes an aperture simulating the entrance to a room. Upon entering this space, the robot eventually exits, since the target location lies outside; only thereafter does the robot proceed along the perimeter of the obstacle, mirroring the behaviour observed in the left scenario.

Figure 3 illustrates the robot’s response to a modification of the obstacle on the right, wherein a new opening has been created.

Although the preceding examples may suggest that the algorithm reliably identifies a path to the target location following modifications or displacements of obstacles, this assumption does not universally hold. A counterexample illustrating its failure is presented in Figure 4. In this instance, the algorithm retains a memory of previously mapped regions within the environment. As the newly introduced opening lies outside the current sensor range, it remains undetected. Consequently, the robot relies on outdated information, interpreting the area as obstructed based on the stored map. As a result, it refrains from exploring this direction, fails to discover an alternative route, and ultimately does not reach the designated target.

2.3. Roadmap Methods

Within roadmap-based methods, the principal strategies are those founded on visibility graphs, Rapidly exploring Random Trees (RRT), Voronoi diagrams and generalized Voronoi diagrams.

A visibility graph is defined as a graph whose vertex set consists of the start position, the target position, and all vertices of the polygonal obstacles [21,35]. The edge set comprises both the obstacle edges themselves and all additional edges connecting every pair of vertices that are mutually visible. For a plane containing polygonal obstacles, the shortest path between any two points can be efficiently determined by constructing the corresponding visibility graph and applying Dijkstra’s algorithm. In the case of a collection of disjoint polygonal obstacles having a total of k edges, the visibility graph can be generated in $\mathcal{O}(k^{2}logk)$ [21].

A notable limitation of shortest paths derived from visibility graphs is that they come into contact with obstacles, either at their vertices or along their edges, thereby compromising safety. This issue can be mitigated through the use of Voronoi diagrams.

The Rapidly exploring Random Tree algorithm begins its growth from the starting position, which serves as the root node of the tree. At each iteration, a random point is generated and connected to the existing tree via the shortest feasible path.

Naturally, any points that fall within obstacle-occupied regions, as well as those whose connecting lines to their nearest neighbours would intersect an obstacle, are excluded from consideration. Given that the start and target positions define the intended direction of the robot’s movement, this directional constraint can be incorporated into the point-generation process. For instance, one may impose a limit on the maximum allowable deviation from this direction for any point generated.

A Voronoi diagram associated with a given set of sites in the plane partitions the plane into a collection of regions, each uniquely corresponding to one site. Every point contained within a particular region lies closer to its associated site than to any other site in the set [2,20,36].

Consider $d(p_i,p_j)$ as the Euclidean distance between $p_i=(x_i,y_i)$ and $p_j=(x_j,y_j)$ in the two-dimensional plane. With this definition, the Voronoi diagram may be rigorously formulated in mathematical terms.

Definition 1. 

Let $P=\{p_1,p_2,\dots ,p_n\}\subset {\mathbb{R}}^2$ denote a finite set of points with the Cartesian coordinates $(x_1,y_1),\dots ,(x_n,y_n)$ where $2<n<\infty$ and $p_i\ne p_j$ for $i\ne j$. We call the region

$$V\left(p_i\right)=\{x\in {\mathbb{R}}^2|d(x,p_i)\le d(x,p_j)\mathrm{for}j\ne i\}$$

(1)

the planar Voronoi polygon associated with $p_i$ (or the Voronoi polygon of $p_i$) and the set given by

$$V=\{V\left(p_i\right),\dots ,V\left(p_n\right)\}$$

(2)

the planar Voronoi diagram generated by P (or the Voronoi diagram of P). We call $p_i$ of $V\left(p_i\right)$ the site or generator point or generator of the i-th Voronoi polygon and the set $P=\{p_1,\dots ,p_n\}$ the generator set of the Voronoi diagram V. Hence, we get

$$V\left(P\right)=\bigcup _{p_i\in P}V\left(p_i\right)=\bigcup _{p_i\in P}\left\{x\in {\mathbb{R}}^2|d(x,p_i)\le d(x,q):\forall q\in (P-\left\{p_i\right\})\right\}=$$

$$=\bigcup _{p_i\in P}\left[\bigcap _{q\in P-p_i}\left\{x\in {\mathbb{R}}^2|d(x,p_i)\le d(x,q)\right\}\right]$$

(3)

For reasons of time complexity, it is necessary to know the properties of the Voronoi diagrams and the algorithms of their constructions.

Let n be the number of sites of the Voronoi diagram, $V_{VD}$ be the set of its vertices, and $E_{VD}$ be the set of its edges. Then the following relations hold:

(i)

$|V_{VD}|\le 2n-5$ and

(ii)

$|E_{VD}|\le 3n-6$.

The algorithms used to construct Voronoi diagrams (divide and conquer, incremental and plane sweep) run in $\mathcal{O}(nlogn)$ time.

Obstacles with polygonal shape were represented solely by their vertex sets, and conventional Voronoi diagram techniques can be employed. However, such trajectories lacked smoothness. Instead of treating each obstacle via its line-approximated boundaries, we can compute Generalized Voronoi diagrams where the exact bisectors between sites—whether point-to-point, point-to-line, or line-to-line—are as follows: (a) The bisector of two points is the perpendicular bisector of the segment connecting them. (b) The bisector between a point and a line is a parabolic arc whose focus is the point and whose directrix is the line. (c) The bisector of two lines is the angle-bisector of their intersecting lines. Thus, the edges of the generalized Voronoi diagram consist of straight-line segments and parabolic arcs.

An illustrative example of RRT, VD and GVD is presented in Figure 5.

2.4. Application of Roadmap Geometric Structures

As a rule, the distance of a newly generated point from the current RRT tree is restricted; see [1]. If a point of the exploring tree reaches a position within a sufficiently small distance from the target, then it is directly connected to the target and further points are not generated.

The drawback of the RRT is a high number of generated edges, resulting in a broken trajectory. It can be smoothed, to a certain extent, using splines, see Figure 6.

Nevertheless, it is evident that RRTs are also applicable in contexts where the environment is entirely or partially unknown.

When the generator set of a Voronoi diagram corresponds exclusively to point-type obstacles and no other forms of obstacles exist in the plane, the robot may traverse exclusively along those Voronoi edges that delineate channels, maximizing clearance from obstacles—excluding only the segments at the start and end of the route. Consequently, the robot’s motion-planning task can be reformulated as a graph-search problem: we introduce a subgraph of the Voronoi diagram comprised solely of edges that the robot can traverse. Should certain Voronoi edges prove impassable, they are excluded from this subgraph.

Of course, upon initially constructing the configuration space of obstacles, it is unnecessary to verify whether the robot can traverse along the edges of the Voronoi diagram.

In environments featuring point obstacles, straight-line barriers, and polygonal shapes, one of the most straightforward methods to identify optimal trajectories is to construct standard Voronoi diagrams based on obstacle vertices, subsequently eliminating any edges that intersect the obstacle regions. To achieve greater solution fidelity, one can approximate the polygonal boundaries by line segments and then apply the aforementioned method [15,38].

An implementation of this approach is described in [38,39]. Using the latter program, we determine the number of line segments used to approximate the edges of polygonal obstacles and compute a Voronoi diagram with more precise geometry. Impassable edges are then removed, and a limited Voronoi decomposition is computed for subsequent snake-robot path planning.

The diagram is ’limited’ in the sense that parallel edges do not extend to infinity, rather we limit the diagram by the surrounding box so that the robot can still use its edges in its planned path, we assume that there are no obstacles in the direct vicinity of the outer scene closer than $0.2$ m that may result in a path impassable for the snake robot.

Figure 7, Figure 8 and Figure 9 show Voronoi diagrams with line, circular and polygonal obstacles. Twenty edge segments are illustrated before and after the redundant edge is removed.

Generalized Voronoi diagrams can also be incrementally constructed during a scene scan using sensors.

3. Hierarchical Motion Planning: Bounded Voronoi Guidance and Simulation-in-the-Loop-Based Locomotion Optimization

This section presents a hierarchical architecture for snake-robot planning and control, combining global path planning via bounded Voronoi decomposition with local trajectory adaptation based on OAL. The local control operates within a Simulation-in-the-Loop (SIL) framework, similar to that presented in [30], providing physically grounded feedback for motion optimization.

First, we compute a bounded GVD within the 2D environment, using obstacle boundaries and a surrounding rectangular hull to truncate infinite edges. This decomposition yields a graph structure that maximizes clearance from obstacles and serves as a roadmap for path search (e.g., with A*). By restricting edges that are impassable or too narrow, we ensure that the resulting path retains maximum clearance corridors. To account for contact interactions required by snake locomotion, we locally expand the configuration space around the vertices of the selected Voronoi path. Neighbourhoods are generated in the vicinity of nearby obstacles, producing an intermediate representation that is larger than the Voronoi skeleton but significantly smaller than the overall free space. This allows for increased maneuverability while maintaining computational efficiency.

At the local level, motion is refined using SIL-based optimization. Successive waypoints along the planned path define motion segments, each of which is optimized to exploit terrain contacts through OAL. The SIL framework couples MATLAB 2024 for control optimization with CoppeliaSim for physics-based simulation [40]. Gait parameters are tuned using Particle Swarm Optimization (PSO), with cost functions defined by simulated contact forces and energy consumption. This integration enables the snake robot to leverage obstacles for propulsion rather than simply avoiding them, resulting in more robust and efficient locomotion.

4. Global Path Planning Using Bounded Voronoi Decomposition

In the present paper, we consider a 10 m × 16 m planar workspace populated with heterogeneous obstacles. The environment is divided into two regions: the lower half contains randomly distributed cylindrical obstacles, while the upper half includes fixed polygonal obstacles that form narrow passages (Figure 7). This setup introduces both stochastic variability and structured constraints, providing a representative testbed for evaluating motion planning strategies.

To generate a global navigation strategy, we employ a bounded Voronoi decomposition as described in [39]. This process partitions the free space into convex corridors that maximize clearance from nearby obstacles (Figure 9), facilitating graph-based path planning. This work adopts an A*-based search to identify a sequence of waypoints that connect the start and goal positions.

Although GVD-based planning ensures obstacle avoidance and safe passage, it inherently favours maximum-clearance paths, which are not always ideal for snake robots. Unlike wheeled or legged robots, snake robots benefit from deliberate contact with obstacles to enhance propulsion and stability through OAL. The interactions with the environment are crucial in OAL and can significantly enhance propulsion efficiency and stability, especially in constrained spaces [41,42].

4.1. Local Obstacle-Aided Locomotion Optimization via Simulation-in-the-Loop

To overcome the limitations of conservative global plans, we integrate a local planning module that dynamically optimizes the robot’s gait to exploit nearby terrain contacts, implementing OAL. This module operates within an SIL system, coupling MATLAB with the CoppeliaSim physics engine, adapting the workflow of our previous work [30]. The SIL framework simulates detailed interactions between the snake robot and the environment for each local path segment (Figure 10 and Figure 11).

The snake robot is actuated via a parameterized serpentine gait. These gait parameters are optimized using Particle Swarm Optimization in MATLAB. During each PSO iteration, candidate gait parameters are transmitted to CoppeliaSim via its remote API. The simulator computes the resulting forward velocity, actuator energy consumption, and reaction forces at each obstacle contact point, which are then returned to MATLAB for evaluation. Performance is quantified through a cost function that balances locomotion efficiency against penalties associated with contact forces.

$$J=\sum _{t=1}^T\left(v_x\left(t\right)-\zeta \sum _{j=1}^M\left|F_{oj}\left(t\right)\right|\right),$$

(4)

where $v_x\left(t\right)$ is the forward velocity at time t, $F_{oj}\left(t\right)$ is the magnitude of the reaction force at the $j^{th}$ obstacle contact point, and $\zeta$ is a weighting factor that governs the trade-off between speed and contact-induced penalties such as material wear or structural stress. Larger reaction forces typically indicate stronger collisions, which may result in surface damage or deformation of the robot body.

The parameter $\zeta$ plays a critical role in shaping the robot’s interaction strategy with obstacles:

• Lower values of $\zeta$ promote aggressive obstacle utilization, enabling the robot to exploit contact forces for enhanced propulsion.
• Higher values of $\zeta$ encourage softer interactions, prioritizing the preservation of structural integrity and minimizing potential damage.

The cost function in (4) can be interpreted as a scalarized multi-objective optimization problem, where forward velocity $v_x\left(t\right)$ promotes rapid progression along the planned path, while the summed obstacle reaction forces ${\sum }_{j=1}^M\left|F_{oj}\left(t\right)\right|$ penalize excessive contact interactions. This weighted-sum formulation is commonly used to balance competing objectives and provides a tunable trade-off between locomotion performance and mechanical safety.

In obstacle-aided locomotion, speed and contact force are inherently coupled: stronger interactions with the environment can increase propulsion, whereas limiting contact forces may constrain achievable speed. The weighting factor $\zeta$ governs this trade-off and shapes the resulting interaction strategy. Importantly, the force penalty does not eliminate obstacle contacts but regularizes their intensity by discouraging impulsive or excessively large reaction forces, which are typically associated with inefficient collisions or unstable body dynamics.

In a previous study [30], four distinct levels of obstacle utilization were defined based on the value of $\zeta$:

• High Utilization: $\zeta =0.01$.
• Medium Utilization: $\zeta =0.3$.
• Low Utilization: $\zeta =0.6$.
• Obstacle Avoidance: $\zeta ={10}^3$.

It was shown that the high utilization setting is particularly well-suited for environments with intermediate to high obstacle density. Accordingly, the value of $\zeta =0.6$ is adopted in this work to leverage contact interactions for improved locomotion performance.

The SIL methodology comprises a bidirectional interface between MATLAB and CoppeliaSim, enabling dynamic evaluation of candidate gaits. At each iteration of PSO, MATLAB generates a set of gait parameters, which are transmitted to CoppeliaSim through its remote API. CoppeliaSim then performs a physics-based simulation, recording contact forces, power consumption, and the resultant motion. These data are returned to MATLAB, where the cost function is evaluated. Over successive iterations, PSO converges toward gait parameters that leverage controlled obstacle interactions to improve locomotion efficiency while maintaining feasible dynamics.

4.2. Discussion

Unlike earlier OAL approaches that primarily emphasized local control in environments densely populated with obstacles (e.g., [12,33]), or relied on carefully designed settings that inherently supported locomotion, the framework presented here integrates OAL into a hierarchical motion planner. The central novelty lies in the fact that obstacle exploitation is not an end in itself but is systematically coordinated with a bounded Voronoi global planner. This allows global paths to be computed with guaranteed clearance, while local interactions with nearby obstacles are selectively exploited to improve locomotion efficiency, offering both global safety and local energetic benefits. Although the SIL-based module is instantiated in this work using a Voronoi-diagram global planner, the SIL methodology itself is more general and can interface with other global planning or space-partitioning schemes. For example, community-detection–based Voronoi task allocation methods and the indoor space partitioning and complementary search planning framework proposed by Chen et al. [43] could, in principle, provide structured traversal inputs to a suitably reformulated SIL layer.

We emphasize that the results presented in this study are obtained entirely through the SIL framework; validating the approach on physical hardware constitutes an important direction for future work.

4.3. Simulation Results and Performance Analysis

The proposed hierarchical planning framework was evaluated in the environment shown in Figure 7. The global path was first generated using the bounded Voronoi decomposition, followed by local trajectory refinement via SIL-based optimization of the OAL controller.

To assess statistical robustness and to test the planner’s adaptability, the experiment was repeated over a set of thirty distinct scenarios. In each scenario, the twelve cylindrical obstacles in the lower half of the workspace were randomly resampled in both size and position, and the start positions were uniformly sampled within the range $y<1.5$ and $x\in [0.3,9.7]$, while maintaining the same target position for all runs. The upper half of the workspace was left unchanged in order to preserve the narrow-passage structure and maintain a consistent global planning challenge. The same thirty scenarios were then rerun after removing a single obstacle in the upper region, thereby creating an open corridor and allowing the planner to autonomously choose whether to pass through the narrow gap or bypass it.

This dual setup provides complementary perspectives: the first set of simulations, with the narrow passage enforced as the only one feasible, tests the robot’s ability to navigate constrained environments safely, while the second set evaluates how the planner adapts when a low-resistance path becomes available. To quantify these effects and have comparative analysis, three representative starting positions were considered dividing the horizontal range into three equal segments, defining distinct starting zones: left ($x\in [0.3,3.4]$), middle ($x\in [3.4,6.6]$), and right ($x\in [6.6,9.7]$).

Each configuration was tested for both the Voronoi-only planner and the SIL-refined OAL controller. The performance metrics include the total trajectory length, total energy consumption across all joints, and the corresponding normalized energy cost per meter. The absolute performance results are summarized in

Table 1. Comparison between SIL-Optimized OAL and Voronoi-Only Planning Across Different Start Positions and Environment Configurations.

Passage Condition	Start Position	Path Length [m]		Energy [J]		Energy/m [J/m]
		Voronoi	SIL	Voronoi	SIL	Voronoi	SIL
Narrow	Left	$23.98$	$22.12$	$4035.5$	$3412.1$	$168.3$	$154.2$
	Middle	$27.55$	$24.44$	$4708.3$	$3824.9$	$170.9$	$156.5$
	Right	$30.59$	$29.01$	$5035.1$	$4581.5$	$164.6$	$157.9$
Open	Left	$23.72$	$22.08$	$3980.2$	$3376.0$	$167.8$	$152.9$
	Middle	$25.8$	$22.4$	$4218.3$	$3380.1$	$163.5$	$150.9$
	Right	$23.6$	$21.3$	$3759.5$	$3224.8$	$159.3$	$151.4$

, while the corresponding average percentage improvements of the SIL-optimized controller over the Voronoi baseline are reported in

Table 2. Percentage improvement of SIL-Optimized OAL over Voronoi-only planning across different start positions and passage configurations.

Passage Cond.	Start Pos.	Path Length [%]	Energy [%]	Energy/m [%]
Narrow	Left	$7.78$	$15.45$	$8.38$
	Middle	$11.29$	$18.76$	$8.43$
	Right	$5.17$	$9.01$	$4.07$
Open	Left	$6.91$	$15.18$	$8.88$
	Middle	$13.18$	$19.87$	$7.71$
	Right	$9.75$	$14.22$	$4.96$

.

Across the thirty randomized scenes, the SIL-optimized OAL controller consistently outperformed the Voronoi-only baseline. Whether squeezed through a narrow passage or given freedom to roam in an open corridor, the SIL-refined controller reconfigures its motion to exploit the very obstacles that slow the global planner down, effectively converting corners and cylindrical barriers into propulsive partners. This results in a notable reduction in total energy expenditure.

When the upper obstacle is removed and additional low-resistance corridors become available, the planner adapts accordingly. In this more permissive environment, the largest relative improvements occur in total energy consumption, while the normalized energy efficiency (energy per meter) also shows a positive mere benefit of approximately $0.2\%$.

Across all starting zones (left, middle, and right), the improvements exhibited a clear and repeatable pattern. The SIL-based controller provided benefits for every initial condition, indicating that the combined global–local planning strategy is robust to variability in both starting position and obstacle configuration. Despite substantial diversity in obstacle placement and path topology, the optimized trajectories converged reliably to efficient solutions.

Overall, these results indicate that the integration of obstacle-aware locomotion within a hierarchical global–local framework materially improves traversal efficiency. Rather than treating obstacles solely as constraints, the SIL-optimized controller identifies when and how to leverage them to reduce energetic cost while maintaining reliable forward progress. This demonstrates the potential of combining Voronoi-based global planning with SIL-tuned local control to produce adaptive, resource-efficient locomotion strategies suitable for complex or partially unknown environments.

5. Conclusions

In this paper, we briefly summarize the main approaches to robot motion planning, highlighting their strengths and limitations, and demonstrate a hierarchical motion planning framework for snake robots that combines bounded Voronoi-based global path planning with SIL optimization of obstacle-aided locomotion. Simulation results demonstrate that the proposed approach significantly improves locomotion efficiency compared to Voronoi-based planning alone. The SIL-optimized controller has achieved average reductions in total energy expenditure of $16.4\%$ and $14.4\%$ in the open and narrow-passage conditions, respectively, along with corresponding reductions in energy per meter traveled of $7.2\%$ and $7\%$, while also producing shorter and more dynamically feasible trajectories. These improvements highlight the importance of incorporating contact-aware strategies, particularly for snake robots, where deliberate obstacle interactions can serve as a functional mechanism for enhanced mobility. Future work will focus on onboarding real-time sensing and, most importantly, experimental hardware validation on physical snake robots to assess robustness.