accel_curve.h

[詳解]

 
#pragma once
 
#include <array>
#include <cmath>     //< for std::sqrt, std::cbrt
#include <iostream>  //< for std::cout
#include <ostream>
 
/* log level definition */
#define CTRL_LOG_LEVEL_NONE 0
#define CTRL_LOG_LEVEL_ERROR 1
#define CTRL_LOG_LEVEL_WARNING 2
#define CTRL_LOG_LEVEL_INFO 3
#define CTRL_LOG_LEVEL_DEBUG 4
/* set log level */
#ifndef CTRL_LOG_LEVEL
#define CTRL_LOG_LEVEL CTRL_LOG_LEVEL_WARNING
#endif
/* Log Error */
#if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_ERROR
#define ctrl_loge (std::cout << "[E][" __FILE__ ":" << __LINE__ << "]\t")
#else
#define ctrl_loge std::ostream(0)
#endif
/* Log Warning */
#if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_WARNING
#define ctrl_logw (std::cout << "[W][" __FILE__ ":" << __LINE__ << "]\t")
#else
#define ctrl_logw std::ostream(0)
#endif
/* Log Info */
#if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_INFO
#define ctrl_logi (std::cout << "[I][" __FILE__ ":" << __LINE__ << "]\t")
#else
#define ctrl_logi std::ostream(0)
#endif
/* Log Debug */
#if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_DEBUG
#define ctrl_logd (std::cout "[D][" << __FILE__ ":" << __LINE__ << "]\t")
#else
#define ctrl_logd std::ostream(0)
#endif
 
namespace ctrl {
 
class AccelCurve {
 public:
  AccelCurve(const float j_max, const float a_max, const float v_start,
             const float v_end) {
    reset(j_max, a_max, v_start, v_end);
  }
  AccelCurve() {
    jm = am = t0 = t1 = t2 = t3 = v0 = v1 = v2 = v3 = x0 = x1 = x2 = x3 = 0;
  }
  void reset(const float j_max, const float a_max, const float v_start,
             const float v_end) {
    /* 符号付きで代入 */
    am = (v_end > v_start) ? a_max : -a_max;  //< 最大加速度の符号を決定
    jm = (v_end > v_start) ? j_max : -j_max;  //< 最大躍度の符号を決定
    /* 初期値と最終値を代入 */
    v0 = v_start;  //< 代入
    v3 = v_end;    //< 代入
    t0 = 0;        //< ここでは初期値をゼロとする
    x0 = 0;        //< ここでは初期値はゼロとする
    /* 速度が曲線となる部分の時間を決定 */
    const auto tc = a_max / j_max;
    /* 等加速度直線運動の時間を決定 */
    const auto tm = (v3 - v0) / am - tc;
    /* 等加速度直線運動の有無で分岐 */
    if (tm > 0) {
      /* 速度: 曲線 -> 直線 -> 曲線 */
      t1 = t0 + tc;
      t2 = t1 + tm;
      t3 = t2 + tc;
      v1 = v0 + am * tc / 2;                 //< v(t) を積分
      v2 = v1 + am * tm;                     //< v(t) を積分
      x1 = x0 + v0 * tc + am * tc * tc / 6;  //< x(t) を積分
      x2 = x1 + v1 * tm;                     //< x(t) を積分
      x3 = x0 + (v0 + v3) / 2 * (t3 - t0);  //< v(t) グラフの台形の面積より
    } else {
      /* 速度: 曲線 -> 曲線 */
      const auto tcp = std::sqrt((v3 - v0) / jm);  //< 変曲までの時間
      t1 = t2 = t0 + tcp;
      t3 = t2 + tcp;
      v1 = v2 = (v0 + v3) / 2;  //< 対称性より中点となる
      x1 = x2 = x0 + v1 * tcp + jm * tcp * tcp * tcp / 6;  //< x(t) を積分
      x3 = x0 + 2 * v1 * tcp;  //< 速度 v(t) グラフの面積より
    }
  }
  float j(const float t) const {
    if (t <= t0)
      return 0;
    else if (t <= t1)
      return jm;
    else if (t <= t2)
      return 0;
    else if (t <= t3)
      return -jm;
    else
      return 0;
  }
  float a(const float t) const {
    if (t <= t0)
      return 0;
    else if (t <= t1)
      return jm * (t - t0);
    else if (t <= t2)
      return am;
    else if (t <= t3)
      return -jm * (t - t3);
    else
      return 0;
  }
  float v(const float t) const {
    if (t <= t0)
      return v0;
    else if (t <= t1)
      return v0 + jm / 2 * (t - t0) * (t - t0);
    else if (t <= t2)
      return v1 + am * (t - t1);
    else if (t <= t3)
      return v3 - jm / 2 * (t - t3) * (t - t3);
    else
      return v3;
  }
  float x(const float t) const {
    if (t <= t0)
      return x0 + v0 * (t - t0);
    else if (t <= t1)
      return x0 + v0 * (t - t0) + jm / 6 * (t - t0) * (t - t0) * (t - t0);
    else if (t <= t2)
      return x1 + v1 * (t - t1) + am / 2 * (t - t1) * (t - t1);
    else if (t <= t3)
      return x3 + v3 * (t - t3) - jm / 6 * (t - t3) * (t - t3) * (t - t3);
    else
      return x3 + v3 * (t - t3);
  }
  float t_end() const { return t3; }
  float v_end() const { return v3; }
  float x_end() const { return x3; }
  float t_0() const { return t0; }
  float t_1() const { return t1; }
  float t_2() const { return t2; }
  float t_3() const { return t3; }
  const std::array<float, 4> getTimeStamps() const {
    return {{t0, t1, t2, t3}};
  }
  void printCsv(std::ostream& os, const float t_interval = 1e-3f) const {
    for (float t = t0; t < t_end(); t += t_interval) {
      os << t << "," << j(t) << "," << a(t) << "," << v(t) << "," << x(t)
         << std::endl;
    }
  }
  friend std::ostream& operator<<(std::ostream& os, const AccelCurve& obj) {
    os << "AccelCurve ";
    os << "\tvs: " << obj.v0;
    os << "\tve: " << obj.v3;
    os << "\tt0: " << obj.t0;
    os << "\tt1: " << obj.t1;
    os << "\tt2: " << obj.t2;
    os << "\tt3: " << obj.t3;
    os << "\td: " << obj.x3 - obj.x0;
    return os;
  }
 
