Skip to content

Commit

Permalink
Multicopter rounded turns (#16376)
Browse files Browse the repository at this point in the history 
* AutoLineSmoothVel: Implement l1-style guidance in turns
  • Loading branch information
@bresch
bresch authored Dec 14, 2020
1 parent 9b065b4 commit fdd1d6d
Show file tree
Hide file tree
Showing 3 changed files with 160 additions and 57 deletions.
4 changes: 2 additions & 2 deletions src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -451,11 +451,11 @@ State FlightTaskAuto::_getCurrentState()
// Target is behind.
return_state = State::target_behind;

} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) {
} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _target_acceptance_radius) {
// Current position is more than cruise speed in front of previous setpoint.
return_state = State::previous_infront;

} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _mc_cruise_speed) {
} else if (Vector2f(Vector2f(_position) - _closest_pt).length() > _target_acceptance_radius) {
// Vehicle is more than cruise speed off track.
return_state = State::offtrack;

Expand Down
205 changes: 150 additions & 55 deletions src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)

void FlightTaskAutoLineSmoothVel::_generateSetpoints()
{
_updateTurningCheck();
_prepareSetpoints();
_generateTrajectory();

Expand All @@ -125,6 +126,26 @@ void FlightTaskAutoLineSmoothVel::_generateSetpoints()
}
}

void FlightTaskAutoLineSmoothVel::_updateTurningCheck()
{
const Vector2f vel_traj(_trajectory[0].getCurrentVelocity(),
_trajectory[1].getCurrentVelocity());
const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition());
const Vector2f target_xy(_target);
const Vector2f u_vel_traj = vel_traj.unit_or_zero();
const Vector2f pos_to_target = Vector2f(target_xy - pos_traj);
const float cos_align = u_vel_traj * pos_to_target.unit_or_zero();

// The vehicle is turning if the angle between the velocity vector
// and the direction to the target is greater than 10 degrees, the
// velocity is large enough and the drone isn't in the acceptance
// radius of the last WP.
_is_turning = vel_traj.longerThan(0.2f)
&& cos_align < 0.98f
&& pos_to_target.longerThan(_target_acceptance_radius);
}

void FlightTaskAutoLineSmoothVel::_generateHeading()
{
// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
Expand All @@ -140,7 +161,7 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);

if ((vel_sp_xy.length() > .1f) &&
(traj_to_target.length() > _target_acceptance_radius)) {
(traj_to_target.length() > 2.f)) {
// Generate heading from velocity vector, only if it is long enough
// and if the drone is far enough from the target
_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
Expand Down Expand Up @@ -183,13 +204,10 @@ float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const

// constrain velocity to go to the position setpoint first if the position setpoint has been modified by an external source
// (eg. Obstacle Avoidance)
bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
bool z_valid = PX4_ISFINITE(_position_setpoint(2));
bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;

Vector3f waypoints[3] = {pos_traj, _target, _next_wp};

if (xy_modified || z_modified) {
if (isTargetModified()) {
waypoints[2] = waypoints[1] = _position_setpoint;
}

Expand Down Expand Up @@ -224,6 +242,64 @@ float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
return max_speed;
}

Vector3f FlightTaskAutoLineSmoothVel::getCrossingPoint() const
{
Vector3f pos_crossing_point{};

if (isTargetModified()) {
// Strictly follow the modified setpoint
pos_crossing_point = _position_setpoint;

} else {
if (_is_turning) {
// Get the crossing point using L1-style guidance
pos_crossing_point.xy() = getL1Point();
pos_crossing_point(2) = _target(2);

} else {
pos_crossing_point = _target;
}
}

return pos_crossing_point;
}

bool FlightTaskAutoLineSmoothVel::isTargetModified() const
{
const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
const bool z_modified = z_valid && fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;

return xy_modified || z_modified;
}

Vector2f FlightTaskAutoLineSmoothVel::getL1Point() const
{
const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition());
const Vector2f target_xy(_target);
const Vector2f u_prev_to_target = Vector2f(target_xy - Vector2f(_prev_wp)).unit_or_zero();
const Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
const Vector2f closest_pt = Vector2f(_prev_wp) + prev_to_closest;

// Compute along-track error using L1 distance and cross-track error
const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();

const float l1 = math::max(_target_acceptance_radius, 5.f);
float alongtrack_error = 0.f;

// Protect against sqrt of a negative number
if (l1 > crosstrack_error) {
alongtrack_error = sqrtf(l1 * l1 - crosstrack_error * crosstrack_error);
}

// Position of the point on the line where L1 intersect the line between the two waypoints
const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;

return l1_point;
}

