mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 09:22:18 +00:00
303 lines
8.1 KiB
C++
303 lines
8.1 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "VelocitySmoothing.hpp"
|
|
|
|
#include <cstdio>
|
|
#include <float.h>
|
|
#include <mathlib/mathlib.h>
|
|
|
|
VelocitySmoothing::VelocitySmoothing(float initial_accel, float initial_vel, float initial_pos)
|
|
{
|
|
reset(initial_accel, initial_vel, initial_pos);
|
|
}
|
|
|
|
void VelocitySmoothing::reset(float accel, float vel, float pos)
|
|
{
|
|
_state.j = 0.f;
|
|
_state.a = accel;
|
|
_state.v = vel;
|
|
_state.x = pos;
|
|
|
|
_state_init = _state;
|
|
}
|
|
|
|
float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max)
|
|
{
|
|
/* Check maximum acceleration, saturate and recompute T1 if needed */
|
|
float accel_T1 = a0 + j_max * T1;
|
|
float T1_new = T1;
|
|
|
|
if (accel_T1 > a_max) {
|
|
T1_new = (a_max - a0) / j_max;
|
|
|
|
} else if (accel_T1 < -a_max) {
|
|
T1_new = (-a_max - a0) / j_max;
|
|
}
|
|
|
|
return T1_new;
|
|
}
|
|
|
|
float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
|
|
{
|
|
float delta = 2.f * a0 * a0 + 4.f * j_max * v3;
|
|
|
|
if (delta < 0.f) {
|
|
// Solution is not real
|
|
return 0.f;
|
|
}
|
|
|
|
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 T3_plus = a0 / j_max + T1_plus;
|
|
float T3_minus = a0 / j_max + T1_minus;
|
|
|
|
float T1 = 0.f;
|
|
|
|
if (T1_plus >= 0.f && T3_plus >= 0.f) {
|
|
T1 = T1_plus;
|
|
|
|
} else if (T1_minus >= 0.f && T3_minus >= 0.f) {
|
|
T1 = T1_minus;
|
|
}
|
|
|
|
T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
|
|
|
|
return math::max(T1, 0.f);
|
|
}
|
|
|
|
float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_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;
|
|
|
|
if (delta < 0.f) {
|
|
// Solution is not real
|
|
return 0.f;
|
|
}
|
|
|
|
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 = a0 / j_max + T1_plus;
|
|
float T3_minus = a0 / j_max + T1_minus;
|
|
|
|
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;
|
|
}
|
|
|
|
T1 = saturateT1ForAccel(a0, j_max, T1, a_max);
|
|
|
|
return T1;
|
|
}
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
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 a0, float j_max)
|
|
{
|
|
float T3 = a0 / j_max + T1;
|
|
return math::max(T3, 0.f);
|
|
}
|
|
|
|
void VelocitySmoothing::updateDurations(float vel_setpoint)
|
|
{
|
|
_vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel);
|
|
_local_time = 0.f;
|
|
_state_init = _state;
|
|
|
|
_direction = computeDirection();
|
|
|
|
if (_direction != 0) {
|
|
updateDurationsMinimizeTotalTime();
|
|
|
|
} else {
|
|
_T1 = _T2 = _T3 = 0.f;
|
|
}
|
|
}
|
|
|
|
int VelocitySmoothing::computeDirection()
|
|
{
|
|
// 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
|
|
_T1 = computeT1(_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(_T1, _T3, _state.a, delta_v, jerk_max_T1);
|
|
}
|
|
|
|
Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d)
|
|
{
|
|
Trajectory traj;
|
|
float jt = d * j;
|
|
float t2 = t * t;
|
|
float t3 = t2 * t;
|
|
|
|
traj.j = jt;
|
|
traj.a = a0 + jt * t;
|
|
traj.v = v0 + a0 * t + 0.5f * jt * t2;
|
|
traj.x = x0 + v0 * t + 0.5f * a0 * t2 + 1.f / 6.f * jt * t3;
|
|
|
|
return traj;
|
|
}
|
|
|
|
void VelocitySmoothing::updateTraj(float dt, float time_stretch)
|
|
{
|
|
_local_time += dt * time_stretch;
|
|
float t_remain = _local_time;
|
|
|
|
float t1 = math::min(t_remain, _T1);
|
|
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
|
|
t_remain -= t1;
|
|
|
|
if (t_remain > 0.f) {
|
|
float t2 = math::min(t_remain, _T2);
|
|
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
|
|
t_remain -= t2;
|
|
}
|
|
|
|
if (t_remain > 0.f) {
|
|
float t3 = math::min(t_remain, _T3);
|
|
_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
|
|
t_remain -= t3;
|
|
}
|
|
|
|
if (t_remain > 0.f) {
|
|
_state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t_remain, 0.f);
|
|
}
|
|
}
|
|
|
|
void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, 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].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);
|
|
}
|