なめらかなスラロームの設計③ C++による実装

git clone -b v1.0.1 https://github.com/kerikun11/micromouse-control-module.git
cd micromouse-control-module

/**
 * @brief 拘束条件を満たす曲線加減速の軌道を生成するクラス
 *
 * - 移動距離の拘束条件を満たす曲線加速軌道を生成する
 * - 各時刻 $t$ における躍度 $j(t)$、加速度 $a(t)$、速度 $v(t)$、位置 $x(t)$
 * を提供する
 */
class AccelDesigner {
public:
  /**
   * @brief 初期化付きコンストラクタ
   *
   * @param j_max     最大躍度の大きさ [m/s/s/s]、正であること
   * @param a_max     最大加速度の大きさ [m/s/s], 正であること
   * @param v_max     最大速度の大きさ [m/s]、正であること
   * @param v_start   始点速度 [m/s]
   * @param v_target  目標速度 [m/s]
   * @param dist      移動距離 [m]
   * @param x_start   始点位置 [m] (オプション)
   * @param t_start   始点時刻 [s] (オプション)
   */
  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);
  /**
   * @brief 時刻 t [s] における躍度 j [m/s/s/s]
   */
  float j(const float t) const;
  /**
   * @brief 時刻 t [s] における加速度 a [m/s/s]
   */
  float a(const float t) const;
  /**
   * @brief 時刻 t [s] における速度 v [m/s]
   */
  float v(const float t) const;
  /**
   * @brief 時刻 t [s] における位置 x [m]
   */
  float x(const float t) const;
  /**
   * @brief 終点時刻 [s]
   */
  float t_end() const;

protected:
  float t0, t1, t2, t3; /**< @brief 境界点の時刻 [s] */
  float x0, x3;         /**< @brief 境界点の位置 [m] */
  AccelCurve ac, dc; /**< @brief 曲線加速、曲線減速オブジェクト */
};

/**
 * @brief 位置姿勢の座標
 */
struct Pose {
  float x, y, th; /*< (x, y, theta) 成分 */
};

/**
 * @brief 軌道制御の状態変数
 */
struct State {
  Pose q;
  Pose dq;
  Pose ddq;
  Pose dddq;
};

/**
 * @brief slalom::Shape スラロームの形状を表す構造体
 *
 * メンバー変数は互いに依存して決定されるので、個別に数値を変更することは許されない、
 * スラローム軌道を得るには slalom::Trajectory を用いる。
 */
struct Shape {
  Pose total; /**< @brief 前後の直線を含めた移動位置姿勢 */
  Pose curve; /**< @brief カーブ部分の移動位置姿勢 */
  float straight_prev; /**< @brief カーブ前の直線の距離 [m] */
  float straight_post; /**< @brief カーブ後の直線の距離 [m] */
  float v_ref;         /**< @brief カーブ部分の基準速度 [m/s] */
  float dddth_max;     /**< @brief 最大角躍度の大きさ [rad/s/s/s] */
  float ddth_max;      /**< @brief 最大角加速度の大きさ [rad/s/s] */
  float dth_max;       /**< @brief 最大角速度の大きさ [rad/s] */

public:
  /**
   * @brief 生成済みスラローム形状を代入するコンストラクタ
   */
  Shape(const Pose total, const Pose curve, float straight_prev,
        const float straight_post, const float v_ref, const float dddth_max,
        const float ddth_max, const float dth_max);
  /**
   * @brief 拘束条件からスラローム形状を生成するコンストラクタ
   *
   * @param total 前後の直線を含めた移動位置姿勢
   * @param y_curve_end
   * $y$方向(進行方向に垂直な方向)の移動距離、
   * カーブの大きさを決めるもので、設計パラメータとなる
   * @param x_adv $x$方向(進行方向)の前後の直線の長さ。180度ターンなどでは
   * y_curve_end で調節できないので、例外的にこの値で調節する。
   * @param dddth_max 最大角躍度の大きさ [rad/s/s/s]
   * @param ddth_max 最大角加速度の大きさ [rad/s/s]
   * @param dth_max 最大角速度の大きさ [rad/s]
   */
  Shape(const Pose total, const float y_curve_end, const float x_adv = 0,
        const float dddth_max = 1200 * M_PI, const float ddth_max = 36 * M_PI,
        const float dth_max = 3 * M_PI);
  /**
   * @brief 軌道の積分を行う関数。ルンゲクッタ法を使用して数値積分を行う。
   *
   * @param ad 角速度分布
   * @param s 状態変数
   * @param v 並進速度
   * @param t 時刻
   * @param Ts 積分時間
   */
  static void integrate(const AccelDesigner &ad, State &s, const float v,
                        const float t, const float Ts);
  /**
   * @brief 情報の表示
   */
  friend std::ostream &operator<<(std::ostream &os, const Shape &obj);
};