void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
{
// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
Expand All @@ -236,74 +312,93 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
// Wait for the yaw setpoint to be aligned
_velocity_setpoint.setAll(0.f);
return;
}

} else {
const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));

if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
// Use 3D position setpoint to generate a 3D velocity setpoint
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
Vector3f u_pos_traj_to_dest = (_position_setpoint - pos_traj).unit_or_zero();

const float xy_speed = _getMaxXYSpeed();
const float z_speed = _getMaxZSpeed();

Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);

for (int i = 0; i < 3; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained(i);

} else {
_velocity_setpoint(i) = vel_sp_constrained(i);
}
}
}
const bool xy_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1));
const bool z_pos_setpoint_valid = PX4_ISFINITE(_position_setpoint(2));

if (xy_pos_setpoint_valid && z_pos_setpoint_valid) {
// Use 3D position setpoint to generate a 3D velocity setpoint
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());

else if (xy_pos_setpoint_valid) {
// Use 2D position setpoint to generate a 2D velocity setpoint
const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero());

// Get various path specific vectors
Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
Vector2f pos_traj_to_dest_xy = Vector2f(_position_setpoint.xy()) - pos_traj;
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
float xy_speed = _getMaxXYSpeed();
const float z_speed = _getMaxZSpeed();

Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();
if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);

for (int i = 0; i < 2; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained_xy(i);
} else {
_max_speed_prev = xy_speed;
}

Vector3f vel_sp_constrained = u_pos_traj_to_dest * sqrtf(xy_speed * xy_speed + z_speed * z_speed);
math::trajectory::clampToXYNorm(vel_sp_constrained, xy_speed, 0.5f);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed, 0.5f);

for (int i = 0; i < 3; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained(i);

} else {
_velocity_setpoint(i) = vel_sp_constrained_xy(i);
}
} else {
_velocity_setpoint(i) = vel_sp_constrained(i);
}
}
}

else if (z_pos_setpoint_valid) {
// Use Z position setpoint to generate a Z velocity setpoint
else if (xy_pos_setpoint_valid) {
// Use 2D position setpoint to generate a 2D velocity setpoint

const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
const float vel_sp_z = z_dir * _getMaxZSpeed();
// Get various path specific vectors
Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
Vector2f pos_traj_to_dest_xy = Vector2f(getCrossingPoint()) - pos_traj;
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());

float xy_speed = _getMaxXYSpeed();

if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);

} else {
_max_speed_prev = xy_speed;
}

Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * xy_speed;

for (int i = 0; i < 2; i++) {
// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) += vel_sp_z;
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained_xy(i);

} else {
_velocity_setpoint(2) = vel_sp_z;
_velocity_setpoint(i) = vel_sp_constrained_xy(i);
}
}
}

_want_takeoff = _velocity_setpoint(2) < -0.3f;
else if (z_pos_setpoint_valid) {
// Use Z position setpoint to generate a Z velocity setpoint

const float z_dir = sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
const float vel_sp_z = z_dir * _getMaxZSpeed();

// If available, use the existing velocity as a feedforward, otherwise replace it
if (PX4_ISFINITE(_velocity_setpoint(2))) {
_velocity_setpoint(2) += vel_sp_z;

} else {
_velocity_setpoint(2) = vel_sp_z;
}
}

_want_takeoff = _velocity_setpoint(2) < -0.3f;
}

void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
Expand Down
8 changes: 8 additions & 0 deletions src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,7 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper

void _generateSetpoints() override; /**< Generate setpoints along line. */
void _generateHeading();
void _updateTurningCheck();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */

static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
Expand All @@ -72,6 +73,13 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
float _getMaxXYSpeed() const;
float _getMaxZSpeed() const;

matrix::Vector3f getCrossingPoint() const;
bool isTargetModified() const;
matrix::Vector2f getL1Point() const;

float _max_speed_prev{};
bool _is_turning{false};

void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _updateTrajConstraints();
void _generateTrajectory();
Expand Down

0 comments on commit fdd1d6d

Please sign in to comment.