(57)【要約】      （修正有）【課題】液体、粉体等を容器内に収容し、容器の開口からこぼさずに、効率良く搬送することができるロボット装置の提供。【解決手段】ロボットアーム先端に装着されたハンドで容器を把持し、容器内に収容された液体、粉体等を搬送する。容器の姿勢は、搬送開始時には重力加速度ベクトルと同方向（真下）を向いているが、加速過程で加速度が増加するにつれて慣性加速度ベクトルが徐々に大きくなり、加速度の向きと逆方向に傾斜が増し、加速度が減少に転ずると慣性加速度ベクトルが徐々に小さくなり、その傾斜は減少する。加速過程が完了して速度一定となると傾斜は解消される。減速過程では、減速度が増すにつれて、加速過程とは逆方向に傾斜が増し、減速度が減少に転ずるとその傾斜は減少し、ロボットが停止すると傾斜は解消され、搬送開始時の真下方向を向く姿勢に戻る。【選択図】            図４
2005001055.tif 000002
【特許請求の範囲】
【請求項１】
容器に入れられた被搬送物を、搬送開始位置から搬送終了位置まで、前記被搬送物が前記容器からこぼれないように搬送するロボットと、前記ロボットを制御する制御装置を備えたロボット装置において、
前記搬送中の被搬送物にかかる慣性加速度ベクトルを所定周期毎に演算する手段と、
前記慣性加速度ベクトルと重力加速度ベクトルとを合成し、合成加速度ベクトルを求める手段と、
該求めた合成加速度ベクトルに基き、前記搬送中の前記ロボットの適正搬送姿勢を求める手段と、
該求められた適正搬送姿勢に基づいて前記ロボットの姿勢を制御する手段とを備えることを特徴とする、ロボット装置。
【請求項２】
容器に入れられた被搬送物を、搬送開始位置から搬送終了位置まで、前記被搬送物が前記容器からこぼれないように搬送するロボットと、前記ロボットを制御する制御装置を備えたロボット装置において、
前記搬送中の被搬送物にかかる慣性加速度ベクトルを所定周期毎に演算する手段と、
前記慣性加速度ベクトルと重力加速度ベクトルとを合成し、合成加速度ベクトルを求める手段と、
前記合成加速度ベクトルの方向と前記重力加速度ベクトルの方向の差異を求める手段と、
前記搬送開始位置における前記ロボットの姿勢を基準にして、前記差異に対応する姿勢変更を施した前記ロボットの適正搬送姿勢を求める手段と、
該求められた適正搬送姿勢に基づいて前記ロボットの姿勢を制御する手段とを備えることを特徴とする、ロボット装置。
【請求項３】
容器に入れられた被搬送物を、搬送開始位置から搬送終了位置まで、前記被搬送物が前記容器からこぼれないように搬送するロボットと、前記ロボットを制御する制御装置を備えたロボット装置において、
前記搬送中の被搬送物にかかる慣性加速度ベクトルを所定周期毎に演算する手段と、
前記慣性加速度ベクトルと重力加速度ベクトルとを合成し、合成加速度ベクトルを求める手段と、
合成加速度ベクトルの初期値を重力加速度ベクトルとして、前回の周期における合成加速度ベクトルの方向と、今回の周期で求めた合成加速度ベクトルの方向の差異に対応する姿勢変更量を求める手段と、
前記ロボットの搬送姿勢の初期値を前記搬送開始位置における前記ロボットの適正搬送姿勢として、前回の周期で求められた適性搬送姿勢を前記姿勢変更量だけ変更して今回の周期における適正搬送姿勢を求める手段と、
該求められた適正搬送姿勢に基づいて前記ロボットの姿勢を制御する手段とを備えることを特徴とする、ロボット装置。
【請求項４】
指令プログラムにより与えられた、搬送開始位置、搬送終了位置、及び速度指令と、所定の加減速処理とに基き、所定周期毎の慣性加速度ベクトルを求めることを特徴とする、請求項１乃至請求項３の内、何れか１項に記載のロボット装置。
【請求項５】
搬送中の慣性加速度ベクトルを演算する手段に代えて、慣性加速度ベクトルを計測する手段を前記容器又はその近傍に設けたことを特徴とする、請求項１乃至請求項３の内、何れか１項に記載のロボット装置。
【請求項６】
指令プログラムの指令により、前記適正搬送姿勢に基づく前記ロボットの姿勢制御を行なわないモードが選択され得ることを特徴とする、請求項１乃至請求項５の内、何れか１項に記載のロボット装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボット（運動機構を備えた本体部）とそれを制御する制御装置を備えたロボット装置に関し、更に詳しく言えば、液体、粉体、または小サイズの成形品（例えば小サイズのネジ）等を容器（バケット等）に収容した状態で搬送するロボット装置に関する。なお、本明細書において、「容器」とは、例えばバケット等のように開口部を持つ容器を指し、密封容器は除くものとする。
【０００２】
【従来の技術】
液体、粉体、又は極めて小サイズの物体の集合などを容器に収容した状態でロボットにより搬送する場合、ロボットの移動中に加速度により液面、粉体の表面あるいは小サイズ物体の最上面が揺れ、容器からこぼれ出る場合がある。従来は、これを防ぐ為に容器の最大容量よりかなり少ない量の液体、粉体あるいは小サイズ物体しか搬送出来ず、運搬効率が低かった。また、移動時の速度及び加速度についてもこれら被搬送物が容器からこぼれ出ないように低くする必要があり、サイクルタイムも長くなっていた。
【０００３】
このような欠点を回避する手法として、容器の傾きについて１自由度の変化を許容する取っ手を容器に取り付け、ロボットはこの取っ手を介して容器を支持して搬送を行なう方法があるが、容器の姿勢変化が１自由度のため、特定の移動方向の搬送時のみにしか有効でない。また、ロボットが停止時等に容器が揺動する場合があり、速度、加速度を液体等がこぼれない様低く設定する必要があった。
【０００４】
また、２自由度の取っ手を取り付けることも考えられるが、その場合でも、加加速度（加速度の変化率）が大きい場合には、液体等がこぼれる可能性が大きい。
【０００５】
更に、本発明に関連性を持つ先行技術として下記特許文献１がある。これは、長軸のパイプなどを把持するために、移動方向に対して非対称な把持力を持つハンドを採用し、ワークにかかる慣性力のみを考慮して、加速度に対して大きな把持力を持つ方向にハンドの姿勢を制御するものである。この先行技術は、特定の非対称な把持力を持つハンドに依存した発明であり、慣性力は考慮しているが重力を考慮しておらず、また液体、粉体などの運搬には適用困難である。
【０００６】
【特許文献】
特開平９−３００２５５号公報
【０００７】
【発明が解決しようとする課題】
上記したように、ロボットを利用して、液体、粉体、又は極めて小サイズの物体の集合など、流動性乃至それに準じた性質を持つ被搬送物を容器に収容した状態でこぼさずに効率良く搬送する技術の開発は十分に進んでいるとは言えない。そこで、本発明の目的は、ロボットに支持される容器に収容される被搬送物が、液体、粉体、又は小サイズの物体の集合などのように流動性乃至それに準じた性質を持っていても、同容器からこぼさずに効率良く搬送することが出来るロボット装置を提供することにある。
【０００８】
【課題を解決するための手段】
本発明は、被搬送物を容器に収容した状態で搬送する際に、同容器乃至ロボットの姿勢を、被搬送物にかかる重カ加速ベクトルと加速する方向の反対方向にかかる慣性加速ベクトルを考慮して重力加速ベクトルと慣性加速ベクトルの合成ベクトルを算出し、この合成加速度ベクトルの方向を指標に用いて、適正な搬送姿勢（以下、「適正搬送姿勢」とも言う）を求め、該適正搬送姿勢が実現されるように、ロボットの姿勢を制御することで、上記技術課題を解決したものである。
【０００９】
ここで、「適正搬送姿勢」とは、被搬送物を容器からこぼさないという観点で、上記合成加速度ベクトルの方向に基づいて算出されるロボット姿勢のことである。但し、この「ロボット姿勢」は、被搬送物を収容した「容器」の姿勢、あるいは、同容器を把持するロボットハンドの姿勢（ロボットハンドで容器を把持した場合）と１対１対応関係にある。従って、ロボットの適正搬送姿勢を求めることと、容器あるいはハンドについて適正搬送姿勢を求めることは実質的に同等である。
【００１０】
本発明は、容器に入れられた被搬送物を、搬送開始位置から搬送終了位置まで、前記被搬送物が前記容器からこぼれないように搬送するロボットと、前記ロボットを制御する制御装置を備えたロボット装置に適用される。本発明の基本的な特徴に従い、同ロボット装置は、前記搬送中の被搬送物にかかる慣性加速度ベクトルを所定周期毎に演算する手段と、前記慣性加速度ベクトルと重力加速度ベクトルとを合成し、合成加速度ベクトルを求める手段と、該求めた合成加速度ベクトルに基き、前記搬送中の前記ロボットの適正搬送姿勢を求める手段と、該求められた適正搬送姿勢に基づいて前記ロボットの姿勢を制御する手段とを備えている。
【００１１】
ロボット姿勢は、前記合成加速度ベクトルの方向の変化に追従して変化するように制御される。そのために、前記合成加速度ベクトルの方向と前記重力加速度ベクトルの方向の差異を求める手段と、前記搬送開始位置における前記ロボットの姿勢を基準にして、前記差異に対応した量だけ姿勢変更した前記ロボットの適正搬送姿勢を求める手段がロボット装置に設けられる。
【００１２】
また、合成加速度ベクトルの初期値を重力加速度ベクトルとして、前回の周期における合成加速度ベクトルの方向と、今回の周期で求めた合成加速度ベクトルの方向の差異に対応する前記ロボットの姿勢変更量を求める手段と、前記ロボットの搬送姿勢の初期値を前記搬送開始位置における前記ロボットの適正搬送姿勢として、前回の周期で求められた適性搬送姿勢を前記姿勢変更量だけ変更して今回の周期における適正搬送姿勢を求める手段を設け、該求められた適正搬送姿勢に基づいて前記ロボットの姿勢を制御するようにしても良い。
【００１３】
所定周期毎の慣性加速度ベクトルの算出は、指令プログラムにより与えられた、搬送開始位置、搬送終了位置、及び速度指令と、所定の加減速処理とに基いて行なうことが出来る。あるいは、搬送中の慣性加速度ベクトルを、前記容器又はその近傍に設けられた慣性加速度ベクトルの計測手段から得るようにしても良い。
なお、例えば空の容器を搬送する場合、被搬送物がこぼれる心配がない場合などに対応するために、指令プログラムの指令により、前記適正搬送姿勢に基づく前記ロボットの姿勢制御を行なわないモードを選択できるようにすることが好ましい。
【００１４】
【発明の実施の形態】
図１は、本発明の実施形態に係るロボット装置による搬送の様子を示した概要配置図である。符号１は搬送を行なうロボット（本体機構部）で、アーム先端に容器３を把持するロボットハンド（以下、単に「ハンド」とも言う）２が装着されている。ロボット１は、ロボット制御装置５に接続され、このロボット制御装置５によってその動作が制御される。容器３は、液体、粉体、あるいは小サイズ物体の集合など、流動性乃至それに準じた性質を持つ被搬送物を収容するためのもので、ここではバケットが用いられている。
【００１５】
容器３には被搬送物の出し入れのための開口４が設けられており、図１に示した状態では、この開口４は真上に向けれらている。以下、このような容器の姿勢（空間内における姿勢）を、「（容器の）基準姿勢」と言うことにする。また、この容器３に基準姿勢をとらせるハンド２の姿勢（即ち、ロボット１の姿勢）を「（ロボットの）基準姿勢」と言うことにする。なお、一般のケースでは、「容器を支持したロボットを静止させた状態で、流動性を持つ被搬送物がこぼれ難い容器の姿勢」を「容器の基準姿勢」と考えれば良い。
【００１６】
そして、空間内での容器３の姿勢を代表するベクトルとして、容器３が静止状態で基準姿勢にある時に鉛直下向き方向を向く単位ベクトルを考え、これをベクトルＨとする。ベクトルＨは、容器３乃至ハンド２上に固定されたベクトルで容器３乃至ハンド２が傾けば当然これに追随して傾く。
【００１７】
さて、図１内に示した矢印Ｓは搬送方向に一例を示し、ロボット１が動作してハンド２が矢印Ｓの方向に移動すれば、当然、容器３に入れられた被搬送物は矢印Ｓの方向に搬送される。このような搬送を行なう場合、ロボット１は、搬送開始位置から始動後しばらくは加速され、指令搬終了位置の手前では減速されることになる。これに伴い、容器３内の被搬送物が不安定になり、例えば被搬送物が液体の場合、液面６が搖れて容器３の開口４からこぼれ出る現象が発生する。
【００１８】
既述のように、本発明では、この現象を、搬送中の容器の姿勢制御（ベクトルＨの空間内での方向制御）、換言すれば、容器を把持するロボットハンドの姿勢制御（より一般化して言えば、容器を支持するロボットの姿勢制御）によって回避する。この姿勢制御は、ベクトルＨの方向が「被搬送物が受けるトータルの加速度」の方向の変化に追随して変化するように行なう。
【００１９】
ロボット１の静止時及び定方向定速度移動時には、容器３内の被搬送物には、重力Ｍｇ  が真下方向に作用している。この重力Ｍｇ  に対応する加速度ベクトルを重力加速度ベクトルＫａ  で表わすことにする。一方、加減速中（加速中または減速中；以下、同し）には、これに慣性力が加わる。図２はその様子を示したものである。図２に示したように、慣性力は、加速度を表わすベクトルａと質量Ｍの積の大きさを持ち、向きはベクトルａと逆となっている。この慣性力を−Ｍａ  で表わし、この慣性力に対応する加速度を慣性加速度ベクトルＫａ  で表わすことにする。そして、これら重力Ｍｇ  と慣性力−Ｍａ  を合成した力をＭｈ  で表わし、この合成力に対応する加速度を合成加速度ベクトルＫｈ  で表わすことにする。
【００２０】
ここで、重力加速度ベクトルＫｇ  は既知の重力定数と設定座標系のデータ（例えばワールド座標系の−Ｚ軸方向）から定めることが出来る。また、慣性加速度は、後述するように、ロボットが持つデータから演算によって求めることが出来るが、加速度センサを利用して取得することも出来る。その場合、加速度センサは、符号８で示したように容器３上に設置するか、符号９で示したようにハンド２など容器３の近傍に設置し、ロボット制御装置５により計算周期毎に検出信号を送るようにすれば良い。
【００２１】
さて、慣性力−Ｍａ  が液体、粉体等に作用すると、慣性加速度ベクトルＫａ  の方向に偏って分布しようとして容器３内で移動を起こし、不安定化する。従って、もしも容器３の姿勢（即ち、ハンド２の姿勢乃至ロボット１の姿勢；以下、同様）を基準姿勢のまま（図１参照）に保ったままであったとすれば、液体、粉体等の被搬送物が開口４からこぼれ出易くなる。
【００２２】
そこで、本発明では、容器３の姿勢を合成加速度ベクトルＫｈ  の方向の変化に追従するように変化させて、被搬送物が容器３からこぼれ出ることを防止する。本実施形態では、図３に示したように、容器３（乃至ハンド２）の姿勢を表わすベクトルＨが重力加速度ベクトルＫｇ  の方向（即ち、静止時の合成加速度ベクトルＫｈ  の方向）から角度Θだけ傾斜して、その時点における合成加速度ベクトルＫｈ  の方向（合成力Ｍｈ  の作用する方向）と一致するように、ロボット１を制御する。
【００２３】
言い換えれば、角度Θは、搬送中の合成加速度ベクトルＫｈ  が重力加速度Ｍｇ  の方向（即ち、静止時の合成加速度ベクトルＫｈ  の方向）となす角度（即ち、慣性加速度ベクトルＫａ  によって生じた合成加速度ベクトルＫｈ  の方向変化）を表わす角度である。
【００２４】
このように、合成加速度ベクトルＫｈ  の方向に容器３乃至ハンド２の姿勢を制御することで、搬送中も常に容器３の底面に対して鉛直方向に合成加速度（合成力）が作用することになり、液体、流体等をこぼさずに搬送することが出来る。図４、図５は、典型的な速度と加速度の推移をもってロボットが移動するケースについて、容器３乃至ハンド２の姿勢の推移を説明する図で、図４は、搬送開始直後の加速からプログラム速度（指令速度）での直線移動に至る過程における容器乃至ハンドの姿勢推移を表わし、図５は、プログラム速度（指令速度）での直線移動から減速して搬送終了に至る過程における容器乃至ハンドの姿勢推移を表わしている。両図において、各姿勢毎に記された矢印は、前述のベクトルＨの方向を表わしている。
【００２５】
これらの図から判るように、搬送開始時にはベクトルＨは真下（重力加速度ベクトルと同方向）を向いているが、加速過程で加速度が増加するにつれて慣性加速度ベクトルが徐々に大きくなり、加速度の向きと逆方向（慣性加速度ベクトルと同じ向き）に傾斜が増し、加速度が減少に転ずると慣性加速度ベクトルが徐々に小さくなり、その傾斜は減少する。そして、加速過程が完了して速度一定（プログラム速度での直線移動）に入ると傾斜は解消され、ベクトルＨは搬送開始時の真下方向を維持する。
【００２６】
次いで、減速過程で減速度（速度を減らす方向の加速度）が増すにつれて慣性加速度ベクトル（加速過程とは向きは逆）が徐々に大きくなり、加速過程とは逆方向に傾斜が増し、減速度が減少に転ずると慣性加速度ベクトル（加速過程とは向きは逆）が徐々に小さくなり、その傾斜は減少する。そして、減速過程が完了してロボット１が停止すると傾斜は解消され、ベクトルＨは搬送開始時の真下方向に戻る。
【００２７】
次に、上記のような姿勢制御を実行する具体的な手順について、一例を説明する。なお、容器の姿勢制御はロボット１の姿勢制御を通して行なわれ、本実施形態では、容器３はハンド２によって把持されており、ロボット１の姿勢制御を行なうことはハンド２の姿勢制御を行なうことに他ならないので、以下、「ロボット１の姿勢制御」を「ハンド２の姿勢制御」として説明する。
【００２８】
前述したように、ロボット１はロボット制御装置５に接続されており（図１参照）、上述した態様のハンド２の姿勢制御がロボット制御装置５によって行なわれる。ロボット制御装置５のハードウェア構成は従来のロボット制御装置と同一であって良く、ソフトウェア構成についても、後記する処理を実行のためのソフトウェアを除けば、特に従来と変わるところはない。
【００２９】
ロボット制御装置の一般構成については、周知なので、図６を参照して概要のみ極く簡単に説明する。図６に示したように、ロボット制御装置５は、プロセッサ５１、ＲＯＭ、ＲＡＭ等からなるメモり５２、表示装置５３、教示操作盤のキー等の入力手段５４、ロボットの各関節軸を駆動するサーボモータを駆動制御するサーボ制御手段５５、ロボットの周辺機器と接続される入出力回路５６等で構成されている。
【００３０】
プロセッサ５１は、メモリ５２に格納された教示プログラムを実行し、サーボ制御手段５５に移動指令を出力する。サーボ制御手段５５は、この移動指令と各軸のサーボモータに取り付けられた位置、速度検出器からの位置、速度フィードバック信号に基づいて、各軸の位置、速度のフィードバック制御を行う。その結果、ロボット１（図１参照）のアーム先端に装着されたハンド２の把持点（ＴＣＰ；ツール先端点）の位置、姿勢、速度が制御される。
【００３１】
ここで、本実施形態では、前述した態様の姿勢制御（慣性加速度ベクトルと重カベクトルを合成した合成加速度ベクトルを追随指標に用いた姿勢制御）を実現するために、移動指令の作成・出力に際して図７のフローチャートに示した処理を実行する。
【００３２】
なお、搬送動作開始直前には、容器３は被搬送物（例えば液体）を収容した状態でハンド２に把持されており、その姿勢は基準姿勢（前述したベクトルＨが真下を向く姿勢）にあるものとする。この状態から、ロボット制御装置５内で教示プログラムのある行（搬送開始位置から搬送終了位置までの搬送を命令する行）が読み込まれ、現在位置よりプログラム実行行に記載の終点までの動作を行なうための処理が開始される。各ステップの要点は下記の通りである。
【００３３】
ステップＡ１：先ず、動作の始点、終点の位置およぴプログラム速度（指令速度）より、動作当りの移動量Ｌｉ  と移動時間Ｔｍ  を算出する。
【００３４】
ステップＡ２：計算周期の回数を表わす指標ｎと時刻指標Ｔｎ  を、それぞれｎ＝１、Ｔｎ  ＝０に初期化する。
ステップＡ３：時刻Ｔｎ  から時刻Ｔｎ＋１  までの移動量△Ｌｔ＝ｎ  を算出し、その値をメモリに格納する。
【００３５】
ステップＡ４：ハンド姿勢制御が有効かどうかを確認し、有効ならば本発明のハンド姿勢制御を行うために、ステップＡ５以下へ進む。もし、無効ならばステップＡ１３へ進む。ハンド姿勢制御が有効かどうかの確認は、例えばプログラムの指令によって指定されるモードフラグ値（１ｏｒ０）のチェックによって行なうことが出来る。また、入力手段５４（図６参照）によりモードフラグ値設定を行ない、これをチェックすることで判別しても良い。
【００３６】
ステップＡ５：移動量△Ｌｔ＝ｎ  及び△Ｌｔ＝ｎ＋１  の差分より、速度△Ｖｔ＝ｎ  を算出する。また、速度△Ｖｔ＝ｎ  と速度△Ｖｔ＝ｎ＋１  の差分より、加速度△Ａｔ＝ｎ  を算出する。なお、初期値は、△Ｌｔ＝１＝０  、△Ａｔ＝１＝０  である。
ステップＡ６：加速度△Ａｔ＝ｎ  より慣性加速度ベクトルＫａ  を算出する。また、容器に作用する重力加速度より重力加速度ベクトルＫｇ  を算出する。なお、重力加速度の大きさのデータは、予めメモリに設定しておく。
【００３７】
ステップＡ７：慣性加速度ベクトルＫａ  と重力加速度ベクトルＫｇ  の合成加速度ベクトルＫｈ  を算出する。
ステップＡ８：合成加速度ベクトルＫｈ  の方向を向くハンド姿勢（適正姿勢）Ｗｈ  、Ｐｈ  、Ｒｈ  を算出する。
ステップＡ９：時刻Ｔｎ  での位置Ｘ、Ｙ、Ｚ、同じく時刻Ｔｎ  でのハンド姿勢Ｗ、Ｐ、Ｒ、及び、ロボットの幾何学式（例えばＤ−Ｈパラメータ）等より、各モータの移動量△Ｍを算出する。
【００３８】
ステップＡ１０：各モータの移動量△Ｍを各モータに指令し、モータを動作させる。
ステップＡ１１：△Ｔだけ待機し、Ｔｎ  ＝Ｔｎ  ＋△Ｔとする。
【００３９】
ステップＡ１２：Ｔｎ  ≧Ｔｍ  かどうかチェックし、イエスであれば、処理を終了する。ノーであれば、ステップＡ３へ戻り、ステップＡ３〜ステップＡ１２を繰り返す。
【００４０】
ステップＡ１３：ステップＡ４でハンド姿勢制御が有効でないと判断された場合には、本発明のハンド姿勢制御を行わず、通常の補間処理に従い、時刻Ｔｎ  での位置、ハンド姿勢Ｗ、Ｐ、Ｒ及びロボットの幾何学式（例えばＤ−Ｈパラメータ）等より、各モータの移動量△Ｍを算出して、ステップＡ１０以下へ進む。
【００４１】
以後は、ステップＡ１０→ステップＡ１１の後、ステップＡ１２→ステップＡ３→ステップＡ４→ステップＡ１３→ステップＡ１０→ステップＡ１１→ステップＡ１２のサイクルが所要回数（ステップＡ１２でイエスが出るまで）繰り返されることになる。なお、このサイクルでロボットを移動させるケースとしては、容器３が空の場合、あるいは、容器３に収容される被搬送物が僅かな量しかない場合などが考えられる。
【００４２】
さて、図７のフローチャートによる処理におけるステップＡ８では、合成加速度ベクトルＫｈ  の方向を向くハンド姿勢Ｗｈ  、Ｐｈ  、Ｒｈ  自体（時刻Ｔｎ  における適正姿勢自体）を算出し、続くステップＡ９では、この適正姿勢を実現するためのモータ移動量ΔＭを、時刻Ｔｎ  での位置Ｘ、Ｙ、Ｚ、時刻Ｔｎ  でのハンド姿勢Ｗ、Ｐ、Ｒ、及び、ロボットの幾何学式より求めている。
【００４３】
しかし、ステップＡ８において計算される適正姿勢（合成加速度ベクトルＫｈ  の方向を向く姿勢）自体に代えて、「適正姿勢を実現するために必要なＷ、Ｐ、Ｒの変位量δＷ、δＰ、δＲ」を計算しても良い。このステップをステップＢ８とする。ここで、変位量計算の起点となる姿勢（δＷ＝δＰ＝δＲ＝０に対応する姿勢）としては、搬送開始時（Ｔｎ  ＝０）におけるハンド姿勢（ハンドの基準姿勢）を採用することが出来る。あるいは、ステップＡ１３で計算される「合成加速度ベクトルを考慮しない場合に計算される姿勢」をδＷ＝δＰ＝δＲ＝０に対応する姿勢として採用しても良い。このようなδＷ＝δＰ＝δＲ＝０に対応する姿勢を便宜的にＷ１  、Ｐ１  、Ｒ１  で表わすことにする。
【００４４】
そして、ステップＢ８に続くステップＢ９として、この姿勢Ｗ１  、Ｐ１  、Ｒ１  に上記変位量δＷ、δＰ、δＲをそれぞれ加算した姿勢Ｗ’、Ｐ’、Ｒ’を計算するステップを採用することが出来る。これらステップＢ８、Ｂ９をステップＡ８、Ａ９に代えて採用した場合の処理の概要を図８のフローチャートに示した。同フローチャートにおいて、ステップＢ１〜ステップＢ７及びステップＢ１０〜ステップＢ１３は、それぞれ上述のステップＡ１〜ステップＡ７及びステップＡ１０〜ステップＡ１３と同様であるから繰り返し説明は省略する。ステップＢ８及びステップＢ９の要点は上述した通りである。
【００４５】
更に、適正姿勢の計算を含めて計算周期毎のインクリメンタル量で計算して行く処理方式を採用することも出来る。その場合の処理の概要を図９のフローチャートに示す。各ステップの要点は下記の通りである。
【００４６】
ステップＣ１：動作の始点、終点の位置およぴプログラム速度（指令速度）より、動作当りの移動量Ｌｉ  と移動時間Ｔｍ  を算出する。
ステップＣ２：計算周期の回数を表わす指標ｎと時刻指標Ｔｎ  を、それぞれｎ＝１、Ｔｎ  ＝０に初期化する。
【００４７】
ステップＣ３：時刻Ｔｎ  から時刻Ｔｎ＋１  までの移動量△Ｌｔ＝ｎ  を算出し、その値をメモリに格納する。
ステップＣ４：ハンド姿勢制御が有効かどうかを確認し、有効ならば本発明のハンド姿勢制御を行うために、ステップＣ５以下へ進む。もし、無効ならばステップＣ１４へ進む。
【００４８】
ステップＣ５：移動量△Ｌｔ＝ｎ  及び△Ｌｔ＝ｎ＋１  の差分より、速度△Ｖｔ＝ｎ  を算出する。また、速度△Ｖｔ＝ｎ  と速度△Ｖｔ＝ｎ＋１  の差分より、加速度△Ａｔ＝ｎ  を算出する。なお、初期値は、△Ｌｔ＝１＝０  、△Ａｔ＝１＝０  である。
【００４９】
ステップＣ６：加速度△Ａｔ＝ｎ  より慣性加速度ベクトルＫａｎを算出する。また、容器に作用する重力加速度より重力加速度ベクトルＫｇｎを算出する。なお、重力加速度ベクトルは実質的に不変で毎回の計算で設定値を用いるが、便宜的にＭｇｎと表記する。
【００５０】
ステップＣ７：慣性加速度ベクトルＫａｎと重力加速度ベクトルＫｇｎの合成加速度ベクトルＫｈｎを算出する。
【００５１】
ステップＣ８：指標ｎがｎ＝１（搬送開始時に対応）であれば、ステップＣ１５へ進み、そうでなければステップＣ９へ進む。
【００５２】
ステップＣ９：適正姿勢（合成加速度ベクトルＫｈｎの方向を向く姿勢）を実現するために必要なＷ、Ｐ、Ｒの変位量ΔＷ、ΔＰ、ΔＲを計算する。ここで、変位量計算の起点となる姿勢（ΔＷ＝ΔＰ＝ΔＲ＝０に対応する姿勢）としては、前回の計算周期で計算されたハンド姿勢Ｗｎ−１  、Ｐｎ−１  、Ｒｎ−１  を採用する。
【００５３】
ステップＣ１０：ハンド姿勢Ｗｎ  ＝Ｗｎ−１  ＋ΔＷ、Ｐｎ  ＝Ｐｎ−１  ＋ΔＰ、Ｒｎ  ＝Ｒｎ−１  ＋ΔＲを計算し、これと時刻Ｔｎ  での位置Ｘ、Ｙ、Ｚ、及び、ロボットの幾何学式等より、各モータの移動量△Ｍを算出する。
【００５４】
ステップＣ１１：各モータの移動量△Ｍを各モータに指令し、モータを動作させる。
【００５５】
ステップＣ１２：△Ｔだけ待機し、Ｔｎ  ＝Ｔｎ  ＋△Ｔとする。
【００５６】
ステップＡ１３：Ｔｎ  ≧Ｔｍ  かどうかチェックし、イエスであれば、処理を終了する。ノーであれば、ステップＣ３へ戻り、ステップＣ３〜ステップＣ１３を繰り返す。
【００５７】
ステップＣ１４：ステップＣ４でハンド姿勢制御が有効でないと判断された場合には、本発明のハンド姿勢制御を行わず、通常の補間処理に従い、時刻Ｔｎ  での位置、ハンド姿勢Ｗ、Ｐ、Ｒ及びロボットの幾何学式（例えばＤ−Ｈパラメータ）等より、各モータの移動量△Ｍを算出して、ステップＣ１１以下へ進む。
【００５８】
以後は、ステップＣ１１１→ステップＣ１２の後、ステップＣ１３→ステップＣ３→ステップＣ４→ステップＡ１４→ステップＣ１１→ステップＣ１２→ステップＣ１３のサイクルが所要回数（ステップＣ１３でイエスが出るまで）繰り返されることになる。
【００５９】
ステップＣ１５：ステップＣ８でｎ＝１（搬送開始時に対応）と判断された場合には、合成加速度ベクトルクトルＭｈｎの所期値として、重力加速度ベクトルＫｇｎを採用する。
【００６０】
なお、以上説明した実施形態では、慣性加速度を加速度△Ａｔ＝ｎ  より算出しているが（ステップＡ６、Ｂ６又はＣ６参照）、慣性加速度のデータを加速度センサを利用して取得することも出来る。その場合、加速度センサは、容器３上に接地するか（図１中の符号８を参照）、ハンド２など容器３の近傍（図１中の符号９を参照）に設置し、ロボット制御装置５により計算周期毎に検出信号を取り込み、それに基づいて慣性加速度ベクトルを求めれば良い。
【００６１】
【発明の効果】
本発明によれば、容器に収容されてロボットで搬送される被搬送物が、液体、粉体、小サイズの物体等のように、流動性あるいはそれに準ずる性質を示すものであっても、搬送中に容器から被搬送物がこぼれ出ることが防止されるように容器の姿勢が制御されるため、容器のほぼ最大容量まで液体、粉体等を収容して搬送を行なっても支障を来さないので容器当たりの運搬効率が向上する。また、搬送時のロボットの速度、加速度を大きくする事が可能となり、短いサイクルタイムで液体、粉体、小サイズの物体等を搬送することが出来るようになる。
【図面の簡単な説明】
【図１】本発明の実施形態に係るロボット装置による搬送の様子を示した概要配置図である。
【図２】搬送中に容器内の被搬送物が受ける加速度について説明する図である。
【図３】搬送中に行なわれる姿勢制御について説明する図である。
【図４】搬送開始直後の加速からプログラム速度（指令速度）での直線移動に至る過程における容器乃至ハンドの姿勢推移について説明する図である。
【図５】減速開始からロボット停止に至る過程における容器乃至ハンドの姿勢推移について説明する図である。
【図６】実施形態で用いるロボット制御装置の構成の概要を説明するブロック図である。
【図７】実施形態で実行される処理の一例の概要を説明するフローチャートである。
【図８】実施形態で実行される処理のもう１つの例の概要を説明するフローチャートである。
【図９】実施形態で実行される処理の更にもう１つの例の概要を説明するフローチャートである。
【符号の説明】
１  ロボット（本体機構部）
２  ロボットハンド
３  容器
４  開口
５  ロボット制御装置
６  液面
８、９  加速度センサ
５１  プロセッサ
５２  メモリ
５３  表示装置
５４  入力手段
５５  サーボ制御手段
５６  入出力回路
Ｓ    搬送方向（ロボットの移動方向）
【図１】
2005001055.tif 000003
【図２】
2005001055.tif 000004
【図３】
2005001055.tif 000005
【図４】
2005001055.tif 000006
【図５】
2005001055.tif 000007
【図６】
2005001055.tif 000008
【図７】
2005001055.tif 000009
【図８】
2005001055.tif 000010
【図９】
2005001055.tif 000011