作業補助アーム
【課題】別途にセンサを必要とせず、任意の案内面上に拘束することができる作業補助アームを得る。
【解決手段】３軸の変位量に基づきその作業座標が決定されるアームを設け、３軸をそれぞれ対応した３つの変速機を介して差動機構で結合し、３つの変速機の変速比を、指示された案内面以外へのアーム手先の移動を拘束するよう制御装置４１によって制御するようにした作業補助アームであって、３軸の回転角度を検出する回転角度検出器４０と、ユーザにより指示された仮想的な案内面を入力する面指示手段４２とを設け、制御装置４１は、検出された回転角度、及び指示された仮想的な案内面に基づき、現在のアーム手先の位置を含み、かつ前記案内面とは平行ではない補正用案内面を計算する補正用案内面計算手段３０１を含み、補正用案内面計算手段３０１により計算された補正用案内面に沿って、前記アーム手先が動くように３つの変速機の変速比を求める。
【解決手段】３軸の変位量に基づきその作業座標が決定されるアームを設け、３軸をそれぞれ対応した３つの変速機を介して差動機構で結合し、３つの変速機の変速比を、指示された案内面以外へのアーム手先の移動を拘束するよう制御装置４１によって制御するようにした作業補助アームであって、３軸の回転角度を検出する回転角度検出器４０と、ユーザにより指示された仮想的な案内面を入力する面指示手段４２とを設け、制御装置４１は、検出された回転角度、及び指示された仮想的な案内面に基づき、現在のアーム手先の位置を含み、かつ前記案内面とは平行ではない補正用案内面を計算する補正用案内面計算手段３０１を含み、補正用案内面計算手段３０１により計算された補正用案内面に沿って、前記アーム手先が動くように３つの変速機の変速比を求める。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、現在のアーム手先位置を含みかつ指定される案内面とは平行でないような補正用案内面を別途計算して、この補正用案内面に沿って操作することで、アームの手先が指定された案内面から外れても復帰することができる作業補助アームに関するものである。
【背景技術】
【０００２】
従来の作業補助アームにおいては、受動的に動作するもので、ロボットのＮ個ある関節間をＮ−１個の無段変速機で結合し、各無段変速機に対して適当な変速比を与えることで、人間がロボットを手で動かしたとき、ロボットが作業座標の所望の１次元の軌道に拘束されるように動作する（例えば、特許文献１参照）。ここで、この無段変速機は速度の比のみを結合し、変位に対しては拘束を与えない。即ち、速比を変化させても変位の関係は変化しない無段変速機である。
【０００３】
また、従来の別の作業補助アームにおいては、受動的に動作するもので、アームの関節にそれぞれ無段変速機を結合し、無段変速機の入力軸同士を差動機構により結合し、各無段変速機に対して適当な変速比を与えることで、人間がアームを手で動かしたとき、アームが所望の２次元の案内面に拘束されるように動作する（例えば、特許文献２参照）。ここで、この無段変速機は速度の比のみを結合し、変位に対しては拘束を与えない。即ち、速比を変化させても変位の関係は変化しない無段変速機である。
【０００４】
【特許文献１】米国特許第５９５２７９６号明細書
【特許文献２】特開２００５−１６９５３６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
上述したような従来の作業補助アームでは、ロボットは１次元の軌道にのみ拘束され、それより次元の大きい拘束、例えば平面に拘束されることは構造上できないという問題点があった。見かけ上、当初指示した１次元の拘束から外れて動作することは可能だが、これは操作者の手の動きをロボット上の力センサなどのセンサで検知して、その動きの方向に新しい１次元の軌道を設けただけであり、このためにはセンサが必須になるという問題点があった。
【０００６】
また、上述したような従来の別の作業補助アームでは、現在アームの手先が置かれた点、すなわち初期値を含む案内面に沿ってしか動作することができないという問題点があった。言い換えれば、初期値から外れた案内面を指定してその案内面に沿って動作することができないという問題点があった。また、アームの手先がなんらかの原因で案内面から外れると案内面に復帰できないという問題があった。
【０００７】
この発明は、上述のような課題を解決するためになされたもので、その目的は、別途にセンサ等を必要とせず、任意の案内面上に拘束することができる作業補助アームを得るものである。
【０００８】
また、この発明は、上述のような課題を解決するためになされたもので、その目的は、初期値から外れた案内面をも指定でき、アームの手先がその案内面に沿って動作することができ、なんらかの原因でアームの手先が案内面から外れても復帰することができる作業補助アームを得るものである。
【課題を解決するための手段】
【０００９】
この発明に係る作業補助アームは、複数の軸の変位量に基づいてその作業座標が決定されるアームを設け、前記複数の軸をそれぞれ対応した複数の変速機を介して差動機構で結合し、前記複数の変速機の変速比を、指示された案内面以外へのアーム手先の移動を拘束するよう制御装置によって制御するようにした作業補助アームであって、前記アームの複数の軸の回転角度を検出する回転角度検出器と、ユーザにより指示された仮想的な案内面を入力する面指示手段とを設け、前記制御装置は、前記回転角度検出器により検出された回転角度、及び前記面指示手段により指示された仮想的な案内面に基づいて、現在のアーム手先の位置を含み、かつ前記指示された案内面とは平行ではない補正用案内面を計算する補正用案内面計算手段を含み、前記補正用案内面計算手段により計算された補正用案内面に沿って、前記アーム手先が動くように前記複数の変速機の変速比を求めるものである。
【発明の効果】
【００１０】
この発明に係る作業補助アームは、別途にセンサ等を必要とせず、任意の案内面上に拘束することができ、さらに、初期値から外れた案内面をも指定でき、アームの手先がその案内面に沿って動作することができ、なんらかの原因でアームの手先が案内面から外れても復帰することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１１】
実施の形態１．
この発明の実施の形態１に係る作業補助アームについて図１から図５までを参照しながら説明する。図１は、この発明の実施の形態１に係る作業補助アームの全体の構成を示す斜視図である。また、図２は、この発明の実施の形態１に係る作業補助アームの駆動部分をアームの正面から見た構成を示す図である。なお、以降では、各図中、同一符号は同一又は相当部分を示す。
【００１２】
図１において、この発明の実施の形態１に係る作業補助アームは、平行リンク式の垂直多関節型ロボットアームと同様であって、第１軸１と、第２軸２と、第３軸３と、変速機（無段変速機）４ａ、４ｂ、４ｃと、入力軸５ａ、５ｂ、５ｃ（図２参照）と、出力軸６ａ、６ｂ、６ｃ（図２参照）と、傘歯車７と、傘歯車８と、差動傘歯車９と、差動傘歯車１０と、差動傘歯車１１と、差動傘歯車軸１２（図２参照）と、キャリア１３と、減速機１４ａ、１４ｂ、１４ｃと、出力軸１５ａ、１５ｂ、１５ｃ（図２参照）と、枠体１６と、ベース１７と、第１アーム１８と、第２アーム１９と、下節２０と、後節２１と、軸２２、２３、２４と、ブレーキ２５（図２参照）と、アーム手先２６と、回転角度検出器４０ａ、４０ｂ、４０ｃと、制御装置４１と、面指示手段４２とが設けられている。
【００１３】
なお、入力軸５ａ、５ｂ、５ｃ、出力軸６ａ、６ｂ、６ｃ、差動傘歯車軸１２、出力軸１５ａ、１５ｂ、１５ｃ、ブレーキ２５については、図２に示されている。
【００１４】
作業補助アームは、図１に示すように、ベース１７に枠体１６が第１軸１回りに回転又は回動自在に取り付けられている。枠体１６の片側側面に第１アーム１８の下端が第２軸２回りに回動自在に取り付けられており、また、枠体１６の反対側側面に下節２０の一端が第３軸３回りに回動自在に取り付けられている。第２軸２と第３軸３は同軸であり、これらは第１軸１と直交している。
【００１５】
第１アーム１８上端には軸２４が取り付けられ、この軸２４には第２アーム１９の略中央部分が回動自在に取り付けられている。なお、軸２４による第２アーム１９の支持位置は、作業補助アーム各部の構成等に応じて中央部以外の部分であってもよい。一方、下節２０の他端には軸２２が取り付けられ、この軸２２には後節２１の下端が回動自在に取り付けられている。後節２１の上端には軸２３が取り付けられ、この軸２３には第２アーム１９の後端が回動自在に取り付けられている。第１アーム１８と後節２１、並びに下節２０と第２アーム１９はそれぞれ平行であって、これらは平行リンクをなしている。第２アーム１９の先端のアーム手先２６には作業に応じて手首（図示していない）やツール（図示していない）が取り付けられるようになっている。
【００１６】
ベース１７に対する枠体１６の第１軸１回りの回転又は回動角度、枠体１６に対する第１アーム１８の第２軸２回りの回動角度、枠体１６に対する第３軸３回りの下節２０の回動角度は、それぞれ回転角度検出器４０ａ、４０ｂ、４０ｃにより検出される。なお、枠体１６に対する第３軸３回りの下節２０の回動角度は第２アーム１９の枠体１６に対する回動角度に等しい。また、第１アーム１８、第２アーム１９、下節２０、後節２１、軸２２、２３、２４で、第１軸１〜第３軸３の変位量に基づいてその作業座標が決定されるアームを構成している。
【００１７】
また、回転角度検出器４０ａ、４０ｂ、４０ｃは、第１軸１〜第３軸３のそれぞれの変位量として回転角度を検出し、例えば、ローターリーエンコーダ等を用いて構成されている。パソコンなどの制御装置４１は、面指示手段４２から与えられた自由度の情報と、回転角度検出器４０ａ、４０ｂ、４０ｃで検出された変位量に基づいて、アーム手先２６の作業座標が、要求された自由度以外に対しては拘束され、残りの自由度に関しては受動的に動作可能となるよう、変速機４ａ、４ｂ、４ｃの変速比を制御し、パソコンなどで構成されている。また、面指示手段４２は、ユーザが所望する自由度を入力するためのハードウエアとソフトウエアからなる機能部であり、ディスプレイ、マウス、キーボードなどを使用して必要な情報を入力する。
【００１８】
図２において、減速機１４ａは、枠体１６（図１参照）の底部に取り付けられ、減速機１４ａの減速機出力軸１５ａはベース１７（図１参照）に接続されている。変速機４ａの変速機入力軸５ａは傘歯車７に、変速機出力軸６ａは減速機１４ａにそれぞれ接続されている。変速機入力軸５ａ、変速機４ａ、変速機出力軸６ａ、減速機１４ａ、減速機出力軸１５ａは、第１軸１と同軸である。
【００１９】
また、図２において、減速機１４ｂは、枠体１６（図１参照）の側面に取り付けられ、減速機１４ｂの減速機出力軸１５ｂは第１アーム１８（図１参照）の下端に接続されている。変速機４ｂの変速機入力軸５ｂは差動傘歯車１０に、変速機出力軸６ｂはブレーキ２５及び減速機１４ｂにそれぞれ接続されている。変速機入力軸５ｂ、変速機４ｂ、変速機出力軸６ｂ、ブレーキ２５、減速機１４ｂ、減速機出力軸１５ｂは、第２軸２と同軸である。
【００２０】
さらに、図２において、減速機１４ｃは、枠体１６（図１参照）の側面に取り付けられ、減速機１４ｃの減速機出力軸１５ｃは下節２０（図１参照）の一端に接続されている。変速機４ｃの変速機入力軸５ｃは差動傘歯車１１に、変速機出力軸６ｃはブレーキ２５及び減速機１４ｃにそれぞれ接続されている。変速機入力軸５ｃ、変速機４ｃ、変速機出力軸６ｃ、ブレーキ２５、減速機１４ｃ、減速機出力軸１５ｃは、第３軸３と同軸である。
【００２１】
図３は、この発明の実施の形態１に係る作業補助アームの要部の構成を示すブロック図である。
【００２２】
図３において、この発明の実施の形態１に係る作業補助アームは、変速機（無段変速機）４と、ブレーキ２５と、回転角度検出器４０と、制御装置４１と、面指示手段４２とが設けられている。また、制御装置４１には、補正用案内面計算手段３０１が設けられている。
【００２３】
なお、ブロックＤ１は、回転角度検出器４０により検出された第１軸１、第２軸２、第３軸３の各軸の角度Θ＝（θ１，θ２，θ３）を示し、ブロックＤ２は、補正用法線方向ベクトルに基づく（式２４）を展開して変速比を計算することを示し、ブロックＤ３は、計算された変速機の変速比を示す。
【００２４】
図４は、この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【００２５】
図４において、補正用案内面計算手段３０１は、前処理フロー３０２と、補正用案内面計算フローＡ３０３とからなる。
【００２６】
また、前処理フロー３０２は、回転角度検出器４０により検出されたアームの各軸の回転角度を作業座標に変換する正変換手段３０５を含む。
【００２７】
さらに、補正用案内面計算フローＡ３０３は、案内面外の点ｐを与えたとき、ｐに最も近い案内面上の点（垂線の足）ｑを求める、つまり任意の点から案内面に下ろした垂線の足の座標を計算する垂線の足計算手段３０６と、案内面外の点ｐｃを通る適当なベクトルと案内面との交点ｑｃを計算し、交点ｑｃが複数あるときには点ｐｃに近い方を選ぶ交点計算手段３０７と、速度ベクトルｄと誤差ベクトルｅから接平面投影速度ベクトル（チルダｄ）を計算する接平面投影速度ベクトル計算手段３０８と、上記各手段の出力に基づき、つまり（式２０）から補正用法線方向ベクトルを計算する補正用法線方向ベクトル計算手段Ａ（第１の補正用法線方向ベクトル計算手段）３０９とを含む。
【００２８】
つぎに、この実施の形態１に係る作業補助アームの動作について図面を参照しながら説明する。図５は、この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【００２９】
図３に示すように、回転角度検出器４０により検出されたアームの各軸の回転角度と、面指示手段４２により指示された仮想的な案内面の情報は、制御装置４１に送られ、所定の演算が行われ、この演算の結果として変速比が得られ、変速機４の変速比が制御される。また、制御装置４１は、非常停止ボタン（図示していない）が押されたときなどはブレーキ２５を動作させるよう構成されている。
【００３０】
ユーザは、作業補助アームが動作すべき仮想的な案内面１０１を面指示手段４２により指示する。例えば、案内面１０１が平面である場合、案内面１０１に立てた法線方向ベクトル
【００３１】
【数１】
【００３２】
と、案内面上のひとつの点の座標
【００３３】
【数２】
【００３４】
とを指定すれば、ひとつの平面を指定することができる。この場合、面指示手段４２は、これらの値を制御装置４１に伝える。
【００３５】
また、例えば、案内面１０１が水平面の座標ｘとｙの関数で、
【００３６】
【数３】
【００３７】
と表されるような曲面の場合、この曲面のｘ方向、ｙ方向それぞれの接線方向ベクトルを、
【００３８】
【数４】
【００３９】
とすれば、接線方向ベクトルは、
【００４０】
【数５】
【００４１】
となる。（式４）、（式５）のベクトルで張られる平面は、図５に示すように、案内面１０１の曲面の接平面３１４となる。（式４）、（式５）のベクトルを用いて案内面１０１の曲面の法線方向ベクトルｎを次のように表すことができる。
【００４２】
【数６】
【００４３】
ここで、×はベクトルの外積を表す。また、（式６）などの分母の二重線はベクトルの大きさ（長さ）を表す。この場合、面指示手段４２は、（式３）とアーム手先位置の座標をもとに（式４）、（式５）、（式６）を用いて法線方向ベクトルを演算して制御装置４１に伝える。
【００４４】
以下では補正用案内面計算手段３０１におけるひとつの演算の手順について図４と図５を用いて説明する。この実施の形態１の演算の手順を補正用案内面計算フローＡ３０３と呼ぶ。図５でアーム手先２６の進行方向は、左から右としている。
【００４５】
まず、補正用案内面計算フローＡ３０３の演算の手順の概要を説明する。アーム手先位置が本来の案内面１０１上にないとき、前回の速度ベクトルを本来の案内面１０１に投影したベクトルと、誤差ベクトルを用いて、幾何学的に補正した補正用案内面３１２の補正用法線方向ベクトル３１３を求める。このときの補正用法線方向ベクトル３１３は、前回の速度ベクトルが次回も維持されると仮定し、前回の速度ベクトルを本来の案内面１０１に投影したベクトルと、今回の誤差ベクトル３１６と、次回に予想される誤差である離隔ベクトル３１７の合ベクトルを考え、これを含むような補正用案内面３１２に設定する。
【００４６】
次に、補正用案内面計算フローＡ３０３の演算の手順の詳細を説明する。まず、垂線の足計算手段３０６と交点計算手段３０７について説明する。
【００４７】
垂線の足計算手段３０６では以下の演算が行われる。案内面外の点ｐ＝（ｐｘ，ｐｙ，ｐｚ）を与えたとき、ｐに最も近い案内面上の点ｑ＝（ｑｘ，ｑｙ，ｑｚ）を求める。点ｑは垂線の足である。このとき、ベクトルｑ−ｐは、点ｑにおいて案内面に垂直なので次の式が成り立つ。
【００４８】
【数７】
【００４９】
ここで、●はベクトルの内積を表す。また、点ｑは案内面上の点なので（式３）を用いて次の式が成り立つ。
【００５０】
【数８】
【００５１】
（式７）、（式８）、（式９）を連立させれば、与えられた点ｐに対する点ｑを得ることができる。この関数を垂線の足計算手段３０６と呼び、次のように定義する。
【００５２】
【数９】
【００５３】
また、交点計算手段３０７では以下の演算が行われる。案内面外の点ｐ＝（ｐｘ，ｐｙ，ｐｚ）を通る適当なベクトルｖ＝（ｖｘ，ｖｙ，ｖｚ）と案内面との交点ｑ＝（ｑｘ，ｑｙ，ｑｚ）を求める。ｋを適当な係数とすれば次の式が成り立つ。
【００５４】
【数１０】
【００５５】
また、ｑは案内面上の点なので（式９）が成り立つ。（式９）と（式１１）を連立させれば、与えられた点ｐと与えられたベクトルｖに対する点ｑと係数ｋを得ることができる。点ｑが複数あるときには点ｐに近い方を選ぶ。この関数を交点計算手段３０７と呼び、係数ｋは必要ないので次のように定義する。
【００５６】
【数１１】
【００５７】
続いて、補正用案内面計算フローＡ３０３の演算の手順の詳細を説明する。回転角度検出器４０により検出されたアームの各軸の回転角度は、正変換手段３０５により作業座標に変換される。以下座標は作業座標で表現する。
【００５８】
前回のｉ−１回目、今回のｉ回目の案内面１０１外の点の座標を、図５に示すように、それぞれｐｉ−１、ｐｉとする。これらの点から案内面１０１に下ろした垂線の座標をそれぞれｑｉ−１、ｑｉとする。垂線の足計算手段３０６により、（式１０）で定義した関数を用いれば、
【００５９】
【数１２】
【００６０】
などとなる。点ｐｉが案内面１０１に対してもつ誤差ベクトル３１６をｅｉとする。このとき次の式のようになる。
【００６１】
【数１３】
【００６２】
前回の速度ベクトルをｄｉ−１とする。
【００６３】
【数１４】
【００６４】
速度ベクトルｄｉ−１を点ｑｉにおける案内面１０１の接平面３１４に投影すれば、接平面投影速度ベクトル３１５（チルダｄｉ−１）は、誤差ベクトル３１６を用いて次の式のようになる。（なお、チルダｄは、ｄの上に〜があることを表す。）
【００６５】
【数１５】
【００６６】
ここで、速度ベクトルｄｉ−１と誤差ベクトル３１６（ｅｉ）から接平面投影速度ベクトル３１５（チルダｄｉ−１）を得る計算を接平面投影速度ベクトル計算手段３０８と呼ぶ。
【００６７】
点ｑｉからチルダｄｉ−１進んだ点を、図５に示すように、ｐｃｉ＋１とする。
【００６８】
【数１６】
【００６９】
点ｐｃｉ＋１から誤差ベクトル３１６（ｅｉ）方向に直線を伸ばし、交点計算手段３０７により、（式１２）で定義した関数を用いて案内面１０１と交差した点を求め、ｑｃｉ＋１とする。
【００７０】
【数１７】
【００７１】
交点ｑｃｉ＋１と点ｐｃｉ＋１の間の誤差を離隔ベクトル３１７と呼び、ｅｃｉ＋１とおく。
【００７２】
【数１８】
【００７３】
以上を用いて補正用法線方向ベクトル計算手段Ａ３０９により、次回の補正用法線方向ベクトル３１３（ハットｎｉ＋１）を次の式のように定める。（なお、ハットｎは、ｎの上に＾があることを表す。）
【００７４】
【数１９】
【００７５】
ここで、前回の速度ベクトルｄｉ−１と、今回の補正用法線方向ベクトルｎｉの外積をａｉとおいた。
【００７６】
【数２０】
【００７７】
以上のようにして補正用案内面計算フローＡ３０３の演算により補正用法線方向ベクトル３１３（ハットｎ）が得られた。この補正用法線方向ベクトル３１３（ハットｎ）を用いて各変速機４ａ、４ｂ、４ｃの変速比を以下のようにして求める。
【００７８】
関節の角速度ベクトルΩと、直交座標系から見たアーム手先２６の速度ベクトルｖの間には、次の関係が成り立つ。
【００７９】
【数２１】
【００８０】
ここで、Ｊ［Θ］は、ヤコビ行列であり、回転角度検出器４０により検出された第１軸１、第２軸２、第３軸３の各軸の角度Θ＝（θ１，θ２，θ３）の関数である。また、ｖ＝（ｖｘ，ｖｙ，ｖｚ）は、ｘ、ｙ、ｚ方向のアーム手先２６の速度であり、Ω＝（ω１，ω２，ω３）は、第１軸１、第２軸２、第３軸３の角速度である。
【００８１】
法線方向ベクトルｎと、アーム手先２６の速度ベクトルｖの間に、次の式が成り立てば両者は直交する。
【００８２】
【数２２】
【００８３】
この（式２３）に、上記の式（式２２）を代入すれば、次の式が得られる。
【００８４】
【数２３】
【００８５】
この（式２４）を展開して、ω１、ω２、ω３の係数を得れば、各変速機４ａ、４ｂ、４ｃの変速比が求められる。
【００８６】
ここで、上記の法線方向ベクトルｎとして、（式１）又は（式６）で得られるベクトルの代わりに、（式２０）で得た補正用法線方向ベクトル３１３（ハットｎ）を用いれば、アーム手先２６は補正用案内面３１２に沿って動く。
【００８７】
次回の速度ベクトルが前回と同じであれば、アーム手先２６は補正用案内面３１２に沿って動作することで点ｑｃｉ＋１の位置で本来の案内面１０１に復帰する。次回の速度ベクトルが前回と異なっていても、以上のアルゴリズムを毎回繰り返すことで誤差が減少していく。従って、アーム手先位置の初期値から外れた面を案内面として指定したり、案内面１０１に沿って移動中になんらかの原因でアーム手先位置が案内面から外れたりしたときにも案内面１０１に戻って来られる効果がある。
【００８８】
なお、（式１７）で点ｑｉから進んだ点ｐｃｉ＋１の座標を求める際、次回の速度ベクトルが前回の速度ベクトルと同じであるとの仮定で前回の接平面投影速度ベクトル３１５（チルダｄｉ−１）を加えているが、前回の接平面投影速度ベクトル３１５に適当なゲインを与えたものを加えてもよい。また、前回の接平面投影速度ベクトル３１５の代わりに適当な速度ベクトルを予測したものを加えるようにしてもよい。
【００８９】
また、ユーザが面指示手段４２に案内面１０１を指示するとき、必ずしも面を記述する情報として案内面１０１に立てた法線方向ベクトルｎと面上のひとつの点の座標ｐ＝（ｘ０，ｙ０，ｚ０）を指定しなくてもよい。例えば、案内面１０１が平面であれば、面上の点３点の座標を指示するなどでもよい。
【００９０】
実施の形態２．
この発明の実施の形態２に係る作業補助アームについて図６から図９までを参照しながら説明する。図６は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の構成を示す図である。なお、この発明の実施の形態２に係る作業補助アームの基本的な構成は、上記の実施の形態１と同様である。
【００９１】
図６において、補正用案内面計算手段３０１は、前処理フロー３０２と、補正用案内面計算フローＢ３０４とからなる。
【００９２】
また、前処理フロー３０２は、回転角度検出器４０により検出されたアームの各軸の回転角度を作業座標に変換する正変換手段３０５を含む。
【００９３】
さらに、補正用案内面計算フローＢ３０４は、案内面外の点ｐを与えたとき、ｐに最も近い案内面上の点（垂線の足）ｑを求める、つまり任意の点から案内面に下ろした垂線の足の座標を計算する垂線の足計算手段３０６と、案内面外の点ｐｃを通る鉛直方向ベクトルｇと案内面との交点ｑｃを計算し、交点ｑｃが複数あるときには点ｐｃに近い方を選ぶ交点計算手段３０７と、速度ベクトルｄと誤差ベクトルｅから接平面投影速度ベクトル（チルダｄ）を計算する接平面投影速度ベクトル計算手段３０８と、上記各手段の出力に基づき、つまり（式３８）から補正用法線方向ベクトルを計算する補正用法線方向ベクトル計算手段Ｂ（第２の補正用法線方向ベクトル計算手段）３１０とを含む。
【００９４】
つぎに、この実施の形態２に係る作業補助アームの動作について図面を参照しながら説明する。図７は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の例）を説明するための図である。また、図８は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の別の例）を説明するための図である。さらに、図９は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【００９５】
この実施の形態２を説明するに当たり、まず、上記の実施の形態１で生じることがある問題点について説明する。図５に示すように、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面に近づく方向である場合は、上記の実施の形態１で述べた方法でアーム手先２６は補正用案内面３１２に沿って動作することで、本来の案内面１０１に復帰することができる。
【００９６】
これに対して、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面から遠ざかる方向である場合、上記の実施の形態１と同様に前回の速度ベクトルを本来の案内面１０１に投影したベクトルと、誤差ベクトルを用いて案内面を幾何学的に補正する方法を適用した場合を図７と図８に示す。図中、アーム手先２６の進行方向は左から右としている。
【００９７】
図７のように、案内面１０１からの誤差が小さいときは、補正用案内面３１２は右上あがりになるのでアーム手先２６を左から右へ移動させれば、補正用案内面３１２に沿って動作するので案内面１０１へ近づき誤差が小さくなる。
【００９８】
ところが、図８のように、案内面１０１からの誤差が大きいときは、補正用案内面３１２の傾きが鉛直軸を通り越して左上あがりになるので、前回の速度ベクトルの方向と同様にアーム手先２６を左から右へ移動させると案内面１０１からかえって遠ざかってしまう。このため、案内面１０１からの誤差が大きいときには、誤差を補正するためにもとの速度（図では左から右）とは逆向きの速度（図では右から左）を与えてバックさせなくてはならなくなり、直感的に操作することが難しくなる問題がある。
【００９９】
そこで、速度ベクトルの水平方向成分が案内面から遠ざかるときのため、上記の実施の形態１とは別な補正方法を考える。
【０１００】
以下では補正用案内面計算フローＢ３０４の演算の手順について、図６と図９を用いて説明する。まず、手順の概要を説明する。
【０１０１】
ここでは、上記の実施の形態１の離隔ベクトル３１７の代りに、鉛直方向ベクトル３１８を用いる。このときの補正用法線方向ベクトル３１３は、前回の速度ベクトルが次回も維持されると仮定し、前回の速度ベクトルを本来の案内面１０１に投影した接平面投影速度ベクトル３１５の分を今回の位置から延長し、そこから鉛直方向に伸ばした直線と本来の案内面１０１との交点までのベクトルを考え、これらの合ベクトルを含むような補正用案内面３１２に設定する。
【０１０２】
次に、補正用案内面計算フローＢ３０４の演算の手順の詳細を説明する。回転角度検出器４０により検出されたアームの各軸の角度は、正変換手段３０５により作業座標に変換される。以下座標は作業座標で表現する。
【０１０３】
前回のｉ−１回目、今回のｉ回目の案内面外の点の座標を、図９に示すように、それぞれｐｉ−１、ｐｉとする。これらの点から案内面１０１に下ろした垂線の座標をそれぞれｑｉ−１、ｑｉとする。垂線の足計算手段３０６により、（式１０）で定義した関数を用いれば、
【０１０４】
【数２４】
【０１０５】
などとなる。点ｐｉが案内面１０１に対してもつ誤差ベクトルをｅｉとする。このとき、次の式のようになる。
【０１０６】
【数２５】
【０１０７】
前回の速度ベクトルをｄｉ−１とする。
【０１０８】
【数２６】
【０１０９】
速度ベクトルｄｉ−１を案内面１０１に投影すれば、接平面投影速度ベクトル３１５（チルダｄｉ−１）は、誤差ベクトル３１６を用いて次の式のようになる。
【０１１０】
【数２７】
【０１１１】
ここで、速度ベクトルｄｉ−１と誤差ベクトル３１６（ｅｉ）から接平面投影速度ベクトル３１５（チルダｄｉ−１）を得る計算を接平面投影速度ベクトル計算手段３０８と呼ぶ。
【０１１２】
点ｐｉから接平面投影速度ベクトル３１５（チルダｄｉ−１）進んだ点をｐｃｉ＋１とする。
【０１１３】
【数２８】
【０１１４】
この点ｐｃｉ＋１から鉛直方向ベクトル３１８（ｇ）方向に直線を伸ばし、交点計算手段３０７により、（式１２）で定義した関数を用いて案内面１０１と交差した点求め、ｑｃｉ＋１とする。
【０１１５】
【数２９】
【０１１６】
以上を用いて補正用法線方向ベクトル計算手段Ｂ３１０により、次回の補正用法線方向ベクトル３１３（ハットｎｉ＋１）を次の式のように定める。
【０１１７】
【数３０】
【０１１８】
ここで、前回の速度ベクトルｄｉ−１と、今回の法線方向ベクトルｎｉの外積をａｉとおいた。
【０１１９】
【数３１】
【０１２０】
以上で（式３５）、（式３６）、（式３７）、（式３８）が、上記の実施の形態１と異なる。
【０１２１】
以上のようにして補正用案内面計算フローＢ３０４の演算により補正用法線方向ベクトル３１３（ハットｎ）が得られた。この補正用法線方向ベクトル３１３（ハットｎ）を用いて各変速機４ａ、４ｂ、４ｃの変速比は、上記の実施の形態１と同様に求めることができる。
【０１２２】
ここで、法線方向ベクトルｎとして、（式１）又は（式６）で得られるベクトルの代わりに、（式３８）で得た補正用法線方向ベクトル３１３（ハットｎ）を用いれば、アーム手先２６は補正用案内面３１２に沿って動く。
【０１２３】
次回の速度ベクトルが前回と同じであれば、アーム手先２６は補正用案内面３１２に沿って動作することで点ｑｃｉ＋１の位置で本来の案内面１０１に復帰する。次回の速度ベクトルが前回と異なっていても、以上のアルゴリズムを毎回繰り返すことで誤差が減少していく。従って、アーム手先位置の初期値から外れた面を案内面として指定したり、案内面１０１に沿って移動中になんらかの原因でアーム手先位置が案内面から外れたりしたときにも案内面１０１に戻って来られる効果がある。
【０１２４】
さらに、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１から遠ざかる方向であり、かつ案内面１０１からの誤差が大きいときに、前回の速度ベクトルの方向と同じ方向にアーム手先２６を移動させると案内面１０１からかえって遠ざかってしまうことがなくなり、アーム手先２６を補正用案内面３１２に沿って概ね同じ方向へ動作することで本来の案内面１０１に復帰できるようになる効果がある。
【０１２５】
なお、（式３５）で点ｐｉから進んだ点ｐｃｉ＋１の座標を求める際、次回の速度ベクトルが前回の速度ベクトルと同じであるとの仮定で前回の接平面投影速度ベクトル３１５（チルダｄｉ−１）を加えているが、前回の接平面投影速度ベクトル３１５に適当なゲインを与えたものを加えてもよい。また、前回の接平面投影速度ベクトル３１５の代わりに適当な速度ベクトルを予測したものを加えるようにしてもよい。
【０１２６】
また、ユーザが面指示手段４２に案内面１０１を指示するとき、必ずしも面を記述する情報として案内面１０１に立てた法線方向ベクトルｎと面上のひとつの点の座標ｐ＝（ｘ０，ｙ０，ｚ０）を指定しなくてもよい。例えば、案内面１０１が平面であれば面上の点３点の座標を指示するなどでもよい。
【０１２７】
実施の形態３．
この発明の実施の形態３に係る作業補助アームについて図１０から図１２までを参照しながら説明する。
【０１２８】
図１０は、この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの向きの場合分けを説明するための図である。また、図１１は、この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの水平方向成分が案内面に近づく方向か遠ざかる方向かの判定方法を説明するための図である。さらに、図１２は、この発明の実施の形態３に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【０１２９】
図１２において、補正用案内面計算手段３０１は、前処理フロー３０２と、速度ベクトル判定手段３１１と、補正用案内面計算フローＡ（第１の補正用案内面計算詳細手段）３０３と、補正用案内面計算フローＢ（第２の補正用案内面計算詳細手段）３０４とからなる。
【０１３０】
また、前処理フロー３０２は、回転角度検出器４０により検出されたアームの各軸の回転角度を作業座標に変換する正変換手段３０５を含む。
【０１３１】
速度ベクトル判定手段３１１は、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向である場合は、補正用案内面計算フローＡ３０３の演算を選択し、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１から遠ざかる方向である場合は、補正用案内面計算フローＢ３０４の演算を選択する。
【０１３２】
この実施の形態３は、補正用案内面計算手段３０１における演算として、上記の実施の形態１で説明した補正用案内面計算フローＡ３０３の方法と、上記の実施の形態２で説明した補正用案内面計算フローＢ３０４の方法のどちらを適用するかを判定していずれかを処理するものである。
【０１３３】
まず、現在のアーム手先位置
【０１３４】
【数３２】
【０１３５】
が指示された案内面１０１の面外にあるとき、現在のアーム手先２６の速度ベクトル
【０１３６】
【数３３】
【０１３７】
の水平方向成分（ｖｘ，ｖｙ，０）が案内面１０１へ近付く方向である場合と遠ざかる方向である場合に関して、図１０に示すように、場合分けを行う。
【０１３８】
ある点（ｘ０，ｙ０，ｚ０）を通って（式１）で表す法線方向ベクトルに直交する案内面１０１の接平面３１４の式は、次のようになる。
【０１３９】
【数３４】
【０１４０】
（式５１）で表す現在のアーム手先位置と、案内面１０１の上下関係については次のように表現できる。案内面上の点を、
【０１４１】
【数３５】
【０１４２】
とする。このとき、
【０１４３】
【数３６】
【０１４４】
として、点（ｘ０，ｙ０，ｚ０）がｑ近傍であるとすれば、
【０１４５】
【数３７】
【０１４６】
ならば、現在の手先位置は案内面１０１より上側、
【０１４７】
【数３８】
【０１４８】
ならば、現在の手先位置は案内面１０１より下側となる。
【０１４９】
次に、（式５２）で表す現在のアーム手先２６の速度ベクトルと、（式１）で表す案内面１０１の法線方向ベクトルの水平方向成分については、それぞれのベクトルの水平方向成分同士の内積の符号を用いて、
【０１５０】
【数３９】
【０１５１】
ならば、それぞれのベクトルの水平方向成分は逆方向、
【０１５２】
【数４０】
【０１５３】
ならば、それぞれのベクトルの水平方向成分は同じ方向となる。
【０１５４】
これらの関係の組み合わせにより、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向か、遠ざかる方向かを図１１のようにまとめることができる。
【０１５５】
図１２に示す速度ベクトル判定手段３１１は、図１１の場合分けにより、（式５７）と（式５９）を用いて、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向である場合は、上記の実施の形態１で説明した補正用案内面計算フローＡ３０３の演算を選択する。また、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１から遠ざかる方向である場合は、上記の実施の形態２で説明した補正用案内面計算フローＢ３０４の演算を選択する。
【０１５６】
以上の判定と演算により、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向であっても、遠ざかる方向であっても、アーム手先２６は補正用案内面３１２に沿って動作することで本来の案内面１０１に復帰するという効果がある。
【図面の簡単な説明】
【０１５７】
【図１】この発明の実施の形態１に係る作業補助アームの全体の構成を示す斜視図である。
【図２】この発明の実施の形態１に係る作業補助アームの駆動部分をアームの正面から見た構成を示す図である。
【図３】この発明の実施の形態１に係る作業補助アームの要部の構成を示すブロック図である。
【図４】この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【図５】この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【図６】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【図７】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の例）を説明するための図である。
【図８】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の別の例）を説明するための図である。
【図９】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【図１０】この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの向きの場合分けを説明するための図である。
【図１１】この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの水平方向成分が案内面に近づく方向か遠ざかる方向かの判定方法を説明するための図である。
【図１２】この発明の実施の形態３に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【符号の説明】
【０１５８】
４ 変速機、２５ ブレーキ、２６ アーム手先、４０ 回転角度検出器、４１ 制御装置、４２ 面指示手段、３０１ 補正用案内面計算手段、３０２ 前処理フロー、３０３ 補正用案内面計算フローＡ、３０４ 補正用案内面計算フローＢ、３０５ 正変換手段、３０６ 垂線の足計算手段、３０７ 交点計算手段、３０８ 接平面投影速度ベクトル計算手段、３０９ 補正用法線方向ベクトル計算手段Ａ、３１０ 補正用法線方向ベクトル計算手段Ｂ、３１１ 速度ベクトル判定手段。
【技術分野】
【０００１】
この発明は、現在のアーム手先位置を含みかつ指定される案内面とは平行でないような補正用案内面を別途計算して、この補正用案内面に沿って操作することで、アームの手先が指定された案内面から外れても復帰することができる作業補助アームに関するものである。
【背景技術】
【０００２】
従来の作業補助アームにおいては、受動的に動作するもので、ロボットのＮ個ある関節間をＮ−１個の無段変速機で結合し、各無段変速機に対して適当な変速比を与えることで、人間がロボットを手で動かしたとき、ロボットが作業座標の所望の１次元の軌道に拘束されるように動作する（例えば、特許文献１参照）。ここで、この無段変速機は速度の比のみを結合し、変位に対しては拘束を与えない。即ち、速比を変化させても変位の関係は変化しない無段変速機である。
【０００３】
また、従来の別の作業補助アームにおいては、受動的に動作するもので、アームの関節にそれぞれ無段変速機を結合し、無段変速機の入力軸同士を差動機構により結合し、各無段変速機に対して適当な変速比を与えることで、人間がアームを手で動かしたとき、アームが所望の２次元の案内面に拘束されるように動作する（例えば、特許文献２参照）。ここで、この無段変速機は速度の比のみを結合し、変位に対しては拘束を与えない。即ち、速比を変化させても変位の関係は変化しない無段変速機である。
【０００４】
【特許文献１】米国特許第５９５２７９６号明細書
【特許文献２】特開２００５−１６９５３６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
上述したような従来の作業補助アームでは、ロボットは１次元の軌道にのみ拘束され、それより次元の大きい拘束、例えば平面に拘束されることは構造上できないという問題点があった。見かけ上、当初指示した１次元の拘束から外れて動作することは可能だが、これは操作者の手の動きをロボット上の力センサなどのセンサで検知して、その動きの方向に新しい１次元の軌道を設けただけであり、このためにはセンサが必須になるという問題点があった。
【０００６】
また、上述したような従来の別の作業補助アームでは、現在アームの手先が置かれた点、すなわち初期値を含む案内面に沿ってしか動作することができないという問題点があった。言い換えれば、初期値から外れた案内面を指定してその案内面に沿って動作することができないという問題点があった。また、アームの手先がなんらかの原因で案内面から外れると案内面に復帰できないという問題があった。
【０００７】
この発明は、上述のような課題を解決するためになされたもので、その目的は、別途にセンサ等を必要とせず、任意の案内面上に拘束することができる作業補助アームを得るものである。
【０００８】
また、この発明は、上述のような課題を解決するためになされたもので、その目的は、初期値から外れた案内面をも指定でき、アームの手先がその案内面に沿って動作することができ、なんらかの原因でアームの手先が案内面から外れても復帰することができる作業補助アームを得るものである。
【課題を解決するための手段】
【０００９】
この発明に係る作業補助アームは、複数の軸の変位量に基づいてその作業座標が決定されるアームを設け、前記複数の軸をそれぞれ対応した複数の変速機を介して差動機構で結合し、前記複数の変速機の変速比を、指示された案内面以外へのアーム手先の移動を拘束するよう制御装置によって制御するようにした作業補助アームであって、前記アームの複数の軸の回転角度を検出する回転角度検出器と、ユーザにより指示された仮想的な案内面を入力する面指示手段とを設け、前記制御装置は、前記回転角度検出器により検出された回転角度、及び前記面指示手段により指示された仮想的な案内面に基づいて、現在のアーム手先の位置を含み、かつ前記指示された案内面とは平行ではない補正用案内面を計算する補正用案内面計算手段を含み、前記補正用案内面計算手段により計算された補正用案内面に沿って、前記アーム手先が動くように前記複数の変速機の変速比を求めるものである。
【発明の効果】
【００１０】
この発明に係る作業補助アームは、別途にセンサ等を必要とせず、任意の案内面上に拘束することができ、さらに、初期値から外れた案内面をも指定でき、アームの手先がその案内面に沿って動作することができ、なんらかの原因でアームの手先が案内面から外れても復帰することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１１】
実施の形態１．
この発明の実施の形態１に係る作業補助アームについて図１から図５までを参照しながら説明する。図１は、この発明の実施の形態１に係る作業補助アームの全体の構成を示す斜視図である。また、図２は、この発明の実施の形態１に係る作業補助アームの駆動部分をアームの正面から見た構成を示す図である。なお、以降では、各図中、同一符号は同一又は相当部分を示す。
【００１２】
図１において、この発明の実施の形態１に係る作業補助アームは、平行リンク式の垂直多関節型ロボットアームと同様であって、第１軸１と、第２軸２と、第３軸３と、変速機（無段変速機）４ａ、４ｂ、４ｃと、入力軸５ａ、５ｂ、５ｃ（図２参照）と、出力軸６ａ、６ｂ、６ｃ（図２参照）と、傘歯車７と、傘歯車８と、差動傘歯車９と、差動傘歯車１０と、差動傘歯車１１と、差動傘歯車軸１２（図２参照）と、キャリア１３と、減速機１４ａ、１４ｂ、１４ｃと、出力軸１５ａ、１５ｂ、１５ｃ（図２参照）と、枠体１６と、ベース１７と、第１アーム１８と、第２アーム１９と、下節２０と、後節２１と、軸２２、２３、２４と、ブレーキ２５（図２参照）と、アーム手先２６と、回転角度検出器４０ａ、４０ｂ、４０ｃと、制御装置４１と、面指示手段４２とが設けられている。
【００１３】
なお、入力軸５ａ、５ｂ、５ｃ、出力軸６ａ、６ｂ、６ｃ、差動傘歯車軸１２、出力軸１５ａ、１５ｂ、１５ｃ、ブレーキ２５については、図２に示されている。
【００１４】
作業補助アームは、図１に示すように、ベース１７に枠体１６が第１軸１回りに回転又は回動自在に取り付けられている。枠体１６の片側側面に第１アーム１８の下端が第２軸２回りに回動自在に取り付けられており、また、枠体１６の反対側側面に下節２０の一端が第３軸３回りに回動自在に取り付けられている。第２軸２と第３軸３は同軸であり、これらは第１軸１と直交している。
【００１５】
第１アーム１８上端には軸２４が取り付けられ、この軸２４には第２アーム１９の略中央部分が回動自在に取り付けられている。なお、軸２４による第２アーム１９の支持位置は、作業補助アーム各部の構成等に応じて中央部以外の部分であってもよい。一方、下節２０の他端には軸２２が取り付けられ、この軸２２には後節２１の下端が回動自在に取り付けられている。後節２１の上端には軸２３が取り付けられ、この軸２３には第２アーム１９の後端が回動自在に取り付けられている。第１アーム１８と後節２１、並びに下節２０と第２アーム１９はそれぞれ平行であって、これらは平行リンクをなしている。第２アーム１９の先端のアーム手先２６には作業に応じて手首（図示していない）やツール（図示していない）が取り付けられるようになっている。
【００１６】
ベース１７に対する枠体１６の第１軸１回りの回転又は回動角度、枠体１６に対する第１アーム１８の第２軸２回りの回動角度、枠体１６に対する第３軸３回りの下節２０の回動角度は、それぞれ回転角度検出器４０ａ、４０ｂ、４０ｃにより検出される。なお、枠体１６に対する第３軸３回りの下節２０の回動角度は第２アーム１９の枠体１６に対する回動角度に等しい。また、第１アーム１８、第２アーム１９、下節２０、後節２１、軸２２、２３、２４で、第１軸１〜第３軸３の変位量に基づいてその作業座標が決定されるアームを構成している。
【００１７】
また、回転角度検出器４０ａ、４０ｂ、４０ｃは、第１軸１〜第３軸３のそれぞれの変位量として回転角度を検出し、例えば、ローターリーエンコーダ等を用いて構成されている。パソコンなどの制御装置４１は、面指示手段４２から与えられた自由度の情報と、回転角度検出器４０ａ、４０ｂ、４０ｃで検出された変位量に基づいて、アーム手先２６の作業座標が、要求された自由度以外に対しては拘束され、残りの自由度に関しては受動的に動作可能となるよう、変速機４ａ、４ｂ、４ｃの変速比を制御し、パソコンなどで構成されている。また、面指示手段４２は、ユーザが所望する自由度を入力するためのハードウエアとソフトウエアからなる機能部であり、ディスプレイ、マウス、キーボードなどを使用して必要な情報を入力する。
【００１８】
図２において、減速機１４ａは、枠体１６（図１参照）の底部に取り付けられ、減速機１４ａの減速機出力軸１５ａはベース１７（図１参照）に接続されている。変速機４ａの変速機入力軸５ａは傘歯車７に、変速機出力軸６ａは減速機１４ａにそれぞれ接続されている。変速機入力軸５ａ、変速機４ａ、変速機出力軸６ａ、減速機１４ａ、減速機出力軸１５ａは、第１軸１と同軸である。
【００１９】
また、図２において、減速機１４ｂは、枠体１６（図１参照）の側面に取り付けられ、減速機１４ｂの減速機出力軸１５ｂは第１アーム１８（図１参照）の下端に接続されている。変速機４ｂの変速機入力軸５ｂは差動傘歯車１０に、変速機出力軸６ｂはブレーキ２５及び減速機１４ｂにそれぞれ接続されている。変速機入力軸５ｂ、変速機４ｂ、変速機出力軸６ｂ、ブレーキ２５、減速機１４ｂ、減速機出力軸１５ｂは、第２軸２と同軸である。
【００２０】
さらに、図２において、減速機１４ｃは、枠体１６（図１参照）の側面に取り付けられ、減速機１４ｃの減速機出力軸１５ｃは下節２０（図１参照）の一端に接続されている。変速機４ｃの変速機入力軸５ｃは差動傘歯車１１に、変速機出力軸６ｃはブレーキ２５及び減速機１４ｃにそれぞれ接続されている。変速機入力軸５ｃ、変速機４ｃ、変速機出力軸６ｃ、ブレーキ２５、減速機１４ｃ、減速機出力軸１５ｃは、第３軸３と同軸である。
【００２１】
図３は、この発明の実施の形態１に係る作業補助アームの要部の構成を示すブロック図である。
【００２２】
図３において、この発明の実施の形態１に係る作業補助アームは、変速機（無段変速機）４と、ブレーキ２５と、回転角度検出器４０と、制御装置４１と、面指示手段４２とが設けられている。また、制御装置４１には、補正用案内面計算手段３０１が設けられている。
【００２３】
なお、ブロックＤ１は、回転角度検出器４０により検出された第１軸１、第２軸２、第３軸３の各軸の角度Θ＝（θ１，θ２，θ３）を示し、ブロックＤ２は、補正用法線方向ベクトルに基づく（式２４）を展開して変速比を計算することを示し、ブロックＤ３は、計算された変速機の変速比を示す。
【００２４】
図４は、この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【００２５】
図４において、補正用案内面計算手段３０１は、前処理フロー３０２と、補正用案内面計算フローＡ３０３とからなる。
【００２６】
また、前処理フロー３０２は、回転角度検出器４０により検出されたアームの各軸の回転角度を作業座標に変換する正変換手段３０５を含む。
【００２７】
さらに、補正用案内面計算フローＡ３０３は、案内面外の点ｐを与えたとき、ｐに最も近い案内面上の点（垂線の足）ｑを求める、つまり任意の点から案内面に下ろした垂線の足の座標を計算する垂線の足計算手段３０６と、案内面外の点ｐｃを通る適当なベクトルと案内面との交点ｑｃを計算し、交点ｑｃが複数あるときには点ｐｃに近い方を選ぶ交点計算手段３０７と、速度ベクトルｄと誤差ベクトルｅから接平面投影速度ベクトル（チルダｄ）を計算する接平面投影速度ベクトル計算手段３０８と、上記各手段の出力に基づき、つまり（式２０）から補正用法線方向ベクトルを計算する補正用法線方向ベクトル計算手段Ａ（第１の補正用法線方向ベクトル計算手段）３０９とを含む。
【００２８】
つぎに、この実施の形態１に係る作業補助アームの動作について図面を参照しながら説明する。図５は、この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【００２９】
図３に示すように、回転角度検出器４０により検出されたアームの各軸の回転角度と、面指示手段４２により指示された仮想的な案内面の情報は、制御装置４１に送られ、所定の演算が行われ、この演算の結果として変速比が得られ、変速機４の変速比が制御される。また、制御装置４１は、非常停止ボタン（図示していない）が押されたときなどはブレーキ２５を動作させるよう構成されている。
【００３０】
ユーザは、作業補助アームが動作すべき仮想的な案内面１０１を面指示手段４２により指示する。例えば、案内面１０１が平面である場合、案内面１０１に立てた法線方向ベクトル
【００３１】
【数１】
【００３２】
と、案内面上のひとつの点の座標
【００３３】
【数２】
【００３４】
とを指定すれば、ひとつの平面を指定することができる。この場合、面指示手段４２は、これらの値を制御装置４１に伝える。
【００３５】
また、例えば、案内面１０１が水平面の座標ｘとｙの関数で、
【００３６】
【数３】
【００３７】
と表されるような曲面の場合、この曲面のｘ方向、ｙ方向それぞれの接線方向ベクトルを、
【００３８】
【数４】
【００３９】
とすれば、接線方向ベクトルは、
【００４０】
【数５】
【００４１】
となる。（式４）、（式５）のベクトルで張られる平面は、図５に示すように、案内面１０１の曲面の接平面３１４となる。（式４）、（式５）のベクトルを用いて案内面１０１の曲面の法線方向ベクトルｎを次のように表すことができる。
【００４２】
【数６】
【００４３】
ここで、×はベクトルの外積を表す。また、（式６）などの分母の二重線はベクトルの大きさ（長さ）を表す。この場合、面指示手段４２は、（式３）とアーム手先位置の座標をもとに（式４）、（式５）、（式６）を用いて法線方向ベクトルを演算して制御装置４１に伝える。
【００４４】
以下では補正用案内面計算手段３０１におけるひとつの演算の手順について図４と図５を用いて説明する。この実施の形態１の演算の手順を補正用案内面計算フローＡ３０３と呼ぶ。図５でアーム手先２６の進行方向は、左から右としている。
【００４５】
まず、補正用案内面計算フローＡ３０３の演算の手順の概要を説明する。アーム手先位置が本来の案内面１０１上にないとき、前回の速度ベクトルを本来の案内面１０１に投影したベクトルと、誤差ベクトルを用いて、幾何学的に補正した補正用案内面３１２の補正用法線方向ベクトル３１３を求める。このときの補正用法線方向ベクトル３１３は、前回の速度ベクトルが次回も維持されると仮定し、前回の速度ベクトルを本来の案内面１０１に投影したベクトルと、今回の誤差ベクトル３１６と、次回に予想される誤差である離隔ベクトル３１７の合ベクトルを考え、これを含むような補正用案内面３１２に設定する。
【００４６】
次に、補正用案内面計算フローＡ３０３の演算の手順の詳細を説明する。まず、垂線の足計算手段３０６と交点計算手段３０７について説明する。
【００４７】
垂線の足計算手段３０６では以下の演算が行われる。案内面外の点ｐ＝（ｐｘ，ｐｙ，ｐｚ）を与えたとき、ｐに最も近い案内面上の点ｑ＝（ｑｘ，ｑｙ，ｑｚ）を求める。点ｑは垂線の足である。このとき、ベクトルｑ−ｐは、点ｑにおいて案内面に垂直なので次の式が成り立つ。
【００４８】
【数７】
【００４９】
ここで、●はベクトルの内積を表す。また、点ｑは案内面上の点なので（式３）を用いて次の式が成り立つ。
【００５０】
【数８】
【００５１】
（式７）、（式８）、（式９）を連立させれば、与えられた点ｐに対する点ｑを得ることができる。この関数を垂線の足計算手段３０６と呼び、次のように定義する。
【００５２】
【数９】
【００５３】
また、交点計算手段３０７では以下の演算が行われる。案内面外の点ｐ＝（ｐｘ，ｐｙ，ｐｚ）を通る適当なベクトルｖ＝（ｖｘ，ｖｙ，ｖｚ）と案内面との交点ｑ＝（ｑｘ，ｑｙ，ｑｚ）を求める。ｋを適当な係数とすれば次の式が成り立つ。
【００５４】
【数１０】
【００５５】
また、ｑは案内面上の点なので（式９）が成り立つ。（式９）と（式１１）を連立させれば、与えられた点ｐと与えられたベクトルｖに対する点ｑと係数ｋを得ることができる。点ｑが複数あるときには点ｐに近い方を選ぶ。この関数を交点計算手段３０７と呼び、係数ｋは必要ないので次のように定義する。
【００５６】
【数１１】
【００５７】
続いて、補正用案内面計算フローＡ３０３の演算の手順の詳細を説明する。回転角度検出器４０により検出されたアームの各軸の回転角度は、正変換手段３０５により作業座標に変換される。以下座標は作業座標で表現する。
【００５８】
前回のｉ−１回目、今回のｉ回目の案内面１０１外の点の座標を、図５に示すように、それぞれｐｉ−１、ｐｉとする。これらの点から案内面１０１に下ろした垂線の座標をそれぞれｑｉ−１、ｑｉとする。垂線の足計算手段３０６により、（式１０）で定義した関数を用いれば、
【００５９】
【数１２】
【００６０】
などとなる。点ｐｉが案内面１０１に対してもつ誤差ベクトル３１６をｅｉとする。このとき次の式のようになる。
【００６１】
【数１３】
【００６２】
前回の速度ベクトルをｄｉ−１とする。
【００６３】
【数１４】
【００６４】
速度ベクトルｄｉ−１を点ｑｉにおける案内面１０１の接平面３１４に投影すれば、接平面投影速度ベクトル３１５（チルダｄｉ−１）は、誤差ベクトル３１６を用いて次の式のようになる。（なお、チルダｄは、ｄの上に〜があることを表す。）
【００６５】
【数１５】
【００６６】
ここで、速度ベクトルｄｉ−１と誤差ベクトル３１６（ｅｉ）から接平面投影速度ベクトル３１５（チルダｄｉ−１）を得る計算を接平面投影速度ベクトル計算手段３０８と呼ぶ。
【００６７】
点ｑｉからチルダｄｉ−１進んだ点を、図５に示すように、ｐｃｉ＋１とする。
【００６８】
【数１６】
【００６９】
点ｐｃｉ＋１から誤差ベクトル３１６（ｅｉ）方向に直線を伸ばし、交点計算手段３０７により、（式１２）で定義した関数を用いて案内面１０１と交差した点を求め、ｑｃｉ＋１とする。
【００７０】
【数１７】
【００７１】
交点ｑｃｉ＋１と点ｐｃｉ＋１の間の誤差を離隔ベクトル３１７と呼び、ｅｃｉ＋１とおく。
【００７２】
【数１８】
【００７３】
以上を用いて補正用法線方向ベクトル計算手段Ａ３０９により、次回の補正用法線方向ベクトル３１３（ハットｎｉ＋１）を次の式のように定める。（なお、ハットｎは、ｎの上に＾があることを表す。）
【００７４】
【数１９】
【００７５】
ここで、前回の速度ベクトルｄｉ−１と、今回の補正用法線方向ベクトルｎｉの外積をａｉとおいた。
【００７６】
【数２０】
【００７７】
以上のようにして補正用案内面計算フローＡ３０３の演算により補正用法線方向ベクトル３１３（ハットｎ）が得られた。この補正用法線方向ベクトル３１３（ハットｎ）を用いて各変速機４ａ、４ｂ、４ｃの変速比を以下のようにして求める。
【００７８】
関節の角速度ベクトルΩと、直交座標系から見たアーム手先２６の速度ベクトルｖの間には、次の関係が成り立つ。
【００７９】
【数２１】
【００８０】
ここで、Ｊ［Θ］は、ヤコビ行列であり、回転角度検出器４０により検出された第１軸１、第２軸２、第３軸３の各軸の角度Θ＝（θ１，θ２，θ３）の関数である。また、ｖ＝（ｖｘ，ｖｙ，ｖｚ）は、ｘ、ｙ、ｚ方向のアーム手先２６の速度であり、Ω＝（ω１，ω２，ω３）は、第１軸１、第２軸２、第３軸３の角速度である。
【００８１】
法線方向ベクトルｎと、アーム手先２６の速度ベクトルｖの間に、次の式が成り立てば両者は直交する。
【００８２】
【数２２】
【００８３】
この（式２３）に、上記の式（式２２）を代入すれば、次の式が得られる。
【００８４】
【数２３】
【００８５】
この（式２４）を展開して、ω１、ω２、ω３の係数を得れば、各変速機４ａ、４ｂ、４ｃの変速比が求められる。
【００８６】
ここで、上記の法線方向ベクトルｎとして、（式１）又は（式６）で得られるベクトルの代わりに、（式２０）で得た補正用法線方向ベクトル３１３（ハットｎ）を用いれば、アーム手先２６は補正用案内面３１２に沿って動く。
【００８７】
次回の速度ベクトルが前回と同じであれば、アーム手先２６は補正用案内面３１２に沿って動作することで点ｑｃｉ＋１の位置で本来の案内面１０１に復帰する。次回の速度ベクトルが前回と異なっていても、以上のアルゴリズムを毎回繰り返すことで誤差が減少していく。従って、アーム手先位置の初期値から外れた面を案内面として指定したり、案内面１０１に沿って移動中になんらかの原因でアーム手先位置が案内面から外れたりしたときにも案内面１０１に戻って来られる効果がある。
【００８８】
なお、（式１７）で点ｑｉから進んだ点ｐｃｉ＋１の座標を求める際、次回の速度ベクトルが前回の速度ベクトルと同じであるとの仮定で前回の接平面投影速度ベクトル３１５（チルダｄｉ−１）を加えているが、前回の接平面投影速度ベクトル３１５に適当なゲインを与えたものを加えてもよい。また、前回の接平面投影速度ベクトル３１５の代わりに適当な速度ベクトルを予測したものを加えるようにしてもよい。
【００８９】
また、ユーザが面指示手段４２に案内面１０１を指示するとき、必ずしも面を記述する情報として案内面１０１に立てた法線方向ベクトルｎと面上のひとつの点の座標ｐ＝（ｘ０，ｙ０，ｚ０）を指定しなくてもよい。例えば、案内面１０１が平面であれば、面上の点３点の座標を指示するなどでもよい。
【００９０】
実施の形態２．
この発明の実施の形態２に係る作業補助アームについて図６から図９までを参照しながら説明する。図６は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の構成を示す図である。なお、この発明の実施の形態２に係る作業補助アームの基本的な構成は、上記の実施の形態１と同様である。
【００９１】
図６において、補正用案内面計算手段３０１は、前処理フロー３０２と、補正用案内面計算フローＢ３０４とからなる。
【００９２】
また、前処理フロー３０２は、回転角度検出器４０により検出されたアームの各軸の回転角度を作業座標に変換する正変換手段３０５を含む。
【００９３】
さらに、補正用案内面計算フローＢ３０４は、案内面外の点ｐを与えたとき、ｐに最も近い案内面上の点（垂線の足）ｑを求める、つまり任意の点から案内面に下ろした垂線の足の座標を計算する垂線の足計算手段３０６と、案内面外の点ｐｃを通る鉛直方向ベクトルｇと案内面との交点ｑｃを計算し、交点ｑｃが複数あるときには点ｐｃに近い方を選ぶ交点計算手段３０７と、速度ベクトルｄと誤差ベクトルｅから接平面投影速度ベクトル（チルダｄ）を計算する接平面投影速度ベクトル計算手段３０８と、上記各手段の出力に基づき、つまり（式３８）から補正用法線方向ベクトルを計算する補正用法線方向ベクトル計算手段Ｂ（第２の補正用法線方向ベクトル計算手段）３１０とを含む。
【００９４】
つぎに、この実施の形態２に係る作業補助アームの動作について図面を参照しながら説明する。図７は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の例）を説明するための図である。また、図８は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の別の例）を説明するための図である。さらに、図９は、この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【００９５】
この実施の形態２を説明するに当たり、まず、上記の実施の形態１で生じることがある問題点について説明する。図５に示すように、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面に近づく方向である場合は、上記の実施の形態１で述べた方法でアーム手先２６は補正用案内面３１２に沿って動作することで、本来の案内面１０１に復帰することができる。
【００９６】
これに対して、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面から遠ざかる方向である場合、上記の実施の形態１と同様に前回の速度ベクトルを本来の案内面１０１に投影したベクトルと、誤差ベクトルを用いて案内面を幾何学的に補正する方法を適用した場合を図７と図８に示す。図中、アーム手先２６の進行方向は左から右としている。
【００９７】
図７のように、案内面１０１からの誤差が小さいときは、補正用案内面３１２は右上あがりになるのでアーム手先２６を左から右へ移動させれば、補正用案内面３１２に沿って動作するので案内面１０１へ近づき誤差が小さくなる。
【００９８】
ところが、図８のように、案内面１０１からの誤差が大きいときは、補正用案内面３１２の傾きが鉛直軸を通り越して左上あがりになるので、前回の速度ベクトルの方向と同様にアーム手先２６を左から右へ移動させると案内面１０１からかえって遠ざかってしまう。このため、案内面１０１からの誤差が大きいときには、誤差を補正するためにもとの速度（図では左から右）とは逆向きの速度（図では右から左）を与えてバックさせなくてはならなくなり、直感的に操作することが難しくなる問題がある。
【００９９】
そこで、速度ベクトルの水平方向成分が案内面から遠ざかるときのため、上記の実施の形態１とは別な補正方法を考える。
【０１００】
以下では補正用案内面計算フローＢ３０４の演算の手順について、図６と図９を用いて説明する。まず、手順の概要を説明する。
【０１０１】
ここでは、上記の実施の形態１の離隔ベクトル３１７の代りに、鉛直方向ベクトル３１８を用いる。このときの補正用法線方向ベクトル３１３は、前回の速度ベクトルが次回も維持されると仮定し、前回の速度ベクトルを本来の案内面１０１に投影した接平面投影速度ベクトル３１５の分を今回の位置から延長し、そこから鉛直方向に伸ばした直線と本来の案内面１０１との交点までのベクトルを考え、これらの合ベクトルを含むような補正用案内面３１２に設定する。
【０１０２】
次に、補正用案内面計算フローＢ３０４の演算の手順の詳細を説明する。回転角度検出器４０により検出されたアームの各軸の角度は、正変換手段３０５により作業座標に変換される。以下座標は作業座標で表現する。
【０１０３】
前回のｉ−１回目、今回のｉ回目の案内面外の点の座標を、図９に示すように、それぞれｐｉ−１、ｐｉとする。これらの点から案内面１０１に下ろした垂線の座標をそれぞれｑｉ−１、ｑｉとする。垂線の足計算手段３０６により、（式１０）で定義した関数を用いれば、
【０１０４】
【数２４】
【０１０５】
などとなる。点ｐｉが案内面１０１に対してもつ誤差ベクトルをｅｉとする。このとき、次の式のようになる。
【０１０６】
【数２５】
【０１０７】
前回の速度ベクトルをｄｉ−１とする。
【０１０８】
【数２６】
【０１０９】
速度ベクトルｄｉ−１を案内面１０１に投影すれば、接平面投影速度ベクトル３１５（チルダｄｉ−１）は、誤差ベクトル３１６を用いて次の式のようになる。
【０１１０】
【数２７】
【０１１１】
ここで、速度ベクトルｄｉ−１と誤差ベクトル３１６（ｅｉ）から接平面投影速度ベクトル３１５（チルダｄｉ−１）を得る計算を接平面投影速度ベクトル計算手段３０８と呼ぶ。
【０１１２】
点ｐｉから接平面投影速度ベクトル３１５（チルダｄｉ−１）進んだ点をｐｃｉ＋１とする。
【０１１３】
【数２８】
【０１１４】
この点ｐｃｉ＋１から鉛直方向ベクトル３１８（ｇ）方向に直線を伸ばし、交点計算手段３０７により、（式１２）で定義した関数を用いて案内面１０１と交差した点求め、ｑｃｉ＋１とする。
【０１１５】
【数２９】
【０１１６】
以上を用いて補正用法線方向ベクトル計算手段Ｂ３１０により、次回の補正用法線方向ベクトル３１３（ハットｎｉ＋１）を次の式のように定める。
【０１１７】
【数３０】
【０１１８】
ここで、前回の速度ベクトルｄｉ−１と、今回の法線方向ベクトルｎｉの外積をａｉとおいた。
【０１１９】
【数３１】
【０１２０】
以上で（式３５）、（式３６）、（式３７）、（式３８）が、上記の実施の形態１と異なる。
【０１２１】
以上のようにして補正用案内面計算フローＢ３０４の演算により補正用法線方向ベクトル３１３（ハットｎ）が得られた。この補正用法線方向ベクトル３１３（ハットｎ）を用いて各変速機４ａ、４ｂ、４ｃの変速比は、上記の実施の形態１と同様に求めることができる。
【０１２２】
ここで、法線方向ベクトルｎとして、（式１）又は（式６）で得られるベクトルの代わりに、（式３８）で得た補正用法線方向ベクトル３１３（ハットｎ）を用いれば、アーム手先２６は補正用案内面３１２に沿って動く。
【０１２３】
次回の速度ベクトルが前回と同じであれば、アーム手先２６は補正用案内面３１２に沿って動作することで点ｑｃｉ＋１の位置で本来の案内面１０１に復帰する。次回の速度ベクトルが前回と異なっていても、以上のアルゴリズムを毎回繰り返すことで誤差が減少していく。従って、アーム手先位置の初期値から外れた面を案内面として指定したり、案内面１０１に沿って移動中になんらかの原因でアーム手先位置が案内面から外れたりしたときにも案内面１０１に戻って来られる効果がある。
【０１２４】
さらに、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１から遠ざかる方向であり、かつ案内面１０１からの誤差が大きいときに、前回の速度ベクトルの方向と同じ方向にアーム手先２６を移動させると案内面１０１からかえって遠ざかってしまうことがなくなり、アーム手先２６を補正用案内面３１２に沿って概ね同じ方向へ動作することで本来の案内面１０１に復帰できるようになる効果がある。
【０１２５】
なお、（式３５）で点ｐｉから進んだ点ｐｃｉ＋１の座標を求める際、次回の速度ベクトルが前回の速度ベクトルと同じであるとの仮定で前回の接平面投影速度ベクトル３１５（チルダｄｉ−１）を加えているが、前回の接平面投影速度ベクトル３１５に適当なゲインを与えたものを加えてもよい。また、前回の接平面投影速度ベクトル３１５の代わりに適当な速度ベクトルを予測したものを加えるようにしてもよい。
【０１２６】
また、ユーザが面指示手段４２に案内面１０１を指示するとき、必ずしも面を記述する情報として案内面１０１に立てた法線方向ベクトルｎと面上のひとつの点の座標ｐ＝（ｘ０，ｙ０，ｚ０）を指定しなくてもよい。例えば、案内面１０１が平面であれば面上の点３点の座標を指示するなどでもよい。
【０１２７】
実施の形態３．
この発明の実施の形態３に係る作業補助アームについて図１０から図１２までを参照しながら説明する。
【０１２８】
図１０は、この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの向きの場合分けを説明するための図である。また、図１１は、この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの水平方向成分が案内面に近づく方向か遠ざかる方向かの判定方法を説明するための図である。さらに、図１２は、この発明の実施の形態３に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【０１２９】
図１２において、補正用案内面計算手段３０１は、前処理フロー３０２と、速度ベクトル判定手段３１１と、補正用案内面計算フローＡ（第１の補正用案内面計算詳細手段）３０３と、補正用案内面計算フローＢ（第２の補正用案内面計算詳細手段）３０４とからなる。
【０１３０】
また、前処理フロー３０２は、回転角度検出器４０により検出されたアームの各軸の回転角度を作業座標に変換する正変換手段３０５を含む。
【０１３１】
速度ベクトル判定手段３１１は、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向である場合は、補正用案内面計算フローＡ３０３の演算を選択し、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１から遠ざかる方向である場合は、補正用案内面計算フローＢ３０４の演算を選択する。
【０１３２】
この実施の形態３は、補正用案内面計算手段３０１における演算として、上記の実施の形態１で説明した補正用案内面計算フローＡ３０３の方法と、上記の実施の形態２で説明した補正用案内面計算フローＢ３０４の方法のどちらを適用するかを判定していずれかを処理するものである。
【０１３３】
まず、現在のアーム手先位置
【０１３４】
【数３２】
【０１３５】
が指示された案内面１０１の面外にあるとき、現在のアーム手先２６の速度ベクトル
【０１３６】
【数３３】
【０１３７】
の水平方向成分（ｖｘ，ｖｙ，０）が案内面１０１へ近付く方向である場合と遠ざかる方向である場合に関して、図１０に示すように、場合分けを行う。
【０１３８】
ある点（ｘ０，ｙ０，ｚ０）を通って（式１）で表す法線方向ベクトルに直交する案内面１０１の接平面３１４の式は、次のようになる。
【０１３９】
【数３４】
【０１４０】
（式５１）で表す現在のアーム手先位置と、案内面１０１の上下関係については次のように表現できる。案内面上の点を、
【０１４１】
【数３５】
【０１４２】
とする。このとき、
【０１４３】
【数３６】
【０１４４】
として、点（ｘ０，ｙ０，ｚ０）がｑ近傍であるとすれば、
【０１４５】
【数３７】
【０１４６】
ならば、現在の手先位置は案内面１０１より上側、
【０１４７】
【数３８】
【０１４８】
ならば、現在の手先位置は案内面１０１より下側となる。
【０１４９】
次に、（式５２）で表す現在のアーム手先２６の速度ベクトルと、（式１）で表す案内面１０１の法線方向ベクトルの水平方向成分については、それぞれのベクトルの水平方向成分同士の内積の符号を用いて、
【０１５０】
【数３９】
【０１５１】
ならば、それぞれのベクトルの水平方向成分は逆方向、
【０１５２】
【数４０】
【０１５３】
ならば、それぞれのベクトルの水平方向成分は同じ方向となる。
【０１５４】
これらの関係の組み合わせにより、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向か、遠ざかる方向かを図１１のようにまとめることができる。
【０１５５】
図１２に示す速度ベクトル判定手段３１１は、図１１の場合分けにより、（式５７）と（式５９）を用いて、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向である場合は、上記の実施の形態１で説明した補正用案内面計算フローＡ３０３の演算を選択する。また、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１から遠ざかる方向である場合は、上記の実施の形態２で説明した補正用案内面計算フローＢ３０４の演算を選択する。
【０１５６】
以上の判定と演算により、現在のアーム手先２６の速度ベクトルの水平方向成分が案内面１０１に近づく方向であっても、遠ざかる方向であっても、アーム手先２６は補正用案内面３１２に沿って動作することで本来の案内面１０１に復帰するという効果がある。
【図面の簡単な説明】
【０１５７】
【図１】この発明の実施の形態１に係る作業補助アームの全体の構成を示す斜視図である。
【図２】この発明の実施の形態１に係る作業補助アームの駆動部分をアームの正面から見た構成を示す図である。
【図３】この発明の実施の形態１に係る作業補助アームの要部の構成を示すブロック図である。
【図４】この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【図５】この発明の実施の形態１に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【図６】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【図７】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の例）を説明するための図である。
【図８】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面の別の例）を説明するための図である。
【図９】この発明の実施の形態２に係る作業補助アームの補正用案内面計算手段の動作（補正用案内面作成方法）を説明するための図である。
【図１０】この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの向きの場合分けを説明するための図である。
【図１１】この発明の実施の形態３に係る作業補助アームおけるアーム手先の速度ベクトルの水平方向成分が案内面に近づく方向か遠ざかる方向かの判定方法を説明するための図である。
【図１２】この発明の実施の形態３に係る作業補助アームの補正用案内面計算手段の構成を示す図である。
【符号の説明】
【０１５８】
４ 変速機、２５ ブレーキ、２６ アーム手先、４０ 回転角度検出器、４１ 制御装置、４２ 面指示手段、３０１ 補正用案内面計算手段、３０２ 前処理フロー、３０３ 補正用案内面計算フローＡ、３０４ 補正用案内面計算フローＢ、３０５ 正変換手段、３０６ 垂線の足計算手段、３０７ 交点計算手段、３０８ 接平面投影速度ベクトル計算手段、３０９ 補正用法線方向ベクトル計算手段Ａ、３１０ 補正用法線方向ベクトル計算手段Ｂ、３１１ 速度ベクトル判定手段。
【特許請求の範囲】
【請求項１】
複数の軸の変位量に基づいてその作業座標が決定されるアームを設け、前記複数の軸をそれぞれ対応した複数の変速機を介して差動機構で結合し、前記複数の変速機の変速比を、指示された案内面以外へのアーム手先の移動を拘束するよう制御装置によって制御するようにした作業補助アームであって、
前記アームの複数の軸の回転角度を検出する回転角度検出器と、
ユーザにより指示された仮想的な案内面を入力する面指示手段とを備え、
前記制御装置は、
前記回転角度検出器により検出された回転角度、及び前記面指示手段により指示された仮想的な案内面に基づいて、現在のアーム手先の位置を含み、かつ前記指示された案内面とは平行ではない補正用案内面を計算する補正用案内面計算手段を含み、
前記補正用案内面計算手段により計算された補正用案内面に沿って、前記アーム手先が動くように前記複数の変速機の変速比を求める
ことを特徴とする作業補助アーム。
【請求項２】
前記補正用案内面計算手段は、
前記回転角度検出器により検出されたアームの複数の軸の回転角度を作業座標に変換する正変換手段と、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する接平面投影速度ベクトル計算手段と、
前記第２の点から接平面投影速度ベクトルだけ進んだ第３の点を通る離隔ベクトルと案内面と交差する第４の点を計算する交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第１の補正用法線方向ベクトル計算手段とを含む
ことを特徴とする請求項１記載の作業補助アーム。
【請求項３】
前記補正用案内面計算手段は、
前記回転角度検出器により検出されたアームの複数の軸の回転角度を作業座標に変換する正変換手段と、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する接平面投影速度ベクトル計算手段と、
前記第１の点から接平面投影速度ベクトルだけ進んだ第３の点を通る鉛直方向ベクトルと案内面と交差する第４の点を計算する交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第２の補正用法線方向ベクトル計算手段とを含む
ことを特徴とする請求項１記載の作業補助アーム。
【請求項４】
前記補正用案内面計算手段は、
前記回転角度検出器により検出されたアームの複数の軸の回転角度を作業座標に変換する正変換手段と、
前記作業座標上の、現在のアーム手先の速度ベクトルの水平方向成分が案内面に近づく方向である場合は、第１の補正用案内面計算詳細手段へ進み、現在のアーム手先の速度ベクトルの前記作業座標上の水平方向成分が案内面から遠ざかる方向である場合は、第２の補正用案内面計算詳細手段へ進む速度ベクトル判定手段と、
前記補正用案内面の補正用法線方向ベクトルを計算する第１の補正用案内面計算詳細手段と、
前記補正用案内面の補正用法線方向ベクトルを計算する第２の補正用案内面計算詳細手段とを含み、
前記第１の補正用案内面計算詳細手段は、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する第１の垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する第１の接平面投影速度ベクトル計算手段と、
前記第２の点から接平面投影速度ベクトルだけ進んだ第３の点を通る離隔ベクトルと案内面と交差する第４の点を計算する第１の交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第１の補正用法線方向ベクトル計算手段とを含むとともに、
前記第２の補正用案内面計算詳細手段は、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する第２の垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する第２の接平面投影速度ベクトル計算手段と、
前記第１の点から接平面投影速度ベクトルだけ進んだ第３の点を通る鉛直方向ベクトルと案内面と交差する第４の点を計算する第２の交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第２の補正用法線方向ベクトル計算手段とを含む
ことを特徴とする請求項１記載の作業補助アーム。
【請求項１】
複数の軸の変位量に基づいてその作業座標が決定されるアームを設け、前記複数の軸をそれぞれ対応した複数の変速機を介して差動機構で結合し、前記複数の変速機の変速比を、指示された案内面以外へのアーム手先の移動を拘束するよう制御装置によって制御するようにした作業補助アームであって、
前記アームの複数の軸の回転角度を検出する回転角度検出器と、
ユーザにより指示された仮想的な案内面を入力する面指示手段とを備え、
前記制御装置は、
前記回転角度検出器により検出された回転角度、及び前記面指示手段により指示された仮想的な案内面に基づいて、現在のアーム手先の位置を含み、かつ前記指示された案内面とは平行ではない補正用案内面を計算する補正用案内面計算手段を含み、
前記補正用案内面計算手段により計算された補正用案内面に沿って、前記アーム手先が動くように前記複数の変速機の変速比を求める
ことを特徴とする作業補助アーム。
【請求項２】
前記補正用案内面計算手段は、
前記回転角度検出器により検出されたアームの複数の軸の回転角度を作業座標に変換する正変換手段と、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する接平面投影速度ベクトル計算手段と、
前記第２の点から接平面投影速度ベクトルだけ進んだ第３の点を通る離隔ベクトルと案内面と交差する第４の点を計算する交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第１の補正用法線方向ベクトル計算手段とを含む
ことを特徴とする請求項１記載の作業補助アーム。
【請求項３】
前記補正用案内面計算手段は、
前記回転角度検出器により検出されたアームの複数の軸の回転角度を作業座標に変換する正変換手段と、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する接平面投影速度ベクトル計算手段と、
前記第１の点から接平面投影速度ベクトルだけ進んだ第３の点を通る鉛直方向ベクトルと案内面と交差する第４の点を計算する交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第２の補正用法線方向ベクトル計算手段とを含む
ことを特徴とする請求項１記載の作業補助アーム。
【請求項４】
前記補正用案内面計算手段は、
前記回転角度検出器により検出されたアームの複数の軸の回転角度を作業座標に変換する正変換手段と、
前記作業座標上の、現在のアーム手先の速度ベクトルの水平方向成分が案内面に近づく方向である場合は、第１の補正用案内面計算詳細手段へ進み、現在のアーム手先の速度ベクトルの前記作業座標上の水平方向成分が案内面から遠ざかる方向である場合は、第２の補正用案内面計算詳細手段へ進む速度ベクトル判定手段と、
前記補正用案内面の補正用法線方向ベクトルを計算する第１の補正用案内面計算詳細手段と、
前記補正用案内面の補正用法線方向ベクトルを計算する第２の補正用案内面計算詳細手段とを含み、
前記第１の補正用案内面計算詳細手段は、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する第１の垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する第１の接平面投影速度ベクトル計算手段と、
前記第２の点から接平面投影速度ベクトルだけ進んだ第３の点を通る離隔ベクトルと案内面と交差する第４の点を計算する第１の交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第１の補正用法線方向ベクトル計算手段とを含むとともに、
前記第２の補正用案内面計算詳細手段は、
前記作業座標上の案内面外の第１の点から案内面に下ろした垂線の足である第２の点の座標を計算する第２の垂線の足計算手段と、
前回及び今回の第１の点間の速度ベクトルと第１及び第２の点間の誤差ベクトルから接平面投影速度ベクトルを計算する第２の接平面投影速度ベクトル計算手段と、
前記第１の点から接平面投影速度ベクトルだけ進んだ第３の点を通る鉛直方向ベクトルと案内面と交差する第４の点を計算する第２の交点計算手段と、
前記各手段の出力、並びに前記案内面の法線方向ベクトルに基づき、前記補正用案内面の補正用法線方向ベクトルを計算する第２の補正用法線方向ベクトル計算手段とを含む
ことを特徴とする請求項１記載の作業補助アーム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２００８−２３８３７１（Ｐ２００８−２３８３７１Ａ）
【公開日】平成２０年１０月９日（２００８．１０．９）
【国際特許分類】
【出願番号】特願２００７−８５３１６（Ｐ２００７−８５３１６）
【出願日】平成１９年３月２８日（２００７．３．２８）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２０年１０月９日（２００８．１０．９）
【国際特許分類】
【出願日】平成１９年３月２８日（２００７．３．２８）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]