Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Improve BendyRuler hesitancy by avoiding significant bearing change

This commit is contained in:
Rishabh 2020-06-13 15:07:40 +05:30 committed by Randy Mackay
parent 03ea646e50
commit 7dd7565d14
4 changed files with 133 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -19,12 +19,52 @@
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
const float OA_BENDYRULER_LOOKAHEAD_DEFAULT = 15.0f;
const float OA_BENDYRULER_RATIO_DEFAULT = 1.5f;
const int16_t OA_BENDYRULER_ANGLE_DEFAULT = 75;
const int16_t OA_BENDYRULER_BEARING_INC = 5; // check every 5 degrees around vehicle
const float OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO = 1.0f; // step2's lookahead length as a ratio of step1's lookahead length
const float OA_BENDYRULER_LOOKAHEAD_STEP2_MIN = 2.0f; // step2 checks at least this many meters past step1's location
const float OA_BENDYRULER_LOOKAHEAD_PAST_DEST = 2.0f; // lookahead length will be at least this many meters past the destination
const float OA_BENDYRULER_LOW_SPEED_SQUARED = (0.2f * 0.2f); // when ground course is below this speed squared, vehicle's heading will be used
const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = {
// @Param: LOOKAHEAD
// @DisplayName: Object Avoidance look ahead distance maximum
// @Description: Object Avoidance will look this many meters ahead of vehicle
// @Units: m
// @Range: 1 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LOOKAHEAD", 1, AP_OABendyRuler, _lookahead, OA_BENDYRULER_LOOKAHEAD_DEFAULT),
// @Param: CONT_RATIO
// @DisplayName: Obstacle Avoidance margin ratio for BendyRuler to change bearing significantly
// @Description: BendyRuler will avoid changing bearing unless ratio of previous margin from obstacle (or fence) to present calculated margin is atleast this much.
// @Range: 1.1 2
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("CONT_RATIO", 2, AP_OABendyRuler, _bendy_ratio, OA_BENDYRULER_RATIO_DEFAULT),
// @Param: CONT_ANGLE
// @DisplayName: BendyRuler's bearing change resistance threshold angle
// @Description: BendyRuler will resist changing current bearing if the change in bearing is over this angle
// @Range: 20 180
// @Increment: 5
// @User: Standard
AP_GROUPINFO("CONT_ANGLE", 3, AP_OABendyRuler, _bendy_angle, OA_BENDYRULER_ANGLE_DEFAULT),
AP_GROUPEND
};
AP_OABendyRuler::AP_OABendyRuler()
{
AP_Param::setup_object_defaults(this, var_info);
_bearing_prev = FLT_MAX;
}
// run background task to find best path and update avoidance_results
// returns true and updates origin_new and destination_new if a best path has been found
bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new)
@ -36,6 +76,9 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
const float bearing_to_dest = current_loc.get_bearing_to(destination) * 0.01f;
const float distance_to_dest = current_loc.get_distance(destination);
// make sure user has set a meaningful value for _lookahead
_lookahead = MAX(_lookahead,1.0f);
// lookahead distance is adjusted dynamically based on avoidance results
_current_lookahead = constrain_float(_current_lookahead, _lookahead * 0.5f, _lookahead);
@ -112,13 +155,18 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
// calculate minimum margin to fence and obstacles for this scenario
float margin2 = calc_avoidance_margin(test_loc, test_loc2);
if (margin2 > _margin_max) {
// if the chosen direction is directly towards the destination avoidance can be turned off
const bool active = (i != 0 || j != 0);
float final_bearing = bearing_test;
float final_margin = margin;
// check if we need ignore test_bearing and continue on previous bearing
const bool ignore_bearing_change = resist_bearing_change(destination, current_loc, active, bearing_test, lookahead_step1_dist, margin, _destination_prev,_bearing_prev, final_bearing, final_margin);
// all good, now project in the chosen direction by the full distance
destination_new = current_loc;
destination_new.offset_bearing(bearing_test, distance_to_dest);
destination_new.offset_bearing(final_bearing, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
// if the chosen direction is directly towards the destination turn off avoidance
const bool active = (i != 0 || j != 0);
AP::logger().Write_OABendyRuler(active, bearing_to_dest, margin, destination, destination_new);
AP::logger().Write_OABendyRuler(active, bearing_to_dest, ignore_bearing_change, final_margin, destination, destination_new);
return active;
}
}
@ -144,13 +192,59 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
destination_new.offset_bearing(chosen_bearing, distance_to_dest);
// log results
AP::logger().Write_OABendyRuler(true, chosen_bearing, best_margin, destination, destination_new);
AP::logger().Write_OABendyRuler(true, chosen_bearing, false, best_margin, destination, destination_new);
return true;
}
/*
This function is called when BendyRuler has found a bearing which is obstacles free at atleast lookahead_step1_dist and then lookahead_step2_dist from the present location
In many situations, this new bearing can be either left or right of the obstacle, and BendyRuler can have a tough time deciding between the two.
It has the tendency to move the vehicle back and forth, if the margin obtained is even slightly better in the newer iteration.
Therefore, this method attempts to avoid changing direction of the vehicle by more than _bendy_angle degrees,
unless the new margin is atleast _bendy_ratio times better than the margin with previously calculated bearing.
We return true if we have resisted the change and will follow the last calculated bearing.
*/
bool AP_OABendyRuler::resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin) const
{
bool resisted_change = false;
// see if there was a change in destination, if so, do not resist changing bearing
bool dest_change = false;
if (!destination.same_latlon_as(prev_dest)) {
dest_change = true;
prev_dest = destination;
}
// check if we need to resist the change in direction of the vehicle. If we have a clear path to destination, go there any how
if (active && !dest_change && is_positive(_bendy_ratio)) {
// check the change in bearing between freshly calculated and previous stored BendyRuler bearing
if ((fabsf(wrap_180(prev_bearing-bearing_test)) > _bendy_angle) && (!is_equal(prev_bearing,FLT_MAX))) {
// check margin in last bearing's direction
Location test_loc_previous_bearing = current_loc;
test_loc_previous_bearing.offset_bearing(wrap_180(prev_bearing), lookahead_step1_dist);
float previous_bearing_margin = calc_avoidance_margin(current_loc,test_loc_previous_bearing);
if (margin < (_bendy_ratio * previous_bearing_margin)) {
// don't change direction abruptly. If margin difference is not significant, follow the last direction
final_bearing = prev_bearing;
final_margin = previous_bearing_margin;
resisted_change = true;
}
}
} else {
// reset stored bearing if BendyRuler is not active or if WP has changed for unnecessary resistance to path change
prev_bearing = FLT_MAX;
}
if (!resisted_change) {
// we are not resisting the change, hence store BendyRuler's presently calculated bearing for future iterations
prev_bearing = bearing_test;
}
return resisted_change;
}
// calculate minimum distance between a segment and any obstacle
float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end)
float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end) const
{
float margin_min = FLT_MAX;
@ -177,7 +271,7 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati
// calculate minimum distance between a path and the circular fence (centered on home)
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin)
bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const
{
// exit immediately if polygon fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
@ -203,7 +297,7 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
// calculate minimum distance between a path and all inclusion and exclusion polygons
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin)
bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const
{
const AC_Fence *fence = AC_Fence::get_singleton();
if (fence == nullptr) {
@ -269,7 +363,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
// calculate minimum distance between a path and all inclusion and exclusion circles
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin)
bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const
{
// exit immediately if fence is not enabled
const AC_Fence *fence = AC_Fence::get_singleton();
@ -345,7 +439,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
// calculate minimum distance between a path and proximity sensor obstacles
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin)
bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const
{
// exit immediately if db is empty
AP_OADatabase *oaDb = AP::oadatabase();

30
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -10,45 +10,55 @@
*/
class AP_OABendyRuler {
public:
AP_OABendyRuler() {}
AP_OABendyRuler();
/* Do not allow copies */
AP_OABendyRuler(const AP_OABendyRuler &other) = delete;
AP_OABendyRuler &operator=(const AP_OABendyRuler&) = delete;
// send configuration info stored in front end parameters
void set_config(float lookahead, float margin_max) { _lookahead = MAX(lookahead, 1.0f); _margin_max = MAX(margin_max, 0.0f); }
void set_config(float margin_max) { _margin_max = MAX(margin_max, 0.0f); }
// run background task to find best path and update avoidance_results
// returns true and populates origin_new and destination_new if OA is required. returns false if OA is not required
bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new);
static const struct AP_Param::GroupInfo var_info[];
private:
// calculate minimum distance between a path and any obstacle
float calc_avoidance_margin(const Location &start, const Location &end);
float calc_avoidance_margin(const Location &start, const Location &end) const;
// determine if BendyRuler should accept the new bearing or try and resist it. Returns true if bearing is not changed
bool resist_bearing_change(const Location &destination, const Location &current_loc, bool active, float bearing_test, float lookahead_step1_dist, float margin, Location &prev_dest, float &prev_bearing, float &final_bearing, float &final_margin) const;
// calculate minimum distance between a path and the circular fence (centered on home)
// on success returns true and updates margin
bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin);
bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const;
// calculate minimum distance between a path and all inclusion and exclusion polygons
// on success returns true and updates margin
bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin);
bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const;
// calculate minimum distance between a path and all inclusion and exclusion circles
// on success returns true and updates margin
bool calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin);
bool calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const;
// calculate minimum distance between a path and proximity sensor obstacles
// on success returns true and updates margin
bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin);
bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const;
// configuration parameters
float _lookahead; // object avoidance will look this many meters ahead of vehicle
// OA common parameters
float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle
// BendyRuler parameters
AP_Float _lookahead; // object avoidance will look this many meters ahead of vehicle
AP_Float _bendy_ratio; // object avoidance will avoid major directional change if change in margin ratio is less than this param
AP_Int16 _bendy_angle; // object avoidance will try avoding change in direction over this much angle
// internal variables used by background thread
float _current_lookahead; // distance (in meters) ahead of the vehicle we are looking for obstacles
float _bearing_prev; // stored bearing in degrees
Location _destination_prev; // previous destination, to check if there has been a change in destination
};

