whoenig
commented
4 years ago I have simple artificial potentials implemented in some branch which I can share. It addressed the issues you raised similar to how you suggest it. One problem is still the downwash. We can also likely share the Glas Neural network, which also has a safety module that can be used without NN.
On Sat, Mar 28, 2020, 18:11 James Alan Preiss notifications@github.com wrote:
In multi-quadrotor applications like mapping and search/rescue, where the task requires the robots to spread out, the risk of collisions is inherently low. We want to ensure collision avoidance, but heavy centralized planning methods that handle maze-like environments and dense formations (like @whoenig https://github.com/whoenig's work) are overkill. A simpler solution is some kind of reactive onboard collision avoidance, where each quadrotor attempts to move towards its goal position in a straight line, but reacts appropriately if another quadrotor is near.
We considered implementing something like this as library functionality in Crazyswarm https://github.com/USC-ACTLab/crazyswarm, but implementing it in the firmware has significant benefits:
- Most importantly: makes the functionality available to a larger set of Crazyflie users.
- Minimizes communication bandwidth - only need to send one packet with the goal position and any algorithm control parameters.
- Minimizes computational load on the base station PC.
- Keeps the Crazyswarm control script simple.
We would like to contribute our implementation back to the main firmware project. I created this issue to get some design feedback before sending any PR. Algorithms
One suitable algorithm is buffered Voronoi cells (BVC) https://www.youtube.com/watch?v=rrDmcE94OQw. Another possibility is optimal reciprocal collision avoidance (ORCA) https://www.youtube.com/watch?v=zmTgj4SKftc. ORCA gets better results when spatial contention is high, but it requires knowing the velocity of neighbors, and imposes stricter constraints on the kind of control laws that can be followed. Buffered Voronoi Cells only constrains that each robot should stay within its cell during the planning horizon.
I already have a prototype of BVC implemented in C and tested. It is not on GitHub yet. I did not use the model-predictive optimal control from the BVC paper. I use a simple projection of the desired direction vector into the Voronoi cell polytope. It runs in less than 1ms on my PC, so I believe it will be fast enough to run at an appropriate rate on the Crazyflie's STM32. Software Design High-level commands
Currently there are only two fundamental control modes in the Crazyflie firmware:
- Follow a piecewise polynomial trajectory from the high-level commander.
- Follow streaming low-level setpoints from the PC.
Go to a goal subject to collision avoidance is a new mode. It cannot be implemented as modification to the trajectory-following mode, because high contention may require stopping and waiting for a long time, and spatial deviations from the original trajectory could be large. The notion of pre-planning the entire trajectory from start to end is no longer valid.
The new mode could be initialized by a new CRTP high-level command COMMAND_GO_TO_COLLISION_AVOID or similar. Knowing other Crazyflies' positions
When a motion capture system is in use, each Crazyflie already receives the radio packets containing positions of all other Crazyflies on its radio channel. Currently, we ignore those. In crtp_localization_service.c, for the EXT_POSE_PACKED (link) https://github.com/bitcraze/crazyflie-firmware/blob/9e47ecfe69677b785f54fe6167f5a6ce048642ba/src/modules/src/crtp_localization_service.c#L179-L189 and EXT_POSITION_PACKED (link) https://github.com/bitcraze/crazyflie-firmware/blob/9e47ecfe69677b785f54fe6167f5a6ce048642ba/src/modules/src/crtp_localization_service.c#L199-L207 packet parsing code, we do nothing if the packet id field doesn't match our own id. But now, we would need to store them.
crtp_localization_service.c currently "pushes" new measurements of the Crazyflie's own position into the state estimator's queue. I do not think that mechanism is appropriate here, because there may be more than one place in the firmware where other Crazyflies' positions are needed. Instead, I would suggest adding APIs to crtp_localization_service.h to allow any other part of the firmware to "pull" the other positions. For example, something like
bool isOtherIDActive(uint8_t id); struct vec getOtherIDPosition(uint8_t id);
To cut down on statically allocated memory, we can store the positions as they come on the radio - as 16-bit integer values of millimeters - instead of the 32-bit floating point values of meters that the rest of the firmware would need.
Currently, this approach means we can only scale collision avoidance up to the number of Crazyflies that can share one radio channel. However, it would not be hard to modify the base station software (such as Crazyswarm) to send all position measurements on all channels. Per-Crazyflie commands and other bandwidth consumers such as logging would still be sent/received only on the appropriate radio. Algorithm code location
I would suggest implementing the core algorithm in a "library" style similar to the current piecewise polynomial library in pptraj.c. This allows compiling the algorithm on a PC for prototyping and testing.
It would probably make sense to put the integration with the overall planning/control framework part in planner.c. However, I am beginning to question the value of the state machine logic in planner.c, since there is also complicated state machine logic in crtp_commander_high_level.c related to emergency timeout stabilization and switching between low- and high-level modes. In the long term, I think it might simplify things to merge them together.
— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/bitcraze/crazyflie-firmware/issues/567, or unsubscribe https://github.com/notifications/unsubscribe-auth/AA3SIPJKTUAMUSDXEZTVL63RJ2N4DANCNFSM4LVZATZQ .
jpreiss
krichardsson
knmcguire
In multi-quadrotor applications like mapping and search/rescue, where the task requires the robots to spread out, the risk of collisions is inherently low. We want to ensure collision avoidance, but heavy centralized planning methods that handle maze-like environments and dense formations (like @whoenig's work) are overkill. A simpler solution is some kind of reactive onboard collision avoidance, where each quadrotor attempts to move towards its goal position in a straight line, but reacts appropriately if another quadrotor is near.
We considered implementing something like this as library functionality in Crazyswarm, but implementing it in the firmware has significant benefits:
We would like to contribute our implementation back to the main firmware project. I created this issue to get some design feedback before sending any PR.
Algorithms
One suitable algorithm is buffered Voronoi cells (BVC). Another possibility is optimal reciprocal collision avoidance (ORCA). ORCA gets better results when spatial contention is high, but it requires knowing the velocity of neighbors, and imposes stricter constraints on the kind of control laws that can be followed. Buffered Voronoi Cells only constrains that each robot should stay within its cell during the planning horizon.
I already have a prototype of BVC implemented in C and tested: https://github.com/bitcraze/crazyflie-firmware/compare/master...jpreiss:collision_avoidance. Most the core math is useful for generic applications so it has been implemented in https://github.com/jpreiss/cmath3d. I did not use the model-predictive optimal control from the BVC paper. I use a simple projection of the desired direction vector into the Voronoi cell polytope. It runs in less than 1ms on my PC, so I believe it will be fast enough to run at an appropriate rate on the Crazyflie's STM32.
Software Design
High-level commands
Currently there are only two fundamental control modes in the Crazyflie firmware:
Go to a goal subject to collision avoidance is a new mode. It cannot be implemented as modification to the trajectory-following mode, because high contention may require stopping and waiting for a long time, and spatial deviations from the original trajectory could be large. The notion of pre-planning the entire trajectory from start to end is no longer valid.
The new mode could be initialized by a new CRTP high-level command
COMMAND_GO_TO_COLLISION_AVOIDor similar.Knowing other Crazyflies' positions
When a motion capture system is in use, each Crazyflie already receives the radio packets containing positions of all other Crazyflies on its radio channel. Currently, we ignore those. In
crtp_localization_service.c, for theEXT_POSE_PACKED(link) andEXT_POSITION_PACKED(link) packet parsing code, we do nothing if the packetidfield doesn't match our ownid. But now, we would need to store them.crtp_localization_service.ccurrently "pushes" new measurements of the Crazyflie's own position into the state estimator's queue. I do not think that mechanism is appropriate here, because there may be more than one place in the firmware where other Crazyflies' positions are needed. Instead, I would suggest adding APIs tocrtp_localization_service.hto allow any other part of the firmware to "pull" the other positions. For example, something likeTo cut down on statically allocated memory, we can store the positions as they come on the radio - as 16-bit integer values of millimeters - instead of the 32-bit floating point values of meters that the rest of the firmware would need.
Currently, this approach means we can only scale collision avoidance up to the number of Crazyflies that can share one radio channel. However, it would not be hard to modify the base station software (such as Crazyswarm) to send all position measurements on all channels. Per-Crazyflie commands and other bandwidth consumers such as logging would still be sent/received only on the appropriate radio.
Algorithm code location
I would suggest implementing the core algorithm in a "library" style similar to the current piecewise polynomial library in
pptraj.c. This allows compiling the algorithm on a PC for prototyping and testing.It would probably make sense to put the integration with the overall planning/control framework part in
planner.c. However, I am beginning to question the value of the state machine logic inplanner.c, since there is also complicated state machine logic incrtp_commander_high_level.crelated to emergency timeout stabilization and switching between low- and high-level modes. In the long term, I think it might simplify things to merge them together.