# How to Implement the Potential Field Approach for Obstacle Avoidance in Peng

> Learn to implement the potential field approach for obstacle avoidance in Peng. Generate real-time velocity commands for quadrotor navigation using attractive repulsive and vortex forces.

- Repository: [Yang Zhou/peng](https://github.com/makeecat/peng)
- Tags: how-to-guide
- Published: 2026-03-06

---

**The potential field approach in Peng uses attractive, repulsive, and vortex forces computed in `ObstacleAvoidancePlanner` to generate real-time velocity commands that steer a quadrotor toward a goal while avoiding obstacles.**

Peng is a Rust-based quadrotor simulation framework that provides a production-ready implementation of artificial potential fields for reactive navigation. By configuring the `ObstacleAvoidancePlanner` struct and its associated gain parameters, you can enable a physical quadrotor or simulated drone to navigate cluttered environments without solving complex global path-planning problems. This guide walks through the source code structure, force calculations, and practical integration steps required to deploy this algorithm in your Peng project.

## Core Architecture and Data Structures

The potential field implementation resides in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) and consists of three primary components: the obstacle data holder, the planner state machine, and the force computation engine.

### The Obstacle Struct

Each obstacle is represented as a simple geometric entity with dynamic properties. In [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) at lines 63–78, the `Obstacle` struct stores:

```rust
pub struct Obstacle {
    pub position: Vector3<f32>,
    pub velocity: Vector3<f32>,
    pub radius: f32,
}

```

You instantiate obstacles using `Obstacle::new(position, velocity, radius)`, where the radius defines the physical size of the object and the velocity vector supports dynamic obstacle tracking.

### The ObstacleAvoidancePlanner

The planner itself, defined at [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) lines 52–77, implements the `Planner` trait and maintains the following critical fields:

- **`target_position`**: The goal waypoint in 3D space.
- **`k_att`, `k_rep`, `k_vortex`**: Gain constants scaling attractive, repulsive, and vortex forces respectively.
- **`d0`**: Influence distance—obstacles beyond this radius are ignored.
- **`d_target`**: Distance threshold where attractive force transitions from exponential to linear.
- **`max_speed`**: Velocity magnitude cap for safety.

### Force Computation Model

The `plan` method (lines 81–110) computes three distinct force vectors that sum to produce the desired motion:

1. **Attractive Force**: Generated by `smooth_attractive_force`, which returns an exponential-decay force when far from the target and a linear force when within `d_target` meters.
2. **Repulsive Force**: Computed as `k_rep * (1/distance - 1/d0) * (1/distance²)` for obstacles where `distance < d0`, pushing the quadrotor away.
3. **Vortex Force**: Calculated as `k_vortex * (velocity.cross(&diff).normalize()) / distance²`, adding a tangential component that helps the drone slide around obstacles rather than head-on repulsion.

The total force vector is normalized, scaled by `max_speed`, and converted into a desired velocity and position setpoint.

## Step-by-Step Implementation Guide

Follow these steps to integrate the potential field planner into your Peng simulation or hardware deployment.

### 1. Import Required Types

Begin by importing the necessary modules from the Peng library:

```rust
use nalgebra::Vector3;
use peng_quad::{Obstacle, ObstacleAvoidancePlanner, PlannerType, PlannerManager};

```

### 2. Define Your Environment

Create a vector of obstacles representing your operational environment:

```rust
let obstacles = vec![
    Obstacle::new(Vector3::new(1.0, 0.0, 1.0), Vector3::zeros(), 0.5),
    Obstacle::new(Vector3::new(-0.5, 2.0, 1.2), Vector3::zeros(), 0.3),
];

```

### 3. Configure the Planner Parameters

Instantiate `ObstacleAvoidancePlanner` with tuned gains appropriate for your quadrotor's dynamics:

```rust
let pf_planner = ObstacleAvoidancePlanner {
    target_position: Vector3::new(0.0, 0.0, 2.0),
    start_time: 0.0,
    duration: 15.0,
    start_yaw: 0.0,
    end_yaw: 0.0,
    obstacles,
    k_att: 1.0,       // Attractive gain
    k_rep: 2.5,       // Repulsive gain  
    k_vortex: 0.8,    // Vortex gain
    d0: 1.2,          // Obstacle influence radius (meters)
    d_target: 0.5,    // Linear attractive region (meters)
    max_speed: 2.0,   // Maximum velocity (m/s)
};

```

### 4. Integrate with PlannerManager

Insert the planner into the state machine that manages flight phases:

```rust
let mut manager = PlannerManager::new(
    Vector3::new(0.0, 0.0, 1.0), // Initial hover position
    0.0,                         // Initial yaw
);
manager.set_planner(PlannerType::ObstacleAvoidance(pf_planner));

```

### 5. Runtime Execution Loop

During each control cycle, query the manager for setpoints:

```rust
let (desired_pos, desired_vel, desired_yaw) = manager
    .update(current_position, current_orientation, current_velocity, current_time, &[])
    .expect("Planner computation failed");

// Feed desired_vel and desired_pos into your PID controller

```

Note that the obstacles vector passed to `update` can be empty if the planner already owns the obstacle list, or you can pass updated obstacle positions for dynamic environments.

## Complete Working Example

Below is a minimal, compilable Rust example demonstrating the full integration from obstacle definition to simulation loop:

