accel_designer.h

00001 /**
00002  * @file accel_designer.h
00003  * @author Ryotaro Onuki (kerikun11+github@gmail.com)
00004  * @see https://www.kerislab.jp/posts/2018-04-29-accel-designer4/
00005  * @brief 距離の拘束を満たす加減速走行軌道を生成するクラスを保持するファイル
00006  * @date 2020-04-19
00007  */
00008 #pragma once
00009 
00010 #include "accel_curve.h"
00011 
00012 #include <algorithm> //< for std::max, std::min
00013 //#include <array>
00014 #include <iostream> //< for std::cout
00015 #include <ostream>
00016 
00017 /**
00018  * @brief 制御関係の名前空間
00019  */
00020 namespace ctrl
00021 {
00022 
00023   /**
00024    * @brief 拘束条件を満たす曲線加減速の軌道を生成するクラス
00025    *
00026    * - 移動距離の拘束条件を満たす曲線加速軌道を生成する
00027    * - 各時刻 $t$ における躍度 $j(t)$，加速度 $a(t)$，速度 $v(t)$，位置 $x(t)$
00028    * を提供する
00029    * - 最大加速度 $a_{\\max}$ と始点速度 $v_s$
00030    * など拘束次第では目標速度 $v_t$ に達することができない場合があるので注意する
00031    */
00032   class AccelDesigner
00033   {
00034   public:
00035     /**
00036      * @brief 初期化付きコンストラクタ
00037      *
00038      * @param j_max     最大躍度の大きさ [m/s/s/s]，正であること
00039      * @param a_max     最大加速度の大きさ [m/s/s], 正であること
00040      * @param v_max     最大速度の大きさ [m/s]，正であること
00041      * @param v_start   始点速度 [m/s]
00042      * @param v_target  目標速度 [m/s]
00043      * @param dist      移動距離 [m]
00044      * @param x_start   始点位置 [m] (オプション)
00045      * @param t_start   始点時刻 [s] (オプション)
00046      */
00047     AccelDesigner(const float j_max, const float a_max, const float v_max,
00048                   const float v_start, const float v_target, const float dist,
00049                   const float x_start = 0, const float t_start = 0)
00050     {
00051       reset(j_max, a_max, v_max, v_start, v_target, dist, x_start, t_start);
00052     }
00053     /**
00054      * @brief 空のコンストラクタ．あとで reset() により初期化すること．
00055      */
00056     AccelDesigner() { t0 = t1 = t2 = t3 = x0 = x3 = 0; }
00057     /**
00058      * @brief 引数の拘束条件から曲線を生成する．
00059      * この関数によって，すべての変数が初期化される．(漏れはない)
00060      *
00061      * @param j_max     最大躍度の大きさ [m/s/s/s]，正であること
00062      * @param a_max     最大加速度の大きさ [m/s/s], 正であること
00063      * @param v_max     最大速度の大きさ [m/s]，正であること
00064      * @param v_start   始点速度 [m/s]
00065      * @param v_target  目標速度 [m/s]
00066      * @param dist      移動距離 [m]
00067      * @param x_start   始点位置 [m] (オプション)
00068      * @param t_start   始点時刻 [s] (オプション)
00069      */
00070     void reset(const float j_max, const float a_max, const float v_max,
00071                const float v_start, const float v_target, const float dist,
00072                const float x_start = 0, const float t_start = 0)
00073     {
00074       /* 目標速度に到達可能か，走行距離から終点速度を決定していく */
00075       float v_end = v_target; /*< 仮代入 */
00076       /* 移動距離の拘束により，目標速度に達し得ない場合の処理 */
00077       const float dist_min = AccelCurve::calcDistanceFromVelocityStartToEnd(
00078           j_max, a_max, v_start, v_end);
00079       if (std::abs(dist) < std::abs(dist_min))
00080       {
00081         ctrl_logd << "vs -> ve != vt" << std::endl;
00082         /* 目標速度$v_t$に向かい，走行距離$d$で到達し得る終点速度$v_e$を算出 */
00083         v_end = AccelCurve::calcReachableVelocityEnd(j_max, a_max, v_start,
00084                                                      v_target, dist);
00085       }
00086       /* 飽和速度の仮置き */
00087       float v_sat = dist > 0 ? std::max(std::max(v_start, v_max), v_end)
00088                              : std::min(std::min(v_start, -v_max), v_end);
00089       /* 曲線を生成 */
00090       ac.reset(j_max, a_max, v_start, v_sat); //< 加速部分
00091       dc.reset(j_max, a_max, v_sat, v_end);   //< 減速部分
00092       /* 最大速度まで加速すると走行距離の拘束を満たさない場合の処理 */
00093       const float d_sum = ac.x_end() + dc.x_end();
00094       if (std::abs(dist) < std::abs(d_sum))
00095       {
00096         ctrl_logd << "vs -> vr -> ve" << std::endl;
00097         /* 走行距離などの拘束から到達可能速度を算出 */
00098         const float v_rm = AccelCurve::calcReachableVelocityMax(
00099             j_max, a_max, v_start, v_end, dist);
00100         /* 無駄な減速を回避 */
00101         v_sat = dist > 0 ? std::max(std::max(v_start, v_rm), v_end)
00102                          : std::min(std::min(v_start, v_rm), v_end);
00103         ac.reset(j_max, a_max, v_start, v_sat); //< 加速
00104         dc.reset(j_max, a_max, v_sat, v_end);   //< 減速
00105       }
00106       /* t23 = nan 回避; vs = ve = d = 0 のときに発生 */
00107       if (v_sat == 0)
00108         v_sat = 1;
00109       /* 各定数の算出 */
00110       const float t23 = (dist - ac.x_end() - dc.x_end()) / v_sat;
00111       x0 = x_start;
00112       x3 = x_start + dist;
00113       t0 = t_start;
00114       t1 = t0 + ac.t_end();                    //< 曲線加速終了の時刻
00115       t2 = t0 + ac.t_end() + t23;              //< 等速走行終了の時刻
00116       t3 = t0 + ac.t_end() + t23 + dc.t_end(); //< 曲線減速終了の時刻
00117 #if 0
00118     /* 出力のチェック */
00119     const float e = 0.01f; //< 数値誤差分
00120     bool show_info = false;
00121     /* 飽和速度時間 */
00122     if (t23 < 0) {
00123       ctrl_logd << t23 << std::endl;
00124       show_info = true;
00125     }
00126     /* 終点速度 */
00127     if (std::abs(v_start - v_end) > e + std::abs(v_start - v_target)) {
00128       std::cerr << "Error: Velocity Target!" << std::endl;
00129       show_info = true;
00130     }
00131     /* 飽和速度 */
00132     if (std::abs(v_sat) >
00133         e + std::max({v_max, std::abs(v_start), std::abs(v_end)})) {
00134       std::cerr << "Error: Velocity Saturation!" << std::endl;
00135       show_info = true;
00136     }
00137     /* タイムスタンプ */
00138     if (!(t0 <= t1 + e && t1 <= t2 + e && t2 <= t3 + e)) {
00139       ctrl_loge << "Error: Time Point Relationship!" << std::endl;
00140       show_info = true;
00141     }
00142     /* 入力情報の表示 */
00143     if (show_info) {
00144       ctrl_loge << "Constraints:"
00145            << "\tj_max: " << j_max << "\ta_max: " << a_max
00146            << "\tv_max: " << v_max << "\tv_start: " << v_start
00147            << "\tv_target: " << v_target << "\tdist: " << dist << std::endl;
00148       ctrl_loge << "ad.reset(" << j_max << ", " << a_max << ", " << v_max << ", "
00149            << v_start << ", " << v_target << ", " << dist << ");" << std::endl;
00150       /* 表示 */
00151       ctrl_loge << "Time Stamp: "
00152            << "\tt0: " << t0 << "\tt1: " << t1 << "\tt2: " << t2
00153            << "\tt3: " << t3 << std::endl;
00154       ctrl_loge << "Position:   "
00155            << "\tx0: " << x0 << "\tx1: " << x0 + ac.x_end()
00156            << "\tx2: " << x0 + (dist - dc.x_end()) << "\tx3: " << x3
00157            << std::endl;
00158       ctrl_loge << "Velocity:   "
00159            << "\tv0: " << v_start << "\tv1: " << v(t1) << "\tv2: " << v(t2)
00160            << "\tv3: " << v_end << std::endl;
00161     }
00162 #endif
00163     }
00164     /**
00165      * @brief 時刻 t [s] における躍度 j [m/s/s/s]
00166      */
00167     float j(const float t) const
00168     {
00169       if (t < t2)
00170         return ac.j(t - t0);
00171       else
00172         return dc.j(t - t2);
00173     }
00174     /**
00175      * @brief 時刻 t [s] における加速度 a [m/s/s]
00176      */
00177     float a(const float t) const
00178     {
00179       if (t < t2)
00180         return ac.a(t - t0);
00181       else
00182         return dc.a(t - t2);
00183     }
00184     /**
00185      * @brief 時刻 t [s] における速度 v [m/s]
00186      */
00187     float v(const float t) const
00188     {
00189       if (t < t2)
00190         return ac.v(t - t0);
00191       else
00192         return dc.v(t - t2);
00193     }
00194     /**
00195      * @brief 時刻 t [s] における位置 x [m]
00196      */
00197     float x(const float t) const
00198     {
00199       if (t < t2)
00200         return x0 + ac.x(t - t0);
00201       else
00202         return x3 - dc.x_end() + dc.x(t - t2);
00203     }
00204     /**
00205      * @brief 終点時刻 [s]
00206      */
00207     float t_end() const { return t3; }
00208     /**
00209      * @brief 終点速度 [m/s]
00210      */
00211     float v_end() const { return dc.v_end(); }
00212     /**
00213      * @brief 終点位置 [m]
00214      */
00215     float x_end() const { return x3; }
00216     /**
00217      * @brief 境界の時刻 [s]
00218      */
00219     float t_0() const { return t0; }
00220     float t_1() const { return t1; }
00221     float t_2() const { return t2; }
00222     float t_3() const { return t3; }
00223     /**
00224      * @brief stdout に軌道のcsvを出力する関数．
00225      */
00226     void printCsv(const float t_interval = 1e-3f) const
00227     {
00228       printCsv(std::cout, t_interval);
00229     }
00230     /**
00231      * @brief std::ostream に軌道のcsvを出力する関数．
00232      */
00233     void printCsv(std::ostream &os, const float t_interval = 1e-3f) const
00234     {
00235       for (float t = t0; t < t_end(); t += t_interval)
00236         os << t << "," << j(t) << "," << a(t) << "," << v(t) << "," << x(t)
00237            << std::endl;
00238     }
00239     /**
00240      * @brief 情報の表示
00241      */
00242     friend std::ostream &operator<<(std::ostream &os, const AccelDesigner &obj)
00243     {
00244       os << "AccelDesigner:";
00245       os << "\td: " << obj.x3 - obj.x0;
00246       os << "\tvs: " << obj.ac.v(0);
00247       os << "\tvm: " << obj.ac.v_end();
00248       os << "\tve: " << obj.dc.v_end();
00249       os << "\tt0: " << obj.t0;
00250       os << "\tt1: " << obj.t1;
00251       os << "\tt2: " << obj.t2;
00252       os << "\tt3: " << obj.t3;
00253       return os;
00254     }
00255     /**
00256      * @brief 境界のタイムスタンプを取得
00257      */
00258     //  const std::array<float, 8> getTimeStamp() const {
00259     //    return {{
00260     //        t0 + ac.t_0(),
00261     //        t0 + ac.t_1(),
00262     //        t0 + ac.t_2(),
00263     //        t0 + ac.t_3(),
00264     //        t2 + dc.t_0(),
00265     //        t2 + dc.t_1(),
00266     //        t2 + dc.t_2(),
00267     //        t2 + dc.t_3(),
00268     //    }};
00269     //  }
00270 
00271   protected:
00272     float t0, t1, t2, t3; /**< @brief 境界点の時刻 [s] */
00273     float x0, x3;         /**< @brief 境界点の位置 [m] */
00274     AccelCurve ac;        /**< @brief 曲線加速用オブジェクト */
00275     AccelCurve dc;        /**< @brief 曲線減速用オブジェクト */
00276   };
00277 
00278 } // namespace ctrl