SHIFT Planner: Speedy Hybrid Iterative Field and Segmented Trajectory Optimization with IKD-tree for Uniform Lightweight Coverage

Cao et al. [5] introduced a hierarchical framework that decomposes complex 3D spaces into manageable subregions, facilitating efficient coverage. However, this approach primarily focuses on spatial decomposition and may not adequately account for dynamic environmental attributes such as varying levels of dirtiness or dryness, which are crucial for adaptive coverage tasks.

Jing et al. [6] proposed a sampling-based view planning algorithm for UAVs to acquire visual geometric information of target objects. While effective in viewpoint selection, this method may not fully integrate real-time sensor data for terrain representation, potentially limiting its responsiveness to dynamic environmental changes.

Feng et al. [7] developed FC-Planner, a skeleton-guided planning framework that decomposes the scene into simple subspaces using skeleton-based space decomposition. Despite its computational efficiency, it may not sufficiently model environmental attributes like dirtiness or dryness, which are essential for tasks requiring adaptive coverage based on such factors.

The APF-CPP approach employs artificial potential fields to facilitate online coverage path planning for multiple robots in dynamic environments. However, it may encounter challenges related to local minima and might not effectively integrate preloaded terrain data or previously established semantic maps, which are vital for informed planning processes.

While existing methods have advanced obstacle avoidance and path optimization, they often fall short in addressing certain critical challenges in practical coverage tasks, such as:

• •

  Uniform Coverage: Ensuring equal attention across the entire workspace without overly focusing on shortest path considerations.

• •

  Environmental Attribute Modeling: Dynamically adjusting robot behavior based on spatial properties like dirtiness or dryness, ensuring coverage planning adapts to real-world conditions.

• •

  3D Environment Adaptability: Extending traditional 2D planning methods to complex 3D surfaces while maintaining efficiency and uniformity in the coverage.

• •

  Real Sensor Data Integration: Utilizing actual terrain data from various sensors, such as monocular cameras, IMUs, GPS, and terrain point clouds, to accurately represent the environment.

Moreover, existing optimization methods based on ESDF (Euclidean Signed Distance Fields) mapping introduce significant memory overhead and may compromise precision, limiting their effectiveness in large or complex scenarios. These methods also tend to focus on optimizing kinematic properties such as velocity, acceleration, jerk, and snap. However, they often lack direct integration with specific planning tasks, leading to a gap between path smoothness and actual task efficiency.

In this paper, we propose a trajectory planning approach that integrates these aspects, specifically addressing the needs of real-world cleaning or coverage tasks. By coupling the optimization process with the requirements of the practical planning task, we ensure efficient and task-relevant trajectory generation that balances uniform coverage with kinematic optimality.

We represent the environment using a parametric surface based on the terrain point cloud collected from sensors:

where $u$ and $v$ are parameters defining the surface.

To capture the intrinsic properties of the surface, we compute the first and second fundamental forms.

First Fundamental Form:

where:

Second Fundamental Form:

where:

and $\mathbf{n}$ is the unit normal vector:

Gaussian Curvature $K$:

Mean Curvature $H$:

These curvature measures help in identifying critical regions on the surface for planning purposes.

To improve the accuracy of the elevation data and ensure the surface model is suitable for path planning, we utilize the Gaussian curvature $K$ and Mean curvature $H$ to identify and smooth out abrupt protrusions and irregularities on the surface.

After obtaining a smooth surface, we proceed to extract the elevation coordinates essential for terrain analysis and path planning.

We begin by discretizing the parameter domain $(u,v)$ of the smoothed surface $\mathbf{S}(u,v)$ to create a uniform grid of parameters $\{(u_{i},v_{j})\}$. This grid serves as the foundation for the placement of landmark points on the terrain.

Parameter Domain Discretization:

Let $u\in[u_{\text{min}},u_{\text{max}}]$ and $v\in[v_{\text{min}},v_{\text{max}}]$. We define:

where $i=0,1,\ldots,N_{u}$, $j=0,1,\ldots,N_{v}$, and $\Delta u$, $\Delta v$ are the parameter increments.

Each grid point $(u_{i},v_{j})$ is mapped onto the 3D surface to obtain the corresponding landmark point $\mathbf{p}_{i,j}$.

Surface Point Evaluation:

Elevation Offset for Clearance:

To ensure the navigating agent maintains a safe distance above the terrain, an elevation offset $z_{\text{offset}}$ is added:

We sequence the adjusted landmark points $\mathbf{p}_{i,j}^{\text{adj}}$ in a boustrophedon (zigzag) pattern to ensure systematic and complete coverage of the terrain.

We use the A* algorithm with an enhanced heuristic that incorporates a potential field derived from obstacle distances.

