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

VelocitySmoothing - Re-enable time stretch, integrate dt to get local time.

Browse Source 
Also split a few functions into smaller ones for readability, fix
formatting, use geters to get the current state of the trajectory
instead of return arguments.
This commit is contained in:
bresch 2019-09-08 15:42:18 +02:00 committed by Matthias Grob
parent c7696488fe
commit 8cc2a7018e
7 changed files with 160 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -349,13 +349,13 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
Vector2f position_xy(_position);
Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity());
Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
//float position_error = drone_to_trajectory_xy.length();
float position_error = drone_to_trajectory_xy.length();
//float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
// Don't stretch time if the drone is ahead of the position setpoint
if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
//time_stretch = 1.f;
time_stretch = 1.f;
}
Vector3f jerk_sp_smooth;
@ -364,16 +364,17 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory()
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
// TODO: fix time stretch
//_trajectory[i].updateTraj(_time_stamp_current, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i));
_trajectory[i].updateTraj(_time, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i));
_trajectory[i].updateTraj(_deltatime, time_stretch);
jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration();
vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity();
pos_sp_smooth(i) = _trajectory[i].getCurrentPosition();
}
_updateTrajConstraints();
for (int i = 0; i < 3; ++i) {
_trajectory[i].updateDurations(_time, _velocity_setpoint(i));
_trajectory[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only

11
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

@ -147,7 +147,7 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
jerk = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get();
_smoothing.setMaxJerk(jerk);
_smoothing.updateDurations(_deltatime, _velocity_setpoint(2));
_smoothing.updateDurations(_velocity_setpoint(2));
if (!_position_lock_z_active) {
_smoothing.setCurrentPosition(_position(2));
@ -156,9 +156,14 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
float pos_sp_smooth;
// TODO: move before updateDurations
_smoothing.updateTraj(_time_stamp_current, _acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth);
_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward
_smoothing.updateTraj(_deltatime);
_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_vel_sp_smooth = _smoothing.getCurrentVelocity();
pos_sp_smooth = _smoothing.getCurrentPosition();
_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward
if (fabsf(_vel_sp_smooth) < 0.1f &&
fabsf(_acceleration_setpoint(2)) < .2f &&

8
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@ -144,8 +144,12 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
_smoothing[i].updateTraj(_time_stamp_current, _acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
_smoothing[i].updateTraj(_deltatime);
_jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
_acceleration_setpoint(i) = _smoothing[i].getCurrentAcceleration();
_vel_sp_smooth(i) = _smoothing[i].getCurrentVelocity();
pos_sp_smooth(i) = _smoothing[i].getCurrentPosition();
}
/* Get yaw setpont, un-smoothed position setpoints.*/
@ -203,7 +207,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_time_stamp_current, _velocity_setpoint(i));
_smoothing[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only

164
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
View File

@ -78,7 +78,7 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
float sqrt_delta = sqrtf(delta);
float T1_plus = (-a0 + 0.5f * sqrt_delta) / j_max;
float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max;
float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max;
float T3_plus = a0 / j_max + T1_plus;
float T3_minus = a0 / j_max + T1_minus;
@ -101,7 +101,7 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max,
{
float a = -j_max;
float b = j_max * T123 - a0;
float delta = T123 * T123 * j_max * j_max + 2.f * T123 *a0 * j_max - a0 * a0 - 4.f * j_max * v3;
float delta = T123 * T123 * j_max * j_max + 2.f * T123 * a0 * j_max - a0 * a0 - 4.f * j_max * v3;
if (delta < 0.f) {
// Solution is not real
@ -134,14 +134,14 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max,
}
float VelocitySmoothing::computeT2(float T1, float T3, float a0 , float v3, float j_max)
float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max)
{
float T2 = 0.f;
float den = a0 + j_max * T1;
if (math::abs_t(den) > FLT_EPSILON) {
T2 = (-0.5f *T1 * T1 * j_max - T1 * T3*j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den;
T2 = (-0.5f * T1 * T1 * j_max - T1 * T3 * j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den;
}
return math::max(T2, 0.f);
@ -156,70 +156,69 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3)
float VelocitySmoothing::computeT3(float T1, float a0, float j_max)
{
float T3 = a0 / j_max + T1;
return math::max(T3, 0.f);
}
void VelocitySmoothing::updateDurations(float t_now, float vel_setpoint)
void VelocitySmoothing::updateDurations(float vel_setpoint)
{
_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
_t0 = t_now;
_local_time = 0.f;
_state_init = _state;
// Compute the velocity at which the trajectory will be
// when the acceleration will be zero
float vel_zero_acc = _state.v;
if (fabsf(_state.a) > FLT_EPSILON) {
float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration
float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration
vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc;
}
/* Depending of the direction, start accelerating positively or negatively */
_direction = math::sign(_vel_sp - vel_zero_acc);
if (_direction == 0) {
// If by braking immediately the velocity is exactly
// the require one with zero acceleration, then brake
_direction = -math::sign(_vel_sp - _state.v);
}
_direction = computeDirection();
if (_direction != 0) {
updateDurations();
updateDurationsMinimizeTotalTime();
} else {
_T1 = _T2 = _T3 = 0.f;
}
}
void VelocitySmoothing::updateDurations(float T123)
int VelocitySmoothing::computeDirection()
{
float T1, T2, T3;
// Compute the velocity at which the trajectory will be
// when the acceleration will be zero
float vel_zero_acc = computeVelAtZeroAcc();
/* Depending of the direction, start accelerating positively or negatively */
int direction = math::sign(_vel_sp - vel_zero_acc);
if (direction == 0) {
// If by braking immediately the velocity is exactly
// the require one with zero acceleration, then brake
direction = -math::sign(_state.a);
}
return direction;
}
float VelocitySmoothing::computeVelAtZeroAcc()
{
float vel_zero_acc = _state.v;
if (fabsf(_state.a) > FLT_EPSILON) {
float j_zero_acc = -math::sign(_state.a) * _max_jerk; // Required jerk to reduce the acceleration
float t_zero_acc = -_state.a / j_zero_acc; // Required time to cancel the current acceleration
vel_zero_acc = _state.v + _state.a * t_zero_acc + 0.5f * j_zero_acc * t_zero_acc * t_zero_acc;
}
return vel_zero_acc;
}
void VelocitySmoothing::updateDurationsMinimizeTotalTime()
{
float jerk_max_T1 = _direction * _max_jerk;
float delta_v = _vel_sp - _state.v;
// compute increasing acceleration time
if (T123 < 0.f) {
T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
} else {
T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel);
}
_T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel);
// compute decreasing acceleration time
T3 = computeT3(T1, _state.a, jerk_max_T1);
_T3 = computeT3(_T1, _state.a, jerk_max_T1);
// compute constant acceleration time
if (T123 < 0.f) {
T2 = computeT2(T1, T3, _state.a, delta_v, jerk_max_T1);
} else {
T2 = computeT2(T123, T1, T3);
}
_T1 = T1;
_T2 = T2;
_T3 = T3;
_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
}
Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d)
@ -237,42 +236,54 @@ Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0
return traj;
}
void VelocitySmoothing::updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth)
void VelocitySmoothing::updateTraj(float dt, float time_stretch)
{
const float t = t_now - _t0;
updateTraj(dt * time_stretch);
}
void VelocitySmoothing::updateTraj(float dt)
{
_local_time += dt;
const float t = _local_time;
float t1 = 0.f;
float t2 = 0.f;
float t3 = 0.f;
float t4 = 0.f;
if (t <= _T1) {
float t1 = t;
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
t1 = t;
} else if (t <= _T1 + _T2) {
float t1 = _T1;
float t2 = t - _T1;
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
t1 = _T1;
t2 = t - _T1;
} else if (t <= _T1 + _T2 + _T3) {
float t1 = _T1;
float t2 = _T2;
float t3 = t - _T1 - _T2;
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
t1 = _T1;
t2 = _T2;
t3 = t - _T1 - _T2;
} else {
float t1 = _T1;
float t2 = _T2;
float t3 = _T3;
float t4 = t - _T1 - _T2 - _T3;
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
_state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f);
t1 = _T1;
t2 = _T2;
t3 = _T3;
t4 = t - _T1 - _T2 - _T3;
}
accel_setpoint_smooth = _state.a;
vel_setpoint_smooth = _state.v;
pos_setpoint_smooth = _state.x;
if (t > 0.f) {
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
}
if (t >= _T1) {
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
}
if (t >= _T1 + _T2) {
_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
}
if (t >= _T1 + _T2 + _T3) {
_state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f);
}
}
void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)
@ -292,8 +303,23 @@ void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)
if (desired_time > FLT_EPSILON) {
for (int i = 0; i < n_traj; i++) {
if (i != longest_traj_index) {
traj[i].updateDurations(desired_time);
traj[i].updateDurationsGivenTotalTime(desired_time);
}
}
}
}
void VelocitySmoothing::updateDurationsGivenTotalTime(float T123)
{
float jerk_max_T1 = _direction * _max_jerk;
float delta_v = _vel_sp - _state.v;
// compute increasing acceleration time
_T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel);
// compute decreasing acceleration time
_T3 = computeT3(_T1, _state.a, jerk_max_T1);
// compute constant acceleration time
_T2 = computeT2(T123, _T1, _T3);
}

