(57)【特許請求の範囲】
【請求項１】
肩に肩軸交点Ｏで交わる３個の回転軸、肘Ｍに１個の回転軸、および手先に手先軸交点Ｗで交わる３個の回転軸があって、肩から肘の間に所定の長さｄｕを有する上腕と肘から手先の間に所定の長さｄｆを有する前腕を備え、上腕と前腕を含む腕平面Ｑが肩軸交点Ｏと手先軸交点Ｗを結ぶ直線Ｌと平行であって、肩と肘の４軸で手先の位置を実現しさらに手先の３軸で姿勢を実現するマスタアームにおいて、前記手先軸交点Ｗの位置と前記腕平面Ｑの前記直線Ｌの周りの回転角すなわち肘角φとに基づいて前記肘Ｍの位置を算定することを特徴とするマスタアームのリンク位置検出方法。
【請求項２】
前記マスタアームがさらに、前記上腕の付け根となる回転軸と前記肩軸交点Ｏの間の肩オフセットリンクが前記前腕の先端にある回転軸と前記手先軸交点Ｗの間の手先オフセットリンクと平行で両者が等しいリンク長ｄｏを有し、前記腕平面Ｑが前記直線Ｌと平行であることを特徴とし、前記肘Ｍの位置を手先軸交点Ｗの座標値（ｘｗ，ｙｗ，ｚｗ）と上腕長ｄｕと前腕長ｄｆと肘角φとから求めることを特徴とする請求項２記載のマスタアームのリンク位置検出方法。
【請求項３】
肘の位置に冗長自由度を有し手先を掴んでロボットハンドの位置制御をするためのマスタアームであって、肘を挟んだ腕を含む面の肩と手先を結ぶ直線の周りの所定の基準面に対する回転角すなわち肘角を指示する肘角指令値と、手先位置を指示する手先位置指令値を入力する入力装置と、入力した前記肘角指令値と手先位置指令値を用いて幾何学的演算によりマスタアームの肘の位置を算出する演算装置を備え、さらに、肩に肩軸交点Ｏで交わる３個の回転軸、肘Ｍに１個の回転軸、および手先に手先軸交点Ｗで交わる３個の回転軸があって、肩から肘の間に所定の長さｄｕを有する上腕と肘から手先の間に所定の長さｄｆを有する前腕を備え、上腕と前腕を含む腕平面Ｑが肩の軸交点Ｏと手先の軸交点Ｗを結ぶ直線Ｌと平行であって、前記上腕の付け根となる回転軸と前記肩軸交点Ｏの間の肩オフセットリンクが前記前腕の先端にある回転軸と前記手先軸交点Ｗの間の手先オフセットリンクと平行で両者が等しいリンク長ｄｏを有し、前記腕平面Ｑが前記直線Ｌと平行であって、肩と肘の４軸で手先の位置を実現しさらに手先の３軸で姿勢を実現するものであることを特徴とし、前記演算装置が、前記手先軸交点Ｗの座標値（ｘｗ，ｙｗ，ｚｗ）と上腕長ｄｕと前腕長ｄｆと、前記腕平面Ｑの前記直線Ｌの周りの回転角すなわち肘角φとに基づいて前記肘Ｍの位置を算定することを特徴とするマスタアーム。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボット制御などに用いるマスタアームの干渉状態検出方法に関し、特に双腕型マニピュレータ操縦装置などで肘が他のリンク機構と干渉することを防止するために肘位置を効率的に求める方法に関する。
【０００２】
【従来の技術】
放射線領域、宇宙空間、海底など、人が容易に近づくことが出来ない場所で作業をする場合に遠隔で操作できるロボットを用いることができる。このような場合、人は安全な場所からマスタアームを介してロボットのマニピュレータを操縦することにより作業する。マスタアームの動きやそれに加わる力に関連した値が指令値としてロボットのマニピュレータに送られ、またマニピュレータで得られる力触覚などの情報がマスタアームに伝達されるので、操縦者はマスタアームの手先を把持して操作することにより、マニピュレータを介してあたかも自身が作業するかのように遠隔作業をすることができる。
ロボットの作業が複雑化すると、人間と同じようにマニピュレータを２本用いて協働して作業する必要が生じ、このような場合は双腕型マスタアームが用いられることになる。
【０００３】
このようなマスタースレーブ型ロボットで作業させる場合には、マスタアームが操縦者や周囲の物と衝突したりすると、操作を妨げることになる。特に双腕型マスタアームを操縦する場合は、マスタアーム同士の機構的な干渉が生じることを防止しなければならない。そこで、常時マスタアームの各部の位置を算定して、マスタアームの干渉を事前に検知する必要が生じる。
従来、マスタアームの干渉を検出するためには、手先や関節など必要とされる箇所に近接センサを設置したり、脇に設置した撮像装置で取得した画像から視覚的に干渉を検知する方法が用いられてきた。
これらの方法では、干渉を検知することを目的として専用の測定手段を設けるためコスト上の負担があり、またセンサの信頼性を考慮する必要があった。
【０００４】
これらの方法以外に、計算機を用いて、マスタアームに設けられる各アクチュエータの位置指令値から関節位置をリアルタイムに計算し、リンク間の干渉を計算する方法がある。この方法は、特別なセンサを用いることなく、数値的に干渉の検出を行うことができるが、各周期毎に関節角度情報から関節位置をいわゆる順運動学により算出するため計算負荷が大きいことが問題となる。
【０００５】
【発明が解決しようとする課題】
そこで、本発明が解決しようとする課題は、リンク位置を幾何学的に計算する負荷の小さい演算方法を用いて、肘の位置をリアルタイムに算定することによりリンク機構との干渉を検知する方法およびその方法を利用したマスタアームを提供することである。
【０００６】
上記課題を解決するため、本発明のマスタアームのリンク位置検出方法は、肘の位置に冗長自由度を有し手先を掴んでロボットハンドの位置制御をするためのマスタアームにおいて、肘を挟んだ腕を含む面の肩と手先を結ぶ直線の周りの回転角すなわち肘角を指示する肘角指令値と手先位置を指示する手先位置指令値を用いて幾何学的演算により肘の位置を算出することを特徴とする。
【０００７】
ロボットを操作するため使用するマスタアームは、操縦者の腕に追従して同じような運動をすることが望ましいため、人間の腕と同様に７自由度を持つように構成することが好ましい。７自由度を有するマニピュレータは、手先の位置姿勢を決定する上で、先端手先の３軸で姿勢を、肩の３軸と肘の１軸で位置を実現することができる。
このとき、手先の位置姿勢は６自由度を有することから、位置決めに対して冗長な自由度が１個存在することになり、この冗長性を利用することによって特異点回避や障害物回避などが可能となって、操作性を増すことができる。
【０００８】
一方で、腕の状態が一意に定まらないことから、操縦者の体と接触したり、環境にある物品と衝突することを回避するため、位置を検出する必要が生ずる。また、双腕型マニピュレータでは２本の腕が相互に干渉を起こさないようにしなければならない。
本発明のリンク位置検出方法は、マスタアームの手先位置を定める指令値と肘角に基づいて、幾何学的演算により肘などリンクの位置を算出するものであるため、演算負荷が小さく、比較的能力の低い演算装置を用いてもリアルタイムで肘位置を算定することができ、他の物体との干渉を回避するようにすることができる。
【０００９】
さらに本発明第２のリンク位置検出方法は、肩に肩軸交点Ｏで交わる３個の回転軸、肘Ｍに１個の回転軸、および手先に手先軸交点Ｗで交わる３個の回転軸があって、肩から肘の間に所定の長さｄｕを有する上腕と肘から手先の間に所定の長さｄｆを有する前腕を備え、上腕と前腕を含む腕平面Ｑが肩の軸交点Ｏと手先の軸交点Ｗを結ぶ直線Ｌと平行であって、肩と肘の４軸で手先の位置を実現しさらに手先の３軸で姿勢を実現するマスタアームにおいて、手先軸交点Ｗの位置と腕平面Ｑが直線Ｌの周りに回転するときの回転角すなわち肘角φとに基づいて肘Ｍなどの位置を算定することを特徴とする。
【００１０】
本発明第２の方法では、肩にある３回転軸と手先にある３回転軸がそれぞれ１点で交差するという条件を備えたマスタアームを対象とするときに適用するもので、拘束条件の存在により演算負荷が著しく低減して、より軽便な演算装置により迅速に結果を得ることができる。
【００１１】
なお、本発明のリンク位置検出方法は、上腕の付け根となる回転軸と肩の軸交点Ｏの間の肩オフセットリンクが前腕の先端にある回転軸と手先の軸交点Ｗの間の手先オフセットリンクと平行で両者が等しいリンク長ｄｏを有し、腕平面Ｑが直線Ｌと平行であるようなマスタアームについて適用する場合に、肘Ｍの位置を手先軸交点Ｗの座標値（ｘｗ，ｙｗ，ｚｗ）と上腕長ｄｕと前腕長ｄｆと肘角φとから求めるようにすることができる。
【００１２】
また、上記課題を解決するため、本発明のマスタアームは、肘の位置に冗長自由度を有し手先を掴んでロボットハンドの位置制御をするためのマスタアームであって、肘角指令値と手先位置指令値を用いて幾何学的にリンク位置を検出する演算装置を備えたことを特徴とする。
【００１３】
なお、本発明のマスタアームは、肩に肩軸交点Ｏで交わる３個の回転軸、肘Ｍに１個の回転軸、および手先に手先軸交点Ｗで交わる３個の回転軸があって、肩から肘の間に所定の長さｄｕを有する上腕と肘から手先の間に所定の長さｄｆを有する前腕を備え、上腕と前腕を含む腕平面Ｑが肩軸交点Ｏと手先軸交点Ｗを結ぶ直線Ｌと平行であって、上腕の付け根となる回転軸と肩軸交点Ｏの間の肩オフセットリンクが前腕の先端にある回転軸と手先軸交点Ｗの間の手先オフセットリンクと平行で両者が等しいリンク長ｄｏを有し、腕平面Ｑが前記直線Ｌと平行になっていて、肩と肘の４軸で手先の位置を実現しさらに手先の３軸で姿勢を実現するものであって、手先軸交点Ｗの座標値（ｘｗ，ｙｗ，ｚｗ）と上腕長ｄｕと前腕長ｄｆと、肘角φとに基づいて前記肘Ｍの位置を算定するようにしてもよい。
【００１４】
本発明のマスタアームにより、肘位置がリアルタイムに把握できて、他のリンク機構との距離計算が容易にできるため、相互の干渉を回避することが可能となり、ロボット操縦を安全に行うことができる。
【００１５】
【発明の実施の形態】
以下、本発明について実施例に基づき図面を参照して詳細に説明する。
【００１６】
【実施例】
本実施例は、本発明のリンク位置検出方法を適用したマスタアームを示すものである。
図１は本実施例の対象とするマスタアームと操縦者の関係を表した概念図、図２はマスタアームのリンク構成図、図３はマスタアームの関係図、図４は本実施例のリンク位置検出方法を適用する干渉検出装置のブロック図、図５は本実施例のリンク検出方法の手順を説明するフロー図、図６は肘投影点を含む面Ｐの概念を表す図面、図７は手先と肩付け根を結ぶ線に垂直な面に対する投影図、図８は肩付け根と手先を結ぶ直線に肘投影点から下ろした垂線の導入を説明する図面である。
【００１７】
本実施例のリンク位置検出方法は、肩の回転中心で３軸が交差し、手先の回転中心で３軸が交差し、さらに肘に１軸を有するような７自由度を持ったマスタアームを対象とするものである。本方法では、各関節の現在角度情報を用いた順変換を計算することなく、手先の位置指令値と肘角指令値を制御装置から抽出して、これらの値を用いた幾何学的計算によってマスタアームの肘関節位置を算出し、他の物体との距離を計算して一定値以下になるときに接触すると判断する。
特に、肘を挟んだ上腕と前腕が含まれる面が肩口と手先を結ぶ線またはそれに平行な線を軸として回転するような構造を持つマスタアームに対して適用することにより、著しく計算負荷を減少させるものである。
【００１８】
例えば双腕型マスタアームによりロボットハンドを操縦する場合は、図１に右腕の部分だけを図式的に表示したように、マスタアーム１の内側に操縦者２が収まり、マスタアーム１の手先Ｗに付けられた指と操縦者２の手指３を連結して、手先の動きで図外のロボットハンドの手先の位置と姿勢を指示し指先を動かすことによりハンドの指を制御する。
肩の部分では肩の付け根の位置に当たる肩軸交点Ｏで第１回転軸Ｊ１、第２回転軸Ｊ２及び第３回転軸Ｊ３の回転３軸が交差している。
【００１９】
上腕Ａｕは所定の長さｄｕを有し、肩先Ｓの部分に第３回転軸Ｊ３に対して垂直の方向に設けられ回転できるように設けられている。上腕Ａｕの先端に第４回転軸Ｊ４が設けられて肘Ｍを構成し、第４回転軸Ｊ４には所定の長さｄｆを有する前腕Ａｆが回転可能に設けられている。
前腕Ａｆの先端に手先の部分が設けられる。手先の部分では指先が付いている手先の位置に当たる手先軸交点Ｗで、第５回転軸Ｊ５、第６回転軸Ｊ６および第７回転軸Ｊ７の回転３軸が交差している。
【００２０】
なお、操縦者２の腕４によりマスタアーム１を操作するためには、腕４がマスタアーム１の内側に納まることが好ましく、マスタアーム１の肩の部分とリンクの肘の部分にはオフセットが必要となる。
こうした必要と演算の便宜のため、本発明が対象とするマスタアーム１は、肩と手先の位置にそれぞれ等しい長さｄｏを有する肩オフセットリンクＦｓと手先オフセットリンクＦｗを設けている。肩オフセットリンクＦｓは肩付け根Ｏと肩先Ｓの間のリンクであって、肩先Ｓの位置で上腕Ａｕと直交している。また手先オフセットＦｗは手先Ｗと手首Ｈの間のリンクであって、前腕Ａｆと手首Ｈで直交している。また、肩オフセットＦｓと手先オフセットＦｗは平行になっている。
なお、肩部分あるいは手首部分の３個の回転軸に対応する回転機構は図１や図２に示すようにそれぞれ独立したものであってもよいが、ボールジョイントのように３軸一体に構成されたものであってもよい。
また、回転軸の連結順は図示した順に限られるのでなく全く任意である。
【００２１】
マスタアームの手先Ｗの位置と姿勢を制御するためには、肘Ｍの位置を決める必要がなく冗長の自由度に従うが、自由な動きを容認すると、操縦者の腕４にぶつかったり周囲の物品に衝突して適正な制御ができなくなるおそれがある。
本発明が対象とするマスタアーム１は、手先Ｗの位置や姿勢を制御すると同時にマスタアーム１の肘Ｍの動きを操縦者２の腕４の動きに追従させることにより操縦者２との干渉を防ぐようにしたものである。このため、マスタアーム１は手先位置姿勢の制御指令値に加えて肘角の指令値を利用して駆動系制御を行う。
【００２２】
ここで、肘角とは、図２により説明するように、マスタアーム１の上腕Ａｕと前腕Ａｆを含む平面Ｑが肩付け根Ｏと手先Ｗを結ぶ直線Ｇの周りに回転する角度をいい、マスタアーム１の標準姿勢を例えば上腕と前腕を含む面Ｑが鉛直面Ｖと平行になる場合を基準とし、これに対する傾き角φで表現することができる。
なお、図中の平面Ｐは直線Ｇを含み平面Ｑと平行な面であり、点Ｅは肘Ｍを平面Ｐに投影した点である。
【００２３】
マスタアーム１を操作する上で他の物体との干渉状態を把握するために、アーム機構のうち、手先Ｗ、手首Ｈ、肘Ｍの位置が問題となる。
本実施例の干渉検出方法では、手先Ｗの位置は指令値から当然に導かれるものとして、手首Ｈと肘Ｍの位置を簡単に算出する。
【００２４】
図３は、マスタアーム１の肘Ｍの位置を指定する演算アルゴリズムを説明するため、マスタアームの機構を単純化して示した図面である。
図中肩先Ｓ、手首Ｈ、肘Ｍを頂点とする三角形ＳＨＭは図２に示した平面Ｑ内にあり、肩軸交点Ｏ、手先軸交点Ｗおよび肘の投影点Ｅを頂点とする三角形ＯＷＥは平面Ｑと平行な平面Ｐ内にある。
肩軸交点Ｏと手先軸交点Ｗを結ぶ直線Ｇに対して肘投影点Ｅから垂線を下ろし、その垂線の足をＩとし、垂線ＥＩの長さをｒとする。
【００２５】
本実施例の干渉検出方法は、手首Ｈと肘Ｍの位置を求めることを目的とし、結局、図３において、肩軸交点Ｏと肘Ｍを結ぶベクトルＯＭ（以下、ＯＭと表記する）と、肩軸交点Ｏと手首Ｈを結ぶベクトルＯＨ（以下、ＯＨ）を求めることに帰着する。なお、重要なリンク関節である手先Ｗの位置は、マスタアームから発せられる位置指令値として直接に与えられる。
【００２６】
図３から、ＯＭは、ベクトルＯＥ（以下、ＯＥ）、およびベクトルＯＳ（以下、ＯＳ）から、
ＯＭ＝ＯＥ＋ｄｏ・ＯＳ／｜ＯＳ｜                            （１）
で表すことができる。また、ＯＨは、肩軸交点Ｏと手先軸交点Ｗを結ぶベクトルＯＷ（以下、ＯＷ）およびＯＳを用いて、
ＯＨ＝ＯＷ＋ｄｏ・ＯＳ／｜ＯＳ｜                            （２）
と表すことができる。
これらの変数の内ＯＷは手先の位置指令値として制御装置に伝達される制御信号から得ることができる。したがって、ＯＥ、ＯＳを算出することにより、手首と肘の位置が求められる。
【００２７】
図４は、本実施例を適用する干渉検出装置の構成図である。
双腕型マスタアーム１０はマスタアーム制御装置１１と接続されていて、操縦者に操作されてマスタアーム１０から発生する回転軸の角度情報、手先位置情報、肘角情報などをマスタアーム制御装置１１に送り、マスタアーム制御装置１１はこれらの情報に基づいてロボットの操縦信号を生成して図外のロボットに伝達する。
干渉検出装置の入力装置１２は、マスタアーム制御装置１１からリンク演算上必要となる手先位置指令値と肘角指令値を抽出して、演算装置１３に送り込む。
【００２８】
演算装置１３は、記憶装置１４を利用しながら、所定の演算手順に従って手首位置と肘位置を算出する。演算に必要なリンク長などマスタアームの形状特性やマスタアームの環境に存在する障害物に関する位置や形状の情報も予め記憶装置１４に登録されている。
演算装置１３は、さらに算出された各リンク関節位置からリンク同士の距離、および環境との距離を算出して、干渉を予測する。
いずれかのリンク位置他の物体と干渉するおそれがある場合は、出力装置１５を介して警報信号を供給する。警報信号は警報ランプや警報ブザー１６、あるいはＨＭＤ１７に送られて操縦者などに危険を報知する。
また、マスタアーム制御装置１１に送って、マスタアームの動きを制約しリンク同士がそれ以上近付かないようにすることもできる。
【００２９】
以下、図５に示した流れ図に基づいて、上記演算装置１３が実行することになる、マスタアームのリンク位置の検出手順を説明する。
初めに、手先位置指令値Ｗ（ｘｗ，ｙｗ，ｚｗ）と肘角指令値φを入力する（Ｓ１）。
次に、肘Ｍの位置を直接算出するのではなく、まず、肘Ｍの平面Ｐへの投影点Ｅの位置を算出する。
図６は、手先Ｗの位置と姿勢を変えずに肘Ｍの位置を変化させる場合を示すもので、平面Ｐが鉛直面Ｖに対して肘角φだけ傾き、平面Ｐ上の投影点Ｅの位置も変化する状態を表す概念図である。
図中、肩軸交点Ｏを原点とし鉛直線上にＺ軸、水平面内にＸ軸とＹ軸を取ったＸＹＺ座標系を設定する。
【００３０】
鉛直面ＶはＺ軸と直線Ｇを含み、平面Ｐは肘投影点Ｅと直線Ｇを含み鉛直面Ｖと直線Ｇにおいて肘角φを持って交差する。
鉛直面Ｖ中の点Ｅ０は平面Ｐ上の三角形ＯＥＷの形状を鉛直面Ｖ内でそのまま保持したときの肘投影点Ｅに対応する点である。
手先Ｗの座標は既知であって（ｘｗ，ｙｗ，ｚｗ）である。図中の点Ｗ’は手先Ｗの点をＸＹ平面に投影した点で、座標は（ｘｗ，ｙｗ，０）になる。
また、ベクトルｐ（以下、ｐ）はＸＹ平面内にあって直線Ｇに垂直な単位ベクトルである。
【００３１】
図７は、原点Ｏ含み直線Ｇに垂直な面を表示したものである。横軸はＸＹ平面を示し、縦軸はＺ軸と直線Ｇを含む鉛直面Ｖを表す。円Ｃは面上に描いた単位円である。単位長としてオフセットリンクの長さｄｏを選ぶと、演算の手数を節減できる。
平面Ｐは縦軸から肘角φの傾きを持った線で表現され、平面Ｐ内の肘投影点Ｅは原点Ｏから距離ｒだけ離れた位置に表現される。
【００３２】
図から、ｐと平面Ｐの交角θと肘角φとは、
θ＝π／２−φ                                              （３）
という関係を有する。
なお、手先ＷをＸＹ平面上に投影した点Ｗ’（ｘｗ，ｙｗ，０）と単位ベクトルｐは直交するので、Ａ＝（ｘｗ２＋ｙｗ２）１／２とおくと、
ｐ＝（ｙｗ／Ａ，−ｘｗ／Ａ，０）                            （４）
として求めることができる（Ｓ２）。
【００３３】
次に、直線Ｇとｐに垂直な単位ベクトルｑ（以下、ｑ）を考える。
ｑはｐとＯＷの外積で求めることができるから、Ｗ（ｘｗ，ｙｗ，ｚｗ）を用いて、
0003565763.tif 000002
と表すことができる（Ｓ３）。ただし、Ｌ＝｜ＯＷ｜である。
【００３４】
ここで、線分ＥＩの長さｒは、図８に示したように原点Ｏと垂線の足Ｉの距離をｕとし垂線の足Ｉと手先Ｗの距離をｗとすると、Ｌ＝ｕ＋ｗから、
ｒ２＝ｄｕ２−ｕ２＝ｄｆ２−ｗ２＝ｄｆ２−（Ｌ−ｕ）２              （６）
すなわち、
ｕ＝（ｄｕ２−ｄｆ２−Ｌ２）／２Ｌ                              （７）
よって、
ｒ＝（ｄｕ２−（ｄｕ２−ｄｆ２−Ｌ２）２／４Ｌ２）１／２              （８）
で求めることができる（Ｓ４）。
【００３５】
また、点Ｉの座標は手先Ｗの座標に対してｕ／Ｌに当たるので、
0003565763.tif 000003
として求める（Ｓ５）。
【００３６】
ＯＳはＯＷに対して垂直であるから図７に示した面の中に存在する。したがって、ＯＳを規準化した規準化ベクトルＯＳ／｜ＯＳ｜、および垂線の足Ｉと投影点Ｅを結ぶベクトルＩＥ（以下、ＩＥ）を規準化した規準化ベクトルＩＥ／｜ＩＥ｜はそれぞれ、肘角φとｐとｑを用いて、
ＯＳ／｜ＯＳ｜＝ｃｏｓφ・ｐ＋ｓｉｎφ・ｑ                      （１０）
ＩＥ／｜ＩＥ｜＝ｓｉｎφ・ｐ−ｃｏｓφ・ｑ                      （１１）
で求められる。
【００３７】
また、原点Ｏから肘投影点Ｅに向かうベクトルＯＥは、原点Ｏから垂線の足ＩまでのベクトルＯＩ（以下、ＯＩ）にＩＥを加えたものであるから、
ＯＥ＝ＯＩ＋ｒ・ＩＥ／｜ＩＥ｜                            （１２）
と表すことができる（Ｓ６）。
以上の結果を
ＯＭ＝ＯＥ＋ｄｏ・ＯＳ／｜ＯＳ｜                          （１３）
ＯＨ＝ＯＷ＋ｄｏ・ＯＳ／｜ＯＳ｜                          （１４）
の関係式に代入すると、検出したい肘Ｍと手首Ｈの位置が求まる（Ｓ７）。
【００３８】
上記演算手順を２本のマスタアームに適用して、手先を含んだそれぞれのリンク関節位置を算定する（Ｓ８）。さらに、リンク関節位置間の距離を算出する（Ｓ９）。この距離が所定の距離より短かければ干渉の警告を発するようにする（Ｓ１０）。
なお、腕の部分が他の腕と干渉する可能性を検証するためには、腕の両端にある関節を結ぶ直線の方程式を算定して、直線間の距離を算出すればよい。
【００３９】
干渉の危険性は、ブザーや表示装置を介して操縦者に知らせる。操縦者がヘッドマウント型デスプレー装置を装着している場合には画面の一部に表示することができる。
また、干渉の危険性が高いときは、マスタアームにインターロックをかけてそれ以上近づかないようにする機構を備えてもよい。
なお、マスタアームが発する指令値に基づいてリンク位置を算出するため、リンク位置の移動方向を予測して干渉の危険性を検知することもできる。
【００４０】
本実施例の検出方法は、手先位置指令値と肘角指令値から幾何学的演算によってリンク位置を算出するものである。したがって従来、各軸の角度情報を用いた順変換で求めるため、膨大な量の計算が必要であったのと比較すると、著しく計算量が減少し、能力の高くない演算装置によってもリンク間距離を迅速に算出して干渉検出を効率的に行うことができる。
【００４１】
【発明の効果】
以上説明したように、本発明のマスタアームのリンク位置検出方法およびそれを用いたマスタアームによれば、マスタアームから取得する手先位置指令値と肘角指令値位置から幾何学的にリンク位置を算出することができるので、簡単な演算処理により効率的にリンク位置を検出あるいは予測して干渉の可能性を検知し、警報を発することができる。したがって、操作する間にマスタアームのリンク同士が干渉したり環境物と衝突したりすることなく安全にロボット操縦を行うことができる。
【図面の簡単な説明】
【図１】本発明の実施例の対象となるマスタアームと操縦者の関係を示す概念図である。
【図２】図１のマスタアームのリンク構成図である。
【図３】図２のマスタアームの関係図である。
【図４】本実施例のマスタアーム装置を表すブロック図である。
【図５】本実施例のリンク検出方法の手順を表すフロー図である。
【図６】肘投影点を含む面の概念を表す図面である。
【図７】本実施例の手先と肩付け根を結ぶ線に垂直な面にリンク機構などを投影した図面である。
【図８】本実施例において肩付け根と手先を結ぶ直線に肘投影点から下ろした垂線の長さを算出する方法について説明する図面である。
【符号の説明】
１  マスタアーム
２  操縦者
３  操縦者の手指
４  腕
１０  双腕型マスタアーム
１１  マスタアーム制御装置
１２  入力装置
１３  演算装置
１４  記憶装置
１５  出力装置
１６  ブザー
１７  ヘッドマウント型デスプレー
【図１】
0003565763.tif 000004
【図２】
0003565763.tif 000005
【図３】
0003565763.tif 000006
【図４】
0003565763.tif 000007
【図５】
0003565763.tif 000008
【図６】
0003565763.tif 000009
【図７】
0003565763.tif 000010
【図８】
0003565763.tif 000011