(57)【要約】【課題】マニピュレータ装置の操作性及び安全性を高めて直接教示を行う。【解決方法】連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、前記移動部に加わる外力を検知し、前記移動部における前記移動部とユーザとの接触箇所を検知し、前記外力が検知された場合に、前記移動部を移動させるために、検知された前記接触箇所の位置及び複数の前記移動部が形成する角度に基づいて前記駆動部の駆動量を制御する。【選択図】図１４
2017202554.tif 000002
【特許請求の範囲】
【請求項１】
連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、
前記移動部に加わる外力を検知する外力検知部と、
前記移動部における前記移動部とユーザとの接触箇所を検知する接触検知部と、
前記外力が検知された場合に、前記移動部を移動させるための駆動部を制御する駆動制御部と、
を含み、
前記駆動制御部は、
検知された前記接触箇所の位置及び複数の前記移動部が形成する角度に基づいて前記駆動部の駆動量を制御することを特徴とするマニピュレータ装置の制御装置。
【請求項２】
前記駆動制御部は、
検知された前記接触箇所が前記交差箇所の近傍である場合に、前記駆動量を減少させることを特徴とする請求項１に記載のマニピュレータ装置の制御装置。
【請求項３】
前記駆動制御部は、
検知された前記接触箇所が前記交差箇所の近傍である場合に、前記駆動量を０にすることを特徴とする請求項１または請求項２に記載のマニピュレータ装置の制御装置。
【請求項４】
前記駆動制御部は、
検知された前記接触箇所が前記交差箇所の近傍である場合に、前記駆動量を増加させることを特徴とする請求項１に記載のマニピュレータ装置の制御装置。
【請求項５】
連結された複数の前記移動部が形成する角度を検知する角度検知部を含み、
前記駆動制御部は、
検知された前記角度に基づいて前記駆動量を制御することを特徴とする請求項１に記載のマニピュレータ装置の制御装置。
【請求項６】
前記駆動制御部は、
検知された前記角度が所定の角度よりも小さい場合に、前記駆動量を減少させることを特徴とする請求項５に記載のマニピュレータ装置の制御装置。
【請求項７】
前記駆動制御部は、
検知された前記角度が所定の角度よりも小さい場合に、前記駆動量を０にすることを特徴とする請求項５または請求項６に記載のマニピュレータ装置の制御装置。
【請求項８】
前記駆動制御部は、
検知された前記接触箇所の位置に基づいて前記駆動量を制御し、
前記角度検知部は、
前記移動部が移動させられた場合に前記角度を検知し、
前記駆動制御部は、
検知された前記角度に基づいて前記駆動量を制御することを特徴とする請求項５ないし請求項７に記載のマニピュレータ装置の制御装置。
【請求項９】
連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御方法であって、
前記移動部に加わる外力を検知し、
前記移動部における前記移動部とユーザとの接触箇所を検知し、
前記外力が検知された場合に、前記移動部を移動させるために、検知された前記接触箇所の位置に基づいて前記移動部を移動させる駆動部の駆動量を制御することを特徴とするマニピュレータ装置の制御方法。
【請求項１０】
連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御プログラムであって、
前記移動部に加わる外力を検知するステップと、
前記移動部における前記移動部とユーザとの接触箇所を検知するステップと、
前記外力が検知された場合に、前記移動部を移動させるために、検知された前記接触箇所の位置に基づいて前記移動部を移動させる駆動部の駆動量を制御するステップと
を実行させることを特徴とするマニピュレータ装置の制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレータ装置の制御装置、マニピュレータ装置の制御方法及びマニピュレータ装置の制御プログラムに関する。
【背景技術】
【０００２】
近年、製造工程を自動化させるファクトリーオートメーションにおいては、本来、人が行う動作や作業をロボットアーム等のマニピュレータ装置に行わせることで製造品質の向上や製造工程を容易に切り替える分散プロセス化が行われている（例えば、特許文献１参照）。
【０００３】
このようなマニピュレータ装置は、ティーチングプレイバック（教示作業）という方法で動作する。このティーチングプレイバックには、操作盤やタブレット端末等の入力装置を用いて、マニピュレータ装置の動作を指定する間接教示と、マニピュレータ装置の先端を模した装置やハンドルをユーザが操作して動作を指定する直接教示とがある。
【０００４】
直接教示においては、マニピュレータ装置を小さな力で直接教示できるようにモータを駆動させるアシスト制御が行われる。
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、特許文献１に開示された技術においては、直接教示を行う際に、マニピュレータ装置の操作箇所によっては大きな力を加えなければならないことがある。また、関節近傍を操作する際には、関節に身体の一部が挟まれる可能性がある。
【０００６】
本発明は、上記実情を考慮してなされたものであり、マニピュレータ装置の操作性及び安全性を高めて直接教示を行うことを目的とする。
【課題を解決するための手段】
【０００７】
上記課題を解決するために、本発明の一態様は、連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、前記移動部に加わる外力を検知する外力検知部と、前記移動部における前記移動部とユーザとの接触箇所を検知する接触検知部と、前記外力が検知された場合に、前記移動部を移動させるための駆動部を制御する駆動制御部と、を含み、前記駆動制御部は、検知された前記接触箇所の位置及び複数の前記移動部が形成する角度に基づいて前記駆動部の駆動量を制御することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、マニピュレータ装置の操作性及び安全性を高めて直接教示を行うことが出来る。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施形態に係るマニピュレータ装置の機械構成図。
【図２】本発明の実施形態に係る制御装置のハードウェア構成を示すブロック図。
【図３】本発明の実施形態に係るマニピュレータ装置の動作機構を示す図。
【図４】本発明の実施形態に係る制御装置の機能構成を示すブロック図。
【図５】本発明の実施形態に係る駆動制御部の機能構成を示すブロック図。
【図６】本発明の実施形態に係るマニピュレータ装置において発生する力について説明する図。
【図７】本発明の実施形態に係る接触検知センサの設置態様を示す図。
【図８】本発明の実施形態に係る接触検知センサの他の設置態様を示す図。
【図９】本発明の実施形態に係るマニピュレータ装置の動作を直接教示する際の動作を例示した図。
【図１０】本発明の実施形態に係るマニピュレータ装置のアシスト制御の説明図。
【図１１】本発明の実施形態に係るマニピュレータ装置のアシスト制御の流れを示すフローチャート。
【図１２】本発明の実施形態に係るマニピュレータ装置の移動力目標値の設定を例示した図。
【図１３】本発明の実施形態に係るマニピュレータ装置の移動力目標値の設定を例示した図。
【図１４】本発明の実施形態に係るマニピュレータ装置におけるユーザの接触箇所及びアームの角度の形成態様を示す図。
【図１５】本発明の実施形態に係るマニピュレータ装置のアシスト量を制御する処理の流れを示したフローチャート。
【図１６】本発明の実施形態に係るマニピュレータ装置のアシスト量を例示した図。
【図１７】本発明の実施形態に係るマニピュレータ装置のアシスト量を例示した図。
【発明を実施するための形態】
【００１０】
以下、図面を参照して、本発明の実施形態を詳細に説明する。本実施形態においては、自由度が「２」である垂直多関節型のロボットアームを例として、マニピュレータ装置の関節にあたる駆動部での動作を補助するアシスト制御について説明する。
【００１１】
図１は、本実施形態に係るマニピュレータ装置１の機械構成図である。図１に示すように、本実施形態に係るマニピュレータ装置１は、アーム１０１、１０２、駆動中心１０３、１０４、減速機１０５、１０８、モータ１０６、１０９、エンコーダ１０７、１１０、土台１１１、移動検知センサ１１２、接触検知センサ１１３、１１４を含む。
【００１２】
アーム１０１、１０２は、その交差箇所を、夫々駆動中心１０３、１０４を回転軸とした回転可能な機構によって支持され、モータ１０６、１０９によって移動させられる移動部として機能する。また、土台１１１は、アーム１０１、１０２を支持する支持部として機能する。アーム１０１の先端には、把持動作を行うロボットハンド等のリンク機構が備えられており、リンク機構とアーム１０１との接続部分には、マニピュレータ装置１が移動したことを検知する移動検知センサ１１２が含まれる。この移動検知センサ１１２が、後述する外力検知部２１７と連動して機能する。
【００１３】
尚、移動検知センサ１１２として、六軸力覚センサ等を用いる構成であってもよい。本実施形態に係るマニピュレータ装置１は、図２で説明する制御装置２が出力する制御信号に基づいて動作を制御される。
【００１４】
接触検知センサ１１３、１１４は、ユーザがマニピュレータ装置１に接触したことを検知するためのセンサであり、例えば、タッチセンサや距離センサを用いることが出来る。尚、この接触検知センサ１１３、１１４が後述する接触検知部２１８と連動して機能する。
【００１５】
図２は、本実施形態に係るマニピュレータ装置１を制御する制御装置２のハードウェア構成を示すブロック図である。図２に示すように、本実施形態に係る制御装置２は、一般的なサーバやＰＣ（Ｐｅｒｓｏｎａｌ  Ｃｏｍｐｕｔｅｒ）等の情報処理端末と同様の構成に加えて、モータ等の駆動機構を制御するエンジンを有する。即ち、本実施形態に係る制御装置２は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）２１、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）２２、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）２３、エンジン２４、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）２５及びＩ／Ｆ２６がバス２９を介して接続されている。Ｉ／Ｆ２６には、センサ２７やスイッチ２８が接続され、スイッチ２８は、マニピュレータ装置１の電源を制御するものである。
【００１６】
また、センサ２７は、図１の移動検知センサ１１２、接触検知センサ１１３、１１４を含み、マニピュレータ装置１の状態を検知するものである。さらに、Ｉ／Ｆ２６にはタブレット端末等の外部装置がネットワークを介して接続され、マニピュレータ装置１の動作に関する設定を入力することが出来る。
【００１７】
ＣＰＵ２１は演算手段であり、マニピュレータ装置１全体の動作を制御する。ＲＡＭ２２は、情報の高速な読み書きが可能な揮発性の記憶媒体であり、ＣＰＵ２１が情報を処理する際の作業領域として用いられる。ＲＯＭ２３は、読み出し専用の不揮発性記憶媒体であり、ファームウェア等のプログラムが格納されている。エンジン２４は、マニピュレータ装置１において実際にモータ駆動を実行する機構である。
【００１８】
ＨＤＤ２５は、情報の読み書きが可能な不揮発性の記憶媒体であり、ＯＳ（Ｏｐｅｒａｔｉｎｇ  Ｓｙｓｔｅｍ）や各種の制御プログラム、アプリケーション・プログラム等が格納されている。Ｉ／Ｆ２６は、バス２９と各種のハードウェアやネットワーク等を接続し制御する。
【００１９】
このようなハードウェア構成において、ＲＯＭ２３に格納されたプログラムや、ＨＤＤ２５若しくは図示しない光学ディスク等の記録媒体からＲＡＭ２２に読み出されたプログラムに従ってＣＰＵ２１が演算を行うことにより、ソフトウェア制御部が構成される。このようにして構成されたソフトウェア制御部と、ハードウェアとの組み合わせによって、本実施形態に係る制御装置２の機能を実現する機能ブロックが構成される。
【００２０】
図３は、本実施形態に係るマニピュレータ装置１の動作機構を示す図である。図１に示したようなマニピュレータ装置１は、制御装置２によって動作を制御される。制御装置２はＰＣ等の演算機能を備えた端末であるため、制御装置２を操作して、マニピュレータ装置１の位置や速度をユーザが任意に設定することが出来る。また、外部装置から制御装置２に入力される命令に基づいてマニピュレータ装置１の動作を制御する構成であってもよい。
【００２１】
エンコーダ１０７、１１０は、回転速度や回転位置を検知するためのマーカーが付加された円板を含み、そのマーカーを光学的に読み取ることによって検知信号を出力する。詳細は後述する。
【００２２】
減速機１０５、１０８は、モータ１０６、１０９の回転数を減少させ、高いトルクを得るための装置である。また、モータ１０６、１０９の回転数とマニピュレータ装置１が必要とする回転数が異なる場合にモータ１０６、１０９の回転数を調節する。
【００２３】
モータ１０６、１０９は、マニピュレータ装置１の動力装置であり、マニピュレータ装置１はモータ１０６、１０９の回転駆動により、動作を行う。従って、モータ１０６、１０９は夫々回転駆動する駆動部１０、１１として機能する。
【００２４】
図４は、本実施形態に係る制御装置２の機能構成を示すブロック図である。図４に示すように、制御装置２は、駆動制御部２１１、回転駆動部２１２、ＦＢ（フィードバック）取得部２１５、位置情報取得部２１６、外力検知部２１７、接触検知部２１８を含む。本実施形態に係る制御装置２は、フィードバック制御部として機能し、マニピュレータ装置１は制御装置２から出力された信号によって動作を制御される。
【００２５】
駆動制御部２１１は、Ｉ／Ｆ２６を介して外部装置から入力される駆動部１０、１１の回転速度の目標値と、ＦＢ取得部２１５から入力されるフィードバック信号とに基づき、駆動部１０、１１を回転させるための制御値を回転駆動部２１２に対して出力する。回転駆動部２１２は、駆動制御部２１１から入力される制御値に基づき、駆動部１０、１１を回転させるためのＰＷＭ信号を生成して出力する。
【００２６】
駆動部１０、１１は、回転駆動部２１２から入力されるＰＷＭ信号に応じて回転するモータである。この駆動部１０、１１の回転によって駆動中心１０３、１０４を回転軸としてマニピュレータ装置１が動作する。駆動部１０、１１としては、ＤＣ（Ｄｉｒｅｃｔ  Ｃｕｒｒｅｎｔ）モータを用いることが可能であり、インナーロータ型ＤＣモータであるブラシレスＤＣモータやブラシ付きＤＣモータを用いることが可能である。本実施形態に係る駆動部１０、１１は、回転速度や回転位置を検知するためのマーカーが付加された円板を回転させ、エンコーダ１０７、１１０によって構成される回転検知部２１４は、そのマーカーを光学的に読み取ることによって検知信号を出力する。
【００２７】
具体的には、駆動部１０、１１の回転に伴って回転する円板に付加されたマーカーが、回転検知部２１４が光学的に読み取る読み取り位置を通過する。これにより、回転検知部２１４による光学的な読み取り状態が変化し、回転検知部２１４がマーカーを検知して検知信号を出力する。
【００２８】
尚、本実施形態に係る回転検知部２１４は２組のセンサを含む。この２組のセンサは、上述した円板に付加されたマーカーの位相差がπ／２（ｒａｄ）となるように配置されている。これにより、本実施形態に係る回転検知部２１４は、π／２（ｒａｄ）の位相差を有する２つの検知信号を出力する。
【００２９】
ＦＢ取得部２１５は、回転検知部２１４の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、マーカーが検知された回数に基づいて駆動部１０、１１の回転位置を計算し、その計算結果から駆動部１０、１１の回転速度を算出する。算出された駆動部１０、１１の回転速度は、フィードバック値として駆動制御部２１１に入力される。また、ＦＢ取得部２１５は、上述したように、π／２（ｒａｄ）の位相差を有する２つの検知信号の位相差を利用して、駆動部１０、１１の回転方向を検知する。
【００３０】
このようにして出力されるＦＢ取得部２１５のフィードバック値に基づき、駆動制御部２１１は、外部装置から入力される目標値と、ＦＢ取得部２１５から入力されるフィードバック値との差分に基づき、駆動部１０、１１を回転させるための制御値を出力する。
【００３１】
位置情報取得部２１６は、上述したような制御装置２におけるフィードバック制御のサイクルにおいて、駆動部１０、１１の回転状態を示す値である特徴量を取得する。図４において破線で示しているように、駆動部１０、１１の回転状態を示す値としては、ＦＢ取得部２１５が取得した回転検知部２１４の検知信号や、ＦＢ取得部２１５が算出した駆動部１０、１１の回転速度を用いることができる。
【００３２】
また駆動制御部２１１が回転駆動部２１２に入力する制御値を用いることができる。更に、回転駆動部２１２が出力するＰＷＭ信号を用いることができる。これらの値が、フィードバック制御において得られるフィードバック値である。位置情報取得部２１６は、取得したマニピュレータ装置１の位置情報を駆動制御部２１１に返信する。
【００３３】
外力検知部２１７は、マニピュレータ装置１の先端部分に設置される移動検知センサ１１２によってアーム１０１、１０２に力が加えられたこと（外力）を検知すると、モータ１０６、１０９への入力トルクやマニピュレータ装置１の位置及び速度等の情報を取得する。そして取得した情報に基づいて外力を検知する。
【００３４】
具体的に言うと、モータ１０６、１０９への入力トルクの情報を駆動制御部２１１から、また、ＦＢ取得部２１５及び位置情報取得部２１６からマニピュレータ装置１の位置及び速度の情報を夫々参照する。従って、外力検知部２１７は、移動検知センサ１１２から送信される制御信号を受信して外力の検知を実行する。
【００３５】
また、移動検知センサ１１２として六軸力覚センサを用いる場合には、外力及びモーメントの大きさと方向の情報を取得する。尚、本実施形態に係る外力は、外部装置を用いてユーザが任意に設定した外力値を超えない場合には、アーム１０１、１０２が移動されて発生した外力と判定しない構成であってもよい。
【００３６】
接触検知部２１８は、アーム１０１、１０２に夫々設置される接触検知センサ１１３、１１４においてアーム１０１、１０２にユーザが接触しているか否かを検知する。尚、接触検知部２１８には、ユーザがマニピュレータ装置１に接触しているか否か判定するための閾値が設定されており、この閾値を超えた場合に、接触検知部２１８は、マニピュレータ装置１にユーザが接触したと判定する。本実施形態においては、接触検知部２１８の検知結果に基づいてマニピュレータ装置１の動作を制御する。
【００３７】
尚、本実施形態に係るフィードバック制御においては、駆動制御部２１１が、駆動部１０、１１の回転速度及び位置の目標値とフィードバック値とに基づいて制御値を出力する。即ち、回転速度に基づいた制御（以降、「速度制御」とする）が行われる。また、駆動部１０、１１の回転位置に基づいた制御（以降、「位置制御」とする）が行われる。アーム１０１、１０２が移動されることで加わる外力と、装置自体の回転速度及び位置情報に基づいて、マニピュレータ装置１の動作を制御することが本実施形態に係る要旨の１つである。
【００３８】
位置制御を行う場合、ＦＢ取得部２１５は、マーカーが検知された回数に基づいて駆動部１０、１１の回転位置を計算し、フィードバック値として駆動制御部２１１に入力する。また、駆動制御部２１１には、回転位置の目標値が入力される。これにより、駆動制御部２１１は、目標値とフィードバック値との差分に基づいて駆動部１０、１１の回転を制御するための制御値を生成して出力する。これにより、駆動部１０、１１の回転位置が、入力された目標値に保たれる。
【００３９】
図５は、本実施形態に係る制御装置２の特徴的な機能を実現する駆動制御部２１１の機能構成を示す図である。図５に示すように、駆動制御部２１１は、移動力目標生成部２２１、位置速度指令生成部２２２を含む。移動力目標生成部２２１は、制御装置２に入力されるマニピュレータ装置１を移動させる移動力を設定する情報に基づいて、アーム１０１、１０２を移動させる時の移動力目標値の設定を行う。また、マニピュレータ装置１の位置や外力の変化から、移動力目標値の設定を変更して駆動部１０、１１を制御するための移動力目標情報を生成する。
【００４０】
位置速度指令生成部２２２は、制御装置２に入力されるマニピュレータ装置１の移動範囲や移動速度を設定する情報に基づいて、アーム１０１、１０２を移動させる位置速度目標値の設定を行う。また、マニピュレータ装置１の位置や移動速度の変化と目標値との差分に基づいて、駆動部１０、１１を制御しマニピュレータ装置１の位置・移動速度の設定を変更するための位置速度指令情報を生成する。
【００４１】
図６（ａ）及び（ｂ）は、本実施形態に係るマニピュレータ装置１において発生する力について説明する図である。本実施形態においては、アーム１０１、１０２が、駆動部１０、１１で接続された２関節マニピュレータの平面運動とアーム１０１の自由末端における力Ｆとの関係から駆動部１０、１１のトルクを算出する。
【００４２】
図６（ａ）は、本実施形態に係るマニピュレータ装置１のｘｙ平面での動作を模式的に示した図である。ここでは、アーム１０１の駆動部１１によってアーム１０２と連結されていない側の末端が自由末端となる。実際のマニピュレータ装置１の動作においては、この自由末端には、図１の移動検知センサ１１２が取り付けられ、外力の検知を行う。
【００４３】
図６（ｂ）は、図６（ａ）にて示したマニピュレータ装置１に発生する物理量を説明する図である。図６（ｂ）においては、夫々、Ｉｉ：慣性モーメント、Ｌｉ：アームの長さ、ｍｉ：質量、θｉ：角度、τｉ：駆動部トルク、ｒｉ：駆動部中心と質量中心との距離、ｇ：重力加速度（ｙ軸負方向）を示している。また、本実施形態においては、自由度２のマニピュレータ装置１を例として説明するので、ｉ＝１，２とする。
【００４４】
図６（ｂ）に示すように、マニピュレータ装置１がｘｙ平面で動作している時の運動方程式は、（数１）で与えられる。
【数１】
2017202554.tif 000003
【００４５】
数１において、ｑは一般化座標を示し、ｑは（数２）のようにあらわされる。
【数２】
2017202554.tif 000004
（数１）において、第一項は慣性力、第二項はコリオリ力、第三項は重力項を示す。
【００４６】
（数３）に示すように、本実施形態に係るマニピュレータ装置１に（数１）に示す運動方程式を適用すると、動作中に駆動部１０、１１に係るトルクを推定することが出来る。
【数３】
2017202554.tif 000005
【００４７】
（数３）で推定したトルクは、マニピュレータ装置１の機械構成や減速機構、剛性、粘性摩擦等を考慮することで更に精度を高めることが出来る。また、数３で推定したトルクと、アーム１０１の自由末端における力Ｆとには、（数４）に示す関係がある。
【数４】
2017202554.tif 000006
Ｊはヤコビ行列を表す。従って、アーム１０１の自由末端における力Ｆは、
【数５】
2017202554.tif 000007
によって求めることが出来る。
【００４８】
また、本実施形態に係るマニピュレータ装置１のヤコビ行列は、
【数６】
2017202554.tif 000008
である。（数６）を（数５）に代入すると、
【数７】
2017202554.tif 000009
となる。このようにして、マニピュレータ装置１の動作からアーム１０１の自由末端における力Ｆを求めることが出来る。この時求めた力Ｆは、アーム１０１に加えられた外力に相当する。
【００４９】
次に、図７及び図８を参照して、本実施形態に係るマニピュレータ装置１にユーザが接触したことを検知する際の態様について説明する。図７は、タッチセンサが用いられている接触検知センサ１１３〜１１８の設置態様を示す図である。図７に示すように、アーム１０１の側面には接触検知センサ１１３、１１４、１１５、アーム１０２の側面には接触検知センサ１１６、１１７、１１８が設置されている。尚、図７においては、接触検知センサ１１３〜１１８としてタッチセンサが用いられている。
【００５０】
接触検知センサ１１３〜１１８として、タッチセンサが用いられている場合、タッチセンサが検知した電気容量や圧力の変化が予め設定されている閾値を超えた場合に、接触検知部２１８は、ユーザがマニピュレータ装置１に接触したと判定する。
【００５１】
また、図８は光学センサが用いられている接触検知センサ１１３、１１６の設置態様を示す図である。図８に示すように、アーム１０１の表面を検出可能な位置に接触検知センサ１１３、アーム１０２の表面を検出可能な位置に接触検知センサ１１６が設置されている。尚、図８においては、接触検知センサ１１３、１１６として光学センサが用いられている。
【００５２】
接触検知センサ１１３、１１６として、光学センサが用いられている場合、光学センサが検知した光学的な変化が予め設定されている閾値を超えた場合に、接触検知部２１８は、ユーザがマニピュレータ装置１に接触したと判定する。
【００５３】
上述した接触検知センサ１１３〜１１８から入力される検知信号は、図４において説明したように制御装置２に入力され、マニピュレータ装置１を制御するための制御信号として機能する。尚、接触検知センサ１１３〜１１８には、ＣＰＵ２１内部で夫々に対して閾値が設定されており、その閾値を超えるとマニピュレータ装置１にユーザが接触したと判断される。
【００５４】
次に、図９を参照して本実施形態に係るマニピュレータ装置１を直接教示する際の動作について説明する。図９は、本実施形態に係るマニピュレータ装置１の動作を直接教示する際にユーザが行う動作を例示した図である。図９に示すように、マニピュレータ装置１を直接教示する場合、装置自体の質量や駆動部１０、１１のトルクによって、マニピュレータ装置１を移動させることが困難な場合がある。
【００５５】
そこで、本実施形態に係るマニピュレータ装置１の制御装置２は図１０に示すように、駆動部１０、１１にユーザが直接教示する方向に働くモータ出力を加える。このように、ユーザによるマニピュレータ装置１の移動を補助するために、駆動部１０、１１に対して行う制御のことを以後、「アシスト制御」とする。図９及び図１０に示すように、ユーザが移動させる方向にマニピュレータ装置１のアシスト制御を行う際に安全性及び操作性を高めて直接教示を行うことが本発明の要旨の１つである。
【００５６】
次に、図１１を参照して本実施形態に係るマニピュレータ装置１のアシスト制御の流れについて説明する。図１１は、本実施形態に係るマニピュレータ装置１が移動された際にアシスト制御する処理を示すフローチャートである。マニピュレータ装置１のアーム１０１、１０２に力が加えられたこと（外力）を検知する（Ｓ１１０１）と、外力検知部２１７は、アーム１０１、１０２が操作されて加えられた外力によるものか否か判定する（Ｓ１１０２）。
【００５７】
外力検知部２１７は、接触検知部２１８によってマニピュレータ装置１にユーザが接触したと検知された場合に、検知された外力がアーム１０１、１０２の操作によって発生したと検知する。
【００５８】
Ｓ１１０１で検知された外力がアーム１０１、１０２の操作によるものである場合（Ｓ１１０２／Ｙｅｓ）、駆動制御部２１１は、外力が加えられた方向にアシスト制御が行われるようにモータ１０６、１０９に出力されるモータ出力値を算出する（Ｓ１１０３）。そして、駆動制御部２１１は、算出したモータ出力値を回転駆動部２１２に出力する。この時、駆動制御部２１１が出力するモータ出力値は、電流と駆動部１０、１１のトルク定数を乗算した値に相当する。
【００５９】
回転駆動部２１２は、モータ出力値に基づいて駆動部１０、１１を駆動させる（Ｓ１１０４）。尚、Ｓ１１０１で検知された外力がアーム１０１、１０２の操作によるものではない場合（Ｓ１１０２／Ｎｏ）、駆動制御部２１１は、本処理を終了させる。例えば、土台１１１のみを移動させて発生したアーム１０１、１０２の移動は外力によるものではないと判断される。
【００６０】
以上説明したように、本実施形態に係るマニピュレータ装置１を制御する制御装置２は、外力及びマニピュレータ装置１への物体の接触状況によってアーム１０１、１０２が操作されたことを検知すると、移動させられた方向にアーム１０１、１０２が移動するようにアシスト制御を実行する。従って、マニピュレータ装置１の質量やトルクが大きく、ユーザが意図するような操作が行いにくい場合であっても、ユーザが操作する方向にアーム１０１、１０２をアシスト制御する。そのために、ユーザは、アーム１０１、１０２に小さな力を加えるのみでマニピュレータ装置１を意図する方向に操作することが出来る。
【００６１】
また、ユーザの操作によっては、アーム１０１、１０２が突然移動する等して、ユーザが意図しない方向にアーム１０１、１０２が移動することも考えられる。これに対して、本実施形態に係る制御装置２においては、図１２に示すように、アーム１０１、１０２を移動させるために必要な移動力目標値を設定し、外力が不感帯を持つように設定する。さらに、予めマニピュレータ装置１が移動可能な位置の範囲や移動速度の設定値である位置速度指令値を設定する。
【００６２】
このような移動力目標値、位置速度指令値を設定してマニピュレータ装置１の動作を制御する処理について以下、図１３を参照して説明する。位置速度指令生成部２２２は、制御装置２に入力されたマニピュレータ装置１が移動可能な範囲や移動速度の設定値の情報に基づいて、位置速度指令値を設定する（Ｓ１３０１）。さらに、移動力目標生成部２２１は、制御装置２に入力されたアーム１０１、１０２を移動させるために必要な移動力目標値の情報に基づいて、移動力目標値を設定する（Ｓ１３０２）。尚、位置速度指令値または移動力目標値は何れか一つを設定する構成であってもよい。
【００６３】
外力検知部２１７によってアーム１０１、１０２に力が加えられたこと（外力）が検知され、アーム１０１、１０２の移動が検知される（Ｓ１３０３）と、位置情報取得部２１６は、駆動部１０、１１の位置を検知し、位置情報を出力する（Ｓ１３０４）。また、外力検知部２１７は、Ｓ１１０２と同様にして、検知した外力がアーム１０１、１０２の操作によるものか否か判定する（Ｓ１３０５）。
【００６４】
アーム１０１、１０２の移動が外力によるものである場合（Ｓ１３０５／Ｙｅｓ）、移動力目標生成部２２１は、位置情報及びマニピュレータ装置１に加えられた外力に基づいてＳ１３０２にて設定した移動力目標値と外力とを比較する。そして、この比較結果に基づいて移動力目標情報を生成し、駆動制御部２１１に送信する（Ｓ１３０６、移動力目標値誤差検知）。駆動制御部２１１は、移動力目標情報に基づいてアーム１０１、１０２が所定の移動力で移動するように制御を行う。
【００６５】
また、位置速度指令生成部２２２は、Ｓ１３０１にて設定した位置速度指令値とマニピュレータ装置１の位置情報及び移動速度とを比較する。そして、この比較結果に基づいて、位置速度指令情報を生成し、駆動制御部２１１に送信する（Ｓ１３０７）。
【００６６】
駆動制御部２１１は、外力と同じ方向にアシスト制御が行われるようにモータ１０６、１０９に出力されるモータ出力値を算出し、算出したモータ出力値を回転駆動部２１２に出力する（Ｓ１３０８）。この時、駆動制御部２１１が出力するモータ出力値は、電流と駆動部１０、１１のトルク定数を乗算した値と、Ｓ１３０６で生成された新たな移動力目標値とＳ１３０７にて生成された位置・速度を指定する情報とを参照し、算出される。
【００６７】
回転駆動部２１２は、モータ出力値に基づいて駆動部１０、１１を駆動させる（Ｓ１３０９）。尚、アーム１０１、１０２の移動が外力によるものではない場合（Ｓ１３０５／Ｎｏ）、駆動制御部２１１は、本処理を終了させる。例えば、土台１１１のみを移動させて発生したアーム１０１、１０２の移動は外力によるものではないと判断される。
【００６８】
以上説明したように、制御装置２に入力される移動力目標値及び位置速度設定値との誤差をフィードバックし、駆動部１０、１１の駆動のアシスト制御を実行する。このようにすることで、マニピュレータ装置１が突然動作することや、意図しない位置に移動することを抑制できる。また、移動力目標値を設定する範囲によっては、マニピュレータ装置１を所定の移動力を加えることで移動させることが可能である。
【００６９】
以上説明したアシスト制御の処理においては、外力と同じ方向のベクトルにマニピュレータ装置１を直接教示する場合を例として説明を行った。これらの処理においては、外力と逆方向（逆ベクトル）にマニピュレータ装置１が移動するように機能を切り替える制御を行うことも可能である。尚、マニピュレータ装置１がアシスト制御される方向を切り替える制御は、スイッチ２８をＯＮ／ＯＦＦ操作して切り替える構成であってもよい。また、所定以上の外力や回転速度を検知した場合に、アシスト制御を実行しない構成であってもよい。
【００７０】
また、図１４に示すように、ユーザがマニピュレータ装置１を移動させるためにアーム１０１、１０２に接触した接触箇所及びアーム１０１、１０２が形成する角度によっては、ユーザがマニピュレータ装置１を操作する際に大きな力を加える必要がある。さらに、ユーザがマニピュレータ装置１を操作する際に、アーム１０１、１０２の間に身体の一部を挟んでしまうおそれがある。
【００７１】
図１４に示すように、例えば、接触検知部２１８が、「Ｃ」地点での接触を検知した場合、アーム１０１、１０２の交差箇所と離れている。しかし、接触検知部２１８が、「Ａ」地点での接触を検知した場合、アーム１０１、１０２の交差箇所付近にユーザが接触してアーム１０１、１０２が操作されるため、安全性を高める必要がある。
【００７２】
また、図１４において、アーム１０２が右側の位置にある場合であって「Ａ」地点での接触を検知した場合、マニピュレータ装置１を移動させるためには、ユーザはより大きな力をアーム１０１、１０２に加える必要がある。
【００７３】
そこで、本実施形態に係るマニピュレータ装置１の制御装置２は、ユーザがマニピュレータ装置１を操作する際の接触箇所及びアーム１０１、１０２が形成する角度に基づいて、アシスト制御を行う際にモータ１０６、１０９に加えられる駆動量であるアシスト量を変化させる。以下、図１５を参照して、ユーザがマニピュレータ装置１へ接触した箇所及びアーム１０１、１０２が形成する角度αに基づいてマニピュレータ装置１のアシスト量を制御する処理の流れについて説明する。
【００７４】
図１５は、本実施形態に係るマニピュレータ装置１のアシスト量を制御して直接教示を行う処理の流れを示すフローチャートである。尚、図１１及び図１３において、駆動制御部２１１がモータ出力値を算出した後の処理であることを前提として以下の説明を行う。
【００７５】
モータ出力値を算出すると（Ｓ１１０３、Ｓ１３０８）、駆動制御部２１１は、接触検知部２１８から、マニピュレータ装置１が操作された箇所の情報を取得する。この時、例えば、接触検知部２１８は、アーム１０１、１０２に設置されている接触検知センサ１１３のうち、どの接触検知センサ１１３で接触を検知したか、もしくは、接触を検知した箇所の位置情報を駆動制御部２１１に送信する構成であってもよい。
【００７６】
駆動制御部２１１は、接触検知部２１８から取得した情報に基づいて、アーム１０１、１０２の交差箇所から所定の範囲内の位置で接触を検知したか否か判定する（Ｓ１５０１）。尚、アーム１０１、１０２の交差箇所からの所定の位置は、あらかじめマニピュレータ装置１のユーザによって設定され、記憶される値を用いてもよい。
【００７７】
アーム１０１、１０２で接触を検知した箇所が所定の範囲より遠い位置である場合（Ｓ１５０１／Ｎｏ）、駆動制御部２１１は、算出されたモータ出力値をそのまま回転駆動部２１２に送信する（Ｓ１５０３）。図１５のフローチャートにおいては、算出されたモータ出力値をそのまま用いる場合を「アシスト量Ａ」の制御を実行するとして記載している。
【００７８】
アーム１０１、１０２において接触が検知された箇所が所定の範囲内の位置である場合（Ｓ１５０１／Ｙｅｓ）、駆動制御部２１１は、位置情報取得部２１６からアーム１０１、１０２の位置情報を取得し、アーム１０１、１０２が形成する角度αを検知する。検知された角度αが所定より大きい場合（Ｓ１５０４／Ｎｏ）、駆動制御部２１１は、アシスト量Ａよりもアシスト量を大きくしたモータ出力値を回転駆動部２１２に出力する（Ｓ１５０５）。従って、位置情報取得部２１６は、角度検知部として機能する。
【００７９】
アーム１０１、１０２において、接触が検知された箇所が所定の範囲内である場合には、マニピュレータ装置１を移動させるために、より大きい力が必要となる。従って、Ｓ１５０５の処理において、駆動制御部２１１は、アシスト量Ａよりもアシスト量を大きくしたモータ出力値を回転駆動部２１２に出力する。尚、図１５のフローチャートにおいては、図１６に示すように算出されたモータ出力値よりも大きいモータ出力値を用いる場合を「アシスト量Ｂ」の制御を実行するとして記載している。
【００８０】
検知された角度αが所定以下の場合（Ｓ１５０４／Ｙｅｓ）、駆動制御部２１１は、マニピュレータ装置１に安全性優先モードが設定されているか否かを確認する（Ｓ１５０６）。「安全性優先モード」とは、ユーザがあらかじめマニピュレータ装置１に対して設定しているものであり、外部装置から入力して設定することが可能である。
【００８１】
安全性優先モードが設定されていない場合（Ｓ１５０６／Ｎｏ）、駆動制御部２１１は、アシスト量Ａよりもアシスト量を小さくしたモータ出力値を回転駆動部２１２に出力する（Ｓ１５０７）。図１５のフローチャートにおいては、図１７に示すように算出されたモータ出力値よりも小さいモータ出力値を用いる場合を「アシスト量Ｃ」の制御を実行するとして記載している。
【００８２】
安全性優先モードが設定されている場合（Ｓ１５０６／Ｙｅｓ）、駆動制御部２１１は、アシスト量を「０」に設定してモータ出力値を回転駆動部２１２に出力する（Ｓ１５０８）。
【００８３】
回転駆動部２１２は、このようにしてアーム１０１、１０２が形成する角度及びアーム１０１、１０２におけるユーザの接触箇所に基づいて算出されたモータ出力値に基づいて駆動部１０、１１を駆動させる。以上説明した処理のステップに従って、本実施形態に係るマニピュレータ装置１の制御装置２は、ユーザがアーム１０１、１０２を操作して直接教示を行う際に、ユーザのアーム１０１、１０２への接触箇所やアーム１０１、１０２が形成する角度に基づいてアシスト量を制御する。
【００８４】
そのため、ユーザがアーム１０１、１０２の交差箇所の近傍を操作している場合には、ユーザの身体がアーム１０１、１０２に挟まれる等の危険性が少なくなり、安全性を高めた直接教示を行う事が出来る。また、マニピュレータ装置１を動かすために、大きな力を加える必要がある場合には、アシスト量を増加させるため、より操作性を高めた直接教示を行う事が出来る。
【００８５】
尚、駆動制御部２１１は、アーム１０１、１０２が移動されている際に、所定の間隔ごとにアーム１０１、１０２が形成する角度αを取得してアシスト量を制御する構成であってもよい。このような制御を行うことにより、アーム１０１、１０２の移動状況の変化に応じたアシスト制御が可能になり、さらに安全性を高めたマニピュレータ装置１の直接教示を行うことが出来る。
【符号の説明】
【００８６】
１  マニピュレータ装置
１０、１１  駆動部
２１  ＣＰＵ
２２  ＲＡＭ
２３  ＲＯＭ
２４  エンジン
２５  ＨＤＤ
２６  Ｉ／Ｆ
２７  センサ
２８  スイッチ
２９  バス
１０１、１０２  アーム
１０３、１０４  駆動中心
１０５、１０８  減速機
１０６、１０９  モータ
１０７、１１０  エンコーダ
１１１  土台
１１２  移動検知センサ
１１３  接触検知センサ
２０１  回転駆動制御部
２１１  駆動制御部
２１２  回転駆動部
２１４  回転検知部
２１５  ＦＢ取得部
２１６  位置情報取得部
２１７  外力検知部
２１８  接触検知部
２２１  移動力目標生成部
２２２  位置速度指令生成部
【先行技術文献】
【特許文献】
【００８７】
【特許文献１】国際公開番号第２０１３／０２７２５０号
【図１】
2017202554.tif 000010
【図２】
2017202554.tif 000011
【図３】
2017202554.tif 000012
【図４】
2017202554.tif 000013
【図５】
2017202554.tif 000014
【図６】
2017202554.tif 000015
【図７】
2017202554.tif 000016
【図８】
2017202554.tif 000017
【図９】
2017202554.tif 000018
【図１０】
2017202554.tif 000019
【図１１】
2017202554.tif 000020
【図１２】
2017202554.tif 000021
【図１３】
2017202554.tif 000022
【図１４】
2017202554.tif 000023
【図１５】
2017202554.tif 000024
【図１６】
2017202554.tif 000025
【図１７】
2017202554.tif 000026