滑らかな加速の設計④ C++による実装例

概要

前回の記事の続きです.

今回の記事では,今までに示した設計法のC++による実装例を紹介します.

滑らかな加速の設計 目次

ソースコード

このライブラリに実体はなく,クラスの宣言が書かれたヘッダファイルのみです.

※使用する際は,これらのファイルはすべて同じディレクトリにおいてください.

設計1 曲線加速

以下のコードは,曲線加速クラスの宣言部分です.

/** @class 加速曲線を生成するクラス
*   @brief 引数に従って加速曲線を生成する
*/
class AccelCurve {
public:
  /** @constructor
  *   @param a_max 最大加速度 [mm/s/s]
  *   @param v_start 始点速度 [mm/s]
  *   @param v_end 終点速度 [mm/s]
  */
  AccelCurve(const float a_max, const float v_start, const float v_end);
  /** @constructor
  *   @brief 空のコンストラクタ.あとで reset() により初期化すること.
  */
  AccelCurve();
  /** @function reset
  *   @brief 引数の拘束条件から曲線を生成する.
  *   この関数によって,すべての変数が初期化される.(漏れはない)
  */
  void reset(const float a_max, const float v_start, const float v_end):
  /** @function a
  *   @brief 時刻$t$における加速度$a$
  *   @param t 時刻[s]
  *   @return 加速度[mm/s/s]
  */
  float a(const float t) const;
  /** @function v
  *   @brief 時刻$t$における速度$v$
  *   @param t 時刻[s]
  *   @return 速度[mm/s]
  */
  float v(const float t) const;
  /** @function x
  *   @brief 時刻$t$における位置$x$
  *   @param t 時刻[s]
  *   @return 位置[mm]
  */
  float x(const float t) const;
  /** @function xx_end
  *   @brief 終端xx
  */
  float t_end() const;
  float v_end() const;
  float x_end() const;
  /** @function calcTimeCurve
  *   @brief 曲線加速部分の時間を決定する関数
  *   @param am 最大加速度の大きさ
  */
  static float calcTimeCurve(const float am);
  /** @function calcVelocityEnd
  *   @brief 走行距離から達しうる終点速度を算出する関数
  *   @param am 最大加速度の大きさ [mm/s/s]
  *   @param vs 始点速度 [mm/s]
  *   @param vt 目標速度 [mm/s]
  *   @param d 走行距離 [mm]
  *   @return vm 最大速度 [mm/s]
  */
  static float calcVelocityEnd(float am, const float vs, const float vt, const float d);
  /** @function calcVelocityMax
  *   @brief 走行距離から達しうる最大速度を算出する関数
  *   @param am 最大加速度の大きさ [mm/s/s]
  *   @param vs 始点速度 [mm/s]
  *   @param ve 終点速度 [mm/s]
  *   @param d 走行距離 [mm]
  *   @return vm 最大速度 [mm/s]
  */
  static float calcVelocityMax(const float am, const float vs, const float va, const float ve, const float d);
private:
  float am; //< 最大加速度 [m/s/s]
  float t0, t1, t2, t3; //< 境界点の時刻 [s]
  float v0, v1, v2, v3; //< 境界点の速度 [m/s]
  float x0, x1, x2, x3; //< 境界点の位置 [m]
  float tc; //< 曲線加速の時間 [s]
  float tm; //< 最大加速度の時間 [s]
};

設計2 曲線加減速

以下のコードは,走行距離の拘束条件を満たした曲線加減速クラスの宣言部分です.

