ctrl::TrajectoryTracker クラス

独立2輪車の軌道追従フィードバック制御器 [詳解]

#include <trajectory_tracker.h>

ctrl::TrajectoryTracker 連携図

クラス
struct	Gain
	フィードバックゲインを格納する構造体 [詳解]
struct	Result
	計算結果を格納する構造体 [詳解]

公開メンバ関数
	TrajectoryTracker (const Gain &gain, const float xi_threshold=kXiThresholdDefault)
	コンストラクタ [詳解]
void	reset (const float vs=0)
	状態の初期化 [詳解]
const Result	update (const Pose &est_q, const Polar &est_v, const Polar &est_a, const State &ref_s, const float Ts=kIntegrationPeriodDefault)
	制御入力の計算 [詳解]
const Result	update (const Pose &est_q, const Polar &est_v, const Polar &est_a, const Pose &ref_q, const Pose &ref_dq, const Pose &ref_ddq, const Pose &ref_dddq, const float Ts=kIntegrationPeriodDefault)
	制御入力の計算 [詳解]

静的公開メンバ関数
static constexpr float	sinc (const float x)
	自作の sinc 関数 sinc(x) := sin(x) / x [詳解]

静的公開変数類
static constexpr const float	kIntegrationPeriodDefault = 1e-3f
	制御周期（積分周期）のデフォルト値 [s] [詳解]
static constexpr const float	kXiThresholdDefault = 150.0f
	制御則の切り替え閾値のデフォルト値 [mm/s] [詳解]

限定公開変数類
Gain	gain
	フィードバックゲイン [詳解]
float	xi
	補助状態変数 [詳解]
float	xi_threshold
	制御則を切り替える閾値 [詳解]

詳解

独立2輪車の軌道追従フィードバック制御器

構築子と解体子

TrajectoryTracker()

ctrl::TrajectoryTracker::TrajectoryTracker ( const Gain &  gain, const float  xi_threshold = kXiThresholdDefault  )

inline

コンストラクタ

引数

[in]	gain	軌道追従フィードバックゲイン
[in]	xi_threshold	制御則を切り替える閾値

      : gain(gain), xi_threshold(xi_threshold) {}

関数詳解

reset()

void ctrl::TrajectoryTracker::reset ( const float  vs = 0 )

inline

状態の初期化

引数

[in]

vs

初期並進速度

{ xi = vs; }

sinc()

static constexpr float ctrl::TrajectoryTracker::sinc ( const float  x )

inlinestaticconstexpr

自作の sinc 関数 sinc(x) := sin(x) / x

引数

[in]

x

戻り値

sinc(x)

                                             {
    const auto xx = x * x;
    const auto xxxx = xx * xx;
    return xxxx * xxxx / 362880 - xxxx * xx / 5040 + xxxx / 120 - xx / 6 + 1;
  }

update() [1/2]

const Result ctrl::TrajectoryTracker::update ( const Pose &  est_q, const Polar &  est_v, const Polar &  est_a, const Pose &  ref_q, const Pose &  ref_dq, const Pose &  ref_ddq, const Pose &  ref_dddq, const float  Ts = kIntegrationPeriodDefault  )

inline

制御入力の計算

引数

[in]	est_q	推定位置
[in]	est_v	推定速度
[in]	est_a	推定加速度
[in]	ref_q	目標位置
[in]	ref_dq	目標速度
[in]	ref_ddq	目標加速度
[in]	ref_dddq	目標躍度
[in]	Ts	制御周期

戻り値

