Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[WIP] Collision Prevention Update #3364

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

[WIP] Collision Prevention Update #3364

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d8cdd14
adapted the collision prevention documentation to reflect the move to…
Claudio-Chies Aug 27, 2024
64e8729
Typos
hamishwillee Aug 27, 2024
0f2dfab
added releases
Claudio-Chies Sep 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions en/computer_vision/collision_prevention.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ Multiple sensors can be used to get information about, and prevent collisions wi
If multiple sources supply data for the _same_ orientation, the system uses the data that reports the smallest distance to an object.
:::

The vehicle restricts the maximum velocity in order to slow down as it gets closer to obstacles, and will stop movement when it reaches the minimum allowed separation.
The vehicle restricts the current velocity in order to slow down as it gets closer to obstacles and adapts the acceleration setpoint in order to disallow collision trajectories.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The first bit makes sense to me - you're approaching a wall, so the vehicle reduces the velocity such that you stop before you hit it.

FMI, what does "adapts the accel. setpoint to disallow collision trajectories" mean?

Assume I'm heading towards a wall at a 45 degree angle, does this mean? I.e. I'm guessing it might mean that the setpoint is adjusted such that you curve into the wall and then move alongside it. Some images showing the behaviour might be nice.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also youtube the video to the PX4 account and include that in the docs too

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yea, some figures might actually help, but for that i first need feedback on my PR from Mathias, as its not yet set in stone how the compensation works.
Regarding "adapts the accel. setpoint to disallow collision trajectories", see comment below.

In order to move away from (or parallel to) an obstacle, the user must command the vehicle to move toward a setpoint that does not bring the vehicle closer to the obstacle.
The algorithm will make minor adjustments to the setpoint direction if it is determined that a "better" setpoint exists within a fixed margin on either side of the requested setpoint.

Expand All @@ -46,7 +46,7 @@ Configure collision prevention by [setting the following parameters](../advanced
| <a id="CP_DELAY"></a>[CP_DELAY](../advanced_config/parameter_reference.md#CP_DELAY) | Set the sensor and velocity setpoint tracking delay. See [Delay Tuning](#delay_tuning) below. |
| <a id="CP_GUIDE_ANG"></a>[CP_GUIDE_ANG](../advanced_config/parameter_reference.md#CP_GUIDE_ANG) | Set the angle (to both sides of the commanded direction) within which the vehicle may deviate if it finds fewer obstacles in that direction. See [Guidance Tuning](#angle_change_tuning) below. |
| <a id="CP_GO_NO_DATA"></a>[CP_GO_NO_DATA](../advanced_config/parameter_reference.md#CP_GO_NO_DATA) | Set to 1 to allow the vehicle to move in directions where there is no sensor coverage (default is 0/`False`). |
| <a id="MPC_POS_MODE"></a>[MPC_POS_MODE](../advanced_config/parameter_reference.md#MPC_POS_MODE) | Set to `Direct velocity` or `Smoothed velocity` to enable Collision Prevention in Position Mode (default is `Acceleration based`). |
| <a id="MPC_POS_MODE"></a>[MPC_POS_MODE](../advanced_config/parameter_reference.md#MPC_POS_MODE) | Set to `Acceleration based` (default), Implementation in `Direct velocity` or `Smoothed velocity` has been removed. |

## Algorithm Description

Expand All @@ -57,13 +57,14 @@ If so, the vehicle velocity is restricted.
This velocity restriction takes into account both the inner velocity loop tuned by [MPC_XY_P](../advanced_config/parameter_reference.md#MPC_XY_P), as well as the [jerk-optimal velocity controller](../config_mc/mc_jerk_limited_type_trajectory.md) via [MPC_JERK_MAX](../advanced_config/parameter_reference.md#MPC_JERK_MAX) and [MPC_ACC_HOR](../advanced_config/parameter_reference.md#MPC_ACC_HOR).
The velocity is restricted such that the vehicle will stop in time to maintain the distance specified in [CP_DIST](#CP_DIST).
The range of the sensors for each sector is also taken into account, limiting the velocity via the same mechanism.
the restricted velocity is then transformed into an acceleration with [MPC_XY_VEL_P_ACC](../advanced_config/parameter_reference.md#MPC_XY_VEL_P_ACC).
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

FWIW as above, I don't really understand what this means. I think that perhaps we're always controlling the acceleration setpoint, so for our controller, so what this says is that we've worked out what our restricted velocity is supposed to be, and this converts it to the value that we need for our accel. setpoint?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
the restricted velocity is then transformed into an acceleration with [MPC_XY_VEL_P_ACC](../advanced_config/parameter_reference.md#MPC_XY_VEL_P_ACC).
The restricted velocity is then transformed into an acceleration with [MPC_XY_VEL_P_ACC](../advanced_config/parameter_reference.md#MPC_XY_VEL_P_ACC).

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yea, its a bit complicated. which is also why the rewrite has not been done before.
we have our velocity and acceleration setpoint coming from the operator. and the problem is that they can point into different directions. so we need to compensate the current velocity if its heading into an obstacle, and restrict the acceleration setpoint from the operator.
the current approach worked fine with velocities. which is why i take the constrained velocity with the old approach, subtract the current velocity to get a velocity error, and just add the proportional gain of the acceleration controller (the input to whom is an velocity error). you can kind of think of the gain as something which modifies the units, input is [m/s] output is [m/s²]. (in an abstract sense)

and for the setpoint of the operator, i just scale it in regard of the distance to the minimum distance. the closer the vehicle is to the minimum distance, the more the normal component gets scaled to zero.


::: info
If there is no sensor data in a particular direction, velocity in that direction is restricted to 0 (preventing the vehicle from crashing into unseen objects).
If you wish to move freely into directions without sensor coverage, this can be enabled by setting [CP_GO_NO_DATA](#CP_GO_NO_DATA) to 1.
:::

Delay, both in the vehicle tracking velocity setpoints and in receiving sensor data from external sources, is conservatively estimated via the [CP_DELAY](#CP_DELAY) parameter.
The delay associated with collision prevention, both in the vehicle tracking velocity setpoints and in receiving sensor data from external sources, is conservatively estimated via the [CP_DELAY](#CP_DELAY) parameter.
This should be [tuned](#delay_tuning) to the specific vehicle.

If the sectors adjacent to the commanded sectors are 'better' by a significant margin, the direction of the requested input can be modified by up to the angle specified in [CP_GUIDE_ANG](#CP_GUIDE_ANG).
Expand Down Expand Up @@ -178,6 +179,7 @@ The system may work well at significantly higher speeds and lower frequency dist
:::

The tested companion software is the _local_planner_ from the [PX4/PX4-Avoidance](https://github.com/PX4/PX4-Avoidance) repo.
This repository is archived/no longer maintained.
For more information on hardware and software setup see: [PX4/PX4-Avoidance > Run on Hardware](https://github.com/PX4/PX4-Avoidance#run-on-hardware).

<!-- hardware platform used for testing not readily available, so have removed -->
Expand Down
Loading