Obstacle avoidance using artificial potential fields method
CopterHack-2022, team Stereo.
The list of team members:
- Denis Konstantinov, @den_konstantinov, engineer and developer.
The project repository is here
This repository contains obstacle avoidance system for quadcopters with Raspberry Pi 4 onboard computer. The code in this repository is designed to work with Clover Raspberry Pi image and special PX4-based firmware modified for easier communication with Raspberry Pi.
Artificial potential fields method is based on considering quadcopter, obstacles and target point as electric-charged points. Quadcopter and obstacles have positive charge, and target point is assigned with negative charge. This results in quadcopter "attracting" itself to the target point, while being repelled by the same-signed charges of obstacles. Using this analogy, you can compute a safe, collision-free trajectory, which can be executed by the vehicle.
It's obvious that you need some sort of geometrical information about the surrounding world if you want to avoid obstacles. This algorithm uses Intel RealSense D435 depth camera - it provides a 3D point cloud which can be easily used for potential fields computation.
This guide is intended to be used on Ubuntu 20.04. Python version is 3.8.10, but it's very likely that it will work on other versions like 3.7, 3.9, 3.10 without any changes.
- Install the Clover simulator.
Install realsense_gazebo_plugin and realsense2_description:
cd ~/catkin_ws/src git clone https://github.com/issaiass/realsense_gazebo_plugin git clone https://github.com/issaiass/realsense2_description cd ~/catkin_ws catkin_make
~/catkin_ws/src/clover/clover_descriptionfolder with one in this repository. This will add RealSense D435 to quadcopter model and make other minor changes so you will be able to reproduce our results.
Install all necessary Python packages
pip3 install -r requirements.txt sudo apt-get install ros-noetic-ros-numpy
Make sure your
PYTHONPATHenv variable is set as
If it's not, add the following paths to
Launch the clover simulator and spawn some obstacles:
roslaunch clover_simulation simulator.launch
Open another terminal window, and launch the takeoff script. The drone will arm its motors and take off to the altitude specified in takeoff.py script:
Then, you should open main.py script and specify the target point (as x, y, z array). It is defined on the line
nav = AvoidanceNavigation(np.array([0, 0, 2]))
Finally, launch main.py to start the obstacle avoidance flight:
Obstacle avoidance algorithm description
This algorithm uses a point cloud produced by stereo camera to infer the flight path by simulating a motion of charged particle. Each point from the point cloud is considered as obstacle, and repels the vehicle with the force directed in 'point->vehicle' direction. The repelling force has a magnitude
q_repel / dist (dist is distance from vehicle to point). Similarly, an attraction point attracts the vehicle with the constant magnitude of
q_attract. The sum of these force vectors is the desired flight direction.. The speed of a particle is set as
speed argument of
AvoidanceNavigation and directed along the resulted force vector. This way, we get a motion equation which we can solve numerically. Now, we have the trajectory which can be executed by the vehicle.
The algorithm is still in development, so we can't guarantee that default parameters will work in all cases. However, you may tweak the parameters yourself. Here is a brief description of each parameter in descending order of importance:
speed- if your vehicle systematically crashes, the first thing you should do is to decrease the flight speed. Values of 1.5-2 m/s show good performance on most obstacle types
dist_threshold- this sets a 'sphere of influence' for individual obstacle point. Larger values of this parameter will result in trajectories with bigger margin from obstacle
lead- this is a smoothing factor of trajectory execution. If this value is too big, vehicle will ignore sharp obstacle avoidance maneuvers and risks crashing into a small/thin obstacle. If this value is too small, vehicle will fly too wobbly.
q_repel- the strength of repelling force is determined by this parameter.