滑らかな加速の設計③ 走行距離を考慮した速度設計

設計2・走行距離を考慮した速度設計

前回の記事の続きです.

この記事では,設計1の曲線加速を組み合わせて,走行距離を考慮した速度設計について考えます.

滑らかな加速の設計 目次

設計内容

設計1によって,始点速度と終点速度を与えれば,それを繋ぐ曲線加速を生成できるようになりました.

今度はそれを組み合わせて,始点速度→加速→最大速度→減速→終点速度,というような速度軌道を設計します.

加速曲線

加速曲線

ここで,図中の赤線である最大速度とその継続時間は,走行距離の拘束条件を満たすように算出します.

拘束条件

与える情報は以下の通りです.

  • 最大加速度 $a_{m}$
  • 始点速度 $v_{s}$
  • 飽和速度 $v_{a}$
  • 目標速度 $v_{t}$
  • 走行距離 $d$
  • 加速度が曲線である時間 $t_c$

設計1との違い

設計1と比べて,飽和速度$v_a$と走行距離$d$が新たに加わりました.

また,終点速度$v_e$が目標速度$v_t$に変わりました.

というのも,走行距離$d$と最大加速度$a_m$の拘束条件により,与えられた目標速度$v_t$に達することができない場合があるからです.

結果的に算出される終点速度$v_e$は,目標速度と$v_t$と異なる場合があるので注意します.

この場合,当初の設計よりも速い速度でターンに突入することがありあますが,無理に減速して姿勢を崩した状態で突入するよりはマシでしょう.

求めた関数

  • 走行距離などの拘束条件から終点速度$v_e$を求める関数
  • 走行距離などの拘束条件から最大速度$v_m$を求める関数
  • 任意の時刻 $t$ における,加速度 $a(t)$,速度 $v(t)$ および位置 $x(t)$ を返す関数

走行距離などから終点速度を求める式

与えられた拘束条件から終点速度を求める式です.