 public:
  static float calcReachableVelocityEnd(const float j_max, const float a_max,
                                        const float vs, const float vt,
                                        const float d) {
    /* 速度が曲線となる部分の時間を決定 */
    const auto tc = a_max / j_max;
    /* 最大加速度の符号を決定 */
    const auto am = (vt > vs) ? a_max : -a_max;
    const auto jm = (vt > vs) ? j_max : -j_max;
    /* 等加速度直線運動の有無で分岐 */
    const auto d_triangle = (vs + am * tc / 2) * tc;  //< distance @ tm == 0
    const auto v_triangle = jm / am * d - vs;         //< v_end @ tm == 0
    // ctrl_logd << "d_tri: " << d_triangle << std::endl;
    // ctrl_logd << "v_tri: " << v_triangle << std::endl;
    if (d * v_triangle > 0 && std::abs(d) > std::abs(d_triangle)) {
      /* 曲線・直線・曲線 */
      ctrl_logd << "v: curve - straight - curve" << std::endl;
      /* 2次方程式の解の公式を解く */
      const auto amtc = am * tc;
      const auto D = amtc * amtc - 4 * (amtc * vs - vs * vs - 2 * am * d);
      const auto sqrtD = std::sqrt(D);
      return (-amtc + (d > 0 ? sqrtD : -sqrtD)) / 2;
    }
    /* 曲線・曲線 (走行距離が短すぎる) */
    /* 3次方程式を解いて、終点速度を算出;
     * 簡単のため、値を一度すべて正に変換して、計算結果に符号を付与して返送 */
    const auto a = std::abs(vs);
    const auto b = (d > 0 ? 1 : -1) * jm * d * d;
    const auto aaa_27 = a * a * a / 27;
    const auto cr = 8 * aaa_27 + b / 2;
    const auto ci_b = 8 * aaa_27 / b + 1.0f / 4;
    if (ci_b >= 0) {
      /* ルートの中が非負のとき、3乗根により解を求める */
      ctrl_logd << "v: curve - curve (accel)" << std::endl;
      const auto c = std::cbrt(cr + std::abs(b) * std::sqrt(ci_b));
      return (d > 0 ? 1 : -1) * (c + 4 * a * a / c / 9 - a / 3);
    } else {
      /* ルートの中が負のとき、極座標変換して解を求める */
      ctrl_logd << "v: curve - curve (decel)" << std::endl;
      const auto ci = std::abs(b) * std::sqrt(-ci_b);
      const auto r = std::hypot(cr, ci);  //< = sqrt(cr^2 + ci^2)
      const auto th = std::atan2(ci, cr);
      return (d > 0 ? 1 : -1) * (2 * std::cbrt(r) * std::cos(th / 3) - a / 3);
    }
  }
  static float calcReachableVelocityMax(const float j_max, const float a_max,
                                        const float vs, const float ve,
                                        const float d) {
    /* 速度が曲線となる部分の時間を決定 */
    const auto tc = a_max / j_max;
    const auto am = (d > 0) ? a_max : -a_max;  //< 加速方向は移動方向に依存
    /* 2次方程式の解の公式を解く */
    const auto amtc = am * tc;
    const auto D = amtc * amtc - 2 * (vs + ve) * amtc + 4 * am * d +
                   2 * (vs * vs + ve * ve);
    if (D < 0) {
      /* 拘束条件がおかしい */
      ctrl_loge << "Error! D = " << D << " < 0" << std::endl;
      /* 入力のチェック */
      if (vs * ve < 0)
        ctrl_loge << "Invalid Input! vs: " << vs << ", ve: " << ve << std::endl;
      return vs;
    }
    const auto sqrtD = std::sqrt(D);
    return (-amtc + (d > 0 ? sqrtD : -sqrtD)) / 2;  //< 2次方程式の解
  }
  static float calcDistanceFromVelocityStartToEnd(const float j_max,
                                                  const float a_max,
                                                  const float v_start,
                                                  const float v_end) {
    /* キャッシュ */
    const auto ve_minus_vs = v_end - v_start;
    /* 符号付きで代入 */
    const auto am = (ve_minus_vs > 0) ? a_max : -a_max;
    const auto jm = (ve_minus_vs > 0) ? j_max : -j_max;
    /* 速度が曲線となる部分の時間を決定 */
    const auto tc = a_max / j_max;
    /* 等加速度直線運動の時間を決定 */
    const auto tm = ve_minus_vs / am - tc;
    /* 始点から終点までの時間を決定 */
    const auto t_all =
        (tm > 0) ? (tc + tm + tc) : (2 * std::sqrt(ve_minus_vs / jm));
    return (v_start + v_end) / 2 * t_all;  //< 速度グラフの面積により
  }
 
 protected:
  float jm;             
  float am;             
  float t0, t1, t2, t3; 
  float v0, v1, v2, v3; 
  float x0, x1, x2, x3; 
};
}  // namespace ctrl