➤ comp.nus.edu.sg/~guoyi / project / urop / code

Safe Control Strategies for Autonomous Robots in Complex Interactive Environments

Undergraduate Research Opportunity Programme [EG2605]

Author: Chen Guoyi
Supervisors: Prof. Prahlad Vadakkepat, Liang Yuanchang
DOI: Pending...

Code Repository




Code Description (ReadMe File)


/MATLAB Simulation / APF.m

The Artificial Potential Field (APF) method employed in this script is pivotal for guiding a robot through an environment peppered with obstacles towards a designated goal. This method hinges on the interplay between attractive and repulsive forces, which respectively draw the robot to its goal and repel it from obstacles. Below are the core mathematical principles that underpin the APF method:

  1. Attractive Force Towards the Goal: The attractive force propels the robot towards its destination. It's quantified by the gradient of an attractive potential field, \( U_{\text{attr}} \), directed towards the goal:
    \[ \mathbf{F}_{\text{attr}} = -\nabla U_{\text{attr}}(\mathbf{q}) \]
    The attractive potential field is commonly modeled as a quadratic function of the distance to the goal, \( q_{\text{goal}} \), given by:
    \[ U_{\text{attr}}(\mathbf{q}) = \frac{1}{2} \zeta \|\mathbf{q} - \mathbf{q}_{\text{goal}}\|^2 \]
    where \( \zeta \) is a scaling factor that modulates the strength of attraction, and \( \|\mathbf{q} - \mathbf{q}_{\text{goal}}\| \) represents the Euclidean distance between the robot's current position \( \mathbf{q} \) and the goal position \( \mathbf{q}_{\text{goal}} \).
  2. Repulsive Force From Obstacles: This force deters the robot from colliding with obstacles by exerting a repulsion that grows as the robot approaches an obstacle. It is quantified as the gradient of a repulsive potential field, \( U_{\text{rep}} \), which is expressed as:
    \[ \mathbf{F}_{\text{rep}} = \nabla U_{\text{rep}}(\mathbf{q}) \]
    The repulsive potential field is defined as:
    \[ U_{\text{rep}}(\mathbf{q}) = \begin{cases} \frac{1}{2} \eta \left( \frac{1}{\|\mathbf{q} - \mathbf{q}_{\text{obs}}\|} - \frac{1}{d_0} \right)^2 & \text{if } \|\mathbf{q} - \mathbf{q}_{\text{obs}}\| \leq d_0 \\ 0 & \text{otherwise} \end{cases} \]
    Here, \( \eta \) is a scaling factor that adjusts the intensity of repulsion, \( \mathbf{q}_{\text{obs}} \) denotes the position of the closest point of the obstacle to the robot, and \( d_0 \) is the distance threshold beyond which the repulsive force ceases to act.
  3. Overall Navigation Force: The robot's motion is steered by the cumulative effect of the attractive and repulsive forces, which ensures a trajectory towards the goal while evading obstacles:
    \[ \mathbf{F}_{\text{total}} = \mathbf{F}_{\text{attr}} + \mathbf{F}_{\text{rep}} \]

This strategic formulation enables the robot to dynamically navigate its path by recalculating these forces at each step based on its present location, the goal's position, and the locations of nearby obstacles detected within its sensor range.

/APF Visualization / potential_field_z.py

This Python script visualizes a combined potential field composed of both repulsive peaks and an attractive plane. The visualization focuses on a top-down view, showcasing the whole potential across a specified domain.


The script uses numpy for numerical operations and matplotlib for plotting. It defines a range for x and y, creates a meshgrid, and calculates the repulsive and attractive potentials separately. The total potential is a combination of these two, visualized in a top-down view to emphasize the whole potential landscape.


Prerequisites


  • Python 3.x
  • numpy
  • matplotlib

Mathematical Formulation


The repulsive potential is given by the formula:

\[ Z_{\text{repulsive}} = \text{peak\_height} \cdot \left(e^{-(x - 0.5)^2 + (y - 0.5)^2} + e^{-(x + 0.5)^2 + (y + 0.5)^2}\right) \]

The attractive potential is represented as a downward sloping plane:

\[ Z_{\text{attractive}} = -0.5 \cdot (x - y) \]

The total potential is the sum of these two components.

/APF Visualization / potential_field.py

This Python script demonstrates the visualization of attractive and repulsive potentials in a 3D space. It showcases how to calculate and visualize these potentials using numpy and matplotlib, including the creation of a 3D surface plot with a colorbar for the attractive potential.


The script defines a domain using a meshgrid and calculates the attractive and repulsive potentials within this domain. The attractive potential is visualized in a 3D plot, emphasizing the gradient of potential across the space. This example focuses on the attractive potential, offering insights into its spatial variation.


Prerequisites


  • Python 3.x
  • numpy
  • matplotlib

Mathematical Formulation


The mathematical model consists of two main components: the repulsive and attractive potentials.

The repulsive potential is defined by:

\[ Z_{\text{repulsive}} = \text{peak\_height} \cdot \left(e^{-((X - 1)^2 + (Y - 1)^2)} + e^{-((X + 1)^2 + (Y + 1)^2)}\right) \]

where \( \text{peak\_height} \) determines the height of the peaks, and \( (X, Y) \) are the coordinates over the defined domain. This component creates two high, steep peaks at positions \((1, 1)\) and \((-1, -1)\).

The attractive potential is simpler, represented by a linear slope in the Y direction:

\[ Z_{\text{attractive}} = -0.5 \cdot Y \]

This creates a plane that slopes downwards along the Y-axis, modeling an attractive force that increases with Y.

The total potential visualized is the sum of these two components, providing insight into how the combined potential landscape would look in a physical system.