$$ \begin{align} v_e(t_c, a_m, v_s, v_t, d) &:= \left\{\begin{array}{ll} \min\{v_t,~v_{e1}\} & (v_t \ge v_s) \newline \max\{v_t,~v_{e1}\} & (v_t < v_s) \end{array}\right. \end{align} $$ ただし, $$ \begin{align} v_{e1} &:= \left\{\begin{array}{ll} v_{e2} & (t_m \ge 0) \newline v_{e3} & (t_m < 0) \end{array}\right. \newline t_m &:= \frac{1}{a_m}(v_t-v_s) - t_c \newline v_{e2} &:= \frac{1}{2}\left(\sqrt{4v_s^2-4v_sa_mt_c+a_m(a_mt_c^2+8d)}-a_mt_c\right) \newline v_{e3} &:= \frac{1}{3}\left( \sqrt[3]{\frac{\sqrt{27b(32a^3+27b)} + 16a^3+27b}{2}} \right. \newline & \quad \left. +4a^2 \sqrt[3]{\frac{2}{\sqrt{27b(32a^3+27b)} + 16a^3+27b}} -a \right) \newline a &:= v_s \newline b &:= \frac{a_md^2}{t_c} \end{align} $$

かなり場合分けが多くなってしまいましたが,一応これで求めることができます.

注意点として,$v_{e3}$の計算する際,減速の場合,2乗根の中身が負になる場合があるので,この計算は複素数で行う必要があります.

答えは必ず実数になるので,どうにか実数で計算できないか考えましたが,今のところ解決策は見つかっていません.

走行距離などから最大速度を求める式

与えられた拘束条件から最大速度を求める式です.

$$ \begin{align} v_m(t_c, a_m, v_s, v_a, v_e, d) &:= \min\{v_a,~v_{m2}\} \end{align} $$ ただし, $$ \begin{align} v_{m2} &:= \max\{v_s, v_e, v_{m1}\} \newline v_{m1} &:= \frac{-a_mt_c + \sqrt{a_m^2t_c^2-(v_s+v_e)a_mt_c+4a_md+2(v_s^2+v_e^2)}}{2} \end{align} $$

任意の時刻 $t$ における,加速度 $a(t)$,速度 $v(t)$ および位置 $x(t)$

ここままで与えられた,もしくは算出した下記パラメータを用いて設計1の曲線加速を組み合わせます.

  • 最大加速度 $a_{m}$
  • 始点速度 $v_{s}$
  • 最大速度 $v_{m}$
  • 終点速度 $v_{e}$
  • 走行距離 $d$
  • 加速度が曲線である時間 $t_c$

前回の記事で設計した設計1の曲線加速に対して,

  • 最大加速度を$a_m$
  • 始点速度を$v_s$
  • 終点速度を$\color{red}{v_m}$
  • 加速度が曲線である時間を$t_c$

としたものを加速曲線 $ac$,

  • 最大加速度を$a_m$
  • 始点速度を$\color{red}{v_m}$
  • 終点速度を$v_e$
  • 加速度が曲線である時間を$t_c$

としたものを減速曲線 $dc$として定義します.

このとき,減速の方は,上図の通り$t_2$だけ時間をシフトした形で使用します.

各境界点時刻の定義は,以下の通りです.

$$ \begin{align} t_0 &:= 0 \newline t_1 &:= ac.t_{end} \newline t_2 &:= t_1 + \frac{d - ac.x_{end} - dc.x_{end}}{v_m} \newline t_3 &:= t_2 + dc.t_{end} \end{align} $$

$ac.t_{end}$などの表記は,察してください.

導出過程

走行距離などから終点速度を求める式

走行距離$d$がある程度小さいとき,与えられた拘束条件では目標速度$v_t$にたどり着けない場合があります.

ここでは,そのような場合にギリギリたどり着ける終点速度$v_e$を求めます.

加速曲線

加速曲線

例によって,上図のように,等加速度直線運動の時間$t_m$の正負に従って場合分けをする必要があります.

$$ t_m := \frac{1}{a_m}(v_t-v_s) - t_c $$

$t_m>0$のとき,求める終点速度を$v_{e2}$とおくと, 対称性により,台形の面積の公式を用いて,

$$ \begin{align} d &= \frac{1}{2}(v_s+v_{e2})(t_3-t_0) \newline &= \frac{1}{2}(v_s+v_{e2})(t_c+t_m+t_c) \newline &= \frac{1}{2}(v_s+v_{e2})\left(t_c+\frac{v_{e2}-v_s}{a_m}\right) \newline \Leftrightarrow v_{e2} &= \frac{1}{2}\left(\sqrt{4v_s^2-4v_sa_mt_c+a_m(a_mt_c^2+8d)}-a_mt_c\right) \end{align} $$

$t_m\le 0$のとき,求める終点速度を$v_{e3}$とおくと,同様に,

$$ \begin{align} d &= \frac{1}{2}(v_s+v_{e3})(t_3-t_0) \newline &= \frac{1}{2}(v_s+v_{e3})2\sqrt{\frac{t_c}{a_m}(v_{e3}-v_s)} \newline \Leftrightarrow v_{e3} &= \frac{1}{3}\left( \sqrt[3]{\frac{\sqrt{27b(32a^3+27b)} + 16a^3+27b}{2}} \right. \newline & \quad \left. +4a^2 \sqrt[3]{\frac{2}{\sqrt{27b(32a^3+27b)} + 16a^3+27b}} -a \right) \newline a &:= v_s \newline b &:= \frac{a_md^2}{t_c} \end{align} $$

以上をまとめると,たどり着き得る終点速度$v_{e1}$は,

$$ \begin{align} v_{e1} &:= \left\{\begin{array}{ll} v_{e2} & (t_m \ge 0) \newline v_{e3} & (t_m < 0) \end{array}\right. \end{align} $$

ここで求めた終点速度$v_{e1}$はあくまで,目標速度$v_t$にたどり着けない場合に使用する速度なので, それ以外は$v_t$で飽和させます.

$$ \begin{align} v_e &:= \left\{\begin{array}{ll} \min\{v_t,~v_{e1}\} & (v_t \ge v_s) \newline \max\{v_t,~v_{e1}\} & (v_t < v_s) \end{array}\right. \end{align} $$

このとき,加速の場合と減速の場合で飽和方向が異なるので注意します.

これで,走行すべき終点速度$v_e$が求まりました.

走行距離などから最大速度を求める式

次に,下図のように走行するために,最大速度$v_m$を求めます.

加速曲線

加速曲線

まずはじめに,飽和速度$v_a$は考えずに,始点速度$v_s$から最大速度$v_{m1}$に達して終点速度$v_e$に達する下図の場合を考えます.

加速曲線

加速曲線

その間の走行距離が$d$となることから$v_{m1}$求めると以下が得られます.

$$ \begin{align} d &= \frac{1}{2}(v_s+v_{m1})(t_1-t_0)+ \frac{1}{2}(v_{m1}+v_e)(t_3-t_2) \newline &= \frac{1}{2}(v_s+v_{m1})\left(t_c+\frac{v_{m1}-v_s}{a_m}\right)+ \frac{1}{2}(v_{m1}+v_e)\left(t_c+\frac{v_e-v_{m1}}{a_m}\right) \newline \Leftrightarrow v_{m1} &:= \frac{-a_mt_c + \sqrt{a_m^2t_c^2-(v_s+v_e)a_mt_c+4a_md+2(v_s^2+v_e^2)}}{2} \end{align} $$

ここで,無駄な減速を避けるため,以下の式で飽和します.

$$ \begin{align} v_{m2} &:= \max\{v_s,~v_e,~v_{m1}\} \end{align} $$

最後に,最大速度$v_m$は,飽和速度$v_a$を超えてはならないので,それで飽和させます.

$$ \begin{align} v_m = \min\{v_a,~v_{m2}\} \end{align} $$

これで,下図のように走行する最大速度$v_m$が求まりました.

加速曲線

加速曲線

まとめ

本記事では,走行距離の拘束条件を満たすような曲線加速を設計しました.

次回の記事では,そのためのC++実装例を紹介します.