ctrl::AccelCurve クラス

走行距離拘束のない曲線加速の軌道を生成するクラス [詳解]

#include <accel_curve.h>

公開メンバ関数
	AccelCurve (const float j_max, const float a_max, const float v_start, const float v_end)
	初期化付きのコンストラクタ [詳解]
	AccelCurve ()
	とりあえずインスタンス化を行う空のコンストラクタ [詳解]
void	reset (const float j_max, const float a_max, const float v_start, const float v_end)
	引数の拘束条件から曲線を生成する関数 [詳解]
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, 4 >	getTimeStamps () const
	境界のタイムスタンプをまとめて取得する関数 [詳解]
void	printCsv (std::ostream &os, const float t_interval=1e-3f) const
	std::ostream に軌道のcsvを出力する関数 [詳解]

静的公開メンバ関数
static float	calcReachableVelocityEnd (const float j_max, const float a_max, const float vs, const float vt, const float d)
	走行距離の拘束から達しうる終点速度を算出する関数 [詳解]
static float	calcReachableVelocityMax (const float j_max, const float a_max, const float vs, const float ve, const float d)
	走行距離の拘束から達しうる最大速度を算出する関数 [詳解]
static float	calcDistanceFromVelocityStartToEnd (const float j_max, const float a_max, const float v_start, const float v_end)
	速度差の拘束から達しうる変位を算出する関数 [詳解]

限定公開変数類
float	jm
	躍度定数 [m/s/s/s] [詳解]
float	am
	加速度定数 [m/s/s] [詳解]
float	t0
float	t1
float	t2
float	t3
	時刻定数 [s] [詳解]
float	v0
float	v1
float	v2
float	v3
	速度定数 [m/s] [詳解]
float	x0
float	x1
float	x2
float	x3
	位置定数 [m] [詳解]

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

詳解

走行距離拘束のない曲線加速の軌道を生成するクラス

• 引数の拘束に従って加速曲線を生成する
• 始点速度と終点速度を滑らかにつなぐ
• 移動距離の拘束はない
• 始点速度および終点速度は、正でも負でも可

構築子と解体子

AccelCurve() [1/2]

ctrl::AccelCurve::AccelCurve ( const float  j_max, const float  a_max, const float  v_start, const float  v_end  )

inline

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

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s], 正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	v_start	始点速度 [m/s]
[in]	v_end	終点速度 [m/s]

                                {
    reset(j_max, a_max, v_start, v_end);
  }

呼び出し関係図:

AccelCurve() [2/2]

ctrl::AccelCurve::AccelCurve ( )

inline

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

注意

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

               {
    jm = am = t0 = t1 = t2 = t3 = v0 = v1 = v2 = v3 = x0 = x1 = x2 = x3 = 0;
  }

関数詳解

a()

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

inline

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

引数

[in]

時刻

t [s]

戻り値

加速度 [m/s/s]

                               {
    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;
  }

calcDistanceFromVelocityStartToEnd()

static float ctrl::AccelCurve::calcDistanceFromVelocityStartToEnd ( const float  j_max, const float  a_max, const float  v_start, const float  v_end  )

inlinestatic

速度差の拘束から達しうる変位を算出する関数

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s], 正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	v_start	始点速度 [m/s]
[in]	v_end	終点速度 [m/s]

戻り値

d 変位 [m]

                                                                     {
    /* キャッシュ */
    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;  //< 速度グラフの面積により
  }

呼び出し関係図:

calcReachableVelocityEnd()

static float ctrl::AccelCurve::calcReachableVelocityEnd ( const float  j_max, const float  a_max, const float  vs, const float  vt, const float  d  )

inlinestatic

走行距離の拘束から達しうる終点速度を算出する関数

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s], 正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	vs	始点速度 [m/s]
[in]	vt	目標速度 [m/s]
[in]	d	走行距離 [m]

戻り値

ve 終点速度 [m/s]

                                                       {
    /* 速度が曲線となる部分の時間を決定 */
    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);
    }
  }

呼び出し関係図:

calcReachableVelocityMax()

static float ctrl::AccelCurve::calcReachableVelocityMax ( const float  j_max, const float  a_max, const float  vs, const float  ve, const float  d  )

inlinestatic

走行距離の拘束から達しうる最大速度を算出する関数

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s], 正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	vs	始点速度 [m/s]
[in]	ve	終点速度 [m/s]
[in]	d	走行距離 [m]

戻り値

vm 最大速度 [m/s]

                                                       {
    /* 速度が曲線となる部分の時間を決定 */
    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次方程式の解
  }

getTimeStamps()

const std::array<float, 4> ctrl::AccelCurve::getTimeStamps ( ) const

inline

境界のタイムスタンプをまとめて取得する関数

                                                 {
    return {{t0, t1, t2, t3}};
  }

j()

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

inline

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

引数

[in]

時刻

t [s]

戻り値

躍度 [m/s/s/s]

                               {
    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;
  }

printCsv()

void ctrl::AccelCurve::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::AccelCurve::reset ( const float  j_max, const float  a_max, const float  v_start, const float  v_end  )

inline

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

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

引数

[in]	j_max	最大躍度の大きさ [m/s/s/s], 正であること
[in]	a_max	最大加速度の大きさ [m/s/s], 正であること
[in]	v_start	始点速度 [m/s]
[in]	v_end	終点速度 [m/s]

                                {
    /* 符号付きで代入 */
    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) グラフの面積より
    }
  }

呼び出し関係図:

t_0()

float ctrl::AccelCurve::t_0 ( ) const

inline

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

{ return t0; }

t_1()

float ctrl::AccelCurve::t_1 ( ) const

inline

等加速度直線運動の開始時刻 [s]

{ return t1; }

t_2()

float ctrl::AccelCurve::t_2 ( ) const

inline

等加速度直線運動の終了時刻 [s]

{ return t2; }

t_3()

float ctrl::AccelCurve::t_3 ( ) const

inline

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

{ return t3; }

t_end()

float ctrl::AccelCurve::t_end ( ) const

inline

終点時刻 [s]

{ return t3; }

v()

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

inline

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

引数

[in]

時刻

t [s]

戻り値

速度 [m/s]

                               {
    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;
  }

v_end()

float ctrl::AccelCurve::v_end ( ) const

inline

終点速度 [m/s]

{ return v3; }

x()

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

inline

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

引数

[in]

時刻

t [s]

戻り値

位置 [m]

                               {
    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);
  }

x_end()

float ctrl::AccelCurve::x_end ( ) const

inline

終点位置 [m]

{ return x3; }

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

operator<<

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

friend

情報の表示

                                                                       {
    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;
  }

メンバ詳解

am

float ctrl::AccelCurve::am

protected

加速度定数 [m/s/s]

jm

float ctrl::AccelCurve::jm

protected

躍度定数 [m/s/s/s]

t0

float ctrl::AccelCurve::t0

protected

t1

float ctrl::AccelCurve::t1

protected

t2

float ctrl::AccelCurve::t2

protected

t3

float ctrl::AccelCurve::t3

protected

時刻定数 [s]

v0

float ctrl::AccelCurve::v0

protected

v1

float ctrl::AccelCurve::v1

protected

v2

float ctrl::AccelCurve::v2

protected

v3

float ctrl::AccelCurve::v3

protected

速度定数 [m/s]

x0

float ctrl::AccelCurve::x0

protected

x1

float ctrl::AccelCurve::x1

protected

x2

float ctrl::AccelCurve::x2

protected

x3

float ctrl::AccelCurve::x3

protected

位置定数 [m]

---

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

• ctrl/accel_curve.h