These three months, since GSOC started, have given me some great memories. I began my journey with bare minimum knowledge about the ArduPilot flight stack, with very basic bug fixes and some documentation as my only experience. Whereas now, I have multiple features, bug fixes, already merged in the Master. It wouldn’t have been possible without @rmackay9@khancyr . I am grateful to them for guiding me through every step and making this period so comfortable by providing immense support through every “obstacle” (pun intended) that came in the way. Thanks a ton for this excellent opportunity, I could not ask for anything more!
Coming to the work done, ArduPilot’s Obstacle Avoidance features have had some significant changes. We completed more than what was initially planned and continue to work towards more improvements. I will be listing them down below so you can have a glance at what all will be coming in the next release. Hopefully, there is something that interests everyone!
Ardupilot has two main avoidance systems:
PathPlanning (for autonomous modes) and Simple Avoidance ( for semi-autonomous modes).
Path Planning:
ArduPilot has two Path Planning algorithms currently, BendyRuler and Dijkstra’s.
BendyRuler has seen a lot of changes in these past months:
BendyRuler has a new type of avoidance. Until now, it could only avoid obstacles by manoeuvring horizontally, but now you can also enable vertical avoidance, in which the vehicle will gain or reduce altitude to avoid the barriers.
A live demo can be seen here:
Simulation:
One thing that most users who have experienced this algorithm would agree is that it is not reliable for large obstacles that suddenly appear in front of the vehicle. This happens because it has a hard time to decide between two equally good obstacle-free paths. To fix this, now the vehicle will try and maintain its chosen path for as long as possible. It will try not to switch over to an equally good course, which might be in the opposite direction to the current path. Therefore we try and avoid any big bearing changes unless this path is significantly better than the one we are on.
This can be seen here(before-left vs after-right):
The object database has now been turned 3-D. The presently released version of ArduCopter and Rover project the obstacle to be always in front. So if the vehicle was to pitch, or roll and see the ground “x” meters away, the object database would continue to think the obstacle is right in front of it “x” meters away instead of below it. This has now been fixed through some rotation matrices, and it makes a massive difference in the performance! A simulation can be seen here:
Another common problem with BendyRuler is that while taking off, the ground may be seen as obstacles which is then stored in the Obstacle Database. This can be very inconvenient, to say the least. The copter will now reject obstacles within a 3-meter radius of the home location up to OA_DB_ALT_MIN height. The parameter can be set zero in case the feature is not needed. This will immensely help with autonomous takeoffs.
There is an entirely new addition to path planning algorithms, that fuses BendyRuler and Dijsktras together.
A major drawback of Dijkstra’s is that (currently) it only works on fences. This is largely because if it was extended to support non-stationary proximity obstacles, we would need to rerun the algorithm every time an obstacle or the vehicle moves. This would require a tremendous amount of computational power, which would keep increasing as the number of obstacles increases (and so will the number of “nodes”).
Therefore this new type of algorithm will run both Dijkstra’s and BendyRuler at the same time and default to Dijkstra’s based navigation when there is no obstacle near the vehicle. This will make sure we have the shortest distance to reach the destination.
IF an obstacle is detected by a proximity (lidar) sensor, we will switch to BendyRuler (which is a ray-based algorithm) to navigate safely away from it.
This feature, therefore, takes away the headache of the user deciding which is the best path planning algorithm for their use case and automatically makes the decision for them.
A simulated demo can be seen here:
Simple Avoidance:
Simple avoidance has two options: Either stop the vehicle in front of the fence/obstacle or (for copter) try and slide across the obstacle. The way this works is that if a user is trying to push the vehicle towards the obstacle, the vehicle will try to “ignore” the velocity in the direction of the present obstacles. So simple avoidance at best, would reduce the speed in a particular direction, and never add anything to it. Therefore, the vehicle did not have that capability to maintain a margin from the obstacle, if the obstacle started to move towards the vehicle. This is no longer the case, and now the vehicle can maintain a margin (safe distance) from fences and obstacles in any combination. This has been tested very thoroughly in the simulators, in the absolute worst-case scenario I could think of. More detailed explanation, with live demonstration and simulations, can be seen here:
The exact code for each of these features can be found in the linked PR’s here:
BendyRuler and Obstacle Database should see obstacles in 3D:
Provide option to search for clear paths vertically in BendyRuler:
Resolve BendyRuler’s hesitancy:
Recovery from BendyRuler avoidance may impact the fence:
Fuse BendyRuler and Dijkstras together:
Back Away from the obstacles in Simple Avoidance:
Other minor improvements:
Reject obstacles near home (easier autonomous takeoff’s:
Bugfix with “Stop mode” never reaching the desired margin:
Check if readings are valid before sending it down to Object Database:
Check if the origin is set before running OA:
I would also like to thank CubePilot( @proficnc) and @MartyMcFly, who were very generous in providing me with hardware and funds. This enabled me to test all my features on a real vehicle, and the work I did would not have been possible without that. Many thanks to you!
The GSOC period is now over, but I will continue working with ArduPilot and hopefully come up with more features that might be useful to all the users. These months have been truly life-changing for me. Kudos to the ArduPilot community and the patient developers for making this a one of a kind experience for me! My gratitude goes towards you.
@rishabsingh3003 well done! I am currently implementing this for a ZED Stereocam based on your realsense camera coding and its working smootly so far.
A few questions though:
will the current OBSTACLE_DISTANCE message be replaced by the new OBSTACLE_DISTANCE_3D message and id not, how will the old OBSTACLE_DISTANCE message behave with 4.1?
Can I use the DISTANCE_SENSOR and OBSTACLE_DISTANCE messages in parallel to OBSTACLE_DISTANCE_3D
Is there a way to turn off the automatic pitch or roll compensation? I do have my ZED on a gimbal and any automatic compensation would read to false results in my understanding.
is there any further sample coding for the 3D object database available? I Built my coding on the code I found on your github https://github.com/rishabsingh3003/Vision-Obstacle-Avoidance. Is the 3x3 matrix you implemented there the best use of the new 3d database capabilities?
@mtbsteve, thanks for testing these things out!
Regarding your questions:
No, we won’t be replacing the OBSTACLE_DISTANCE message.Not now, and probably never will. You are free to use OBSTACLE_DISTANCE. However, I see it as a mavlink rangefinder. I.e. single point horizontal distances. If that’s what you need in your project, then I would highly recommend them. The functionality of OBSTACLE_DISTANCE has not changed from 4.0 -> 4.1
It might work, but I would HIGHLY discourage that. If you are using OBSACLE_DISTANCE_3D, why not just set Z=0, and that would behave something like OBSTACLE_DISTANCE.
It’s certainly not the best way! I am sorry we don’t have more examples, my 3x3 matrix is just something generic and would certainly work. In general, if you can send a few points that cover up the entire region of Interest, ArduPilot backend will filter them and only use the points we really need to care about for avoidance. It’ll have to be a trial and error though. If you do not send enough points, Copter will obviously not know where the obstacles are, and might crash.
A word of caution for Copter Beta: there are known issues with BendyRuler at the moment, because of some changes to WP nav. This should be fixed soon.
All of these features are fairly new and have not been tested by a lot of people. Please feel free to ping me on Discord or here in case you find any troubles with these new features. If it’s a bug, I’ll try my best to fix it asap.
Thanks for your feedback!
I have flown and tested object avoidance with my ZED stereo cam along with the DISTANCE_SENSOR, and OBSTACLE_DISTANCE functions in AC 4.0.7 with good results.
I will try AC 4.1 bendy ruler along with the OBSTACLE_DISTANCE_3D function first in the simulator. And yes, please check back with the dev team on introducing a parameter to configure the pitch/roll compensation in the 3D database.
Here is a screenshot from the ZED camera depth view with the 9 sector computation of the different median depth per sector. (The ZED provides per default a grayscale image)