18
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -24,7 +24,6 @@
extern const AP_HAL::HAL &hal;
// parameter defaults
const float OA_LOOKAHEAD_DEFAULT = 15;
const float OA_MARGIN_MAX_DEFAULT = 5;
#if APM_BUILD_TYPE(APM_BUILD_Rover)
const int16_t OA_OPTIONS_DEFAULT = 1;
@ -42,14 +41,8 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = {
// @User: Standard
AP_GROUPINFO_FLAGS("TYPE", 1, AP_OAPathPlanner, _type, OA_PATHPLAN_DISABLED, AP_PARAM_FLAG_ENABLE),
// @Param: LOOKAHEAD
// @DisplayName: Object Avoidance look ahead distance maximum
// @Description: Object Avoidance will look this many meters ahead of vehicle
// @Units: m
// @Range: 1 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LOOKAHEAD", 2, AP_OAPathPlanner, _lookahead, OA_LOOKAHEAD_DEFAULT),
// Note: Do not use Index "2" for any new parameter
// It was being used by _LOOKAHEAD which was later moved to AP_OABendyRuler
// @Param: MARGIN_MAX
// @DisplayName: Object Avoidance wide margin distance
@ -73,6 +66,10 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = {
// @User: Standard
AP_GROUPINFO("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT),
#endif
// @Group: BR_
// @Path: AP_OABendyRuler.cpp
AP_SUBGROUPPTR(_oabendyruler, "BR_", 6, AP_OAPathPlanner, AP_OABendyRuler),
AP_GROUPEND
};
@ -96,6 +93,7 @@ void AP_OAPathPlanner::init()
case OA_PATHPLAN_BENDYRULER:
if (_oabendyruler == nullptr) {
_oabendyruler = new AP_OABendyRuler();
AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
}
break;
case OA_PATHPLAN_DIJKSTRA:
@ -259,7 +257,7 @@ void AP_OAPathPlanner::avoidance_thread()
if (_oabendyruler == nullptr) {
continue;
}
_oabendyruler->set_config(_lookahead, _margin_max);
_oabendyruler->set_config(_margin_max);
if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new)) {
res = OA_SUCCESS;
}

2
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -90,9 +90,9 @@ private:
// parameters
AP_Int8 _type; // avoidance algorithm to be used
AP_Float _lookahead; // object avoidance will look this many meters ahead of vehicle
AP_Float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle
AP_Int16 _options; // Bitmask for options while recovering from Object Avoidance
// internal variables used by front end
HAL_Semaphore _rsem; // semaphore for multi-thread use of avoidance_request and avoidance_result
bool _thread_created; // true once background thread has been created