accel_curve.h

00001 /**
00002  * @file accel_curve.h
00003  * @author Ryotaro Onuki (kerikun11+github@gmail.com)
00004  * @see https://www.kerislab.jp/posts/2018-04-29-accel-designer4/
00005  * @brief 躍度0次，加速度1次，速度2次，位置3次関数により，滑らかな加速を実現する
00006  * @date 2020-04-19
00007  */
00008 #pragma once
00009 
00010 //#include <array>
00011 #include <cmath>    //< for std::sqrt, std::cbrt
00012 #include <iostream> //< for std::cout
00013 #include <ostream>
00014 
00015 /* log level definition */
00016 #define CTRL_LOG_LEVEL_NONE 0
00017 #define CTRL_LOG_LEVEL_ERROR 1
00018 #define CTRL_LOG_LEVEL_WARNING 2
00019 #define CTRL_LOG_LEVEL_INFO 3
00020 #define CTRL_LOG_LEVEL_DEBUG 4
00021 /* set log level */
00022 #ifndef CTRL_LOG_LEVEL
00023 #define CTRL_LOG_LEVEL CTRL_LOG_LEVEL_WARNING
00024 #endif
00025 /* Log Error */
00026 #if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_ERROR
00027 #define ctrl_loge (std::cout << "[E][" __FILE__ ":" << __LINE__ << "]\t")
00028 #else
00029 #define ctrl_loge std::ostream(0)
00030 #endif
00031 /* Log Warning */
00032 #if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_WARNING
00033 #define ctrl_logw (std::cout << "[W][" __FILE__ ":" << __LINE__ << "]\t")
00034 #else
00035 #define ctrl_logw std::ostream(0)
00036 #endif
00037 /* Log Info */
00038 #if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_INFO
00039 #define ctrl_logi (std::cout << "[I][" __FILE__ ":" << __LINE__ << "]\t")
00040 #else
00041 #define ctrl_logi std::ostream(0)
00042 #endif
00043 /* Log Debug */
00044 #if CTRL_LOG_LEVEL >= CTRL_LOG_LEVEL_DEBUG
00045 #define ctrl_logd (std::cout "[D][" << __FILE__ ":" << __LINE__ << "]\t")
00046 #else
00047 #define ctrl_logd std::ostream(0)
00048 #endif
00049 
00050 /**
00051  * @brief 制御関係の名前空間
00052  */
00053 namespace ctrl
00054 {
00055 
00056   /**
00057    * @brief 加速曲線を生成するクラス
00058    *
00059    * - 引数の拘束に従って加速曲線を生成する
00060    * - 始点速度と終点速度を滑らかにつなぐ
00061    * - 移動距離の拘束はない
00062    * - 始点速度および終点速度は，正でも負でも可
00063    */
00064   class AccelCurve
00065   {
00066   public:
00067     /**
00068      * @brief 初期化付きのコンストラクタ．
00069      *
00070      * @param j_max   最大躍度の大きさ [m/s/s/s], 正であること
00071      * @param a_max   最大加速度の大きさ [m/s/s], 正であること
00072      * @param v_start 始点速度 [m/s]
00073      * @param v_end   終点速度 [m/s]
00074      */
00075     AccelCurve(const float j_max, const float a_max, const float v_start,
00076                const float v_end)
00077     {
00078       reset(j_max, a_max, v_start, v_end);
00079     }
00080     /**
00081      * @brief 空のコンストラクタ．あとで reset() により初期化すること．
00082      */
00083     AccelCurve()
00084     {
00085       jm = am = t0 = t1 = t2 = t3 = v0 = v1 = v2 = v3 = x0 = x1 = x2 = x3 = 0;
00086     }
00087     /**
00088      * @brief 引数の拘束条件から曲線を生成する．
00089      * この関数によって，すべての変数が初期化される．(漏れはない)
00090      *
00091      * @param j_max   最大躍度の大きさ [m/s/s/s], 正であること
00092      * @param a_max   最大加速度の大きさ [m/s/s], 正であること
00093      * @param v_start 始点速度 [m/s]
00094      * @param v_end   終点速度 [m/s]
00095      */
00096     void reset(const float j_max, const float a_max, const float v_start,
00097                const float v_end)
00098     {
00099       /* 符号付きで代入 */
00100       am = (v_end > v_start) ? a_max : -a_max; //< 最大加速度の符号を決定
00101       jm = (v_end > v_start) ? j_max : -j_max; //< 最大躍度の符号を決定
00102       /* 初期値と最終値を代入 */
00103       v0 = v_start; //< 代入
00104       v3 = v_end;   //< 代入
00105       t0 = 0;       //< ここでは初期値をゼロとする
00106       x0 = 0;       //< ここでは初期値はゼロとする
00107       /* 速度が曲線となる部分の時間を決定 */
00108       const float tc = a_max / j_max;
00109       /* 等加速度直線運動の時間を決定 */
00110       const float tm = (v3 - v0) / am - tc;
00111       /* 等加速度直線運動の有無で分岐 */
00112       if (tm > 0)
00113       {
00114         /* 速度: 曲線 -> 直線 -> 曲線 */
00115         t1 = t0 + tc;
00116         t2 = t1 + tm;
00117         t3 = t2 + tc;
00118         v1 = v0 + am * tc / 2;                //< v(t) を積分
00119         v2 = v1 + am * tm;                    //< v(t) を積分
00120         x1 = x0 + v0 * tc + am * tc * tc / 6; //< x(t) を積分
00121         x2 = x1 + v1 * tm;                    //< x(t) を積分
00122         x3 = x0 + (v0 + v3) / 2 * (t3 - t0);  //< v(t) グラフの台形の面積より
00123       }
00124       else
00125       {
00126         /* 速度: 曲線 -> 曲線 */
00127         const float tcp = std::sqrt((v3 - v0) / jm); //< 変曲までの時間
00128         t1 = t2 = t0 + tcp;
00129         t3 = t2 + tcp;
00130         v1 = v2 = (v0 + v3) / 2;                            //< 対称性より中点となる
00131         x1 = x2 = x0 + v1 * tcp + jm * tcp * tcp * tcp / 6; //< x(t) を積分
00132         x3 = x0 + 2 * v1 * tcp;                             //< 速度 v(t) グラフの面積より
00133       }
00134     }
00135     /**
00136      * @brief 時刻 t [s] における躍度 j [m/s/s/s]
00137      */
00138     float j(const float t) const
00139     {
00140       if (t <= t0)
00141         return 0;
00142       else if (t <= t1)
00143         return jm;
00144       else if (t <= t2)
00145         return 0;
00146       else if (t <= t3)
00147         return -jm;
00148       else
00149         return 0;
00150     }
00151     /**
00152      * @brief 時刻 t [s] における加速度 a [m/s/s]
00153      */
00154     float a(const float t) const
00155     {
00156       if (t <= t0)
00157         return 0;
00158       else if (t <= t1)
00159         return jm * (t - t0);
00160       else if (t <= t2)
00161         return am;
00162       else if (t <= t3)
00163         return -jm * (t - t3);
00164       else
00165         return 0;
00166     }
00167     /**
00168      * @brief 時刻 t [s] における速度 v [m/s]
00169      */
00170     float v(const float t) const
00171     {
00172       if (t <= t0)
00173         return v0;
00174       else if (t <= t1)
00175         return v0 + jm / 2 * (t - t0) * (t - t0);
00176       else if (t <= t2)
00177         return v1 + am * (t - t1);
00178       else if (t <= t3)
00179         return v3 - jm / 2 * (t - t3) * (t - t3);
00180       else
00181         return v3;
00182     }
00183     /**
00184      * @brief 時刻 t [s] における位置 x [m]
00185      */
00186     float x(const float t) const
00187     {
00188       if (t <= t0)
00189         return x0 + v0 * (t - t0);
00190       else if (t <= t1)
00191         return x0 + v0 * (t - t0) + jm / 6 * (t - t0) * (t - t0) * (t - t0);
00192       else if (t <= t2)
00193         return x1 + v1 * (t - t1) + am / 2 * (t - t1) * (t - t1);
00194       else if (t <= t3)
00195         return x3 + v3 * (t - t3) - jm / 6 * (t - t3) * (t - t3) * (t - t3);
00196       else
00197         return x3 + v3 * (t - t3);
00198     }
00199     /**
00200      * @brief 終点時刻 [s]
00201      */
00202     float t_end() const { return t3; }
00203     /**
00204      * @brief 終点速度 [m/s]
00205      */
00206     float v_end() const { return v3; }
00207     /**
00208      * @brief 終点位置 [m]
00209      */
00210     float x_end() const { return x3; }
00211     /**
00212      * @brief 境界の時刻 [s]
00213      */
00214     float t_0() const { return t0; }
00215     float t_1() const { return t1; }
00216     float t_2() const { return t2; }
00217     float t_3() const { return t3; }
00218     /**
00219      * @brief 境界のタイムスタンプを取得
00220      */
00221     //  const std::array<float, 4> getTimeStamp() const { return {{t0, t1, t2, t3}}; }
00222     /**
00223      * @brief std::ostream に軌道のcsvを出力する関数．
00224      */
00225     void printCsv(std::ostream &os, const float t_interval = 1e-3f) const
00226     {
00227       for (float t = t0; t < t_end(); t += t_interval)
00228       {
00229         os << t << "," << j(t) << "," << a(t) << "," << v(t) << "," << x(t)
00230            << std::endl;
00231       }
00232     }
00233     /**
00234      * @brief 情報の表示
00235      */
00236     friend std::ostream &operator<<(std::ostream &os, const AccelCurve &obj)
00237     {
00238       os << "AccelCurve ";
00239       os << "\tvs: " << obj.v0;
00240       os << "\tve: " << obj.v3;
00241       os << "\tt0: " << obj.t0;
00242       os << "\tt1: " << obj.t1;
00243       os << "\tt2: " << obj.t2;
00244       os << "\tt3: " << obj.t3;
00245       os << "\td: " << obj.x3 - obj.x0;
00246       return os;
00247     }
00248 
00249   public:
00250     /**
00251      * @brief 走行距離から達しうる終点速度を算出する関数
00252      *
00253      * @param j_max 最大躍度の大きさ [m/s/s/s], 正であること
00254      * @param a_max 最大加速度の大きさ [m/s/s], 正であること
00255      * @param vs    始点速度 [m/s]
00256      * @param vt    目標速度 [m/s]
00257      * @param d     走行距離 [m]
00258      * @return ve   終点速度 [m/s]
00259      */
00260     static float calcReachableVelocityEnd(const float j_max, const float a_max,
00261                                           const float vs, const float vt,
00262                                           const float d)
00263     {
00264       /* 速度が曲線となる部分の時間を決定 */
00265       const float tc = a_max / j_max;
00266       /* 最大加速度の符号を決定 */
00267       const float am = (vt > vs) ? a_max : -a_max;
00268       const float jm = (vt > vs) ? j_max : -j_max;
00269       /* 等加速度直線運動の有無で分岐 */
00270       const float d_triangle = (vs + am * tc / 2) * tc; //< distance @ tm == 0
00271       const float v_triangle = jm / am * d - vs;        //< v_end @ tm == 0
00272       // ctrl_logd << "d_tri: " << d_triangle << std::endl;
00273       // ctrl_logd << "v_tri: " << v_triangle << std::endl;
00274       if (d * v_triangle > 0 && std::abs(d) > std::abs(d_triangle))
00275       {
00276         /* 曲線・直線・曲線 */
00277         ctrl_logd << "v: curve - straight - curve" << std::endl;
00278         /* 2次方程式の解の公式を解く */
00279         const float amtc = am * tc;
00280         const float D = amtc * amtc - 4 * (amtc * vs - vs * vs - 2 * am * d);
00281         const float sqrtD = std::sqrt(D);
00282         return (-amtc + (d > 0 ? sqrtD : -sqrtD)) / 2;
00283       }
00284       /* 曲線・曲線 (走行距離が短すぎる) */
00285       /* 3次方程式を解いて，終点速度を算出;
00286        * 簡単のため，値を一度すべて正に変換して，計算結果に符号を付与して返送 */
00287       const float a = std::abs(vs);
00288       const float b = (d > 0 ? 1 : -1) * jm * d * d;
00289       const float aaa_27 = a * a * a / 27;
00290       const float cr = 8 * aaa_27 + b / 2;
00291       const float ci_b = 8 * aaa_27 / b + 1.0f / 4;
00292       if (ci_b >= 0)
00293       {
00294         /* ルートの中が非負のとき，3乗根により解を求める */
00295         ctrl_logd << "v: curve - curve (accel)" << std::endl;
00296         const float c = std::cbrt(cr + std::abs(b) * std::sqrt(ci_b));
00297         return (d > 0 ? 1 : -1) * (c + 4 * a * a / c / 9 - a / 3);
00298       }
00299       else
00300       {
00301         /* ルートの中が負のとき，極座標変換して解を求める */
00302         ctrl_logd << "v: curve - curve (decel)" << std::endl;
00303         const float ci = std::abs(b) * std::sqrt(-ci_b);
00304         const float r = std::hypot(cr, ci); //< = sqrt(cr^2 + ci^2)
00305         const float th = std::atan2(ci, cr);
00306         return (d > 0 ? 1 : -1) * (2 * std::cbrt(r) * std::cos(th / 3) - a / 3);
00307       }
00308     }
00309     /**
00310      * @brief 走行距離から達しうる最大速度を算出する関数
00311      *
00312      * @param j_max 最大躍度の大きさ [m/s/s/s], 正であること
00313      * @param a_max 最大加速度の大きさ [m/s/s], 正であること
00314      * @param vs    始点速度 [m/s]
00315      * @param ve    終点速度 [m/s]
00316      * @param d     走行距離 [m]
00317      * @return vm   最大速度 [m/s]
00318      */
00319     static float calcReachableVelocityMax(const float j_max, const float a_max,
00320                                           const float vs, const float ve,
00321                                           const float d)
00322     {
00323       /* 速度が曲線となる部分の時間を決定 */
00324       const float tc = a_max / j_max;
00325       const float am = (d > 0) ? a_max : -a_max; /*< 加速方向は移動方向に依存 */
00326       /* 2次方程式の解の公式を解く */
00327       const float amtc = am * tc;
00328       const float D = amtc * amtc - 2 * (vs + ve) * amtc + 4 * am * d +
00329                       2 * (vs * vs + ve * ve);
00330       if (D < 0)
00331       {
00332         /* 拘束条件がおかしい */
00333         ctrl_loge << "Error! D = " << D << " < 0" << std::endl;
00334         /* 入力のチェック */
00335         if (vs * ve < 0)
00336           ctrl_loge << "Invalid Input! vs: " << vs << ", ve: " << ve << std::endl;
00337         return vs;
00338       }
00339       const float sqrtD = std::sqrt(D);
00340       return (-amtc + (d > 0 ? sqrtD : -sqrtD)) / 2; //< 2次方程式の解
00341     }
00342     /**
00343      * @brief 速度差から変位を算出する関数
00344      *
00345      * @param j_max   最大躍度の大きさ [m/s/s/s], 正であること
00346      * @param a_max   最大加速度の大きさ [m/s/s], 正であること
00347      * @param v_start 始点速度 [m/s]
00348      * @param v_end   終点速度 [m/s]
00349      * @return d      変位 [m]
00350      */
00351     static float calcDistanceFromVelocityStartToEnd(const float j_max,
00352                                                     const float a_max,
00353                                                     const float v_start,
00354                                                     const float v_end)
00355     {
00356       /* 符号付きで代入 */
00357       const float am = (v_end > v_start) ? a_max : -a_max;
00358       const float jm = (v_end > v_start) ? j_max : -j_max;
00359       /* 速度が曲線となる部分の時間を決定 */
00360       const float tc = a_max / j_max;
00361       /* 等加速度直線運動の時間を決定 */
00362       const float tm = (v_end - v_start) / am - tc;
00363       /* 始点から終点までの時間を決定 */
00364       const float t_all =
00365           (tm > 0) ? (tc + tm + tc) : (2 * std::sqrt((v_end - v_start) / jm));
00366       return (v_start + v_end) / 2 * t_all; //< 速度グラフの面積により
00367     }
00368 
00369   protected:
00370     float jm;             /**< @brief 躍度定数 [m/s/s/s] */
00371     float am;             /**< @brief 加速度定数 [m/s/s] */
00372     float t0, t1, t2, t3; /**< @brief 時刻定数 [s] */
00373     float v0, v1, v2, v3; /**< @brief 速度定数 [m/s] */
00374     float x0, x1, x2, x3; /**< @brief 位置定数 [m] */
00375   };
00376 } // namespace ctrl