Collision avoidance pos ctrl #10710
Collision avoidance pos ctrl #10710
Conversation
| * @boolean | ||
| * @group Multicopter Position Control | ||
| */ | ||
| PARAM_DEFINE_INT32(MPC_USE_OBS_SENS, 0); |
There was a problem hiding this comment.
You might want to move the OBS forward so that the relevant parameters are grouped together when viewed in the param tree.
MPC_OBS_SENS_EN - enable use of obstacle avoidance sensors
MPC_OBS_MIN_DIST - obstacle avoidance sensors min distance
msg/vehicle_constraints.msg
Outdated
| @@ -10,6 +10,7 @@ float32 speed_down # in meters/sec | |||
| float32 tilt # in radians [0, PI] | |||
| float32 min_distance_to_ground # in meters | |||
| float32 max_distance_to_ground # in meters | |||
| float32[4] velocity_limits # maximum velocities determined by range sensor measurements [x positive, y positive, x negative, y negative] direction. | |||
There was a problem hiding this comment.
Suggestion - consider splitting this into separate arrays for velocity min (x & y) and velocity max (x & y). Then in the code you can directly create a Vector2f.
| //apply the velocity reductions to form velocity limits | ||
| _constraints.velocity_limits[0] = v_max_x - _constraints.velocity_limits[0]; | ||
| _constraints.velocity_limits[1] = v_max_y - _constraints.velocity_limits[1]; | ||
| _constraints.velocity_limits[2] = v_max_x - _constraints.velocity_limits[2]; |
There was a problem hiding this comment.
Whitespace error, check CI output.
|
This is awesome! Do we want this directly in the position controller though? Can't we split this same functionality into a separate low-level avoidance module like I did in my branch? This also allows slightly more advanced behaviour in the future if we decide to implement it. (e.g reciprocal velocity avoidance). |
|
Link to branch : mhkabir@883ed61 |
|
I've been wanting to create an automotive radar based distance sensor. This would be a great combo. |
There was a problem hiding this comment.
Nice!
Why don't we put this new feature in a library so that it can be called from any flight task, for example auto?
I think it would also be useful to directly use the distance_sensor along with obstacle_distance for vehicles that only have one sonar/time of flight sensor.
| MulticopterPositionControl::update_range_constraints(const obstacle_distance_s &obstacle_distance, | ||
| vehicle_constraints_s &constraints) | ||
| { | ||
| constraints.velocity_limits_x[0] = 0.0; |
There was a problem hiding this comment.
0.0 -> 0.0f there are a couple spread in the code
|
I still don't understand why this needs to live within the multicopter position controller, as a library or otherwise. Can't obstacle avoidance be it's own module, which in the future is extended, for example, with a more complex algorithm, or avoidance for geofences, or even with strategies for fixedwings sharing airspace with manned aviation. Building the avoidance module with a similar module structure as land detector at this stage would future proof it and avoid adding crud to the position controller, most of which was cleaned up thanks to FlightTasks. Fundamentally, I think avoidance has no place inside the controller. @dagar @LorenzMeier @bkueng thoughts? |
I thought that was one of the main intentions with FlightTasks, to provide a clean interface for things like obstacle avoidance? |
|
As a start simply keeping the avoidance logic in a separate library could help work towards being a full module applicable to all vehicle types. |
|
we flew some tests today, it behaved reasonable. It was a bit wobbly, and also it sometimes approached a bit closer than the specified distance if the throttle was kept towards the obstacle. This might also be due to the sensor measurements. We used the local_planner to generate range data form 3 realsense cameras. I did not have time yet to look at the logs but I put them here for anyone who is interested: Here a video, where the drone was commanded to go forward at full throttle towards the tree, max velocity was set to 5m/s. Here some flight logs: |
| for(int i = 0; i<72; i++){ | ||
|
|
||
| //determine if distance bin is valid and contains a valid distance measurement | ||
| if(obstacle_distance.distances[i] < obstacle_distance.max_distance && |
There was a problem hiding this comment.
I would add a check on the obstacle_distance timestamp to make sure you're not using stale data
I observe this with my approach as well. We might need to pursue some kind of "reciprocal velocity" approach to combat this. |
I'm already doing that. The velocity goes negative if you get ranges, which state that you are too close to an obstacle. |
|
➤ Wrike Bot commented: @tanja Baumann All subtasks have been completed or deferred, you might want to complete this task as well. |
|
closing this because got replaced by #10785 |
Use range sensor measurements to avoid collisions in position control mode.
Range sensor data is read in and transformed into velocity constraints which prevent the vehicle from approaching any obstacle closer than a specified distance. Only the velocity component towards the obstacles is limited all other velocity components of the stick input are preserved and executed.
SITL tested, flight tests pending