Collision Prevention for manual 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.
hereby its important to note that scaling currently only happens in surface-normal direction, which means that the drone is still able to move freely tangentially.
whereby the acceleration constraining is done in the distance between CP_DIST and MPC_VEL_MANUAL / MPC_XY_P

Drawbacks

The main drawback of this approach is that it might allow collision causing accelerations if flying into inside corners, as when your closer to one wall than the other, you can command maximum acceleration into the other wall.
might introduce normal and tangential damping parameters to solve this.

Changelog Entry

For release notes:

Feature: Collision prevention for FlightModeManual Acceleration
New parameter: TBD
Documentation: Need to clarify page ... / done, read docs.px4.io/...

Drawbacks

• Difficulty with inside corners
• 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 a 2D scanning lidar
• Hardware Testing planned

Context

CP.mp4

[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.