Collision Prevention for multicopter acceleration based position flight mode #23507
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
4 times, most recently
from
a3d8d8b to
d130612
Compare
Claudio-Chies
changed the title
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
2 times, most recently
from
dcfa190 to
9cf4838
Compare
bresch
reviewed
Aug 12, 2024
|
@Claudio-Chies is this something I can pull down to flight test? Or is it needing more time in the SITL? |
|
@dirksavage88 it should be ready to test, the only thing i haven't tested is how non-default gains in the position and velocity controller change the CP behavior |
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
6 times, most recently
from
7fa1fcc to
077226b
Compare
1 task
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
2 times, most recently
from
1e40431 to
bd72ecf
Compare
MaEtUgR
reviewed
Oct 1, 2024
There was a problem hiding this comment.
After an in person discussion I see a lot of value in simplifying the logic. The main pain points are:
- Separate input data processing from guidance logic e.g. distance and direction to closest obstacle and distance to next obstacle in current velocity direction are the interface between obstacle map processing and guidance.
- If possible avoid cases in guidance logic. E.g. three things to take care of:
- Stick should not accelerate over the configured collision prevention minimum distance
- Brake from current velocity before before minimum allowed distance
- If obstacle is closer/"comes closer" push away from it to minimum distance
Probably these rules can run all the time, get superimposed and be readable, predictable (logged).
I added a commit for cosmetic and commenting things that I saw at a glance.
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
2 times, most recently
from
e0b6707 to
d346da5
Compare
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
2 times, most recently
from
6d72e11 to
1075492
Compare
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
from
c5834b2 to
de8fc84
Compare
The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation.
…tests to reflect the change rewrite of obstacle_distance merging methods, and fix of various issues
The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation.
…cle_dsitance message
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
from
f654979 to
ec1efcc
Compare
Claudio-Chies
commented
Nov 21, 2024
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
2 times, most recently
from
59edc23 to
3b4efab
Compare
…f we are closer to the obstacle than the minimal distance
Claudio-Chies
force-pushed
the
pr-Collision_Prevention
branch
from
3b4efab to
f05f032
Compare
FLASH Analysispx4_fmu-v5xpx4_fmu-v6x |
MaEtUgR
approved these changes
Nov 21, 2024
There was a problem hiding this comment.
Nice! Thanks for putting the effort in to not only make this work with FlightTaskManualAcceleration (current default Position mode) but also adding some improvements and bearing through all my partly unrelated review. I think it's at a good big iteration state now and we can follow up with individual improvements iteratively.
|
💵 To receive payouts, sign up on Algora, link your Github account and connect with Stripe. |
mahimayoga
reviewed
Jan 21, 2025
| for (int i = 0; i < BIN_COUNT; i++) { | ||
| // check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data | ||
| if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get() | ||
| || (_param_cp_go_no_data.get() && _data_fov[i]))) { |
There was a problem hiding this comment.
@Claudio-Chies would you mind explaining why we need to check for (_param_cp_go_no_data.get() && _data_fov[i]))? Why should the setpoint not be feasible if we don't have data in the direction we want to travel when it is in our fov, if _param_cp_go_no_data is set to true?
There was a problem hiding this comment.
This if statement was taken from the previous implementation, there it was
if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
if (!move_no_data || (move_no_data && _data_fov[i])) {
vel_max = 0.f;
}
}
i'll have to test it, but the more i look at it, it would never be true, if _param_cp_go_no_data is true.
because there wouldnt ever be a case where _obstacle_map_body_frame.distances[i]== UINT16_MAX and _data_fov[i] is true.
EDIT: i can't test it until next week,
There was a problem hiding this comment.
So that's what I thought as well, but actually somehow when flying collision prevention with the SF45, the _data_fov array is often initialized incorrectly (true in all bins). I am trying to debug this separately, but generally wasn't sure why this condition exists in the if statement - because I thought that as long as _param_cp_go_no_data is set to true, you should be able to fly in any direction.
There was a problem hiding this comment.
yea, the problem with that lies most likely in that _data_fov, is only set to 0 in the constructor. and the sf45 can generate wierd measurements during initialisation, one of the reason why i had to implement the timeout logic in the driver. because without it, it displayed measurements in regions where it cant measure. the effectos of that is probably what your are seeing here, meaning these measurements are still sent in the beginning, _data_fov gets set to true because it once got a measurement in that bin.
you could either solve that in the driver, to not publish measurements until they are "sensible", or adapt the logic here
(probably somewhere here
hope this helps
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Solved Problem
Ports and extends the current collision prevention algorithm to FlightModeManualAcceleration
Associated PR's
Solution
The algorithm has two parts, velocity compensation, and constraining of the commanded acceleration.
Velocity compensation
For this, I have ported over the existing methods from the velocity based collision prevention method and transformed the velocity into an acceleration by subtracting the velocity set-point from the collision prevention velocity and applying a proportional gain ( MPC_XY_VEL_P_ACC ) to it.
Acceleration constraining
For this I'm calculating the minimal distance to the obstacle, and scaling it quadratic in the range of [0,1] distances proportional to the maximum velocity. (See image below)
For this we split the acceleration setpoint coming from the operator into a surface normal and tangential component, then using the normal and tangential obstacle distance we scale these components individual.
This way, if we are in front of a big even wall, we are free to move tangentially but are restricted in the surface normal direction.
whereby, the acceleration constraining is done in the distance between CP_DIST and MPC_VEL_MANUAL / MPC_XY_P
Drawbacks
Removed in 87a0ee6
Changelog Entry
For release notes:
Documentation: Need to update https://docs.px4.io/main/en/computer_vision/collision_prevention.html
WIP PX4/PX4-user_guide#3364
Drawbacks
Alternatives
Test coverage
Context
CP.mp4
Hardware Tests
1D Lidar
The oscillations which are present are due to flying close to the ground, and when pitching the distance sensor picks up the ground.
EDIT1:
Updated description to include tangential scaling
/claim #22464