Greetings to the ArduPiilot community! This year I have the pleasure to work alongside @rmackay9 and @khancyr for GSOC 2020. For the past few months, my contribution has been towards adding new documentation and fixing small bugs. I now have the opportunity to work on one of the most interesting features that ArduPilot has to offer.
In the coming months, I will be working towards Improvements to Object Avoidance in Rover & Multicopter. This would include a wide array of enhancements and new features. Obstacle Avoidance and path planning are relatively new features in the ArduPilot codebase. I aim to make these algorithms safer and more reliable to the average user while adding new features to it.
ArduPilot currently offers three types of Avoidance Protocols:
- Simple Avoidance: STOP or SLIDE just before hitting Fence/object
- BendyRuler: Avoid Obstacles with the least deviation to the goal
- Djikstras: Shortest path planning algorithm (No dynamic obstacle avoidance)
It has come to my attention many times in the past, that several users are confused on how these algorithms work, especially BendyRuler. My focus on this blog will be to explain the idea behind the algorithm:
Bendy Ruler
BendyRuler is an obstacle avoidance algorithm that was initially developed by Canberra UAV. It was later adapted to ArduPilot’s Copter and Rover with some changes.
The algorithm takes input from two sources:
- The fences that are stored in the AC_FENCE library:
- Inclusion/Exclusion Polygon Fence
- Inclusion/Exclusion Circle Fence
- Circular Fence
- Dynamically moving obstacles:
- Sensed using lidars in the Proximity Library
This algorithm is broadly divided into two steps:
STEP 1:
- Bearing(angle the vehicle has to turn to point towards the destination) and distance to the destination from the present location is calculated.
- Next, a probe is done in steps of 5 degrees deviation from the bearing to the goal. Hence we start with 0 degrees deviation and a location is projected to some distance. This new location is stored as a “Test Location”.
- Distance (margin) from the Test location to the obstacles (Fence, Lidar data, etc.) is calculated. The least angular deviation that gives a margin greater than MARGIN_MAX (the user-settable parameter that controls how far the obstacle avoidance algorithm should detect obstacles) is stored as “best bearing” and the algorithm proceeds to step 2. If 0 degrees does not give us a safe path, the deviation is changed to +5 degrees(Then -5 degrees, and so on), and Step 1 is run again. Therefore, the algorithm can possibly(but not always) do an entire 360-degree search in order to find a safe margin.
STEP 2:
- The second part of the algorithm is to probe in three extra directions from the already extrapolated test location in part 1 that is at least MARGIN_MAX distance away from the obstacle. These three test bearings are: [0, 45, -45] degrees with respect to the vehicle’s bearing to the destination from the first Test Location.
- After extrapolating a new test location, called “Test Location 2” (First with 0-degree deviation), we check the margin from fences and obstacles similar to what we did in Step 1. If the Margin is greater than MARGIN_MAX, then we have a safe direction to travel in. Test Location is then passed to the WayPoint library to continue safe navigation. Otherwise, we check the other two deviations one by one.
- If this newfound bearing is the same as the original path (i.e the straight line from the present location to the destination), then Bendy Ruler adjusted navigation is turned off (Since obstacle avoidance is not needed now) and we have a clear path to travel.
LAST STRAW:
- Normally we should never reach here, but it can happen that the above two Steps (Even after doing a complete 360-degree probe) did not give us a safe path. Therefore, we choose the path which has the maximum margin from Step 1 to travel to as a last resort.
*Note: It may seem that Step 1 and Step 2 are disconnected from each other, but in reality they are actually interwoven. I have broken them down in two steps to make explanation easier to the reader. To keep the algorithm efficient, Step 1 will go to Step 2 as soon as it finds a clear path (i.e a 360-degree probe is almost never required in normal circumstances)
This entire process is repeated at 1hz (Once per second) and the waypoint library is updated with either of the following possibilities:
- No obstacle avoidance needed, continue on course
- The updated “obstacle-free” waypoint is set.
The same procedure is depicted below with the help of an illustration:
Case: Obstacle has been detected
Case: Obstacle has not been detected OR obstacle has been cleared with the help of BendyRuler adjusted navigation
Additional Resources for learning about the algorithm:
See it in action:
- https://www.youtube.com/watch?v=SPu0a23FGKc&feature=youtu.be
- https://www.youtube.com/watch?v=eQQJ8OPJVDk
- https://www.youtube.com/watch?v=0AEW1TvaviY
ArduPilot Developer Conference 2020:
Coming to the work this summer, my key focus will be on:
- Getting BendyRuler to work in 3D: Currently all the objects that the vehicle senses using Lidar are stored as 2D vectors. Also, the probing for obstacles that was explained before is also done in 2-D. I would like to turn both of these to 3-D. Therefore the vehicle will be able to move up and down to avoid an obstacle and not just move left or right as the current implementation does.
- BendyRuler is hesitant in making decisions, as can be seen in the video here. It also gets stuck in front of obstacles sometimes. Some improvements in the path choosing algorithm are required.
The rest of my work can be seen in detail here: https://github.com/ArduPilot/ardupilot/issues/14405
I look forward to any suggestions, advice, questions that you may have. Kindly post them in the comments below.
Thank you for taking out the time to read through this!