/**
 * @brief slalom::Trajectory スラローム軌道を生成するクラス
 *
 * スラローム形状 Shape と並進速度をもとに、各時刻における位置や速度を提供する。
 */
class Trajectory {
public:
  /**
   * @brief コンストラクタ
   *
   * @param shape スラローム形状
   * @param mirror_x スラローム形状を$x$軸反転(進行方向に対して左右反転)する
   */
  Trajectory(const Shape &shape, const bool mirror_x = false);
  /**
   * @brief 並進速度を設定して軌道を初期化する関数
   *
   * @param velocity 並進速度 [m/s]
   * @param th_start 初期姿勢 [rad] (オプション)
   * @param t_start 初期時刻 [s] (オプション)
   */
  void reset(const float velocity, const float th_start = 0,
             const float t_start = 0);
  /**
   * @brief 軌道の更新
   *
   * @param state 次の時刻に更新する現在状態
   * @param t 現在時刻 [s]
   * @param Ts 積分時間 [s]
   */
  void update(State &state, const float t, const float Ts) const;
  /**
   * @brief 並進速度を取得
   */
  float getVelocity() const;
  /**
   * @brief ターンの合計時間を取得
   */
  float getTimeCurve() const;
  /**
   * @brief スラローム形状を取得
   */
  const Shape &getShape() const;
  /**
   * @brief 角速度設計器を取得
   */
  const AccelDesigner &getAccelDesigner() const;

protected:
  Shape shape;      /**< @brief スラロームの形状 */
  AccelDesigner ad; /**< @brief 角速度用の曲線加速生成器 */
  float velocity;   /**< @brief 並進速度 */
};

#include "slalom.h"
#include <iostream>

using namespace ctrl; //< 非推奨な書き方

int main(void) {
  /* 事前の形状設計 */
  /* 設計パラメータを定義 */
  const Pose pose_total = Pose(45, 45, M_PI / 2); //< 探索90度ターンを想定
  const float y_curve = 40;
  /* スラローム形状を作成 */
  slalom::Shape shape(pose_total, y_curve);
  /* 生成結果パラメータの表示 */
  std::cout << shape << std::endl;

  /* 走行時の軌道生成 */
  slalom::Trajectory trajectory(shape); //< 軌道生成器にスラローム形状を渡す
  /* 制御周期 */
  const float Ts = 0.001f;
  /* 走行する並進速度を用いて軌道の生成 */
  const float v = 600;
  trajectory.reset(v);
  /* 状態変数を定義 */
  State s;
  /* スラローム走行 */
  for (float t = 0; t < trajectory.getTimeCurve(); t += Ts) {
    /* 軌道の更新 */
    trajectory.update(s, t, Ts);
    /* 軌道の csv 出力 */
    std::cout << t;              //< 時刻
    std::cout << "," << v;       //< 並進速度
    std::cout << "," << s.dq.th; //< 角速度
    std::cout << "," << s.q.x;   //< 位置 x
    std::cout << "," << s.q.y;   //< 位置 y
    std::cout << "," << s.q.th;  //< 角度
    std::cout << std::endl;
  }
  return 0;
}

Slalom Shape
        total:  (45, 45, 1.5708)
        curve:  (40, 40, 1.5708)
        v_ref:  241.59
        straight_prev:  5.00001
        straight_post:  5
        integral error: (0, 0, 1.19209e-07)

0,600,0.0288747,0.6,1.44374e-06,9.62491e-06
0.001,600,0.115499,1.2,2.30998e-05,7.69993e-05
0.002,600,0.259873,1.8,0.000116943,0.000259873
0.003,600,0.461996,2.4,0.000369597,0.000615994
--- 省略 ---
0.111,600,0.0158959,40,39.5548,1.57079
0.112,600,0,40,40.1548,1.5708