fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 16:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use the new trajectory constraints to synchronize and limit velocities

Browse Source
This commit is contained in:
Julian Kent
2019-12-12 14:51:58 +01:00
committed by Mathieu Bresciani
parent 025c044530
commit 75d7a049c1
4 changed files with 88 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+83 -69
View File
@@ -36,8 +36,8 @@
*/
#include "FlightTaskAutoLineSmoothVel.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
#include "TrajectoryConstraints.hpp"
using namespace matrix;
@@ -178,53 +178,55 @@ float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
return math::sign(val) * math::min(fabsf(val), fabsf(max));
}
float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const
float FlightTaskAutoLineSmoothVel::_getMaxXYSpeed() const
{
// Compute the maximum allowed speed at the waypoint assuming that we want to
// connect the two lines (prev-current and current-next)
// with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius
// The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius.
// This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that
// the real acceptance radius is smaller.
// It can be that the next waypoint is the last one or that the drone will have to stop for some other reason
// so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint.
float speed_at_target = 0.0f;
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
const float distance_current_next = (_target - _next_wp).xy().norm();
const bool waypoint_overlap = (_target - _prev_wp).xy().norm() < _target_acceptance_radius;
const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
math::trajectory::VehicleDynamicLimits config;
config.z_accept_rad = _param_nav_mc_alt_rad.get();
config.xy_accept_rad = _target_acceptance_radius;
config.max_acc_xy = _trajectory[0].getMaxAccel();
config.max_jerk = _trajectory[0].getMaxJerk();
config.max_speed_xy = _mc_cruise_speed;
config.max_acc_xy_radius_scale = _param_mpc_xy_traj_p.get();
Vector3f waypoints[3];
waypoints[0] = pos_traj;
if (distance_current_next > 0.001f &&
!waypoint_overlap &&
yaw_align_check_pass) {
Vector3f pos_traj;
pos_traj(0) = _trajectory[0].getCurrentPosition();
pos_traj(1) = _trajectory[1].getCurrentPosition();
pos_traj(2) = _trajectory[2].getCurrentPosition();
// Max speed between current and next
const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next, next_target_speed);
const float alpha = acosf(Vector2f((_target - pos_traj).xy()).unit_or_zero().dot(
Vector2f((_target - _next_wp).xy()).unit_or_zero()));
// We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account
// that there is a jerk limit (a direct transition from line to circle is not possible)
// MPC_XY_TRAJ_P should be between 0 and 1.
float accel_tmp = _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get();
float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(alpha,
accel_tmp,
_target_acceptance_radius);
speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed);
// constrain velocity to go to the position setpoint first if our tracking is really bad
if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) {
waypoints[1] = _position_setpoint;
waypoints[2] = _target;
} else {
waypoints[1] = _target;
waypoints[2] = _next_wp;
}
return speed_at_target;
float max_speed = math::trajectory::computeXYSpeedFromWaypoints<3>(waypoints, config);
return max_speed;
}
float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const
float FlightTaskAutoLineSmoothVel::_getMaxZSpeed() const
{
float max_speed = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_auto.get(),
_param_mpc_acc_hor.get(),
braking_distance,
final_speed);
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
float z_setpoint = _target(2);
if ((pos_traj - _position_setpoint).longerThan(_target_acceptance_radius)) {
z_setpoint = _position_setpoint(2);
}
const float distance_start_target = fabs(z_setpoint - pos_traj(2));
const float arrival_z_speed = 0.f;
float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
_trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
distance_start_target, arrival_z_speed));
return max_speed;
}
@@ -242,33 +244,43 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
_velocity_setpoint.setAll(0.f);
} else {
if (PX4_ISFINITE(_position_setpoint(0)) &&
PX4_ISFINITE(_position_setpoint(1))) {
// Use position setpoints to generate velocity setpoints
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);
math::trajectory::clampToZNorm(vel_sp_constrained, z_speed);
for (int i = 0; i < 3; i++) {
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(i))) {
_velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained(i), _velocity_setpoint(i));
} else {
_velocity_setpoint(i) = vel_sp_constrained(i);
}
}
}
else if (xy_pos_setpoint_valid) {
// Use 2D position setpoint to generate a 2D velocity setpoint
// Get various path specific vectors
Vector3f pos_traj;
pos_traj(0) = _trajectory[0].getCurrentPosition();
pos_traj(1) = _trajectory[1].getCurrentPosition();
pos_traj(2) = _trajectory[2].getCurrentPosition();
Vector2f pos_traj_to_dest_xy = (_position_setpoint - pos_traj).xy();
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());
const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
// If the drone has to change altitude, stop at the waypoint, otherwise fly through
const float arrival_speed = has_reached_altitude ? _getSpeedAtTarget(0.f) : 0.f;
const Vector2f max_arrival_vel = u_pos_traj_to_dest_xy * arrival_speed;
Vector2f vel_abs_max_xy(_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)), max_arrival_vel(0)),
_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)), max_arrival_vel(1)));
const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed;
// Constrain the norm of each component using min and max values
Vector2f vel_sp_constrained_xy(_constrainAbs(vel_sp_xy(0), vel_abs_max_xy(0)),
_constrainAbs(vel_sp_xy(1), vel_abs_max_xy(1)));
Vector2f vel_sp_constrained_xy = u_pos_traj_to_dest_xy * _getMaxXYSpeed();
for (int i = 0; i < 2; i++) {
// If available, constrain the velocity using _velocity_setpoint(.)
@@ -281,9 +293,11 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
}
}
if (PX4_ISFINITE(_position_setpoint(2))) {
const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
_param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
else if (z_pos_setpoint_valid) {
// Use Z position setpoint to generate a Z velocity setpoint
const float z_dir = math::sign(_position_setpoint(2) - _trajectory[2].getCurrentPosition());
const float vel_sp_z = z_dir * _getMaxZSpeed();
// If available, constrain the velocity using _velocity_setpoint(.)
if (PX4_ISFINITE(_velocity_setpoint(2))) {
@@ -292,9 +306,9 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
} else {
_velocity_setpoint(2) = vel_sp_z;
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
_want_takeoff = _velocity_setpoint(2) < -0.3f;
}
}
@@ -361,7 +375,7 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
_trajectory[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
VelocitySmoothing::timeSynchronization(_trajectory, 3);
_jerk_setpoint = jerk_sp_smooth;
_acceleration_setpoint = accel_sp_smooth;
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+3 -5
View File
@@ -71,9 +71,8 @@ protected:
static float _constrainAbs(float val, float max); /** Constrain the value -max <= val <= max */
/** Give 0 if next is the last target **/
float _getSpeedAtTarget(float next_target_speed) const;
float _getMaxSpeedFromDistance(float braking_distance, float final_speed) const;
float _getMaxXYSpeed() const;
float _getMaxZSpeed() const;
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _updateTrajConstraints();
@@ -91,7 +90,6 @@ protected:
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
(ParamFloat<px4::params::MPC_Z_TRAJ_P>) _param_mpc_z_traj_p
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p
);
};
src/lib/mathlib/math/TrajMath.hpp
+2 -1
View File
@@ -66,7 +66,8 @@ inline float computeMaxSpeedFromDistance(const float jerk, const float accel, co
float c = - 2.0f * accel * braking_distance - sqr(final_speed);
float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c));
return max_speed;
// don't slow down more than the end speed, even if the conservative accel ramp time requests it
return max(max_speed, final_speed);
}
/* Compute the maximum tangential speed in a circle defined by two line segments of length "d"
src/modules/mc_pos_control/mc_pos_control_params.c
-10
View File
@@ -259,16 +259,6 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
*/
PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
/**
* Proportional gain for vertical trajectory position error
*
* @min 0.1
* @max 1.0
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_TRAJ_P, 0.3f);
/**
* Cruise speed when angle prev-current/current-next setpoint
* is 90 degrees. It should be lower than MPC_XY_CRUISE.