...
For a more detailed read on the working and motivation behind this approach, please skim through this link: https://www.cs.cmu.edu/~motionplanning/papers/sbp_papers/integrated1/borenstein_VFHisto.pdf
System Workflow
detection
The system configures LiDAR settings and fetches LiDAR readings
These readings contain distance and angle values that represent the environment around the drone.
The Detection module ensures that valid LiDAR detections are processed while filtering out invalid or noisy data.
These detections are passed to the LiDAR Parser for further processing in the form of LidarDetection objects.
lidar_parser
The lidar_parser module clusters continuous LiDAR readings into oscillations.
An oscillation represents a full sweep of the LiDAR sensor from one end to the other and back.
The parser tracks the angle direction (UP or DOWN) to detect when a full oscillation is completed.
Once an oscillation is detected, it is converted into a LidarOscillation object, which is passed to the VFH module for sector-based density calculation.
vfh
The vfh module takes a LidarOscillation as input and generates a polar obstacle density representation.
The polar obstacle density represents obstacle distribution across different angular sectors.
Unlike traditional clustering methods, this approach does not attempt to classify obstacles by shape. Instead, it prioritises density-based representations.
The density is calculated by:
Dividing the scan into angular sectors.
Assigning each sector a density value based on LiDAR readings.
Applying confidence factors and decay rates to fine-tune obstacle representation
The VFH module calculates obstacle density by weighting LiDAR detections based on confidence and distance, giving higher priority to closer obstacles.
...
The generated PolarObstacleDensity object is passed to the vfh_decision module for trajectory planning.
vfh_decision
The vfh_decision module processes the PolarObstacleDensity to determine the safest and most efficient steering direction. It identifies valleys—clear paths between obstacles—and evaluates them based on:
Density threshold: Ensures a sector is safe for navigation if its density falls below this value.
Minimum consecutive sectors: Prevents the drone from choosing narrow, unreliable gaps.
Wide valley check: In AUTO mode, the drone prioritises its original route and only deviates if absolutely necessary.
The system then selects the best valley based on:
Proximity to the target direction (favoring paths aligned with the mission).
Valley width, ensuring smooth and stable navigation.
If no safe valleys exist, the system issues a "REVERSE" command to halt or backtrack. Once a direction is chosen, the command is sent to the Flight Interface for execution.