開発目的

モーター動作によるリンク機構の制御を応用した実用的なシステムを作成する。

システム概要

手の動作を認識しマニピュレータの先端が前にある物体の動きに追従してく。
figure1にシステム全体と動作のイメージを示す。

figure1

システム全体は3つのモーターと平行四節リンクで構成されている。
また、マニピュレータの先端には距離センサが3つ付けられている。

figure1はfigure2のように簡略化できる。
ここでモーター1,2とリンクの結合部を原点にとり右手座標系を採用している。

figure2

黒丸●はマニピュレータ先端を示す。
このうち $\theta_0$ はfigure1のモーター0で制御する。
これをY-Q平面で見たときの図をfigure3に示す。

figure3

$\theta_1$ はモータ1で、 $\theta_2$ はモータ2でそれぞれ制御する。
マニピュレータ先端P(x,y,z)に向けて逆運動学によって $\theta_0,\theta_1,\theta_2$ を決定する。
マニピュレータの腕の長さをどちらも $l$ とすると逆運動学より

$\theta_0=tan^{-1} \dfrac{z}{x}$

$\theta_1=tan^{-1} \dfrac{x}{\sqrt{x^2+z^2}} + cos^{-1} \dfrac{\sqrt{𝑥^2+y^2+𝑧^2}}{l}$

$\theta_2=tan^{-1} \dfrac{x}{\sqrt{x^2+z^2}} - cos^{-1} \dfrac{\sqrt{𝑥^2+y^2+𝑧^2}}{l}$

が求まる。

制御したい位置の座標P(x,y,z)から上の式を用いて $\theta_0,\theta_1,\theta_2$ を計算し、PID方式によって各モータの角度を制御する。

上で述べたように、マニピュレータ先端には3つの距離センサが取り付けられている。
その様子をfigure4に示す。

figure4

各センサは、目の前に物体があるとき、その物体との距離を返す。
検出可能な範囲内に物体がないときは非常に大きな値を返す。

この特性を利用して、目の前にある物体が移動した方向を以下のような原理で検出する。

例えば、物体がfigure5のように現在位置から右に移動したとする。

figure5

移動前、全てのセンサからは物体との距離が返されている。
移動後、センサ1,2からは物体との距離が返されるが、センサ0の前には物体が無いのでそのことを表す"非常に大きな値"がセンサから返される。
これを検知し、物体は「右に動いた」と判断する。
そして、物体の動いた方向にマニピュレータの先端も動かす。

次に物体とセンサが一定距離を保つようにする動作原理を説明する。
マニピュレータ先端に取り付けているのは距離センサなのでこれを利用する。

figure6

figure6に示す平面 $\alpha$ はマニピュレータ先端の距離センサで構成される平面である。
物体が平面 $\alpha$ に近づくと距離センサが返す値は小さくなるので「近づいた」と判断し、物体から離れるために物体の動きと同じ方向にマニピュレータ先端を移動させる。

同様にして、センサから遠ざかるように動いたときは平面 $\alpha$ から距離が離れ、センサの値は大きくなるので、マニピュレータ先端は物体を追うように動く。

このふたつの仕組みによって、距離センサで物体の動きを追跡することが可能になる。

センサが認識した物体の位置と、現在のマニピュレーターの状態とを、OpenGLを用いて可視化できるようにした。
figure7にその様子を示す。

figure7