Simple onboard collision avoidance for multiple Crazyflies #567
Comments
|
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 ***@***.***> 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
<#567>, or
unsubscribe
<https://github.com/notifications/unsubscribe-auth/AA3SIPJKTUAMUSDXEZTVL63RJ2N4DANCNFSM4LVZATZQ>
.
|
|
I currently account for downwash in BVC using the ellipsoid collision model by finding the separating planes in a stretched coordinate system. Based on my understanding, BVC might be superior to potential fields because it should not be as sensitive to tuning parameters. (You mentioned this in a previous email on this topic.) Can you link to the branch with the potential field implementation for reference on software design discussions? |
|
Edit: my implementation of the core BVC algorithm is in master...jpreiss:collision_avoidance |
|
A public copy of the firmware code for our GLAS paper is available at https://github.com/whoenig/crazyflie-firmware/tree/GLAS-iros2020. Knowing other Crazyflies positionThere are two new functions in the localization service: Algorithm code locationSimilar to what you suggested, we put our algorithm in a separate file that can be easily tested on a PC: platform-agnostic code, FreeRTOS Glue Code. Now the central issue is how to include it in the firmware. Our solution is somewhat hacky, but works well with the high-level mode and can be easily enabled and disabled. In particular, we put our code in the planner. First, planning the current goal is augmented. This augmentation has different modes (we have a simple artificial potential and the neural network) and one can switch between the modes using a parameter. In each mode, we augment the current desired (high-level) setpoint by keeping a hidden state of a double integrator. The hidden state is continuously updated whenever none of the modes is active. One could also put this logic somewhere else, especially if you want to be able to enable this collision avoidance in low-level flight as well. However, the principle will likely remain the same (this logic would then likely move into the stabilizer in order to work in all possible flight modes). |
|
Hi! I think this is an interesting feature that would benefit the platform and I have some input on the design. We (Bitcraze) are happy to add any functionality that we believe is beneficial for a larger group of users and that is useful in multiple use cases if possible (not too application specific). The intention of my comments/suggestions is to tweak and broaden your proposal in this direction if possible. Some might be hard to implement, but I just want to bring them up for consideration. Knowing other Crazyflies positionIt would be nice to be able to use the functionality with other positioning systems (for instance the Lighthouse) and without Crazyswarm, that is without broadcasting position information from a central system. I'm thinking that it might be possible to generalize the functionality of knowing other Crazyflies positions into a module, and that position data for other CFs could be fed (or sourced) from P2P communication. Another possible source of positioning data could be a future UWB mixed mode (TWR + TDoA) scheme where other CFs positions are transmitted in UWB packets. I could imagine other use cases, other than collision avoidance that could use the positions of other CFs as well. This could probably be a useful module on its own right. The SITAW (Situation awareness module)There is already a SITAW module, would it be possible to extend it with this new functionality? From an application point of view it would make sense to get notifications of possible collisions for instance. Collision avoidance as a utilityFrom an application point of view it would be nice to have access to all parts of the solution (as modules or utilities), where the application "glues" it all together. That approach makes it possible to tap in and modify at various points in the chain as opposed to a solution that is part of the platform. Maybe the "glue" part should be an (out of tree build) application, while the bulk of the code is part of the platform? |
|
@whoenig your "Knowing other Crazyflies position" implementation is close to what I had in mind. The hard-coded limit on number of neighbors could be a limitation for large swarms, especially if the system is extended to support decentralized localization. Maybe this is an appropriate use case for dynamic memory allocation? Either way, this is a minor issue IMO. I initially thought it wouldn't be possible to implement collision avoidance as a modification/augmentation layer in between the commander and controller in the stabilizer, but looking at your implementation where you track the position and velocity of the @krichardsson I agree that accessing the positions of other Crazyflies ought to be available for other localization methods besides mocap. P2P seems really promising because it would allow each CF to perform its own sensor fusion with its IMU and share a higher quality state estimate. If I understand your proposal correctly: we add something like Re. SITAW, I am not too familiar with this module, but it seems like some high level functionality like Re. utility/application architecture, I agree the "glue" part is pretty small. Aside from peer localization, the main computational task for BVC is projecting a vector into a convex polytope, which I have already implemented as a standalone function in |
Yes, if you need to perform computations that risk to block the radio task, the best option is probably to create a new task that runs that computation. Queues is a good tool for task-to-task communication.
Not sure, my comment was more on a philosophical level :-)
It is very sparse, it will not take long https://github.com/bitcraze/crazyflie-firmware/blob/master/docs/userguides/app_layer.md |
|
Hi all, I was thinking more about how the collision avoidance module is best integrated into the rest of the planning and control framework. At first I thought that we needed a new planner mode, but after more consideration, I think we can make it work in a more flexible way. However, I have one unresolved design question, denoted in bold near the end. I suggest the following architecture: Collision avoidance is implemented as a layer in between the commander and the controller. It consumes a Aside from the Voronoi cell itself, relevant parameters are:
Using the shorthand
|
|
Just a few notes:
In my understanding, this is exactly what the situation awareness framework already does. Currently, it has support to detect attitude issues (e.g., drone flipped upside down), but it is generic enough to handle position setpoint adjustments, too. I prefer thinking about the different modes in terms of typical use-cases; not all combinations make sense. In particular:
As a side note: I don't like the setpoint struct that much for the reason that not all combinations make sense. It would be nicer to have an enum and union for the desired "flight mode". Then each controller could also decide which flight modes it can actually support properly. In the situation awareness framework, we would only enable the collision avoidance in some of the flight modes. The firmware controllers already go that route and only support setpoint mode combinations that are "common", but it is hard to decipher which combinations are even valid. |
|
I agreee SitAw could be a home for this functionality. I guess the alternative would be a new module implementing something like Re. your case 3, can you mathematically define "scale the velocity term so that it is a consistent feedforward term with respect to the updated/constrained position to help the tracking controller"? I agree with your comments about the setpoint struct, but any changes should be deferred to a standalone project IMO. I think the concept of "flight mode" could also potentially subsume the state machine logic in |
|
For case 3, this would need to solve a planning problem instance. IMO the easiest way would be to use the one-piece trajectory planner from planner.c to plan from the current state to stop at a position within your BVC. Then the velocity is the derivative of the polynomial at t=dt. Another option would be to use a double integrator model instead. Agreed that any setpoint struct changes are not within the scope of this issue. I only pointed it out to argue that we do not need to support all possible setpoint struct configurations in this design, but can focus on commonly used ones. |
|
@whoenig OK, I think I misinterpreted your initial comment before. I was breaking down the cases in terms of the commander's intent, but it sounds like you are breaking down the cases in terms of the controller's abilities. So if I understand correctly, when you say:
you are disagreeing not with my statement
but only with my statement
and the reason you disagree is because setting You propose to instead generate an appropriate velocity command by solving a polynomial trajectory planning problem subject to:
and then evaluating the velocity of the polynomial trajectory at If my interpretation is correct, then won't the resulting velocity command be very close to the current velocity in most cases? |
|
Your interpretation is correct! I think the velocity command will be close to the current velocity if dt << H. Further, H might be something that we'll need to compute/estimate dynamically, based on the dynamics limits (e.g., v_max, a_max). If we only consider those two derivatives, it might be easier to go with a double integrator model, where we estimate H using the maximum possible deceleration. At the end, we try to approximate an optimal control problem with constraints to stay within our BVC, similar to how the original BVC paper had it formulated. We could also try to use cvxgen or similar tools to generate code that can actually solve the optimal control problem online (although I feel this might be "overkill" for the intended use-case.) |
|
I was thinking that H would be a user specified parameter they can send over the radio when enabling collision avoidance, or a param. I agree that CVXGEN is most likely overkill. I also think it is not a good dependency to introduce into the crazyflie-firmware project because it requires an account on the CVXGEN website to generate code, the code generator is not open source, and the generated code is hard to understand. I lean towards keeping the collision avoidance really simple and allowing it to do things like generate discontinuous velocity commands. We could then put more effort into making the controllers handle step inputs smoothly, which would have a much larger payoff across many Crazyflie use cases. Personally, I think it's OK to include the caveat that collision avoidance is only designed to handle slow movement, at least in the initial version. |
|
Sounds good! One comment: OSQP is a good open-source alternative to cvxgen, see https://stanford.edu/~boyd/papers/pdf/osqp_embedded.pdf. (Again, I don't think we should use it for this issue, but just for future reference.) |
|
OK, so to summarize: in the case when Now, returning to my previous question about what to do if
In case i., I think it is not the responsibility of the collision avoidance module to track trajectories, so we should assume that the downstream controller will quickly reject the disturbance and reach In case ii., scaling the velocity in the same way effectively is reducing the gain of the upstream controller, which is probably not a big deal. So in the end, the same behavior seems to make sense for all 3 cases. |
|
btw, since this has been merged already, can we close this? |
fix parsing xml qtm
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: 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.The text was updated successfully, but these errors were encountered: