ctrl::AccelDesigner クラス

拘束条件を満たす曲線加減速の軌道を生成するクラス [詳解]

#include <accel_designer.h>

ctrl::AccelDesigner の継承関係図

ctrl::AccelDesigner 連携図

公開メンバ関数
	AccelDesigner (const float j_max, const float a_max, const float v_max, const float v_start, const float v_target, const float dist, const float x_start=0, const float t_start=0)
	初期化付きコンストラクタ [詳解]
	AccelDesigner ()
	とりあえずインスタンス化を行う空のコンストラクタ [詳解]
void	reset (const float j_max, const float a_max, const float v_max, const float v_start, const float v_target, const float dist, const float x_start=0, const float t_start=0)
	引数の拘束条件から曲線を生成する関数 [詳解]
float	j (const float t) const
	任意の時刻 t [s] における躍度 j [m/s/s/s] を返す関数 [詳解]
float	a (const float t) const
	任意の時刻 t [s] における加速度 a [m/s/s] を返す関数 [詳解]
float	v (const float t) const
	任意の時刻 t [s] における速度 v [m/s] を返す関数 [詳解]
float	x (const float t) const
	任意の時刻 t [s] における位置 x [m] を返す関数 [詳解]
float	t_end () const
	終点時刻 [s] [詳解]
float	v_end () const
	終点速度 [m/s] [詳解]
float	x_end () const
	終点位置 [m] [詳解]
float	t_0 () const
	曲線加速の開始時刻 [s] [詳解]
float	t_1 () const
	最高速度に達する時刻 [s] [詳解]
float	t_2 () const
	曲線減速の開始時刻 [s] [詳解]
float	t_3 () const
	曲線減速の終了時刻 [s] [詳解]
const std::array< float, 8 >	getTimeStamps () const
	曲線加速の境界のタイムスタンプを取得 [詳解]
void	printCsv (const float t_interval=1e-3f) const
	stdout に軌道のcsvを出力する関数。 [詳解]
void	printCsv (std::ostream &os, const float t_interval=1e-3f) const
	std::ostream に軌道のcsvを出力する関数。 [詳解]

限定公開変数類
float	t0
float	t1
float	t2
float	t3
	境界点の時刻 [s] [詳解]
float	x0
float	x3
	境界点の位置 [m] [詳解]
AccelCurve	ac
	曲線加速用オブジェクト [詳解]
AccelCurve	dc
	曲線減速用オブジェクト [詳解]

フレンド
std::ostream &	operator<< (std::ostream &os, const AccelDesigner &obj)
	情報の表示 [詳解]

詳解

拘束条件を満たす曲線加減速の軌道を生成するクラス

• 目標速度や移動距離などの拘束条件を満たす曲線加速軌道を生成する
• 任意の時刻 $t$ における躍度 $j(t)$、加速度 $a(t)$、速度 $v(t)$、位置 $x(t)$ を返す連続な関数を提供する
• 最大加速度 $a_{\max}$ と始点速度 $v_s$ など拘束次第では目標速度 $v_t$ に達することができない場合があるので注意する

構築子と解体子

AccelDesigner() [1/2]

ctrl::AccelDesigner::AccelDesigner ( const float  j_max, const float  a_max, const float  v_max, const float  v_start, const float  v_target, const float  dist, const float  x_start = 0, const float  t_start = 0  )

inline

初期化付きコンストラクタ

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s]、正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	v_max	最大速度の大きさ [m/s]、正であること
[in]	v_start	始点速度 [m/s]
[in]	v_target	目標速度 [m/s]
[in]	dist	移動距離 [m]
[in]	x_start	始点位置 [m] (オプション)
[in]	t_start	始点時刻 [s] (オプション)

                                                                  {
    reset(j_max, a_max, v_max, v_start, v_target, dist, x_start, t_start);
  }

呼び出し関係図:

AccelDesigner() [2/2]

ctrl::AccelDesigner::AccelDesigner ( )

inline

とりあえずインスタンス化を行う空のコンストラクタ

注意

別途 reset() により初期化すること。

{ t0 = t1 = t2 = t3 = x0 = x3 = 0; }

関数詳解

a()

float ctrl::AccelDesigner::a ( const float  t ) const

inline

任意の時刻 t [s] における加速度 a [m/s/s] を返す関数

引数

[in]

時刻

t [s]

戻り値

加速度 [m/s/s]

                               {
    if (t < t2)
      return ac.a(t - t0);
    else
      return dc.a(t - t2);
  }

呼び出し関係図:

getTimeStamps()

const std::array<float, 8> ctrl::AccelDesigner::getTimeStamps ( ) const

inline

曲線加速の境界のタイムスタンプを取得

                                                 {
    return {{
        t0 + ac.t_0(),
        t0 + ac.t_1(),
        t0 + ac.t_2(),
        t0 + ac.t_3(),
        t2 + dc.t_0(),
        t2 + dc.t_1(),
        t2 + dc.t_2(),
        t2 + dc.t_3(),
    }};
  }

呼び出し関係図:

j()

float ctrl::AccelDesigner::j ( const float  t ) const

inline

任意の時刻 t [s] における躍度 j [m/s/s/s] を返す関数

引数

[in]

時刻

t [s]

戻り値

躍度 [m/s/s/s]

                               {
    if (t < t2)
      return ac.j(t - t0);
    else
      return dc.j(t - t2);
  }

呼び出し関係図:

printCsv() [1/2]

void ctrl::AccelDesigner::printCsv ( const float  t_interval = 1e-3f ) const

inline

stdout に軌道のcsvを出力する関数。

                                                      {
    printCsv(std::cout, t_interval);
  }

printCsv() [2/2]

void ctrl::AccelDesigner::printCsv ( std::ostream &  os, const float  t_interval = 1e-3f  ) const

inline

std::ostream に軌道のcsvを出力する関数。

                                                                      {
    for (float t = t0; t < t_end(); t += t_interval)
      os << t << "," << j(t) << "," << a(t) << "," << v(t) << "," << x(t)
         << std::endl;
  }

呼び出し関係図:

reset()

void ctrl::AccelDesigner::reset ( const float  j_max, const float  a_max, const float  v_max, const float  v_start, const float  v_target, const float  dist, const float  x_start = 0, const float  t_start = 0  )

inline

引数の拘束条件から曲線を生成する関数

この関数によってもれなくすべての変数が初期化される。

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s]、正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	v_max	最大速度の大きさ [m/s]、正であること
[in]	v_start	始点速度 [m/s]
[in]	v_target	目標速度 [m/s]
[in]	dist	移動距離 [m]
[in]	x_start	始点位置 [m] (オプション)
[in]	t_start	始点時刻 [s] (オプション)

                                                               {
    /* 目標速度に到達可能か、走行距離から終点速度を決定していく */
    auto v_end = v_target;  //< 仮代入
    /* 移動距離の拘束により、目標速度に達し得ない場合の処理 */
    const auto dist_min = AccelCurve::calcDistanceFromVelocityStartToEnd(
        j_max, a_max, v_start, v_end);
    if (std::abs(dist) < std::abs(dist_min)) {
      ctrl_logd << "vs -> ve != vt" << std::endl;
      /* 目標速度$v_t$に向かい、走行距離$d$で到達し得る終点速度$v_e$を算出 */
      v_end = AccelCurve::calcReachableVelocityEnd(j_max, a_max, v_start,
                                                   v_target, dist);
    }
    /* 飽和速度の仮置き */
    auto v_sat = dist > 0 ? std::max({v_start, v_max, v_end})
                          : std::min({v_start, -v_max, v_end});
    /* 曲線を生成 */
    ac.reset(j_max, a_max, v_start, v_sat);  //< 加速部分
    dc.reset(j_max, a_max, v_sat, v_end);    //< 減速部分
    /* 最大速度まで加速すると走行距離の拘束を満たさない場合の処理 */
    const auto d_sum = ac.x_end() + dc.x_end();
    if (std::abs(dist) < std::abs(d_sum)) {
      ctrl_logd << "vs -> vr -> ve" << std::endl;
      /* 走行距離などの拘束から到達可能速度を算出 */
      const auto v_rm = AccelCurve::calcReachableVelocityMax(
          j_max, a_max, v_start, v_end, dist);
      /* 無駄な減速を回避 */
      v_sat = dist > 0 ? std::max({v_start, v_rm, v_end})
                       : std::min({v_start, v_rm, v_end});
      ac.reset(j_max, a_max, v_start, v_sat);  //< 加速
      dc.reset(j_max, a_max, v_sat, v_end);    //< 減速
    }
    /* t23 = nan 回避; vs = ve = d = 0 のときに発生 */
    if (std::abs(v_sat) < std::numeric_limits<float>::epsilon()) v_sat = 1;
    /* 各定数の算出 */
    const auto t23 = (dist - ac.x_end() - dc.x_end()) / v_sat;
    x0 = x_start;
    x3 = x_start + dist;
    t0 = t_start;
    t1 = t0 + ac.t_end();                     //< 曲線加速終了の時刻
    t2 = t0 + ac.t_end() + t23;               //< 等速走行終了の時刻
    t3 = t0 + ac.t_end() + t23 + dc.t_end();  //< 曲線減速終了の時刻
#if 0
    /* 出力のチェック */
    const auto e = 0.01f;  //< 数値誤差分
    bool show_info = false;
    /* 飽和速度時間 */
    if (t23 < 0) {
      ctrl_logd << t23 << std::endl;
      show_info = true;
    }
    /* 終点速度 */
    if (std::abs(v_start - v_end) > e + std::abs(v_start - v_target)) {
      std::cerr << "Error: Velocity Target!" << std::endl;
      show_info = true;
    }
    /* 飽和速度 */
    if (std::abs(v_sat) >
        e + std::max({v_max, std::abs(v_start), std::abs(v_end)})) {
      std::cerr << "Error: Velocity Saturation!" << std::endl;
      show_info = true;
    }
    /* タイムスタンプ */
    if (!(t0 <= t1 + e && t1 <= t2 + e && t2 <= t3 + e)) {
      ctrl_loge << "Error: Time Point Relationship!" << std::endl;
      show_info = true;
    }
    /* 入力情報の表示 */
    if (show_info) {
      ctrl_loge << "Constraints:"
                << "\tj_max: " << j_max << "\ta_max: " << a_max
                << "\tv_max: " << v_max << "\tv_start: " << v_start
                << "\tv_target: " << v_target << "\tdist: " << dist
                << std::endl;
      ctrl_loge << "ad.reset(" << j_max << ", " << a_max << ", " << v_max
                << ", " << v_start << ", " << v_target << ", " << dist << ");"
                << std::endl;
      /* 表示 */
      ctrl_loge << "Time Stamp: "
                << "\tt0: " << t0 << "\tt1: " << t1 << "\tt2: " << t2
                << "\tt3: " << t3 << std::endl;
      ctrl_loge << "Position:   "
                << "\tx0: " << x0 << "\tx1: " << x0 + ac.x_end()
                << "\tx2: " << x0 + (dist - dc.x_end()) << "\tx3: " << x3
                << std::endl;
      ctrl_loge << "Velocity:   "
                << "\tv0: " << v_start << "\tv1: " << v(t1) << "\tv2: " << v(t2)
                << "\tv3: " << v_end << std::endl;
    }
#endif
  }

呼び出し関係図:

t_0()

float ctrl::AccelDesigner::t_0 ( ) const

inline

曲線加速の開始時刻 [s]

{ return t0; }

t_1()

float ctrl::AccelDesigner::t_1 ( ) const

inline

最高速度に達する時刻 [s]

{ return t1; }

t_2()

float ctrl::AccelDesigner::t_2 ( ) const

inline

曲線減速の開始時刻 [s]

{ return t2; }

t_3()

float ctrl::AccelDesigner::t_3 ( ) const

inline

曲線減速の終了時刻 [s]

{ return t3; }

t_end()

float ctrl::AccelDesigner::t_end ( ) const

inline

終点時刻 [s]

{ return t3; }

v()

float ctrl::AccelDesigner::v ( const float  t ) const

inline

任意の時刻 t [s] における速度 v [m/s] を返す関数

引数

[in]

時刻

t [s]

戻り値

速度 [m/s]

                               {
    if (t < t2)
      return ac.v(t - t0);
    else
      return dc.v(t - t2);
  }

呼び出し関係図:

v_end()

float ctrl::AccelDesigner::v_end ( ) const

inline

終点速度 [m/s]

{ return dc.v_end(); }

呼び出し関係図:

x()

float ctrl::AccelDesigner::x ( const float  t ) const

inline

任意の時刻 t [s] における位置 x [m] を返す関数

引数

[in]

時刻

t [s]

戻り値

位置 [m]

                               {
    if (t < t2)
      return x0 + ac.x(t - t0);
    else
      return x3 - dc.x_end() + dc.x(t - t2);
  }

呼び出し関係図:

x_end()

float ctrl::AccelDesigner::x_end ( ) const

inline

終点位置 [m]

{ return x3; }

フレンドと関連関数の詳解

operator<<

std::ostream& operator<< ( std::ostream &  os, const AccelDesigner &  obj  )

friend

情報の表示

                                                                          {
    os << "AccelDesigner:";
    os << "\td: " << obj.x3 - obj.x0;
    os << "\tvs: " << obj.ac.v(0);
    os << "\tvm: " << obj.ac.v_end();
    os << "\tve: " << obj.dc.v_end();
    os << "\tt0: " << obj.t0;
    os << "\tt1: " << obj.t1;
    os << "\tt2: " << obj.t2;
    os << "\tt3: " << obj.t3;
    return os;
  }

メンバ詳解

ac

AccelCurve ctrl::AccelDesigner::ac

protected

曲線加速用オブジェクト

dc

AccelCurve ctrl::AccelDesigner::dc

protected

曲線減速用オブジェクト

t0

float ctrl::AccelDesigner::t0

protected

t1

float ctrl::AccelDesigner::t1

protected

t2

float ctrl::AccelDesigner::t2

protected

t3

float ctrl::AccelDesigner::t3

protected

境界点の時刻 [s]

x0

float ctrl::AccelDesigner::x0

protected

x3

float ctrl::AccelDesigner::x3

protected

境界点の位置 [m]

---

このクラス詳解は次のファイルから抽出されました:

• ctrl/accel_designer.h