Dynamic Window Approach

元論文

www.researchgate.net

運動学の確認

本論文で扱われている、進行速度と角速度がコントロールできる平面上のロボットの運動学を整理する。時刻 tにおけるロボットの位置を x(t), y(t)とすると

\begin{eqnarray} x(t_{n}) &=& x(t_{0}) + \int_{t_{0}}^{t_{n}} v(t^{'}) \cos \theta(t^{'}) dt^{'} \\ &=& x(t_{0}) + \sum_{i=0}^{n-1} \int_{t_{i}}^{t_{i+1}} v(t^{'}) \cos \theta(t^{'}) dt^{'} \\ &=& x(t_{0}) + \sum_{i=0}^{n-1} \int_{t_{i}}^{t_{i+1}} \left[v(t_{i}) + \dot{v}({t_{i}}) \right] \cos \left[ \theta(t_{i}) + \omega(t_{i})(t^{'} - t_{i}) + \frac{1}{2} \dot{\omega}(t_{i})(t^{'} - t_{i})^{2} \right] dt^{'} \end{eqnarray}

ここで、進行速度と角速度の変化率は t_{i} \le t \le t_{i+1}の間で0であるとすると、 \dot{v}(t_{i}) = 0, \dot{w}(t_{i}) = 0となり、

\begin{eqnarray} x(t_{n}) &\sim& x(t_{0}) + \sum_{i=0}^{n-1} \int_{t_{i}}^{t_{i+1}} v(t_{i}) \cos \left[ \theta(t_{i}) + \omega(t_{i})(t^{'} - t_{i}) \right] dt^{'} \\ &\equiv& x(t_{0}) + \sum_{i=0}^{n-1} F_{x}^{i}(t_{i+1}) \end{eqnarray}

F_{x}^{i}(t_{i+1})は積分可能であり、 w(t_{i}) \ne 0のとき、

\begin{eqnarray} F_{x}^{i}(t_{i+1}) &=& \int_{t_{i}}^{t_{i+1}} v(t_{i}) \cos \left[ \theta(t_{i}) + \omega(t_{i})(t^{'} - t_{i}) \right] dt^{'} \\ &=& \frac{v(t_{i})}{\omega(t_{i})} \left[ \sin(\theta(t_{i}) + \omega(t_{i})(t_{i+1} - t_{i})) - \sin \theta(t_{i})\right] \end{eqnarray}

w(t_{i}) = 0のとき、

\begin{eqnarray} F_{x}^{i}(t_{i+1}) &=& \int_{t_{i}}^{t_{i+1}} v(t_{i}) \cos \theta(t_{i}) dt^{'} \\ &=& v(t_{i}) (t_{i+1} - t_{i}) \cos \theta(t_{i}) \end{eqnarray}

同様の計算をすれば、

\begin{eqnarray} x(t_{n}) &\sim& y(t_{0}) + \sum_{i=0}^{n-1} F_{y}^{i}(t_{i+1}) \\ F_{y}^{i}(t_{i+1}) &=& \int_{t_{i}}^{t_{i+1}} v(t_{i}) \sin \left[ \theta(t_{i}) + \omega(t_{i})(t^{'} - t_{i}) \right] dt^{'} \\ &=& -\frac{v(t_{i})}{\omega(t_{i})} \left[ \cos(\theta(t_{i}) + \omega(t_{i})(t_{i+1} - t_{i})) - \cos \theta(t_{i})\right]\ \ \ \ \ (\omega(t_{i}) \ne 0) \\ &=& v(t_{i}) (t_{i+1} - t_{i}) \sin \theta(t_{i})\ \ \ \ \ (\omega(t_{i}) = 0) \end{eqnarray}