From 22780efcd052377df71e3cccb88a5cba1ff89acf Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 3 Oct 2018 14:22:31 +0200
Subject: [PATCH] Trajectory - Add time synchronization between trajectories.
Split update(...) function into updateDurations(...) and integrate(...) to be
able to insert time synchronization in between.
---
.../FlightTaskManualPositionSmoothVel.cpp | 11 +-
.../tasks/Utility/VelocitySmoothing.cpp | 111 ++++++++++++++----
.../tasks/Utility/VelocitySmoothing.hpp | 64 ++++++++--
3 files changed, 157 insertions(+), 29 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 06403a254a..1bfeda50e2 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -60,6 +60,8 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_smoothing[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
_smoothing[0].setMaxVel(_constraints.speed_xy);
_smoothing[1].setMaxVel(_constraints.speed_xy);
+ _smoothing[0].setDt(_deltatime);
+ _smoothing[1].setDt(_deltatime);
Vector2f vel_xy = Vector2f(&_velocity(0));
float jerk = _jerk_max.get();
@@ -76,10 +78,15 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
for (int i = 0; i < 2; ++i) {
_smoothing[i].setMaxJerk(jerk);
+ _smoothing[i].updateDurations(_velocity_setpoint(i));
+ }
+
+ VelocitySmoothing::timeSynchronization(_smoothing, 2);
+
+ for (int i = 0; i < 2; ++i) {
float smoothed_velocity_setpoint, smoothed_position_setpoint;
- _smoothing[i].update(_deltatime, _position(i), _velocity_setpoint(i),
- smoothed_velocity_setpoint, smoothed_position_setpoint);
+ _smoothing[i].integrate(_position(i), smoothed_velocity_setpoint, smoothed_position_setpoint);
_position_setpoint(i) = smoothed_position_setpoint;
_velocity_setpoint(i) = smoothed_velocity_setpoint;
}
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index 62b9154419..84d03b06ed 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -87,6 +87,34 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
return math::max(T1, 0.f);
}
+float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk)
+{
+ float a = -max_jerk;
+ float b = max_jerk * T123 - accel_prev;
+ float delta = T123 * T123 * max_jerk * max_jerk + 2.f * T123 * accel_prev * max_jerk - accel_prev * accel_prev
+ + 4.f * max_jerk * (vel_prev - vel_setpoint);
+
+ float sqrt_delta = sqrtf(delta);
+ float denominator_inv = 1.f / (2.f * a);
+ float T1_plus = math::max((-b + sqrt_delta) * denominator_inv, 0.f);
+ float T1_minus = math::max((-b - sqrt_delta) * denominator_inv, 0.f);
+
+ float T3_plus = computeT3(T1_plus, accel_prev, max_jerk);
+ float T3_minus = computeT3(T1_minus, accel_prev, max_jerk);
+
+ float T13_plus = T1_plus + T3_plus;
+ float T13_minus = T1_minus + T3_minus;
+
+ float T1 = 0.f;
+ if (T13_plus > T123) {
+ T1 = T1_minus;
+ } else if (T13_minus > T123){
+ T1 = T1_plus;
+ }
+
+ return T1;
+}
+
float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint,
float max_jerk)
@@ -97,16 +125,22 @@ float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float v
return math::max(T2, 0.f);
}
+float VelocitySmoothing::computeT2(float T123, float T1, float T3)
+{
+ float T2 = T123 - T1 - T3;
+ return math::max(T2, 0.f);
+}
+
float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
{
float T3 = accel_prev / max_jerk + T1;
return math::max(T3, 0.f);
}
-void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev, float dt,
+void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
float &accel_out, float &vel_out, float &pos_out)
{
- accel_out = jerk * dt + accel_prev;
+ accel_out = jerk * _dt + accel_prev;
if (accel_out > _max_accel) {
accel_out = _max_accel;
@@ -115,7 +149,7 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
accel_out = -_max_accel;
}
- vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
+ vel_out = _dt * 0.5f * (accel_out + accel_prev) + vel_prev;
if (vel_out > _max_vel) {
vel_out = _max_vel;
@@ -124,50 +158,68 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
vel_out = -_max_vel;
}
- pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos;
+ pos_out = _dt / 3.f * (vel_out + accel_prev * _dt * 0.5f + 2.f * vel_prev) + _pos;
}
-void VelocitySmoothing::update(float dt, float pos, float vel_setpoint, float &vel_setpoint_smooth,
- float &pos_setpoint_smooth)
+void VelocitySmoothing::updateDurations(float vel_setpoint, float T123)
{
+ float T1, T2, T3;
+
/* Depending of the direction, start accelerating positively or negatively */
- const float max_jerk = (vel_setpoint - _vel > 0.f) ? _max_jerk : -_max_jerk;
+ _max_jerk_T1 = (vel_setpoint - _vel > 0.f) ? _max_jerk : -_max_jerk;
// compute increasing acceleration time
- float T1 = computeT1(_accel, _vel, vel_setpoint, max_jerk);
+ if (PX4_ISFINITE(T123)) {
+ T1 = computeT1(T123, _accel, _vel, vel_setpoint, _max_jerk_T1);
+ } else {
+ T1 = computeT1(_accel, _vel, vel_setpoint, _max_jerk_T1);
+ }
/* Force T1/2/3 to zero if smaller than an epoch to avoid chattering */
- if (T1 < dt) {
+ if (T1 < _dt) {
T1 = 0.f;
}
// compute decreasing acceleration time
- float T3 = computeT3(T1, _accel, max_jerk);
+ T3 = computeT3(T1, _accel, _max_jerk_T1);
- if (T3 < dt) {
+ if (T3 < _dt) {
T3 = 0.f;
}
// compute constant acceleration time
- float T2 = computeT2(T1, T3, _accel, _vel, vel_setpoint, max_jerk);
+ if (PX4_ISFINITE(T123)) {
+ T2 = computeT2(T123, T1, T3);
+ } else {
+ T2 = computeT2(T1, T3, _accel, _vel, vel_setpoint, _max_jerk_T1);
+ }
- if (T2 < dt) {
+ if (T2 < _dt) {
T2 = 0.f;
}
+ _T1 = T1;
+ _T2 = T2;
+ _T3 = T3;
+ _vel_sp = vel_setpoint;
+}
+
+void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
+ float &pos_setpoint_smooth)
+{
/* Integrate the trajectory */
float accel_new, vel_new, pos_new;
- integrateT(_jerk, _accel, _vel, _pos, dt, accel_new, vel_new, pos_new);
+ integrateT(_jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
/* Apply correct jerk (min, max or zero) */
- if (T1 > 0.f) {
- _jerk = max_jerk;
+ if (_T1 > 0.f) {
+ _jerk = _max_jerk_T1;
- } else if (T2 > 0.f) {
+ } else if (_T2 > 0.f) {
_jerk = 0.f;
- } else if (T3 > 0.f) {
- _jerk = -max_jerk;
+ } else if (_T3 > 0.f) {
+ _jerk = -_max_jerk_T1;
} else {
_jerk = 0.f;
@@ -188,4 +240,23 @@ void VelocitySmoothing::update(float dt, float pos, float vel_setpoint, float &v
pos_setpoint_smooth = _pos;
}
-
+void VelocitySmoothing::timeSynchronization(VelocitySmoothing traj[3], int n_traj)
+{
+ float desired_time = 0.f;
+ int longest_traj_index = 0;
+
+ for (int i = 0; i < n_traj; i++) {
+ const float T123 = traj[i].getTotalTime();
+ if (T123 > desired_time) {
+ desired_time = T123;
+ longest_traj_index = i;
+ }
+ }
+ if (desired_time > FLT_EPSILON) {
+ for (int i = 0; i < n_traj; i++) {
+ if (i != longest_traj_index) {
+ traj[i].updateDurations(traj[i].getVelSp(), desired_time);
+ }
+ }
+ }
+}
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index e5fd7c6d4a..6d78b6d5e9 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -39,6 +39,19 @@
* @class VelocitySmoothing
*
* TODO: document the algorithm
+ * |T1| T2 |T3|
+ * ___
+ * __| |____ __ Jerk
+ * |_|
+ * ___
+ * / \ Acceleration
+ * ___/ \___
+ * ___
+ * ;"
+ * /
+ * / Velocity
+ * ;
+ * ----"
*/
class VelocitySmoothing
{
@@ -55,18 +68,22 @@ public:
void reset(float accel, float vel, float pos);
/**
- * Update the setpoint and get the smoothed setpoints. This should be called on every cycle.
- * @param dt delta time between last update() call and now [s]
- * @param pos Current vehicle's position
+ * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle.
* @param vel_setpoint velocity setpoint input
+ * @param T123 optional parameter. If set, the total trajectory time will be T123, if not,
+ * the algorithm optimizes for time.
+ */
+ void updateDurations(float vel_setpoint, float T123 = NAN);
+
+ /**
+ * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
+ * @param pos Current vehicle's position (used for position error clamping)
* @param vel_setpoint_smooth returned smoothed velocity setpoint
* @param pos_setpoint_smooth returned smoothed position setpoint
*/
- void update(float dt, float pos, float vel_setpoint, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
-
+ void integrate(float pos, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
/* Get / Set constraints (constraints can be updated at any time) */
-
float getMaxJerk() const { return _max_jerk; }
void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; }
@@ -76,15 +93,38 @@ public:
float getMaxVel() const { return _max_vel; }
void setMaxVel(float max_vel) { _max_vel = max_vel; }
+ /* Other getters and setters */
+ float getTotalTime() const {return _T1 + _T2 + _T3; }
+ float getVelSp() const {return _vel_sp; }
+
+ void setDt(float dt) {_dt = dt; } // delta time between last update() call and now [s]
+
+ /**
+ * Synchronize several trajectories to have the same total time. This is required to generate
+ * straight lines.
+ * The resulting total time is the one of the longest trajectory.
+ * @param traj[3] a table of VelocitySmoothing objects
+ * n_traj the number of trajectories to be synchronized
+ */
+ static void timeSynchronization(VelocitySmoothing traj[3], int n_traj);
+
private:
/**
* Compute increasing acceleration time
*/
inline float computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
+ /**
+ * Compute increasing acceleration time using total time constraint
+ */
+ inline float computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
/**
* Compute constant acceleration time
*/
inline float computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk);
+ /**
+ * Compute constant acceleration time using total time constraint
+ */
+ inline float computeT2(float T123, float T1, float T3);
/**
* Compute decreasing acceleration time
*/
@@ -93,7 +133,7 @@ private:
/**
* Integrate the jerk, acceleration and velocity to get the new setpoints and states.
*/
- inline void integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev, float dt,
+ inline void integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
float &accel_out, float &vel_out, float &pos_out);
/* Constraints */
@@ -107,5 +147,15 @@ private:
float _vel;
float _pos;
+ float _max_jerk_T1 = 0.f; ///< jerk during phase T1 (with correct sign)
+
+ /* Duration of each phase */
+ float _T1 = 0.f; // Increasing acceleration
+ float _T2 = 0.f; // Constant acceleration
+ float _T3 = 0.f; // Decreasing acceleration
+
+ float _vel_sp;
+ float _dt = 0.f;
+
static constexpr float max_pos_err = 1.f; ///< maximum position error (if above, the position setpoint is locked)
};
--
GitLab