37
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
View File

@ -74,21 +74,18 @@ public:
/**
* Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle
* and before evaluateTraj().
* and before updateTraj().
* @param vel_setpoint velocity setpoint input
* @param t current time
*/
void updateDurations(float t_now, float vel_setpoint);
void updateDurations(float vel_setpoint);
/**
* Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
* @param dt optional integration period. If not given, the integration period provided during updateDuration call is used.
* A dt different from the one given during the computation of T1-T3 can be used to fast-forward or slow-down the trajectory.
* @param acc_setpoint_smooth returned smoothed acceleration setpoint
* @param vel_setpoint_smooth returned smoothed velocity setpoint
* @param pos_setpoint_smooth returned smoothed position setpoint
*/
void updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
void updateTraj(float dt, float time_stretch);
void updateTraj(float dt);
/* Get / Set constraints (constraints can be updated at any time) */
float getMaxJerk() const { return _max_jerk; }
@ -129,10 +126,28 @@ private:
/**
* Compute T1, T2, T3 depending on the current state and velocity setpoint.
* @param T123 optional parameter. If set, the total trajectory time will be T123, if not,
* the algorithm optimizes for time.
* Minimize the total time of the trajectory
*/
void updateDurations(float T123 = -1.f);
void updateDurationsMinimizeTotalTime();
/**
* Compute T1, T2, T3 depending on the current state and velocity setpoint.
* @param T123 desired total time of the trajectory
*/
void updateDurationsGivenTotalTime(float T123);
/**
* Compute the direction of the jerk to be applied in order to drive the current state
* to the desired one
*/
int computeDirection();
/**
* Compute the velocity at which the trajectory will be if the maximum jerk is applied
* during the time required to cancel the current acceleration
*/
float computeVelAtZeroAcc();
/**
* Compute increasing acceleration time
*/
@ -187,5 +202,5 @@ private:
float _T2 = 0.f; ///< Constant acceleration [s]
float _T3 = 0.f; ///< Decreasing acceleration [s]
float _t0 = 0.f; ///< Starting time
float _local_time = 0.f; ///< Current local time
};