/** @class 加減速曲線を生成するクラス
*   @brief 引数に従って速度計画をし,加減速曲線を生成する
*/
class AccelDesigner{
public:
  /** @constructor
  *   @param a_max 最大加速度 [mm/s/s]
  *   @param v_start 始点速度 [mm/s]
  *   @param v_end 終点速度 [mm/s]
  */
  AccelDesigner(const float a_max, const float v_start, const float v_sat, const float v_target, const float distance, const float x_start = 0);
  /** @constructor
  *   @brief 空のコンストラクタ.あとで reset() により初期化すること.
  */
  AccelDesigner();
  /** @function reset
  *   @brief 引数の拘束条件から曲線を生成する.
  *   この関数によって,すべての変数が初期化される.(漏れはない)
  */
  void reset(const float a_max, const float v_start, const float v_sat, const float v_target, const float distance, const float x_start = 0);
  /** @function a
  *   @brief 時刻$t$における加速度$a$
  *   @param t 時刻[s]
  *   @return 加速度[mm/s/s]
  */
  float a(const float t) const;
  /** @function v
  *   @brief 時刻$t$における速度$v$
  *   @param t 時刻[s]
  *   @return 速度[mm/s]
  */
  float v(const float t) const;
  /** @function x
  *   @brief 時刻$t$における位置$x$
  *   @param t 時刻[s]
  *   @return 位置[mm]
  */
  float x(const float t) const;
  /** @function xx_end
  *   @brief 終端xx
  */
  float t_end() const;
  float v_end() const;
  float x_end() const;
  /** @function printCsv
  *   @brief stdoutに軌道のcsvを出力する関数.
  */
  void printCsv(const float t_interval = 0.001f) const;
  /** @function printCsv
  *   @brief std::ofstream に軌道のcsvを出力する関数.
  */
  void printCsv(std::ofstream& f, const float t_interval = 0.001f) const;
private:
  float t1, t2, t3; //< 境界点の時刻 [s]
  float x0, x3; //< 境界点の位置 [mm]
  AccelCurve ac, dc; //< 曲線加速,曲線減速
};

使用例

シンプル

一番簡単な例として以下のC++コードをご覧ください. 実行すると,標準出力にCSVが表示されます.

#include <cstdio>
#include <fstream>
#include <iostream>
#include "AccelDesigner.h"

int main(void){
  std::ofstream f("out.csv");
  AccelDesigner sd;

  const float a_max    = 3000;
  const float v_start  =    0;
  const float v_sat    = 2400;
  const float v_target =  600;
  const float distance =  720;
  sd.reset(a_max, v_start, v_sat, v_target, distance); //< 曲線を生成
  // CSV出力
  for(float t=0; t<sd.t_end(); t+=0.001f){
    printf("%f, %f, %f\n", sd.a(t), sd.v(t), sd.x(t));
  }

  return 0;
}

連続使用

加減速をいくつか繰り返す例です.

前回の終点の速度や位置を,次回の始点速度,位置に使っています.

#include <cstdio>
#include <fstream>
#include <iostream>
#include "AccelDesigner.h"

int main(void){
  std::ofstream of("out.csv");
  AccelDesigner sd;

  sd.reset(6000, sd.v_end(), 2400, 1200, 1800, sd.x_end()); //< 曲線の生成
  sd.printCsv(of); //< CSVファイル出力
  sd.reset(6000, sd.v_end(), 2400,  600,  360, sd.x_end()); //< 曲線の生成
  sd.printCsv(of); //< CSVファイル出力
  sd.reset(6000, sd.v_end(), 2400,    0,  720, sd.x_end()); //< 曲線の生成
  sd.printCsv(of); //< CSVファイル出力

  return 0;
}

CSVをMATLABでプロット

以下のMATLABコードで出力されたout.csvファイルをプロットできます.

clear; close all;
data = csvread('out.csv');
plot(data, '-','LineWidth', 4); grid on;
xlabel('時刻', 'Interpreter', 'Latex', 'FontSize', 12)
legend({'加速度', '速度', '距離'}, 'FontSize', 14, 'Location', 'SouthWest');
title('曲線加速','fontsize', 14);

上記の連続使用のコード例をプロットしたのが以下の図です.

CSVをMATLABでプロット

CSVをMATLABでプロット

実装上の工夫

移植性の考慮

今回のクラス設計では,

  • 走行距離$d$を考慮して終点速度$v_e$を求める関数AccelCurve::calcVelocityEnd()
  • 走行距離$d$を考慮して最大速度$v_m$を求める関数AccelCurve::calcVelocityMax()

は,設計1のAccelCurveクラスに実装しました.

というのも,曲線生成部分がすべてAccelCurveクラスにまとまっているので, 今後,曲線部分を2次関数ではない別の関数に変えたくなったら,AccelCurveクラスの中身を変更するだけで済むようになっています.

まとめ

さて,滑らかな加速について考えてきました.

今回の設計によって,拘束条件を満たす走行軌道を生成することができました.

あとは,機体がこれを追従するコードを書くだけですね!

といっても,それもまた難しいですが…

免責

設計には欠陥やミスがあるかもしれません.くれぐれも自己責任ご使用ください.

もしミスや改良点を見つけたら教えてください!