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