next up previous
Next: 逆運動学によるPI制御 Up: 2軸スカラ型マニピュレータのシミュレーション Previous: 目標軌道の生成

逆動力学によるフィードフォワード

次に逆動力学によるロボットのフィードフォワード制御を行います. まず,入力${\bf u}(t)$をつぎのように構成することにします.  
 \begin{displaymath}
{\bf u}(t) =
J({\bf q})\ddot{\bf q_r} + C({\bf q},\dot{\bf q})
+ D {\bf q} + G({\bf q})\end{displaymath} (12)
(12)を(10)に代入してみましょう. 次式を得ます.

\begin{displaymath}
\ddot{\bf q} = \ddot{\bf q_r} + J({\bf q})^{-1} {\bf d}\end{displaymath}

もしも${\bf d}(t)=0$,すなわち外乱がゼロならば

\begin{displaymath}
\ddot{\bf q} = \ddot{\bf q_r}\end{displaymath}

が成立します.この式を2回積分して整理すると,

\begin{displaymath}
{\bf q}(t) - {\bf q_r}(t)
=
\left( \dot{\bf q}(t_0) - \dot{\bf q_r}(t_0) \right) (t-t_0)
+ {\bf q}(t_0) - {\bf q_r}(t_0)\end{displaymath}

となるので,もしも外乱がゼロで 初期状態や速度が一致しているのなら, 完全に軌道の追従が出来ることになります.

それでは外乱が無い(d(t)=0)場合について シミュレーションを行ってみましょう. うまく行けば,追従誤差(${\bf q}(t)-{\bf q_r}(t)$)はゼロのはずです.

\includegraphics [width=7cm,clip]{manip2track_plot_q.ps}
\includegraphics [width=7cm,clip]{manip2track_plot_u.ps}
\includegraphics [width=7cm,clip]{manip2track_plot_e.ps}

左上:${\bf q}(t)$


右上:${\bf u}(t)$  
左下:${\bf q}(t)-{\bf q_r}(t)$  
   
逆運動学に基づく制御プログラムをつくった場合は,このように, フィードバック制御も外乱も加えないでうまく追従するかを まずは試してみるのが良いでしょう. プログラムは次節のPI制御の場合のプログラムを参照してください.



Kenichiro Nonaka
5/15/1998