Heuristic Function:

where:

• •

  $g(\mathbf{p})$: Cost from the start node to current node $\mathbf{p}$.

• •

  $h(\mathbf{p})$: Heuristic estimate from $\mathbf{p}$ to the goal.

• •

  $\Psi(\mathbf{p})$: Potential field function.

• •

  $\alpha$: Weighting factor.

We define the potential field $\Psi(\mathbf{p})$ based on the inverse of the distance to the nearest obstacle:

where $d(\mathbf{p})$ is the Euclidean distance to the nearest obstacle, and $\epsilon$ is a small constant.

When a segment of the path is within a safety distance $d_{\text{safe}}$ from obstacles, we perform local optimization.

Cost Function $J$:

Our system generates a Panoptic Occupancy Local Semantic Map by processing data from monocular cameras and an IMU. This map provides detailed information about the environment, including the location and classification of obstacles. The localization information is aligned with the IMU data to accurately position the autonomous agent within the map.

To initiate the planning process, we utilize preloaded terrain data or previously established semantic information, such as a prior semantic map. We implement the proposed Radiant Field-Informed Coverage Planning (RFICP) algorithm, which computes coverage trajectories and velocity assignments for efficient path planning.

During navigation, the autonomous agent must continuously assess the planned path for compliance with safety and efficiency criteria, especially in the presence of dynamic obstacles. We introduce the Circular Judgment and Assessment Module to identify regions along the path that violate the defined path cost function due to obstacles or environmental changes.

To optimize the identified non-compliant path segments $\mathcal{S}_{\text{nc}}$, we introduce the Incremental IKD-tree Sliding Window Optimization (IKD-SWOpt) algorithm. This method leverages an incremental KD-tree for efficient obstacle representation and employs BFGS optimization to refine the path within a sliding window.

where:

• •

  $\mathbf{P}*i$: Control points.

• •

  $N*{i,k}(t)$: B-spline basis functions of degree $k$.

• •

  $t\in[0,1]$.

The coverage effectiveness at a point depends on the robot’s speed and the time spent over that point.

Coverage Function: The cumulative coverage $C(x,y,t)$ at point $(x,y)$ over time $t$.

In RFICP, the diffusion field captures how a robot’s activity at one position influences surrounding areas. This model ensures that coverage is not treated in isolation but instead incorporates spatial relationships to achieve uniformity.

To address varying environmental attributes, RFICP sets a desired coverage $C_{\text{desired}}(x,y)$ proportional to the attribute $A(x,y)$, such as dirtiness or dryness:

The time $t(x,y)$ required to achieve the desired coverage can be derived by assuming the robot’s activity near $(x,y)$ dominates its effective coverage. Simplifying the total effective coverage:

Solving for $t(x,y)$:

The RFICP framework applies the speed adjustment strategy as follows:

1. 1.

   Initialization:

   • •

     Input environmental attribute map $A(x,y)$, coverage parameters $C_{\text{target}}$, and robot constraints $v_{\text{min}},v_{\text{max}},a_{\text{max}}$.

2. 2.

   Coverage Time Computation:

   • •

     Compute the required time $t(x,y)$ at each position using the derived formula.

3. 3.

   Dynamic Speed Planning:

   • •

     Derive speed $v(x,y)$ from $t(x,y)$.

   • •

     Smooth speed transitions by enforcing acceleration limits.

4. 4.

   Trajectory Integration:

   • •

     Integrate the computed speed profile $v(x,y)$ into the global trajectory, ensuring smooth and efficient coverage.

Platforms:

• •

  Robotic Vacuum Cleaner: Tested in indoor environments with varying levels of dirtiness.

  

  Fig. 12 shows a real robot scenario in a semantic-labeled environment (dirtiness map). The robot, guided by SHIFT, slows down in dirty areas to ensure thorough coverage, and speeds up in cleaner regions to optimize efficiency.

  VI-B Additional Comparative Experiments

• •

  Agricultural Drone: Tested in outdoor terrains with varying crop conditions.

Sensors: LiDAR, camera, IMU, odometer.

• •

  Terrain Point Clouds: Collected using onboard LiDAR sensors.

• •

  Environmental Attributes: Dirtiness levels detected using camera sensors and semantic segmentation.

Our method efficiently generates feasible paths without the need for complex region decomposition.

• •

  Computation Time: Reduced by approximately 30% compared to traditional methods.

• •

  Memory Usage: Lower due to the elimination of cell decomposition data structures.

The local optimization effectively adjusts the trajectory in critical areas.

• •

  Convergence: The optimization converges within a few iterations due to the analytical gradient computation.

• •

  Safety Margin: Maintained a safe distance from obstacles, improving overall safety.