18
src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp
View File

@ -48,7 +48,7 @@ class VelocitySmoothingTest : public ::testing::Test
public:
void setConstraints(float j_max, float a_max, float v_max);
void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos);
void updateTrajectories(float t, Vector3f velocity_setpoints);
void updateTrajectories(float dt, Vector3f velocity_setpoints);
VelocitySmoothing _trajectories[3];
};
@ -71,16 +71,14 @@ void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vecto
}
}
void VelocitySmoothingTest::updateTrajectories(float t, Vector3f velocity_setpoints)
void VelocitySmoothingTest::updateTrajectories(float dt, Vector3f velocity_setpoints)
{
float dummy; // We don't care about the immediate result
for (int i = 0; i < 3; i++) {
_trajectories[i].updateTraj(t, dummy, dummy, dummy);
_trajectories[i].updateTraj(dt);
}
for (int i = 0; i < 3; i++) {
_trajectories[i].updateDurations(t, velocity_setpoints(i));
_trajectories[i].updateDurations(velocity_setpoints(i));
}
VelocitySmoothing::timeSynchronization(_trajectories, 2);
@ -132,15 +130,13 @@ TEST_F(VelocitySmoothingTest, testConstantSetpoint)
// Compute the number of steps required to reach desired value
// The updateTrajectories is fist called once to compute the total time
float t = 0.f;
const float dt = 0.01;
updateTrajectories(t, velocity_setpoints);
updateTrajectories(0.f, velocity_setpoints);
float t123 = _trajectories[0].getTotalTime();
int nb_steps = ceil(t123 / dt);
for (int i = 0; i < nb_steps; i++) {
t += dt;
updateTrajectories(t, velocity_setpoints);
updateTrajectories(dt, velocity_setpoints);
}
// THEN: All the trajectories should have reach their
@ -167,7 +163,7 @@ TEST_F(VelocitySmoothingTest, testZeroSetpoint)
// WHEN: We run a few times the algorithm
for (int i = 0; i < 60; i++) {
updateTrajectories(t, velocity_setpoints);
updateTrajectories(dt, velocity_setpoints);
t += dt;
}

19
src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp
View File

@ -60,35 +60,36 @@ int main(int argc, char *argv[])
trajectory[i].setCurrentVelocity(v0[i]);
}
float t = 0.f;
const float dt = 0.01f;
float velocity_setpoint[3] = {1.f, 0.f, -1.f};
for (int i = 0; i < 3; i++) {
trajectory[i].updateDurations(t, velocity_setpoint[i]);
trajectory[i].updateDurations(velocity_setpoint[i]);
}
float t123 = trajectory[2].getTotalTime();
float t123 = trajectory[0].getTotalTime();
int nb_steps = ceil(t123 / dt);
printf("Nb steps = %d\n", nb_steps);
for (int i = 0; i < nb_steps; i++) {
t += dt;
for (int i = 0; i < 3; i++) {
trajectory[i].updateTraj(t, a0[i], v0[i], x0[i]);
trajectory[i].updateTraj(dt);
}
for (int i = 0; i < 3; i++) {
trajectory[i].updateDurations(t, velocity_setpoint[i]);
trajectory[i].updateDurations(velocity_setpoint[i]);
}
VelocitySmoothing::timeSynchronization(trajectory, 2);
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 1; i++) {
printf("Traj[%d]\n", i);
printf("jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n", trajectory[i].getCurrentJerk(),
a0[i], v0[i], x0[i]);
printf("jerk = %.3f\taccel = %.3f\tvel = %.3f\tpos = %.3f\n",
trajectory[i].getCurrentJerk(),
trajectory[i].getCurrentAcceleration(),
trajectory[i].getCurrentVelocity(),
trajectory[i].getCurrentPosition());
printf("T1 = %.3f\tT2 = %.3f\tT3 = %.3f\n", trajectory[i].getT1(), trajectory[i].getT2(), trajectory[i].getT3());
printf("\n");
}