mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 00:47:34 +08:00
new bearing feasibility for handling zero airspeed
- simpler bearing feasibility function - get rid of wind ratio - replace wind ratio buffer with airspeed buffer
This commit is contained in:
Thomas Stastny
committed by
JaeyoungLim
JaeyoungLim
parent
5398428683
commit
4dce637ab7
+35
-77
@@ -56,36 +56,10 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
|
||||
Vector2f air_vel = ground_vel - wind_vel;
|
||||
const float airspeed = air_vel.norm();
|
||||
|
||||
if (airspeed < MIN_AIRSPEED) {
|
||||
// this case should only ever happen if we have not launched, the wind
|
||||
// estimator has failed, or the aircraft is legitimately in a very sad
|
||||
// situation
|
||||
airspeed_ref_ = airspeed_nom_;
|
||||
lateral_accel_ = 0.0f;
|
||||
feas_ = 0.0f;
|
||||
feas_on_track_ = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
const float wind_speed = wind_vel.norm();
|
||||
const float wind_ratio = wind_speed / airspeed;
|
||||
|
||||
float track_error = fabsf(signed_track_error);
|
||||
|
||||
// on-track wind triangle projections
|
||||
float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
|
||||
float wind_dot_upt = wind_vel.dot(unit_path_tangent);
|
||||
|
||||
// calculate the bearing feasibility on the track at the current closest point
|
||||
feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, wind_speed, wind_ratio);
|
||||
|
||||
// update control parameters considering upper and lower stability bounds (if enabled)
|
||||
// must be called before trackErrorBound() as it updates time_const_
|
||||
adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_ratio, track_error,
|
||||
path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
|
||||
p_gain_ = pGain(adapted_period_, damping_);
|
||||
time_const_ = timeConst(adapted_period_, damping_);
|
||||
|
||||
// track error bound is dynamic depending on ground speed
|
||||
track_error_bound_ = trackErrorBound(ground_speed, time_const_);
|
||||
float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
|
||||
@@ -95,6 +69,20 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
|
||||
|
||||
bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
|
||||
|
||||
// on-track wind triangle projections
|
||||
float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
|
||||
float wind_dot_upt = wind_vel.dot(unit_path_tangent);
|
||||
|
||||
// calculate the bearing feasibility on the track at the current closest point
|
||||
feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
|
||||
|
||||
// update control parameters considering upper and lower stability bounds (if enabled)
|
||||
// must be called before trackErrorBound() as it updates time_const_
|
||||
adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
|
||||
path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
|
||||
p_gain_ = pGain(adapted_period_, damping_);
|
||||
time_const_ = timeConst(adapted_period_, damping_);
|
||||
|
||||
// specific waypoint logic complications... handles case where we are following
|
||||
// waypoints and are in front of the first of the segment.
|
||||
// TODO: find a way to get this out of the main eval method!
|
||||
@@ -118,9 +106,9 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
|
||||
|
||||
wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
|
||||
wind_dot_upt = wind_vel.dot(unit_path_tangent);
|
||||
feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, wind_speed, wind_ratio);
|
||||
feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
|
||||
|
||||
adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_ratio, track_error,
|
||||
adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
|
||||
path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
|
||||
p_gain_ = pGain(adapted_period_, damping_);
|
||||
time_const_ = timeConst(adapted_period_, damping_);
|
||||
@@ -133,7 +121,7 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
|
||||
const float wind_dot_bearing = wind_vel.dot(bearing_vec_);
|
||||
|
||||
// continuous representation of the bearing feasibility
|
||||
feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, wind_speed, wind_ratio);
|
||||
feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
|
||||
|
||||
// we consider feasibility of both the current bearing as well as that on the track at the current closest point
|
||||
const float feas_combined = feas_ * feas_on_track_;
|
||||
@@ -152,7 +140,7 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
|
||||
|
||||
// lateral acceleration needed to stay on curved track (assuming no heading error)
|
||||
lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
|
||||
wind_cross_upt, airspeed, wind_speed, wind_ratio, signed_track_error, path_curvature);
|
||||
wind_cross_upt, airspeed, wind_speed, signed_track_error, path_curvature);
|
||||
|
||||
// total lateral acceleration to drive aircaft towards track as well as account
|
||||
// for path curvature. The full effect of the feed-forward acceleration is smoothly
|
||||
@@ -161,13 +149,13 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
|
||||
lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_;
|
||||
} // evaluate
|
||||
|
||||
float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_ratio,
|
||||
float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
|
||||
const float track_error, const float path_curvature, const Vector2f &wind_vel,
|
||||
const Vector2f &unit_path_tangent, const float feas_on_track) const
|
||||
{
|
||||
float period = period_;
|
||||
const float air_turn_rate = fabsf(path_curvature * airspeed);
|
||||
const float wind_factor = windFactor(wind_ratio);
|
||||
const float wind_factor = windFactor(airspeed, wind_speed);
|
||||
|
||||
if (en_period_lb_) {
|
||||
// lower bound the period for stability w.r.t. roll time constant and current flight condition
|
||||
@@ -213,10 +201,15 @@ float NPFG::normalizedTrackError(const float track_error, const float track_erro
|
||||
return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
|
||||
}
|
||||
|
||||
float NPFG::windFactor(const float wind_ratio) const
|
||||
float NPFG::windFactor(const float airspeed, const float wind_speed) const
|
||||
{
|
||||
// See [TODO: include citation] for definition/elaboration of this approximation.
|
||||
return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_ratio)));
|
||||
if (wind_speed > airspeed || airspeed < EPSILON) {
|
||||
return 2.0f;
|
||||
|
||||
} else {
|
||||
return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed)));
|
||||
}
|
||||
} // windFactor
|
||||
|
||||
float NPFG::periodUB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
|
||||
@@ -419,58 +412,23 @@ Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bea
|
||||
return air_vel_ref.normalized() * airspeed;
|
||||
} // infeasibleAirVelRef
|
||||
|
||||
float NPFG::bearingFeasibility(const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
|
||||
const float wind_ratio) const
|
||||
float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
|
||||
const float wind_speed) const
|
||||
{
|
||||
float sin_cross_wind_ang; // in [0, 1] (constant after 90 deg)
|
||||
|
||||
if (wind_dot_bearing <= 0.0f) {
|
||||
sin_cross_wind_ang = 1.0f;
|
||||
if (wind_dot_bearing < 0.0f) {
|
||||
wind_cross_bearing = wind_speed;
|
||||
|
||||
} else {
|
||||
sin_cross_wind_ang = fabsf(wind_cross_bearing / wind_speed);
|
||||
wind_cross_bearing = fabsf(wind_cross_bearing);
|
||||
}
|
||||
|
||||
// upper and lower feasibility barriers
|
||||
float wind_ratio_ub, wind_ratio_lb;
|
||||
|
||||
if (sin_cross_wind_ang < CROSS_WIND_ANG_CO) { // small angle approx.
|
||||
// linear feasibility function (avoid singularity)
|
||||
|
||||
const float wind_ratio_ub_co = ONE_DIV_SIN_CROSS_WIND_ANG_CO;
|
||||
wind_ratio_ub = wind_ratio_ub_co + CO_SLOPE * (CROSS_WIND_ANG_CO - sin_cross_wind_ang);
|
||||
|
||||
const float wind_ratio_lb_co = (ONE_DIV_SIN_CROSS_WIND_ANG_CO - 2.0f) * wind_ratio_buffer_ + 1.0f;
|
||||
wind_ratio_lb = wind_ratio_lb_co + wind_ratio_buffer_ * CO_SLOPE * (CROSS_WIND_ANG_CO - sin_cross_wind_ang);
|
||||
|
||||
} else {
|
||||
const float one_div_sin_cross_wind_ang = 1.0f / sin_cross_wind_ang;
|
||||
wind_ratio_ub = one_div_sin_cross_wind_ang;
|
||||
wind_ratio_lb = (one_div_sin_cross_wind_ang - 2.0f) * wind_ratio_buffer_ + 1.0f;
|
||||
}
|
||||
|
||||
// calculate bearing feasibility
|
||||
float feas = 1.0f; // feasible
|
||||
|
||||
if (wind_ratio > wind_ratio_ub) {
|
||||
// infeasible
|
||||
feas = 0.0f;
|
||||
|
||||
} else if (wind_ratio > wind_ratio_lb) {
|
||||
// partially feasible
|
||||
// smoothly transition from fully feasible to fully infeasible
|
||||
feas = cosf(M_PI_F * 0.5f * math::constrain((wind_ratio - wind_ratio_lb) / (wind_ratio_ub - wind_ratio_lb), 0.0f,
|
||||
1.0f));
|
||||
feas *= feas;
|
||||
}
|
||||
|
||||
return feas;
|
||||
float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / airspeed_buffer_, 0.0f, 1.0f));
|
||||
return sin_arg * sin_arg;
|
||||
} // bearingFeasibility
|
||||
|
||||
float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel,
|
||||
const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
|
||||
const float wind_speed, const float wind_ratio, const float signed_track_error,
|
||||
const float path_curvature) const
|
||||
const float wind_speed, const float signed_track_error, const float path_curvature) const
|
||||
{
|
||||
// NOTE: all calculations within this function take place at the closet point
|
||||
// on the path, as if the aircraft were already tracking the given path at
|
||||
|
||||
+20
-29
@@ -118,12 +118,12 @@ public:
|
||||
/*
|
||||
* Set the nominal airspeed reference [m/s].
|
||||
*/
|
||||
void setAirspeedNom(float airsp) { airspeed_nom_ = math::constrain(airsp, MIN_AIRSPEED, airspeed_max_); }
|
||||
void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }
|
||||
|
||||
/*
|
||||
* Set the maximum airspeed reference [m/s].
|
||||
*/
|
||||
void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, airspeed_nom_); }
|
||||
void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); }
|
||||
|
||||
/*
|
||||
* Set the autopilot roll response time constant [s].
|
||||
@@ -131,9 +131,9 @@ public:
|
||||
void setRollTimeConst(float tc) { roll_time_const_ = math::max(tc, 0.1f); }
|
||||
|
||||
/*
|
||||
* Set the wind ratio buffer size.
|
||||
* Set the airspeed buffer size.
|
||||
*/
|
||||
void setWindRatioBuffer(float buf) { wind_ratio_buffer_ = math::constrain(buf, 0.01f, 0.2f); }
|
||||
void setAirspeedBuffer(float buf) { airspeed_buffer_ = math::max(buf, 0.1f); }
|
||||
|
||||
/*
|
||||
* @return Controller proportional gain [rad/s]
|
||||
@@ -335,22 +335,14 @@ public:
|
||||
private:
|
||||
|
||||
static constexpr float EPSILON = 1.0e-4;
|
||||
static constexpr float MIN_AIRSPEED = 1.0f; // constrain airspeed to avoid singularities [m/s]
|
||||
static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]
|
||||
static constexpr float PERIOD_SAFETY_FACTOR = 4.0f; // multiplier for period lower bound
|
||||
static constexpr float PERIOD_SAFETY_FACTOR = 4.0f; // multiplier for period lower bound [s]
|
||||
|
||||
/* pre-computed constants for linear cut-off function for bearing feasibility calculation */
|
||||
static constexpr float CROSS_WIND_ANG_CO =
|
||||
0.02; // cross wind angle cut-off below which the feasibility barrier function is finite and linear [rad] (= approx. 1 deg)
|
||||
static constexpr float ONE_DIV_SIN_CROSS_WIND_ANG_CO = 50.003333488895450; // 1/sin(CROSS_WIND_ANG_CO)
|
||||
static constexpr float CO_SLOPE =
|
||||
2499.833309998360; // cross wind angle cut-off slope = cos(CROSS_WIND_ANG_CO)/sin(CROSS_WIND_ANG_CO)^2
|
||||
|
||||
float period_{30.0f}; // nominal (desired) period -- user defined [s]
|
||||
float damping_{0.25f}; // nominal (desired) damping ratio -- user defined
|
||||
float p_gain_{0.12566f}; // proportional gain (computed from period_ and damping_) [rad/s]
|
||||
float time_const_{9.0f}; // time constant (computed from period_ and damping_) [s]
|
||||
float adapted_period_{30.0f}; // auto-adapted period (if stability bounds enabled) [s]
|
||||
float period_{20.0f}; // nominal (desired) period -- user defined [s]
|
||||
float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined
|
||||
float p_gain_{0.4442}; // proportional gain (computed from period_ and damping_) [rad/s]
|
||||
float time_const_{14.142f}; // time constant (computed from period_ and damping_) [s]
|
||||
float adapted_period_{20.0f}; // auto-adapted period (if stability bounds enabled) [s]
|
||||
|
||||
bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period
|
||||
bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled)
|
||||
@@ -368,7 +360,7 @@ private:
|
||||
// ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached
|
||||
float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible)
|
||||
float feas_on_track_{1.0f}; // continuous bearing feasibility "on track"
|
||||
float wind_ratio_buffer_{0.1f}; // a buffer region below unity wind ratio allowing continuous transition between feasible and infeasible conditions/commands
|
||||
float airspeed_buffer_{1.5f}; // size of the region above the feasibility boundary (into feasible space) where a continuous transition from feasible to infeasible is imposed [m/s]
|
||||
|
||||
float track_error_bound_{135.0f}; // the current ground speed dependent track error bound [m]
|
||||
float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track
|
||||
@@ -420,7 +412,7 @@ private:
|
||||
*
|
||||
* @param[in] ground_speed Vehicle ground speed [m/s]
|
||||
* @param[in] airspeed Vehicle airspeed [m/s]
|
||||
* @param[in] wind_ratio Wind speed to airspeed ratio
|
||||
* @param[in] wind_speed Wind speed [m/s]
|
||||
* @param[in] track_error Track error (magnitude) [m]
|
||||
* @param[in] path_curvature Path curvature at closest point on track [m^-1]
|
||||
* @param[in] wind_vel Wind velocity vector in inertial frame [m/s]
|
||||
@@ -429,7 +421,7 @@ private:
|
||||
* @param[in] feas_on_track Bearing feasibility on track at the closest point
|
||||
* @return Adapted period [s]
|
||||
*/
|
||||
float adaptPeriod(const float ground_speed, const float airspeed, const float wind_ratio,
|
||||
float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
|
||||
const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel,
|
||||
const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const;
|
||||
|
||||
@@ -445,10 +437,11 @@ private:
|
||||
/*
|
||||
* Cacluates an approximation of the wind factor (see [TODO: include citation]).
|
||||
*
|
||||
* @param[in] wind_ratio Wind speed to airspeed ratio
|
||||
* @param[in] airspeed Vehicle airspeed [m/s]
|
||||
* @param[in] wind_speed Wind speed [m/s]
|
||||
* @return Non-dimensional wind factor approximation
|
||||
*/
|
||||
float windFactor(const float wind_ratio) const;
|
||||
float windFactor(const float airspeed, const float wind_speed) const;
|
||||
|
||||
/*
|
||||
* Calculates a theoretical upper bound on the user defined period to maintain
|
||||
@@ -622,12 +615,12 @@ private:
|
||||
*
|
||||
* @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
|
||||
* @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
|
||||
* @param[in] airspeed Vehicle airspeed [m/s]
|
||||
* @param[in] wind_speed Wind speed [m/s]
|
||||
* @param[in] wind_ratio Wind speed to airspeed ratio
|
||||
* @return bearing feasibility
|
||||
*/
|
||||
float bearingFeasibility(const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
|
||||
const float wind_ratio) const;
|
||||
float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
|
||||
const float wind_speed) const;
|
||||
|
||||
/*
|
||||
* Calculates an additional feed-forward lateral acceleration demand considering
|
||||
@@ -639,7 +632,6 @@ private:
|
||||
* @param[in] wind_vel Wind velocity vector [m/s]
|
||||
* @param[in] airspeed Vehicle airspeed [m/s]
|
||||
* @param[in] wind_speed Wind speed [m/s]
|
||||
* @param[in] wind_ratio Wind speed to airspeed ratio
|
||||
* @param[in] signed_track_error Signed error to track at closest point (sign
|
||||
* determined by path normal direction) [m]
|
||||
* @param[in] path_curvature Path curvature at closest point on track [m^-1]
|
||||
@@ -647,8 +639,7 @@ private:
|
||||
*/
|
||||
float lateralAccelFF(const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &ground_vel,
|
||||
const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
|
||||
const float wind_speed, const float wind_ratio, const float signed_track_error,
|
||||
const float path_curvature) const;
|
||||
const float wind_speed, const float signed_track_error, const float path_curvature) const;
|
||||
|
||||
/*
|
||||
* Calculates a lateral acceleration demand from the heading error.
|
||||
|
||||
@@ -118,7 +118,7 @@ FixedwingPositionControl::parameters_update()
|
||||
_npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get());
|
||||
_npfg.setNormalizedTrackErrorFraction(_param_npfg_nte_fraction.get());
|
||||
_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
|
||||
_npfg.setWindRatioBuffer(_param_npfg_wind_ratio_buf.get());
|
||||
_npfg.setAirspeedBuffer(_param_npfg_airspeed_buffer.get());
|
||||
_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
|
||||
_npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get()));
|
||||
|
||||
|
||||
@@ -387,7 +387,7 @@ private:
|
||||
(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
|
||||
(ParamFloat<px4::params::NPFG_NTE_FRAC>) _param_npfg_nte_fraction,
|
||||
(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
|
||||
(ParamFloat<px4::params::NPFG_WR_BUF>) _param_npfg_wind_ratio_buf,
|
||||
(ParamFloat<px4::params::NPFG_ASPD_BUF>) _param_npfg_airspeed_buffer,
|
||||
|
||||
(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
|
||||
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
|
||||
|
||||
@@ -222,19 +222,20 @@ PARAM_DEFINE_FLOAT(NPFG_NTE_FRAC, 0.5f);
|
||||
PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
|
||||
|
||||
/**
|
||||
* NPFG wind ratio buffer
|
||||
* NPFG airspeed buffer
|
||||
*
|
||||
* The size of the feasibility transition region at cross wind angle >= 90 deg.
|
||||
* This must be non-zero to avoid jumpy airspeed incrementation while using wind
|
||||
* excess handling logic.
|
||||
* excess handling logic. Similarly used as buffer region around zero airspeed.
|
||||
*
|
||||
* @min 0.01
|
||||
* @max 0.30
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @unit m/s
|
||||
* @min 0.1
|
||||
* @max 5.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group FW NPFG Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NPFG_WR_BUF, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(NPFG_ASPD_BUF, 1.5f);
|
||||
|
||||
/**
|
||||
* Cruise throttle
|
||||
|
||||
Reference in New Issue
Block a user