fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Multicopter rounded turns (#16376)

Browse Source 
* AutoLineSmoothVel: Implement l1-style guidance in turns
This commit is contained in:
Mathieu Bresciani 2020-12-14 13:04:01 +01:00 committed by GitHub
parent 9b065b4975
commit fdd1d6d244
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 163 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -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;

211
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -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));
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();
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());
const float xy_speed = _getMaxXYSpeed();
const float z_speed = _getMaxZSpeed();
const Vector3f u_pos_traj_to_dest((getCrossingPoint() - pos_traj).unit_or_zero());
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);
float xy_speed = _getMaxXYSpeed();
const float z_speed = _getMaxZSpeed();
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);
if (_is_turning) {
// Lock speed during turn
xy_speed = math::min(_max_speed_prev, xy_speed);
} else {
_velocity_setpoint(i) = vel_sp_constrained(i);
}
}
} else {
_max_speed_prev = xy_speed;
}
else if (xy_pos_setpoint_valid) {
// Use 2D position setpoint to generate a 2D velocity setpoint
// 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());
Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();
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 {
_velocity_setpoint(i) = vel_sp_constrained_xy(i);
}
}
}
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();
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(2))) {
_velocity_setpoint(2) += vel_sp_z;
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) += vel_sp_constrained(i);
} else {
_velocity_setpoint(2) = vel_sp_z;
_velocity_setpoint(i) = vel_sp_constrained(i);
}
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
else if (xy_pos_setpoint_valid) {
// Use 2D position setpoint to generate a 2D velocity setpoint
// 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(i))) {
_velocity_setpoint(i) += vel_sp_constrained_xy(i);
} else {
_velocity_setpoint(i) = vel_sp_constrained_xy(i);
}
}
}
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()

8
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View File

@ -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();