u 制御入力

                                                                  {
    /* Prepare Variable */
    const float x = est_q.x;
    const float y = est_q.y;
    const float theta = est_q.th;
    const float cos_theta = std::cos(theta);
    const float sin_theta = std::sin(theta);
    const float dx = est_v.tra * cos_theta;
    const float dy = est_v.tra * sin_theta;
    const float ddx = est_a.tra * cos_theta;
    const float ddy = est_a.tra * sin_theta;
    /* Feedback Gain Design */
    const float zeta = gain.zeta;
    const float omega_n = gain.omega_n;
    const float kx = omega_n * omega_n;
    const float kdx = 2 * zeta * omega_n;
    const float ky = kx;
    const float kdy = kdx;
    /* Determine Reference */
    const float dddx_r = ref_dddq.x;
    const float dddy_r = ref_dddq.y;
    const float ddx_r = ref_ddq.x;
    const float ddy_r = ref_ddq.y;
    const float dx_r = ref_dq.x;
    const float dy_r = ref_dq.y;
    const float x_r = ref_q.x;
    const float y_r = ref_q.y;
    const float th_r = ref_q.th;
    const float cos_th_r = std::cos(th_r);
    const float sin_th_r = std::sin(th_r);
    const float u1 = ddx_r + kdx * (dx_r - dx) + kx * (x_r - x);
    const float u2 = ddy_r + kdy * (dy_r - dy) + ky * (y_r - y);
    const float du1 = dddx_r + kdx * (ddx_r - ddx) + kx * (dx_r - dx);
    const float du2 = dddy_r + kdy * (ddy_r - ddy) + ky * (dy_r - dy);
    const float d_xi = u1 * cos_th_r + u2 * sin_th_r;
    /* integral the state(s) */
    xi += d_xi * Ts;
    /* determine the output signal */
    Result res;
    if (std::abs(xi) < xi_threshold) {
      const auto b = gain.low_b;        //< b > 0
      const auto zeta = gain.low_zeta;  //< zeta \in [0,1]
      const auto v_d = ref_dq.x * cos_th_r + ref_dq.y * sin_th_r;
      const auto w_d = ref_dq.th;
      const auto k1 = 2 * zeta * std::sqrt(w_d * w_d + b * v_d * v_d);
      const auto k2 = b;
      const auto k3 = k1;
      const auto v = v_d * std::cos(th_r - theta) +
                     k1 * (cos_theta * (x_r - x) + sin_theta * (y_r - y));
      const auto w = w_d +
                     k2 * v_d * sinc(th_r - theta) *
                         (-sin_theta * (x_r - x) + cos_theta * (y_r - y)) +
                     k3 * (th_r - theta);
      res.v = v;
      res.w = w;
      res.dv = ref_ddq.x * cos_th_r + ref_ddq.y * sin_th_r;
      res.dw = ref_ddq.th;
      // res.v = ref_dq.x * cos_th_r + ref_dq.y * sin_th_r;
      // res.w = red_dq.th;
      // res.dv = ref_ddq.x * cos_th_r + ref_ddq.y * sin_th_r;
      // res.dw = ref_ddq.th;
    } else {
      res.v = xi;
      res.dv = d_xi;
      res.w = (u2 * cos_th_r - u1 * sin_th_r) / xi;
      res.dw = -(2 * d_xi * res.w + du1 * sin_th_r - du2 * cos_th_r) / xi;
    }
    return res;
  }

呼び出し関係図:

update() [2/2]

const Result ctrl::TrajectoryTracker::update ( const Pose &  est_q, const Polar &  est_v, const Polar &  est_a, const State &  ref_s, const float  Ts = kIntegrationPeriodDefault  )

inline

制御入力の計算

引数

[in]	est_q	推定位置
[in]	est_v	推定速度
[in]	est_a	推定加速度
[in]	ref_s	目標状態
[in]	Ts	制御周期

戻り値

u 制御入力

                                                                  {
    return update(est_q, est_v, est_a, ref_s.q, ref_s.dq, ref_s.ddq, ref_s.dddq,
                  Ts);
  }

メンバ詳解

gain

Gain ctrl::TrajectoryTracker::gain

protected

フィードバックゲイン

kIntegrationPeriodDefault

constexpr const float ctrl::TrajectoryTracker::kIntegrationPeriodDefault = 1e-3f

staticconstexpr

制御周期（積分周期）のデフォルト値 [s]

kXiThresholdDefault

constexpr const float ctrl::TrajectoryTracker::kXiThresholdDefault = 150.0f

staticconstexpr

制御則の切り替え閾値のデフォルト値 [mm/s]

xi

float ctrl::TrajectoryTracker::xi

protected

補助状態変数

xi_threshold

float ctrl::TrajectoryTracker::xi_threshold

protected

制御則を切り替える閾値

---

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

• ctrl/trajectory_tracker.h