Collision Prevention for multicopter acceleration based position flight mode

[Claudio-Chies]

Solved Problem

Ports and extends the current collision prevention algorithm to FlightModeManualAcceleration

Associated PR's

• add world for collision prevention PX4-gazebo-models#52
• [WIP] Collision Prevention Update PX4-user_guide#3364

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:

Feature: Collision prevention for FlightModeManual Acceleration

Documentation: Need to update https://docs.px4.io/main/en/computer_vision/collision_prevention.html
WIP PX4/PX4-user_guide#3364

Drawbacks

• If velocity is not close to set point, might cause crashes

Alternatives

• dont use the max velocity and position error gain for the scaling factor and use a new normal and tangential scaling parameter.

Test coverage

• Tested in SITL with 1D and 2D lidar
• Hardware Testing in progress

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

[dirksavage88]

@Claudio-Chies is this something I can pull down to flight test? Or is it needing more time in the SITL?

[Claudio-Chies]

@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

[MaEtUgR]

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.

[dirksavage88]

@MaEtUgR can we change this to be a param?

PX4-Autopilot/src/lib/collision_prevention/CollisionPrevention.cpp

Line 46 in 6764c14

static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly

The original collision prevention used point-lidar sensors with low resolution, but now we have advanced rotating lidars with <1 degree resolution per sample, so being able to set to the lowest increment value of 5 makes sense

[dirksavage88]

@dirksavage88

big issue in indoors with an inside corner. Is there a w

Yea, which is why i mentioned it. i just added a commit which looks at the distance in the tangential direction and then scales the tangential component based on that distance. this should fix this issue.

I just tested with subtfinals on an inside corner and it no longer ping pongs between the corner walls, but it does get stuck. But overall much better. I think the lidar model on the x500 does not have rear facing coverage

[MaEtUgR]

can we change this to be a param?

This should rather be dependent on the input interface resolution

PX4-Autopilot/msg/ObstacleDistance.msg

Line 17 in 4f00df6

float32 increment # Angular width in degrees of each array element.

or if that's too complicated we just run with the highest resolution and just fill the unknown directions accordingly when the resolution is lower.

[dirksavage88]

@MaEtUgR we have the obstacle distance increment set to 5 deg on the SF45 driver currently. However @Claudio-Chies is wanting to change it to 10 to match the collision prevention lib.

[MaEtUgR]

@dirksavage88 I see. I'd say let's change both to 5° and check it doesn't break things. Ideally, collision prevention would adapt to the input data but I assume it doesn't explode to run through the 72 bins.

[Claudio-Chies]

@dirksavage88 sounds good, then let's switch the driver and Collision Prevention to 5.
my assumption was that it was 10 because of computational resources constraints and not because of sensor hardware limitations.

[MaEtUgR]

Well well, your commit breaks the unit tests and mine breaks the unit test build 🙈 Let's roll back and fix. I have multiple suggestions already but I'd like to have constantly passing unit tests as a first barrier to not break things.

[MaEtUgR]

I fixed all my commits but had to remove yours which both line changes broke the unit tests. Here's it for reference: 6be5dd0
I can add it back tomorrow making collision prevention use 72 bins again but have to check if anything is hardcoded in the tests.

[hamishwillee]

Note, this will need a docs update (but should be fairly easy)

[Claudio-Chies]

One open point for discussion:

• should we expand the collision_constraint message to differentiate between the velocity compensation and setpoint scaling? It might help with debugging to see where what contributed how much to the constrained setpoint?