```rust
use nalgebra::{Vector3, UnitQuaternion};
use peng_quad::{
    Obstacle, ObstacleAvoidancePlanner, PlannerManager,
    PlannerType, SimulationError,
};

fn main() -> Result<(), SimulationError> {
    // Define static obstacles in the flight path
    let obstacles = vec![
        Obstacle::new(Vector3::new(1.0, 0.0, 1.0), Vector3::zeros(), 0.5),
        Obstacle::new(Vector3::new(-0.8, 0.5, 0.9), Vector3::zeros(), 0.4),
    ];

    // Configure potential field parameters
    let pf = ObstacleAvoidancePlanner {
        target_position: Vector3::new(0.0, 0.0, 2.0),
        start_time: 0.0,
        duration: 12.0,
        start_yaw: 0.0,
        end_yaw: std::f32::consts::PI / 2.0, // 90-degree final heading
        obstacles,
        k_att: 1.2,
        k_rep: 3.0,
        k_vortex: 0.5,
        d0: 1.0,
        d_target: 0.3,
        max_speed: 2.0,
    };

    // Initialize the planner manager
    let mut manager = PlannerManager::new(Vector3::new(0.0, 0.0, 1.0), 0.0);
    manager.set_planner(PlannerType::ObstacleAvoidance(pf));

    // Simulation loop at 50Hz
    let mut time = 0.0;
    let dt = 0.02;
    let mut pos = Vector3::new(0.0, 0.0, 1.0);
    let mut vel = Vector3::zeros();
    let mut ori = UnitQuaternion::identity();

    while time < 12.0 {
        let (desired_pos, desired_vel, desired_yaw) =
            manager.update(pos, ori, vel, time, &[])?;

        // In a real system, feed these to motor controllers
        pos = desired_pos;
        vel = desired_vel;
        ori = UnitQuaternion::from_euler_angles(0.0, 0.0, desired_yaw);
        
        time += dt;
    }

    Ok(())
}

```

This example uses [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) components to initialize the potential field planner, insert it into the `PlannerManager`, and extract velocity commands over a 12-second mission.

## Parameter Tuning and Optimization

Achieving stable obstacle avoidance requires careful tuning of the potential field gains based on your specific vehicle dynamics and environment density.

### Adjusting Influence Distances

Set **`d0`** to at least twice the physical radius of your largest obstacle plus a safety buffer. If `d0` is too small, the quadrotor will react too late to avoid collisions; if too large, distant objects will cause unnecessary deviations from the optimal path.

### Balancing Force Gains

- **Increase `k_rep`** when the drone passes too close to obstacles, but beware of oscillation in narrow corridors.
- **Decrease `k_att`** if the vehicle overshoots the target position or exhibits aggressive acceleration toward the goal.
- **Tune `k_vortex`** between 0.3 and 1.0 to encourage smooth circumnavigation; setting this to zero removes the tangential force component entirely.

### Performance Considerations

The force calculation loop in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) runs in **O(N)** time complexity relative to the number of obstacles. For environments with fewer than 50 obstacles, the computational overhead is negligible on modern flight controllers. For dense obstacle fields, consider implementing spatial partitioning (e.g., a KD-tree) to reduce the number of distance checks per cycle.

## Summary

- **Peng's `ObstacleAvoidancePlanner`** implements a three-component potential field (attractive, repulsive, vortex) in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) lines 52–110.
- The **`plan`** method converts force vectors into velocity-limited setpoints suitable for closed-loop control.
- Configuration requires tuning **`k_att`**, **`k_rep`**, **`k_vortex`**, and **`d0`** to match your quadrotor's agility and obstacle density.
- Integration follows a four-step workflow: import types, define obstacles, configure gains, and query the `PlannerManager` during the control loop.
- The algorithm complexity scales linearly with obstacles, making it suitable for real-time embedded systems with moderate clutter.

## Frequently Asked Questions

### What is the purpose of the vortex force in Peng's potential field implementation?

The vortex force adds a tangential component perpendicular to both the quadrotor's velocity vector and the obstacle direction vector. According to the source code in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs), this force helps the vehicle "slide" around obstacles rather than experiencing pure head-on repulsion, which reduces the risk of getting stuck in local minima between closely spaced obstacles.

### How does Peng handle the local minimum problem inherent in potential field methods?

Peng mitigates local minima through the **vortex force** component and by allowing dynamic re-planning. The tangential force introduced by `k_vortex` breaks symmetry in the force field, encouraging the quadrotor to circulate around obstacles rather than oscillating between them. Additionally, the `PlannerManager` can switch between different planner types (e.g., from potential field to waypoint tracking) if `is_finished` returns false for an extended period.

### Can the potential field planner handle moving obstacles in real-time?

Yes, the `Obstacle` struct includes a **`velocity`** field, and you can update obstacle positions each control cycle by passing a refreshed obstacle list to `manager.update()` or by modifying the planner's internal obstacles vector. For highly dynamic environments, ensure your control loop runs at sufficient frequency (50–100Hz) to react to fast-moving obstacles within the `d0` influence radius.

### What causes the "jerky" motion when approaching the target, and how do I fix it?

Jerky behavior near the goal typically indicates excessive **attractive gain** (`k_att`) or an insufficient **`d_target`** value. The `smooth_attractive_force` function in [`src/lib.rs`](https://github.com/makeecat/peng/blob/main/src/lib.rs) transitions from exponential to linear attraction at `d_target` meters. Increase `d_target` to enlarge the linear region, or reduce `k_att` to soften the approach velocity, ensuring the quadrotor decelerates smoothly as it converges on the waypoint.