From 0b112981f9b9d1755dc68775f66d8e31349bfb6d Mon Sep 17 00:00:00 2001 From: Rishabh Date: Tue, 21 Jul 2020 22:31:53 +0530 Subject: [PATCH] AC_Avoid: Back away if vehicle breaches margin to obstacle/fence --- libraries/AC_Avoidance/AC_Avoid.cpp | 322 +++++++++++++++++++++++----- libraries/AC_Avoidance/AC_Avoid.h | 39 +++- 2 files changed, 296 insertions(+), 65 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index c2b98b1f46..8fc0aec3f9 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -18,7 +18,7 @@ #include // Failsafe fence library #include #include - +#include #include #if APM_BUILD_TYPE(APM_BUILD_Rover) @@ -68,6 +68,14 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO_FRAME("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), + // @Param: BACKUP_SPD + // @DisplayName: Avoidance maximum backup speed + // @Description: Maximum speed that will be used to back away from obstacles in GPS modes (m/s). Set zero to disable + // @Units: m/s + // @Range: 0 2 + // @User: Standard + AP_GROUPINFO("BACKUP_SPD", 6, AC_Avoid, _backup_speed_max, 0.5f), + AP_GROUPEND }; @@ -79,7 +87,7 @@ AC_Avoid::AC_Avoid() AP_Param::setup_object_defaults(this, var_info); } -void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, bool &backing_up,float dt) { // exit immediately if disabled if (_enabled == AC_AVOID_DISABLED) { @@ -88,20 +96,77 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); + // make a copy of desired velocity since the one that is passed will be modified + const Vector2f desired_vel_orig = desired_vel_cms; + + // maximum component of desired backup velocity in each quadrant + Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) { - adjust_velocity_circle_fence(kP, accel_cmss_limited, desired_vel_cms, dt); - adjust_velocity_inclusion_and_exclusion_polygons(kP, accel_cmss_limited, desired_vel_cms, dt); - adjust_velocity_inclusion_circles(kP, accel_cmss_limited, desired_vel_cms, dt); - adjust_velocity_exclusion_circles(kP, accel_cmss_limited, desired_vel_cms, dt); + // Store velocity needed to back away from fence + Vector2f backup_vel_fence; + + adjust_velocity_circle_fence(kP, accel_cmss_limited, desired_vel_cms, backup_vel_fence, dt); + find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + + // backup_vel_fence is set to zero after each fence incase the velocity is unset from previous methods + backup_vel_fence.zero(); + adjust_velocity_inclusion_and_exclusion_polygons(kP, accel_cmss_limited, desired_vel_cms, backup_vel_fence, dt); + find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + + backup_vel_fence.zero(); + adjust_velocity_inclusion_circles(kP, accel_cmss_limited, desired_vel_cms, backup_vel_fence, dt); + find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + + backup_vel_fence.zero(); + adjust_velocity_exclusion_circles(kP, accel_cmss_limited, desired_vel_cms, backup_vel_fence, dt); + find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); } if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) { - adjust_velocity_beacon_fence(kP, accel_cmss_limited, desired_vel_cms, dt); + // Store velocity needed to back away from beacon fence + Vector2f backup_vel_beacon; + adjust_velocity_beacon_fence(kP, accel_cmss_limited, desired_vel_cms, backup_vel_beacon, dt); + find_max_quadrant_velocity(backup_vel_beacon, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); } if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) { - adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, dt); + // Store velocity needed to back away from physical obstacles + Vector2f backup_vel_proximity; + adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel_cms, backup_vel_proximity, dt); + find_max_quadrant_velocity(backup_vel_proximity, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); + } + + // Desired backup velocity is sum of maximum velocity component in each quadrant + Vector2f desired_backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; + + const float max_back_spd_cms = _backup_speed_max * 100.0f; + if (!desired_backup_vel.is_zero() && is_positive(max_back_spd_cms)) { + backing_up = true; + // Constrain backing away speed + if (desired_backup_vel.length() > max_back_spd_cms) { + desired_backup_vel = desired_backup_vel.normalized() * max_back_spd_cms; + } + if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { + // project desired velocity towards backup velocity + Vector2f projected_vel = desired_vel_cms.projected(desired_backup_vel); + // subtract this projection since we are already going in that direction + desired_vel_cms -= projected_vel; + desired_vel_cms += desired_backup_vel; + } else { + // back away to stopping position + desired_vel_cms = desired_backup_vel; + } + } + + // only log results if velocity has been modified to avoid fence/obstacle + if (desired_vel_orig != desired_vel_cms) { + // log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!) + uint32_t now = AP_HAL::millis(); + if ((now - _last_log_ms) > 100) { + _last_log_ms = now; + AP::logger().Write_SimpleAvoidance(true, desired_vel_orig, desired_vel_cms, backing_up); + } } } @@ -125,9 +190,22 @@ void AC_Avoid::adjust_speed(float kP, float accel, float heading, float &speed, Vector2f vel_xy; vel_xy.x = cosf(heading) * speed * 100.0f; vel_xy.y = sinf(heading) * speed * 100.0f; - adjust_velocity(kP, accel * 100.0f, vel_xy, dt); - // adjust speed towards zero + bool backing_up = false; + adjust_velocity(kP, accel * 100.0f, vel_xy, backing_up, dt); + + if (backing_up) { + // back up + if (fabsf(wrap_180(degrees(vel_xy.angle())) - degrees(heading)) > 90.0f) { + // Big difference between the direction of velocity vector and actual heading therefore we need to reverse the direction + speed = -vel_xy.length() * 0.01f; + } else { + speed = vel_xy.length() * 0.01f; + } + return; + } + + // No need to back up so adjust speed towards zero if needed if (is_negative(speed)) { speed = -vel_xy.length() * 0.01f; } else { @@ -274,6 +352,59 @@ void AC_Avoid::limit_velocity(float kP, float accel_cmss, Vector2f &desired_vel_ } } +/* + * Compute the back away velocity required to avoid breaching margin + * INPUT: This method requires the breach in margin distance (back_distance_cm), direction towards the breach (limit_direction) + * It then calculates the desired backup velocity and passes it on to "find_max_quadrant_velocity" method to distribute the velocity vectors into respective quadrants + * OUTPUT: The method then outputs four velocities (quad1/2/3/4_back_vel_cms), which correspond to the maximum final desired backup velocity in each quadrant + */ +void AC_Avoid::calc_backup_velocity(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt) +{ + if (limit_direction.is_zero()) { + // protect against divide by zero + return; + } + // speed required to move away the exact distance that we have breached the margin with + const float back_speed = get_max_speed(kP, 0.4f * accel_cmss, fabsf(back_distance_cm), dt); + + // direction to the obstacle + limit_direction.normalize(); + + // move in the opposite direction with the required speed + Vector2f back_direction_vel = limit_direction * (-back_speed); + // divide the vector into quadrants, find maximum velocity component in each quadrant + find_max_quadrant_velocity(back_direction_vel, quad1_back_vel_cms, quad2_back_vel_cms, quad3_back_vel_cms, quad4_back_vel_cms); +} + +/* + * Calculate maximum velocity vector that can be formed in each quadrant + * This method takes the desired backup velocity, and four other velocities corresponding to each quadrant + * The desired velocity is then fit into one of the 4 quadrant velocities as per the sign of its components + * This ensures that if we have multiple backup velocities, we can get the maximum of all of those velocities in each quadrant +*/ +void AC_Avoid::find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel) +{ + if (desired_vel.is_zero()) { + return; + } + // first quadrant: +ve x, +ve y direction + if (is_positive(desired_vel.x) && is_positive(desired_vel.y)) { + quad1_vel = Vector2f{MAX(quad1_vel.x, desired_vel.x), MAX(quad1_vel.y,desired_vel.y)}; + } + // second quadrant: -ve x, +ve y direction + if (is_negative(desired_vel.x) && is_positive(desired_vel.y)) { + quad2_vel = Vector2f{MIN(quad2_vel.x, desired_vel.x), MAX(quad2_vel.y,desired_vel.y)}; + } + // third quadrant: -ve x, -ve y direction + if (is_negative(desired_vel.x) && is_negative(desired_vel.y)) { + quad3_vel = Vector2f{MIN(quad3_vel.x, desired_vel.x), MIN(quad3_vel.y,desired_vel.y)}; + } + // fourth quadrant: +ve x, -ve y direction + if (is_positive(desired_vel.x) && is_negative(desired_vel.y)) { + quad4_vel = Vector2f{MAX(quad4_vel.x, desired_vel.x), MIN(quad4_vel.y,desired_vel.y)}; + } +} + /* * Computes the speed such that the stopping distance * of the vehicle will be exactly the input distance. @@ -290,7 +421,7 @@ float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, flo /* * Adjusts the desired velocity for the circular fence. */ -void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) { AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -331,12 +462,27 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f // get the margin to the fence in cm const float margin_cm = _fence.get_margin() * 100.0f; + if (margin_cm > fence_radius) { + return; + } + // get vehicle distance from home const float dist_from_home = position_xy.length(); if (dist_from_home > fence_radius) { // outside of circular fence, no velocity adjustments return; } + const float distance_to_boundary = fence_radius - dist_from_home; + + // for backing away + Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + + // back away if vehicle has breached margin + if (is_negative(distance_to_boundary - margin_cm)) { + calc_backup_velocity(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_cm - distance_to_boundary, position_xy, dt); + } + // desired backup velocity is sum of maximum velocity component in each quadrant + backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; // vehicle is inside the circular fence if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { @@ -389,7 +535,7 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f /* * Adjusts the desired velocity for the exclusion polygons */ -void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) { const AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -401,14 +547,18 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float return; } + // for backing away + Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + // iterate through inclusion polygons const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count(); for (uint8_t i = 0; i < num_inclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points); - + Vector2f backup_vel_inc; // adjust velocity - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, true); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel_inc, boundary, num_points, true, fence->get_margin(), dt, true); + find_max_quadrant_velocity(backup_vel_inc, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); } // iterate through exclusion polygons @@ -416,16 +566,19 @@ void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float for (uint8_t i = 0; i < num_exclusion_polygons; i++) { uint16_t num_points; const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points); - + Vector2f backup_vel_exc; // adjust velocity - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, false); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel_exc, boundary, num_points, true, fence->get_margin(), dt, false); + find_max_quadrant_velocity(backup_vel_exc, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); } + // desired backup velocity is sum of maximum velocity component in each quadrant + backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; } /* * Adjusts the desired velocity for the inclusion circles */ -void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) { const AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -443,13 +596,6 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec return; } - // get desired speed - const float desired_speed = desired_vel_cms.length(); - if (is_zero(desired_speed)) { - // no avoidance necessary when desired speed is zero - return; - } - // get vehicle position Vector2f position_NE; if (!AP::ahrs().get_relative_position_NE_origin(position_NE)) { @@ -461,18 +607,26 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec // get the margin to the fence in cm const float margin_cm = fence->get_margin() * 100.0f; + // get desired speed + const float desired_speed = desired_vel_cms.length(); + // get stopping distance as an offset from the vehicle Vector2f stopping_offset; - switch ((AC_Avoid::BehaviourType)_behavior.get()) { - case BEHAVIOR_SLIDE: - stopping_offset = desired_vel_cms*(get_stopping_distance(kP, accel_cmss, desired_speed)/desired_speed); - break; - case BEHAVIOR_STOP: - // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence - stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); - break; + if (!is_zero(desired_speed)) { + switch ((AC_Avoid::BehaviourType)_behavior.get()) { + case BEHAVIOR_SLIDE: + stopping_offset = desired_vel_cms*(get_stopping_distance(kP, accel_cmss, desired_speed)/desired_speed); + break; + case BEHAVIOR_STOP: + // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence + stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); + break; + } } + // for backing away + Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + // iterate through inclusion circles for (uint8_t i = 0; i < num_circles; i++) { Vector2f center_pos_cm; @@ -488,6 +642,21 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec continue; } + const float radius_with_margin = radius_cm - margin_cm; + if (is_negative(radius_with_margin)) { + return; + } + + const float margin_breach = radius_with_margin - safe_sqrt(dist_sq_cm); + // back away if vehicle has breached margin + if (is_negative(margin_breach)) { + calc_backup_velocity(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_breach, position_NE_rel, dt); + } + if (is_zero(desired_speed)) { + // no avoidance necessary when desired speed is zero + continue; + } + switch ((AC_Avoid::BehaviourType)_behavior.get()) { case BEHAVIOR_SLIDE: { // implement sliding behaviour @@ -519,6 +688,8 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec // otherwise user is backing away from fence so do not apply limits if (stopping_point_plus_margin.length() >= dist_cm) { desired_vel_cms.zero(); + // desired backup velocity is sum of maximum velocity component in each quadrant + backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; return; } } else { @@ -537,12 +708,14 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec } } } + // desired backup velocity is sum of maximum velocity component in each quadrant + backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; } /* * Adjusts the desired velocity for the exclusion circles */ -void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) { const AC_Fence *fence = AP::fence(); if (fence == nullptr) { @@ -560,13 +733,6 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec return; } - // get desired speed - const float desired_speed = desired_vel_cms.length(); - if (is_zero(desired_speed)) { - // no avoidance necessary when desired speed is zero - return; - } - // get vehicle position Vector2f position_NE; if (!AP::ahrs().get_relative_position_NE_origin(position_NE)) { @@ -578,13 +744,20 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec // get the margin to the fence in cm const float margin_cm = fence->get_margin() * 100.0f; + // for backing away + Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + + // get desired speed + const float desired_speed = desired_vel_cms.length(); + // calculate stopping distance as an offset from the vehicle (only used for BEHAVIOR_STOP) // add a margin so we look forward far enough to intersect with circular fence Vector2f stopping_offset; - if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_STOP) { - stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); + if (!is_zero(desired_speed)) { + if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_STOP) { + stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed); + } } - // iterate through exclusion circles for (uint8_t i = 0; i < num_circles; i++) { Vector2f center_pos_cm; @@ -596,14 +769,28 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec // if we are inside this circle do not limit velocity for this circle const float dist_sq_cm = position_NE_rel.length_squared(); const float radius_cm = (radius * 100.0f); + if (radius_cm < margin_cm) { + return; + } if (dist_sq_cm < sq(radius_cm)) { continue; } + + const Vector2f vector_to_center = center_pos_cm - position_NE; + const float dist_to_boundary = vector_to_center.length() - radius_cm; + // back away if vehicle has breached margin + if (is_negative(dist_to_boundary - margin_cm)) { + calc_backup_velocity(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_cm - dist_to_boundary, vector_to_center, dt); + } + if (is_zero(desired_speed)) { + // no avoidance necessary when desired speed is zero + continue; + } switch ((AC_Avoid::BehaviourType)_behavior.get()) { case BEHAVIOR_SLIDE: { // vector from current position to circle's center - Vector2f limit_direction = center_pos_cm - position_NE; + Vector2f limit_direction = vector_to_center; if (limit_direction.is_zero()) { // vehicle is exactly on circle center so do not limit velocity continue; @@ -629,6 +816,7 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec // otherwise user is backing away from fence so do not apply limits if (stopping_point_plus_margin.length() <= dist_cm) { desired_vel_cms.zero(); + backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; return; } } else { @@ -647,12 +835,14 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec } } } + // desired backup velocity is sum of maximum velocity component in each quadrant + backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; } /* * Adjusts the desired velocity for the beacon fence. */ -void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) { AP_Beacon *_beacon = AP::beacon(); @@ -673,13 +863,13 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f if (AP::fence()) { margin = AP::fence()->get_margin(); } - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, margin, dt, true); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, true, margin, dt, true); } /* * Adjusts the desired velocity based on output from the proximity sensor */ -void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) +void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt) { // exit immediately if proximity sensor is not present AP_Proximity *proximity = AP::proximity(); @@ -696,24 +886,19 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &d // get boundary from proximity sensor uint16_t num_points = 0; const Vector2f *boundary = _proximity.get_boundary_points(num_points); - adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, false, _margin, dt, true); + adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, false, _margin, dt, true); } /* * Adjusts the desired velocity for the polygon fence. */ -void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt, bool stay_inside) +void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt, bool stay_inside) { // exit if there are no points if (boundary == nullptr || num_points == 0) { return; } - // exit immediately if no desired velocity - if (desired_vel_cms.is_zero()) { - return; - } - const AP_AHRS &_ahrs = AP::ahrs(); // do not adjust velocity if vehicle is outside the polygon fence @@ -737,6 +922,7 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des // We need a separate vector in case adjustment fails, // e.g. if we are exactly on the boundary. Vector2f safe_vel(desired_vel_cms); + Vector2f desired_back_vel_cms; // if boundary points are in body-frame, rotate velocity vector from earth frame to body-frame if (!earth_frame) { @@ -749,8 +935,14 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des // for stopping const float speed = safe_vel.length(); - const Vector2f stopping_point_plus_margin = position_xy + safe_vel*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed); + Vector2f stopping_point_plus_margin; + if (!desired_vel_cms.is_zero()) { + stopping_point_plus_margin = position_xy + safe_vel*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, speed))/speed); + } + // for backing away + Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; + for (uint16_t i=0; i= num_points) { @@ -759,9 +951,20 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des // end points of current edge Vector2f start = boundary[j]; Vector2f end = boundary[i]; + Vector2f vector_to_boundary = Vector2f::closest_point(position_xy, start, end) - position_xy; + // back away if vehicle has breached margin + if (is_negative(vector_to_boundary.length() - margin_cm)) { + calc_backup_velocity(kP, accel_cmss, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel, margin_cm-vector_to_boundary.length(), vector_to_boundary, dt); + } + + // exit immediately if no desired velocity + if (desired_vel_cms.is_zero()) { + continue; + } + if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_SLIDE) { // vector from current position to closest point on current edge - Vector2f limit_direction = Vector2f::closest_point(position_xy, start, end) - position_xy; + Vector2f limit_direction = vector_to_boundary; // distance to closest point const float limit_distance_cm = limit_direction.length(); if (!is_zero(limit_distance_cm)) { @@ -798,14 +1001,19 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des } } } + // desired backup velocity is sum of maximum velocity component in each quadrant + desired_back_vel_cms = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; - // set modified desired velocity vector + // set modified desired velocity vector or back away velocity vector if (earth_frame) { desired_vel_cms = safe_vel; + backup_vel = desired_back_vel_cms; } else { // if points were in body-frame, rotate resulting vector back to earth-frame desired_vel_cms.x = safe_vel.x * _ahrs.cos_yaw() - safe_vel.y * _ahrs.sin_yaw(); desired_vel_cms.y = safe_vel.x * _ahrs.sin_yaw() + safe_vel.y * _ahrs.cos_yaw(); + backup_vel.x = desired_back_vel_cms.x * _ahrs.cos_yaw() - desired_back_vel_cms.y * _ahrs.sin_yaw(); + backup_vel.y = desired_back_vel_cms.x * _ahrs.sin_yaw() + desired_back_vel_cms.y * _ahrs.cos_yaw(); } } diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h index bc95efa6f6..0ecf07eceb 100644 --- a/libraries/AC_Avoidance/AC_Avoid.h +++ b/libraries/AC_Avoidance/AC_Avoid.h @@ -23,6 +23,7 @@ /* * This class prevents the vehicle from leaving a polygon fence in * 2 dimensions by limiting velocity (adjust_velocity). + * Additionally the vehicle may back up if the margin to obstacle is breached */ class AC_Avoid { public: @@ -45,8 +46,12 @@ public: * before the fence/object. * Note: Vector3f version is for convenience and only adjusts x and y axis */ - void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); + void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt) { + bool backing_up = false; + adjust_velocity(kP, accel_cmss, desired_vel_cms, backing_up, dt); + } void adjust_velocity(float kP, float accel_cmss, Vector3f &desired_vel_cms, float dt); + void adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel_cms, bool &backing_up,float dt); // adjust desired horizontal speed so that the vehicle stops before the fence or object // accel (maximum acceleration/deceleration) is in m/s/s @@ -99,28 +104,28 @@ private: /* * Adjusts the desired velocity for the circular fence. */ - void adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); + void adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity for inclusion and exclusion polygon fences */ - void adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); + void adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity for the inclusion and exclusion circles */ - void adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); - void adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); + void adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); + void adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity for the beacon fence. */ - void adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); + void adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity based on output from the proximity sensor */ - void adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt); + void adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, float dt); /* * Adjusts the desired velocity given an array of boundary points @@ -128,13 +133,29 @@ private: * margin is the distance (in meters) that the vehicle should stop short of the polygon * stay_inside should be true for fences, false for exclusion polygons */ - void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt, bool stay_inside); + void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, Vector2f &backup_vel, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt, bool stay_inside); /* * Computes distance required to stop, given current speed. */ float get_stopping_distance(float kP, float accel_cmss, float speed_cms) const; + /* + * Compute the back away velocity required to avoid breaching margin + * INPUT: This method requires the breach in margin distance (back_distance_cm), direction towards the breach (limit_direction) + * It then calculates the desired backup velocity and passes it on to "find_max_quadrant_velocity" method to distribute the velocity vector into respective quadrants + * OUTPUT: The method then outputs four velocities (quad1/2/3/4_back_vel_cms), which correspond to the final desired backup velocity in each quadrant + */ + void calc_backup_velocity(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &qua2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt); + + /* + * Calculate maximum velocity vector that can be formed in each quadrant + * This method takes the desired backup velocity, and four other velocities corresponding to each quadrant + * The desired velocity is then fit into one of the 4 quadrant velocities as per the sign of its components + * This ensures that we have multiple backup velocities, we can get the maximum of all of those velocities in each quadrant + */ + void find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel); + /* * methods for avoidance in non-GPS flight modes */ @@ -151,9 +172,11 @@ private: AP_Float _dist_max; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes AP_Float _margin; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes AP_Int8 _behavior; // avoidance behaviour (slide or stop) + AP_Float _backup_speed_max; // Maximum speed that will be used to back away (in m/s) bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable) uint32_t _last_limit_time; // the last time a limit was active + uint32_t _last_log_ms; // the last time simple avoidance was logged static AC_Avoid *_singleton; };