From fdd1d6d2446daa7c9b905fbd7bbd9cebd0f7555a Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Mon, 14 Dec 2020 13:04:01 +0100 Subject: [PATCH] Multicopter rounded turns (#16376) * AutoLineSmoothVel: Implement l1-style guidance in turns --- .../tasks/Auto/FlightTaskAuto.cpp | 4 +- .../FlightTaskAutoLineSmoothVel.cpp | 205 +++++++++++++----- .../FlightTaskAutoLineSmoothVel.hpp | 8 + 3 files changed, 160 insertions(+), 57 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index bef3be7fc9..40e44fa2af 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -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; diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index acfd907e6c..342ecb7989 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -116,6 +116,7 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi) void FlightTaskAutoLineSmoothVel::_generateSetpoints() { + _updateTurningCheck(); _prepareSetpoints(); _generateTrajectory(); @@ -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 @@ -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); @@ -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; } @@ -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. @@ -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() diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 1a4457ab60..c8ef2a2980 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -63,6 +63,7 @@ protected: 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 */ @@ -72,6 +73,13 @@ protected: 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();