Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Extend BendyRuler to search for paths up and down

Browse Source
This commit is contained in:
Rishabh 2020-07-01 11:41:09 +05:30 committed by Randy Mackay
parent 55923d8b7b
commit b62455fd3f
4 changed files with 239 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

215
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -19,16 +19,21 @@
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
// parameter defaults
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_TYPE_DEFAULT = 1;
const int16_t OA_BENDYRULER_BEARING_INC = 5; // check every 5 degrees around vehicle
const int16_t OA_BENDYRULER_BEARING_INC_XY = 5; // check every 5 degrees around vehicle
const int16_t OA_BENDYRULER_BEARING_INC_VERTICAL = 90;
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
#define VERTICAL_ENABLED APM_BUILD_TYPE(APM_BUILD_ArduCopter)
const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = {
// @Param: LOOKAHEAD
@ -56,6 +61,13 @@ const AP_Param::GroupInfo AP_OABendyRuler::var_info[] = {
// @User: Standard
AP_GROUPINFO("CONT_ANGLE", 3, AP_OABendyRuler, _bendy_angle, OA_BENDYRULER_ANGLE_DEFAULT),
// @Param{Copter}: TYPE
// @DisplayName: Type of BendyRuler
// @Description: BendyRuler will search for clear path along the direction defined by this parameter
// @Values: 1:Horizontal search, 2:Vertical search
// @User: Standard
AP_GROUPINFO_FRAME("TYPE", 4, AP_OABendyRuler, _bendy_type, OA_BENDYRULER_TYPE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER),
AP_GROUPEND
};
@ -68,7 +80,7 @@ AP_OABendyRuler::AP_OABendyRuler()
// 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)
{
{
// bendy ruler always sets origin to current_loc
origin_new = current_loc;
@ -97,9 +109,28 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
} else {
ground_course_deg = degrees(ground_speed_vec.angle());
}
bool ret;
switch (get_type()) {
case OABendyType::OA_BENDY_VERTICAL:
#if VERTICAL_ENABLED
ret = search_vertical_path(current_loc, destination, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest);
break;
#endif
case OABendyType::OA_BENDY_HORIZONTAL:
default:
ret = search_xy_path(current_loc, destination, ground_course_deg, destination_new, lookahead_step1_dist, lookahead_step2_dist, bearing_to_dest, distance_to_dest);
}
return ret;
}
// Search for path in the horizontal directions
bool AP_OABendyRuler::search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step1_dist, float lookahead_step2_dist, float bearing_to_dest, float distance_to_dest )
{
// check OA_BEARING_INC definition allows checking in all directions
static_assert(360 % OA_BENDYRULER_BEARING_INC == 0, "check 360 is a multiple of OA_BEARING_INC");
static_assert(360 % OA_BENDYRULER_BEARING_INC_XY == 0, "check 360 is a multiple of OA_BEARING_INC");
// search in OA_BENDYRULER_BEARING_INC degree increments around the vehicle alternating left
// and right. For each direction check if vehicle would avoid all obstacles
@ -108,14 +139,14 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
float best_margin = -FLT_MAX;
float best_margin_bearing = best_bearing;
for (uint8_t i = 0; i <= (170 / OA_BENDYRULER_BEARING_INC); i++) {
for (uint8_t i = 0; i <= (170 / OA_BENDYRULER_BEARING_INC_XY); i++) {
for (uint8_t bdir = 0; bdir <= 1; bdir++) {
// skip duplicate check of bearing straight towards destination
if ((i==0) && (bdir > 0)) {
continue;
}
// bearing that we are probing
const float bearing_delta = i * OA_BENDYRULER_BEARING_INC * (bdir == 0 ? -1.0f : 1.0f);
const float bearing_delta = i * OA_BENDYRULER_BEARING_INC_XY * (bdir == 0 ? -1.0f : 1.0f);
const float bearing_test = wrap_180(bearing_to_dest + bearing_delta);
// ToDo: add effective groundspeed calculations using airspeed
@ -125,7 +156,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
Location test_loc = current_loc;
test_loc.offset_bearing(bearing_test, lookahead_step1_dist);
// calculate margin from fence for this scenario
// calculate margin from obstacles for this scenario
float margin = calc_avoidance_margin(current_loc, test_loc);
if (margin > best_margin) {
best_margin_bearing = bearing_test;
@ -155,7 +186,8 @@ 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
// if the chosen direction is directly towards the destination avoidance can be turned off
// i == 0 && j == 0 implies no deviation from bearing to destination
const bool active = (i != 0 || j != 0);
float final_bearing = bearing_test;
float final_margin = margin;
@ -166,7 +198,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
destination_new = current_loc;
destination_new.offset_bearing(final_bearing, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
AP::logger().Write_OABendyRuler(active, bearing_to_dest, ignore_bearing_change, final_margin, destination, destination_new);
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, active, bearing_to_dest, 0.0f, ignore_bearing_change, final_margin, destination, destination_new);
return active;
}
}
@ -192,11 +224,135 @@ 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, false, best_margin, destination, destination_new);
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_HORIZONTAL, true, chosen_bearing, 0.0f, false, best_margin, destination, destination_new);
return true;
}
// Search for path in the vertical directions
bool AP_OABendyRuler::search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest)
{
// check OA_BEARING_INC_VERTICAL definition allows checking in all directions
static_assert(360 % OA_BENDYRULER_BEARING_INC_VERTICAL == 0, "check 360 is a multiple of OA_BEARING_INC_VERTICAL");
float best_pitch = 0.0f;
bool have_best_pitch = false;
float best_margin = -FLT_MAX;
float best_margin_pitch = best_pitch;
const uint8_t angular_limit = 180 / OA_BENDYRULER_BEARING_INC_VERTICAL;
for (uint8_t i = 0; i <= angular_limit; i++) {
for (uint8_t bdir = 0; bdir <= 1; bdir++) {
// skip duplicate check of bearing straight towards destination or 180 degrees behind
if (((i==0) && (bdir > 0)) || ((i == angular_limit) && (bdir > 0))) {
continue;
}
// bearing that we are probing
const float pitch_delta = i * OA_BENDYRULER_BEARING_INC_VERTICAL * (bdir == 0 ? 1.0f : -1.0f);
Location test_loc = current_loc;
test_loc.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, lookahead_step1_dist);
// calculate margin from obstacles for this scenario
float margin = calc_avoidance_margin(current_loc, test_loc);
if (margin > best_margin) {
best_margin_pitch = pitch_delta;
best_margin = margin;
}
if (margin > _margin_max) {
// this path avoids the obstacles with the required margin, now check for the path ahead
if (!have_best_pitch) {
best_pitch = pitch_delta;
have_best_pitch = true;
}
const float test_pitch_step2[] { 0.0f, 90.0f, -90.0f, 180.0f};
float bearing_to_dest2;
if (is_equal(fabsf(pitch_delta), 90.0f)) {
bearing_to_dest2 = bearing_to_dest;
} else {
bearing_to_dest2 = test_loc.get_bearing_to(destination) * 0.01f;
}
float distance2 = constrain_float(lookahead_step2_dist, OA_BENDYRULER_LOOKAHEAD_STEP2_MIN, test_loc.get_distance(destination));
for (uint8_t j = 0; j < ARRAY_SIZE(test_pitch_step2); j++) {
float bearing_test2 = wrap_180(test_pitch_step2[j]);
Location test_loc2 = test_loc;
test_loc2.offset_bearing_and_pitch(bearing_to_dest2, bearing_test2 ,distance2);
// 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 we might turn off avoidance
// i == 0 && j == 0 implies no deviation from bearing to destination
bool active = (i != 0 || j != 0);
if (!active) {
// do a sub test to confirm if we should really turn of BendyRuler
const float sub_test_pitch_step2[] {-90.0f, 90.0f};
for (uint8_t k = 0; k < ARRAY_SIZE(sub_test_pitch_step2); k++) {
Location test_loc_sub_test = test_loc;
test_loc_sub_test.offset_bearing_and_pitch(bearing_to_dest2, sub_test_pitch_step2[k], _margin_max);
float margin_sub_test = calc_avoidance_margin(test_loc, test_loc_sub_test);
if (margin_sub_test < _margin_max) {
// BendyRuler will remain active
active = true;
break;
}
}
}
// project in the chosen direction by the full distance
destination_new = current_loc;
destination_new.offset_bearing_and_pitch(bearing_to_dest,pitch_delta, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new);
return active;
}
}
}
}
}
float chosen_pitch;
if (have_best_pitch) {
// none of the directions tested were OK for 2-step checks. Choose the direction
// that was best for the first step
chosen_pitch = best_pitch;
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.05f);
} else {
// none of the possible paths had a positive margin. Choose
// the one with the highest margin
chosen_pitch = best_margin_pitch;
_current_lookahead = MAX(_lookahead * 0.5f, _current_lookahead * 0.9f);
}
// calculate new target based on best effort
destination_new = current_loc;
destination_new.offset_bearing_and_pitch(bearing_to_dest, chosen_pitch, distance_to_dest);
// log results
AP::logger().Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, true, bearing_to_dest, chosen_pitch,false, best_margin, destination, destination_new);
return true;
}
AP_OABendyRuler::OABendyType AP_OABendyRuler::get_type() const
{
switch (_bendy_type) {
case (uint8_t)OABendyType::OA_BENDY_VERTICAL:
#if VERTICAL_ENABLED
return OABendyType::OA_BENDY_VERTICAL;
#endif
case (uint8_t)OABendyType::OA_BENDY_HORIZONTAL:
default:
return OABendyType::OA_BENDY_HORIZONTAL;
}
// should never reach here
return OABendyType::OA_BENDY_HORIZONTAL;
}
/*
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.
@ -252,6 +408,15 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati
if (calc_margin_from_circular_fence(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
}
#if VERTICAL_ENABLED
// alt fence only is only needed in vertical avoidance
if (get_type() == OABendyType::OA_BENDY_VERTICAL) {
if (calc_margin_from_alt_fence(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
}
}
#endif
if (calc_margin_from_object_database(start, end, latest_margin)) {
margin_min = MIN(margin_min, latest_margin);
@ -295,6 +460,38 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
return true;
}
// calculate minimum distance between a path and the altitude fence
// on success returns true and updates margin
bool AP_OABendyRuler::calc_margin_from_alt_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();
if (fence == nullptr) {
return false;
}
if ((fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) == 0) {
return false;
}
int32_t alt_above_home_cm_start, alt_above_home_cm_end;
if (!start.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm_start)) {
return false;
}
if (!end.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_above_home_cm_end )) {
return false;
}
// safe max alt = fence alt - fence margin
const float max_fence_alt = fence->get_safe_alt_max();
const float margin_start = max_fence_alt - alt_above_home_cm_start * 0.01f;
const float margin_end = max_fence_alt - alt_above_home_cm_end * 0.01f;
// margin is minimum distance to fence from either start or end location
margin = MIN(margin_start,margin_end);
return true;
}
// 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) const

22
libraries/AC_Avoidance/AP_OABendyRuler.h
View File

@ -23,6 +23,21 @@ public:
// 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);
enum class OABendyType {
OA_BENDY_DISABLED = 0,
OA_BENDY_HORIZONTAL = 1,
OA_BENDY_VERTICAL = 2,
};
// return type of BendyRuler in use
OABendyType get_type() const;
// search for path in XY direction
bool search_xy_path(const Location& current_loc, const Location& destination, float ground_course_deg, Location &destination_new, float lookahead_step_1_dist, float lookahead_step_2_dist, float bearing_to_dest, float distance_to_dest);
// search for path in the Vertical directions
bool search_vertical_path(const Location& current_loc, const Location& destination,Location &destination_new, const float &lookahead_step1_dist, const float &lookahead_step2_dist, const float &bearing_to_dest, const float &distance_to_dest);
static const struct AP_Param::GroupInfo var_info[];
private:
@ -37,6 +52,10 @@ private:
// on success returns true and updates margin
bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const;
// calculate minimum distance between a path and the altitude fence
// on success returns true and updates margin
bool calc_margin_from_alt_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) const;
@ -56,7 +75,8 @@ private:
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
AP_Int8 _bendy_type; // Type of BendyRuler to run
// 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

9
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -107,6 +107,15 @@ void AP_OAPathPlanner::init()
start_thread();
}
// return type of BendyRuler in use
AP_OABendyRuler:: OABendyType AP_OAPathPlanner::get_bendy_type() const
{
if (_oabendyruler == nullptr) {
return AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED;
}
return _oabendyruler->get_type();
}
// pre-arm checks that algorithms have been initialised successfully
bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{

3
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -62,6 +62,9 @@ public:
uint16_t get_options() const { return _options;}
// helper function to return type of BendyRuler in use. This is used by AC_WPNav_OA
AP_OABendyRuler::OABendyType get_bendy_type() const;
static const struct AP_Param::GroupInfo var_info[];
private: