Brainstorm: What would it take to get a graph based shortest path planner to work in real time with Proximity sensors?

[rishabsingh3003]

Background: For path planning, we have BendyRuler ( a local planner, which can avoid both fences and proximity-based live obstacles), and Dijkstra's ( a global planner, which pre plans a path avoiding static fences only) Most of us are aware of the computational and memory costs that come with graph-based planners, and throughout history, it has been suggested that these algorithms should not be used in real-time avoidance.

BendyRuler has its advantages: it's simple, it'll get you across small objects in a wide space. But it has similar drawbacks to what most of these local planners do: have problems in cluttered space, will not allow you to navigate narrow places unless you really reduce a lot of the search parameters.. but then that will create other problems, turning towards the wrong direction, etc etc. Also recently, I have read a lot of papers that have gained success with graph-based path planners, with algorithms that are slightly modified to A-Star.

What I have done till now:

1. https://github.com/ArduPilot/ardupilot/pull/18806
2. https://github.com/ArduPilot/ardupilot/pull/18820

These two PR's should significantly improve the performance of our global planner, Dijkstra's.

Things I am considering doing now:

1. Study and deploy a different algorithm to construct a "reduced visibility graph" instead of the full set of vertices that we are constructing right now (thanks to @hendjoshsr71 for the idea).
2. Implement modified A* or RRT which are faster than several times faster than what we have in master.
3. Restrict the number of obstacles that can be pushed to the Obstacle database (or maybe enlarge each obstacle so that we can store less of them eventually)
4. Make Obstacle Database more efficient in removing old obstacles

Even after all this, it might just happen that it's not possible to implement these ideas inside a tiny Flight Controller, but it seems worth a try!

This also comes as motivation from my recent work with the OAK-D camera, where we can actually track obstacles: https://youtu.be/KtecldGqP5U .. which means that we can have very few obstacles present in the Obstacle Database and it can be highly optimized.

I am looking for suggestions, research papers, ideas that can help in this effort!

[hendjoshsr71]

Here is an overview paper of various methods that may be of interest. Especially the references.

https://www.researchgate.net/publication/324416551_Overview_of_Path_Planning_and_Obstacle_Avoidance_Algorithms_for_UAVs_A_Comparative_Study

[IamPete1]

Just having a read over the implementation to see if I can spot anything.

We should get a approx 50% speedup in generated the lines by early returning on the line - polygon intersection checks, don't need to know where the intersection is, just if there is one. https://github.com/ArduPilot/ardupilot/blob/b0aa456daad5e732d9477481a055f5fa1acd8a77/libraries/AP_Math/polygon.cpp#L165

We should be checking that points are inside all fences before adding to the list of points. That would cut out unnecessary line - polygon intersection checks in favor of faster point polygon checks. It also mean we don't need the inclusion circle line intersection check, if the two end point are within the inclusion the line will also be.

[rishabsingh3003]

@IamPete1 this is super helpful! Seems obvious now, but I didn't think of it back then. An update from my side: https://youtu.be/CSn69OUmRZM This basically demonstrates a working real-time A* algorithm for obstacle avoidance with a 360-lidar. But on SITL, it's taking an average of 500 ms to come up with the path (~495 ms just to come up with the visgraph, and the rest on the algorithm itself). So its clear we need to highly optimize the construction of the visgraph, and this is a good start