冗長マニピュレータの制御を行う場合、どの関節をどの程度動かすかを決める必要がある。

ここでは、その逆運動学計算を行う際に、擬似逆行列を用いる方法について述べる。

 

マニピュレータの先端位置・姿勢ベクトルを$\boldsymbol{r}$、関節角度ベクトルを$\boldsymbol{\theta}$とすると、順運動学は(\ref{eq:FK})式となる。

\begin{equation} \label{eq:FK}\boldsymbol{r}=f(\boldsymbol{\theta})\tag{1}\end{equation}

なお、冗長マニピュレータであることから、ここでは$\boldsymbol{r}$は$m$次元ベクトル、$\boldsymbol{\theta}$は$n$次元ベクトル($m < n$)であるとする。

(\ref{eq:FK})式の両辺を時間微分すると(\ref{eq:FK_diff})式となる。

\begin{equation} \label{eq:FK_diff} \frac{d\boldsymbol{r}}{dt}=\boldsymbol{J}\frac{d\boldsymbol{\theta}}{dt} \tag{2} \end{equation}

または(\ref{eq:FK_diff})式の両辺に$dt$をかけて(\ref{eq:FK_diff2})式となる。

\begin{equation} \label{eq:FK_diff2} d\boldsymbol{r}=\boldsymbol{J}d\boldsymbol{\theta} \tag{3} \end{equation}

ここで$\boldsymbol{J}$はヤコビ行列($m\times n$)と呼ばれる。

 

さて、(\ref{eq:FK_diff2})式の両辺に左からヤコビ逆行列$\boldsymbol{J}^{-1}$をかけると、先端位置姿勢の変化量に対する関節角度の変化量が求まるが、冗長マニピュレータの場合はヤコビ行列$\boldsymbol{J}$が正方行列ではないため、逆行列を持たない(正則行列ではない。)。そこで擬似逆行列$\boldsymbol{J}^{+}$を導入する。

\begin{equation} \label{eq:pinvJ} \boldsymbol{J}^{+}=\boldsymbol{J}^{t}(\boldsymbol{J}\boldsymbol{J}^{t})^{-1} \tag{4} \end{equation}

(\ref{eq:FK_diff2})式に左から$\boldsymbol{J}^{+}$をかけると、先端位置姿勢の変化量に対する関節角度の変化量が(\ref{eq:IK})式のように求まる。

\begin{equation} \label{eq:IK} d\boldsymbol{\theta}=\boldsymbol{J}^{+}d\boldsymbol{r} \tag{5} \end{equation}

これを積分することで、逆運動学の数値解が得られる。