(57)【要約】【課題】本発明が解決しようとする課題は、ロボットアームの先端に加わる外力を高精度に推定することができるロボット制御装置を提供することである。【解決手段】実施形態のロボット制御装置は、リンクと、前記リンクを回転駆動するモータと、を含むロボットアームに用いられるロボット制御装置であって、前記モータの角速度と電流指令値に基づいて前記リンクの回転角の変化量を含む第１の推定値と前記モータの回転角の変化量を含む第２の推定値を導き、前記第１の推定値と前記第２の推定値の差分に基づいて前記ロボットアームに生じる外力を導出する導出部と、を備える。【選択図】図１
2018083274.tif 000002
【特許請求の範囲】
【請求項１】
リンクと、前記リンクを回転駆動するモータと、を含むロボットアームに用いられるロボット制御装置であって、
前記モータの角速度と電流指令値に基づいて前記リンクの回転角の変化量を含む第１の推定値と前記モータの回転角の変化量を含む第２の推定値を導き、前記第１の推定値と前記第２の推定値の差分に基づいて前記ロボットアームに生じる外力を導出する導出部と、を備えるロボット制御装置。
【請求項２】
閾値を指定する指定部と、前記閾値と前記外力に基づいて前記外力の有無を判定する判定部と、を更に備え、
前記判定部は、前記外力が前記閾値より大きい場合に外力有と判定し、前記外力が前記閾値より小さい場合に外力無と判定する請求項１に記載のロボット制御装置。
【請求項３】
前記判定部が外力有と判定したときの前記モータの回転角を記憶する記憶部を更に備える請求項２に記載のロボット制御装置。
【請求項４】
前記判定部が外力有と判定したときに、前記外力を緩和するための前記リンクの動作を生成する生成部を更に備える請求項２または３に記載のロボット制御装置。
【請求項５】
前記生成部は、前記記憶部が記憶した前記モータの回転角と前記リンクの長さに基づいて前記外力が生じた際の前記リンクの位置情報を導出する請求項４に記載のロボット制御装置。
【請求項６】
前記導出部は、前記差分を座標変換する請求項１乃至５のいずれか１項に記載のロボット制御装置。
【請求項７】
前記第１及び第２の推定値は、前記リンク及び前記モータの角加速度であり、前記外力は、前記リンクに生じる外部からの加速度である請求項１乃至６のいずれか１項に記載のロボット制御装置。
【請求項８】
前記第１及び第２の推定値は、前記リンク及び前記モータの角速度であり、前記外力は、前記リンクに生じる外部からの速度である請求項１乃至６のいずれか１項に記載のロボット制御装置。
【請求項９】
リンクと、前記リンクを回転駆動するモータと、を含むロボットアームに用いられるロボット制御装置であって、
前記モータの角速度と電流指令値に基づいて前記リンク及び前記モータのＰＩ制御出力値を導き、前記ＰＩ制御出力値に基づいて前記ロボットアームに生じる外乱トルクを導出する導出部と、を備えるロボット制御装置。
【請求項１０】
リンクと、前記リンクを回転駆動するモータと、を含むロボットアームに用いられるロボット制御装置におけるロボット制御方法であって、
前記モータの角速度と電流指令値に基づいて前記リンクの回転角の変化量を含む第１の推定値と前記モータの回転角の変化量を含む第２の推定値を導き、前記第１の推定値と前記第２の推定値の差分に基づいて前記ロボットアームに生じる外力を導出するロボット制御方法。
【請求項１１】
請求項１乃至９のいずれか１項に記載のロボット制御装置と、
保持機構と移動機構の少なくとも一方を含むピッキング装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、ロボット制御装置、ロボット制御方法及びピッキング装置に関する。
【背景技術】
【０００２】
ロボットアームを用いた自動化ラインでは、作業者がアーム先端の位置データ等を予め記憶させるティーチングという作業を行っている。ティーチングは、時間と手間を要し作業者の負担が大きいため、ティーチングレスが求められている。ティーチングレスのため、ロボットに視覚センサや力覚センサを併用することが多いが、視覚センサは、奥行き方向に誤差を有しており、高い精度を出すにはコストがかかる。また、アーム先端に取り付けた力覚センサを用いる場合は、突き当て動作（アーム先端を作業対象物に軽く接触させること）により得られる接触情報から奥行き方向を計測することはできるが、力覚センサは高価でかつ壊れやすい。力覚センサの代わりに接触力を検知するための機構を付加することも、コスト高の要因になる。
【０００３】
そこで、追加のセンサ等を用いずにロボットアームの各関節を駆動するモータのエンコーダ値等に基づいてアーム先端に生じる外力を推定することが求められている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１３−１９８９５５公報
【特許文献２】特許第３８７８０５４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
本発明が解決しようとする課題は、ロボットアームの先端に加わる外力を高精度に推定することができるロボット制御装置を提供することである。
【課題を解決するための手段】
【０００６】
実施形態のロボット制御装置は、リンクと、前記リンクを回転駆動するモータと、を含むロボットアームに用いられるロボット制御装置であって、前記モータの角速度と電流指令値に基づいて前記リンクの回転角の変化量を含む第１の推定値と前記モータの回転角の変化量を含む第２の推定値を導き、前記第１の推定値と前記第２の推定値の差分に基づいて前記ロボットアームに生じる外力を導出する導出部と、を備える。
【０００７】
実施形態のロボット制御方法は、リンクと、前記リンクを回転駆動するモータと、を含むロボットアームに用いられるロボット制御装置におけるロボット制御方法であって、前記モータの角速度と電流指令値に基づいて前記リンクの回転角の変化量を含む第１の推定値と前記モータの回転角の変化量を含む第２の推定値を導き、前記第１の推定値と前記第２の推定値の差分に基づいて前記ロボットアームに生じる外力を導出する。
【０００８】
実施形態のピッキング装置は、上記ロボット制御装置と、保持機構と移動機構の少なくとも一方を含む。
【図面の簡単な説明】
【０００９】
【図１】第１の実施形態にかかるロボット制御装置の制御対象の２リンクロボットアームの断面図の一例を示す図である。
【図２】ロボットアームの１リンク分を示す模式図である。
【図３】ロボットアーム各軸の位置、速度制御系を示すブロック図である。
【図４】たわみ加速度を用いたロボット制御装置のブロック図を示す。
【図５】第１の実施形態にかかるオブザーバ部を示すブロック図である。
【図６】図４に示すオブザーバ部に基づいたアーム先端のたわみ加速度を推定するオブザーバ部を示すブロック図である。
【図７】第１の実施形態にかかるロボット制御装置を用いて推定したリンク先端のたわみ加速度推定値を示す図である。
【図８】第２の実施形態にかかるロボット制御装置のオブザーバ部の一例を示す図である。
【図９】第２の実施形態にかかるロボット制御装置を用いて推定したリンク先端のたわみ速度推定値を示す図である。
【図１０】第３の実施形態にかかるロボット制御装置のオブザーバ部の一例を示す図である。
【図１１】第３の実施形態にかかるロボット制御装置を用いてい推定したＰＩ制御出力値を示す図である。
【図１２】第１乃至３の実施形態にかかるロボット制御装置を含むピッキング装置の一例を示す側面図と正面図である。
【図１３】物品Ｇを載置した棚と対向した場合のピッキング装置を示す図である。
【発明を実施するための形態】
【００１０】
以下、図面を参照して実施形態にかかるロボット制御装置について説明する。同じ符号が付されているものは同様のものを示す。なお、図面は模式的または概念的なものであり、各部分の厚みと幅との関係や部分間の大きさの比係数などは、必ずしも現実のものと同一とは限らない。また、同じ部分を表す場合であっても、図面により互いの寸法や比係数が異なって表される場合もある。
【００１１】
（第１の実施形態）
第１の実施形態について図１を参照して説明する。図１は第１の実施形態にかかるロボット制御装置の制御対象の２リンクロボットアームの断面図の一例を示す図である。
【００１２】
このロボットアームは、基台１の上部に第１リンク３の一端が取り付けられており、第１リンク３の他端には第２リンク８が取り付けられている。
【００１３】
基台１は、第１モータ４、第１減速機５、第１エンコーダ６を有する。第１リンク３は、第２モータ９、第２減速機１０、第２エンコーダ１１を有する。第２リンク８の先端には、負荷１２を有し、第２リンク８の先端の負荷１２で作業対象物（ワーク）と接触する。
【００１４】
ロボット制御装置１３は、第１モータ４及び第１エンコーダ６とバネ特性を持つ第１減速機５との組合せにより、第１軸２を中心として第１リンク３を水平旋回させる。好ましくは、第１モータ４、第１エンコーダ６及び第１減速機５は、第１軸２を中心に同軸上に配置されるのが良い。また、ロボット制御装置１３は、第２モータ９及び第２エンコーダ１１とバネ特性を持つ第２減速機１０との組合せにより、第２軸７を中心として第１リンク３に対して第２リンク８を水平旋回させる。好ましくは、第２モータ９、第２エンコーダ１１及び第２減速機１０は、第２軸７を中心に同軸上に配置される。
【００１５】
基台１は、第１軸２のまわりに回転可能な第１リンク３を支持する。また、基台１は、金属製の筐体であり外部から第１モータ４等に衝撃が加わるのを防止する。基台１は、金属製に限定されず樹脂製等であっても良い。
【００１６】
第１リンク３は、一端が基台１内の第１減速機５と繋がる。第１リンク３の他端には、第２モータ９、第２減速機１０と第２エンコーダ１１が設置される。第２軸７のまわりに回転可能な第２リンク８の一端は、第２減速機１０と繋がる。第１リンク３及び第２リンク８は、ロボットのアーム部にあたる。
【００１７】
第１減速機５、第２減速機１０は、第１モータ４、第２モータ９にそれぞれ設置されモータの回転速度を減速し、第１リンク３、第２リンク８の回転速度を定める。減速比は、モータの回転数等により適宜調整される。
【００１８】
第１エンコーダ６、第２エンコーダ１１は、第１モータ４、第２モータ９にそれぞれ設置されモータの回転角等をセンシングしてロボット制御装置１３に出力する。第１エンコーダ６、第２エンコーダ１１は、位置検出のセンサやトランスデューサ等で良い。
【００１９】
負荷１２は、第２リンク８の先端部であり作業対象物と接触する箇所である。負荷１２は、第２リンク８の一部であるが作業対象物に接触しやすい形状となっているのが好ましい。例えば、突起等を設けても良い。負荷１２の位置は、第２リンク８の先端部に限定されず、第２リンク８の側面部等も含む。
【００２０】
図２は、ロボットアームの１リンク分を示す模式図である。これは、２慣性系と呼ばれる。このロボットアームは、弾性関節を有するシリアル２リンクアームとして非線形動力学モデルで表すことができる。図２では、１リンク分の非線形動力学モデルの導出に必要な物理パラメータ、慣性モーメント、摩擦係数、及びバネ係数等について第１軸を代表して示す。図２に示すように、モータ２０によってリンク３０が減速機２５を介して駆動制御される。第１軸に関しては、モータ２０のトルク入力がｕ１で表され、モータ２０の慣性モーメントがｍＭ１で表され、モータ２０の回転角、すなわちエンコーダによって検出された出力がθＭ１で表され、モータ２０の粘性摩擦係数がｄＭ１で表され、モータ２０のクーロン動摩擦係数がｆＭ１で表され、減速機２５の減衰係数がｄＧ１で表され、減速機２５のバネ係数がｋＧ１で表され、リンク３０の慣性モーメントがｍＬ１で表され、リンク３０の粘性摩擦係数がｄＬ１で表され、リンク３０の回転角がθＬ１で表される。
【００２１】
図示を省略するが、第２軸に関しては、モータ２０のトルク入力がｕ２で表され、モータ２０の慣性モーメントがｍＭ２で表され、モータ２０の回転角、すなわちエンコーダによって検出された出力がθＭ２で表され、モータ２０の粘性摩擦係数がｄＭ２で表され、モータ２０のクーロン動摩擦係数がｆＭ２で表され、減速機２５の減衰係数がｄＧ２で表され、減速機２５のバネ係数がｋＧ２で表され、リンク３０の慣性モーメントがｍＬ２で表され、リンク３０の粘性摩擦係数がｄＬ２で表され、リンク３０の回転角がθＬ２で表される。
【００２２】
これにより、２リンクアームの非線形動力学モデルでは、２次元ベクトルとして、モータ２０のトルク入力がｕで表され、モータ２０の慣性モーメントがｍＭで表され、モータ２０の回転角、すなわちエンコーダによって検出された出力がθＭで表され、モータ２０の粘性摩擦係数がｄＭで表され、モータ２０のクーロン動摩擦係数がｆＭで表され、減速機２５の減衰係数がｄＧで表され、減速機２５のバネ係数がｋＧで表され、リンク３０の慣性モーメントがｍＬで表され、リンク３０の粘性摩擦係数がｄＬで表され、リンク３０の回転角がθＬで表される。
【００２３】
弾性関節を有するシリアル２リンクアームの非線形動力学モデルは、モータ側が式（１）で与えられ、リンク側が式（２）で与えられる。
【数１】
2018083274.tif 000003
【数２】
2018083274.tif 000004
【００２４】
ただし、
【数３】
2018083274.tif 000005
である。
【００２５】
ここで、α、β、γをリンクの長さや重心位置、質量、慣性により構成されるパラメータとすると、リンクの慣性行列は式（３）のように表される。
【数４】
2018083274.tif 000006
【００２６】
さらに、パラメータα、β、γの詳細は、式（４）のようになる。
【数５】
2018083274.tif 000007
【００２７】
ただし、
【数６】
2018083274.tif 000008
である。
【００２８】
遠心力、コリオリ力ベクトルについては式（５）のようになる。
【数７】
2018083274.tif 000009
【００２９】
モータ角速度制御系については、ＰＩ（比例、積分）制御をＦＦ−Ｉ−Ｐ（フィードフォワード−積分−比例）制御として２自由度ＰＩ制御にした式（６）で与えられる。
【数８】
2018083274.tif 000010
【００３０】
ただし、
【数９】
2018083274.tif 000011
である。
【００３１】
図３は、ロボットアーム各軸の位置、速度制御系を示すブロック図である。
図３に示すように、２自由度ＰＩ速度制御系は、位置制御系（Ｐ制御）の内側にカスケード接続された制御系として構成される。なお、図３においては、第１軸に関する２自由度ＰＩ速度制御系を示している。
【００３２】
以下、位置制御系を除いたモータの角速度制御系に焦点を当て、制御周期が十分短いものと仮定し、これによりモータの角度制御系が連続系であるとして説明を行う。
【００３３】
まず、式（１）の物理パラメータを公知の方法で同定し、次に、同定された物理パラメータを式（１）及び速度フィードバック制御則を表す式（６）に代入する。これにより、２リンクロボットアームの速度制御系のシミュレーションモデルが構築される。
【００３４】
本実施形態では、このシミュレーションモデルを用いて、ロボットアームの先端たわみ加速度等を推定するオブザーバ部（演算部または導出部とも称される）を構成する。
【００３５】
図４は、ロボット制御装置１３のブロック図の一例を示す。
【００３６】
ロボット制御装置１３は、速度ＰＩ制御された動力学モデルに基づくオブザーバ部１３１と、閾値指定部１３２と、外力判定部１３３と、接触位置記憶部１３４と、反転起動生成部１３５とを有する。
【００３７】
オブザーバ部１３１では、第１モータ４、第２モータ９のトルク入力（電流指令値）と、  第１エンコーダ６、第２エンコーダ１１からのエンコーダ値とを受け取り、２リンクアーム非線形動力学モデルを用いてたわみ加速度の推定値を求めている。
【００３８】
閾値指定部１３２では、作業者（ユーザ）等があらかじめ指定するアーム先端に加わる外力の閾値が指定される。閾値指定部１３２（指定部とも称される）には、図示しない入力部があり、作業者がその入力部により直接閾値を指定しても良い。また、アームの剛性等を考慮して所定の値が設定されていても良い。入力部は、コンピュータや携帯端末等で入力しても良い。ここでいう外力とは、リンクに生じる外部からの加速度のことを示すが、加速度以外にも速度や外乱トルク等を含む。
【００３９】
外力判定部１３３（判定部とも称される）は、閾値指定部１３２で指定された閾値と、オブザーバ部１３１で導出されたアーム先端のたわみ加速度とを比較する。外力判定部１３３は、閾値とたわみ加速度を比較してたわみ加速度が閾値以上の場合に外部からの外力有りと判定し、たわみ加速度が閾値より小さい場合に外部からの外力無と判定する。上記判定は、たわみ加速度が閾値より大きい場合に外部からの外力有りと判定し、たわみ加速度が閾値以下の場合に外部からの外力無と判定しても良い。外部からの外力とは、突き当て動作によって、ロボットアーム先端の第２リンク８の負荷部１２に生じる外力である。
【００４０】
接触位置記憶部１３４（記憶部とも称される）は、外力判定部１３３が比較する閾値とたわみ加速度から、たわみ加速度が閾値を超えたときの（たわみ加速度と閾値が等しい場合でも良い）、各軸のモータのエンコーダ値を記憶する。エンコーダ値は、モータの回転角、回転数、モータの速度、モータの負荷等の値である。接触位置記憶部１３４は、例えば、磁気テープやカセットテープ等のテープ系、フロッピー（登録商標）ディスク／ハードディスク等の磁気ディスクやＣＤ−ＲＯＭ／ＭＯ／ＭＤ／ＤＶＤ／ＣＤ−Ｒ等の光ディスクを含むディスク系、ＩＣカード（メモリカードを含む）／光カード等のカード系、あるいはマスクＲＯＭ／ＥＰＲＯＭ／ＥＥＰＲＯＭ／フラッシュＲＯＭ等の半導体メモリ系などを用いることができる。
【００４１】
反転起動生成部１３５（生成部とも称される）は、突き当て動作によって発生する外力による衝撃を抑制するために、アームを作業対象物とは逆方向に動作させる軌道を生成する。この動作により、アームを作業対象物の表面から離して停止させる。
【００４２】
オブザーバ部１３１、外力判定部１３３及び反転起動生成部１３５での計算は、コンピュータのＣＰＵ（Central Processing Unit：中央演算処理装置）等で行われる。
【００４３】
次に、本実施形態のオブザーバ部について詳しく説明する。
【００４４】
図５は、第１の実施形態にかかるオブザーバ部を示すブロック図である。
【００４５】
図５に示すように、オブザーバ部１３１は、第１および第２軸のＰＩ制御器１３１１、１３１２と、２リンクアームの非線形動力学モデル１３１３と、非線形動力学モデル１３１３を積分する積分器群１３１４ａ〜１３１４ｄ、１３１５ａ〜１３１５ｄとを有する。ＰＩ制御器１３１１は、第１軸を駆動するモータの速度ｄθＭ１／ｄｔと第１軸を駆動するモータの回転速度の推定値との偏差に基づいてＰＩ制御する。ＰＩ制御器１３１２は、第２軸を駆動するモータの速度ｄθＭ２／ｄｔと第２軸を駆動するモータの回転速度の推定値との偏差に基づいてＰＩ制御する。２リンクアームの非線形動力学モデル１３１３は、ＰＩ制御器１３１１の出力と第１軸の操作量ｕ１との和となる第１入力τ１およびＰＩ制御器１３１２の出力と第２軸の操作量ｕ２との和となる第２入力τ２に基づいて非線形動力学モデルに基づいて第１および第２リンクの角加速度の推定を行うとともに第１および第２リンクをそれぞれ駆動するモータの角加速度の推定を行い、推定した角加速度を出力する。積分器１３１４ａは、非線形動力学モデル１３１３から出力される第１リンクの角加速度の推定値を積分し、角速度の推定値を出力する。積分器１３１４ｂは、積分器１３１４ａの出力を積分し、第１リンクの回転角の推定値を出力する。積分器１３１４ｃは、非線形動力学モデル１３１３から出力される第１リンクを駆動するモータの角加速度の推定値を積分し、上記モータの角速度の推定値を出力する。積分器１３１４ｄは、積分器１３１４ｃの出力を積分し、第１リンクを駆動するモータの回転角の推定値を出力する。積分器１３１５ａは、非線形動力学モデル１３１３から出力される第２リンクの角加速度の推定値を積分し、角速度の推定値を出力する。積分器１３１４ｂは、積分器１３１４ａの出力を積分し、第２リンクの回転角の推定値を出力する。積分器１３１４ｃは、非線形動力学モデル１３１３から出力される第２リンクを駆動するモータの角加速度の推定値を積分し、上記モータの角速度の推定値を出力する。積分器１３１４ｄは、積分器１３１４ｃの出力を積分し、第２リンクを駆動するモータの回転角の推定値を出力する。
【００４６】
これらの第１および第２リンクの角加速度の推定値、第１および第２リンクの角速度の推定値、第１および第２リンクを駆動するモータの角速度の推定値、およびモータの回転角の推定値は、それぞれオブザーバ部１３１から出力される。
【００４７】
すなわち、図５に示すオブザーバ部１３１は、ロボットアームの非線形動力学モデル１３１３を内蔵し、ＰＩ制御器１３１１、１３１２のオブザーバゲインは、ロボットアーム各軸の既存の速度制御系のＰＩ制御ゲインをそのまま使用するシミュレーションモデルである。つまり、通常のオブザーバゲイン調整にかかるエンジニアリングコストが全くない。これは、前記物理パラメータが精度良く同定されていることによって達成される。
【００４８】
非線形動力学モデルに基づくＰＩ制御型オブザーバは、式（１）を変形した式（７）のような２階微分形式で表される。なお、式（７）において、＾は推定値を表す。
【数１０】
2018083274.tif 000012
【００４９】
ただし、
【数１１】
2018083274.tif 000013
である。
【００５０】
このように、オブザーバ部１３１には、モータの角速度（エンコーダ差分）とモータドライバへの入力電圧（電流指令値）が入力される。ここでＰＩ制御系１３１１、１３１２のオブザーバゲインは、実機の速度ループのＦＦ−Ｉ−Ｐ制御（２自由度ＰＩ制御）のＰとＩのゲインと同じものを選んでおり、この場合、オブザーバの追従性能の１自由度だけを考えれば良いので、ＦＦ＝ＰのようにＰＩ制御として設定する。このシステムは、ゲインが一定の非線形オブザーバの一種である。
【００５１】
式（７）をロボット制御装置１３に実装し、リアルタイムで積分器群１３１４、１３１５のように２回積分（実際は数値積分）することによって状態推定ができる。
【００５２】
これにより、軸ねじれ角加速度の推定値は、各リンクの角加速度推定値とモータの角加速度推定値との差分から、式（８）のように求められる。
【数１２】
2018083274.tif 000014
【００５３】
ここで、ロボットアームの各リンクの長さをｌ１、ｌ２、アーム先端の座標をＰ（ｘ、ｙ）とすると、以下の式（９）により導出できる。
【数１３】
2018083274.tif 000015
【００５４】
式（９）の両辺を微分すると、ヤコビ行列Ｊ（θＬ）を用いて、式（１０）が得られる。
【数１４】
2018083274.tif 000016
【００５５】
式（１０）を用いると、軸ねじれ角速度とアーム先端のたわみ速度の関係として、式（１１）が得られる。
【数１５】
2018083274.tif 000017
【００５６】
さらに、式（１１）の両辺を微分すると、軸ねじれ角加速度とアーム先端のたわみ加速度の関係として、式（１２）が導かれる。
【数１６】
2018083274.tif 000018
【００５７】
ただし、本実施形態で想定されるアームの動作速度は小さく、微分の過程で現れるｄＪ（θＬ）／ｄｔは無視できるほど小さいと仮定した。なお、軸ねじれ角は微小であるのでＪ（θＬ）は、Ｊ（θＬ）＝Ｊ（ｎＧθＭ）として、計測できるモータ角度から導出可能である。
【００５８】
図６は、図５に示すオブザーバ部に基づいたアーム先端のたわみ加速度を推定するオブザーバ部を示すブロック図である。図６のオブザーバ部を用いることにより、軸ねじれ角加速度を導出し、式（１２）の座標変換を用いることにより、アーム先端のたわみ加速度を求めることができる。
【００５９】
詳しく述べると、図６に示すように、第１リンク３の角加速度の推定値、第１リンク３とこの第１リンク３を駆動する第１モータ４との間に設けられる第１減速機５の減速比ｎＧ１、および第１リンク３を駆動する第１モータ４の角加速度の推定値に基づいて、第１軸ねじれ角加速度の推定値を演算し、この推定値が出力される（式（８）に基づく）。同様に、第２リンク８の角加速度の推定値、第２リンク８とこの第２リンク８を駆動する第２モータ９との間に設けられる第２減速機１０の減速比ｎＧ２、および第２リンク８を駆動する第２モータ９の角加速度の推定値に基づいて、第２軸ねじれ角加速度の推定値を演算し、この推定値が出力される（式（８）に基づく）。この第１軸ねじれ角加速度推定値と第２軸ねじれ角加速度推定値を式（１２）の座標変換することにより、アーム先端のたわみ加速度が求められる。
【００６０】
また、接触位置記憶部１３４に記憶された外力判定部１３３が外力有と判定した時の第１エンコーダ６と第２エンコーダ１１の値と、各リンクの長さを用いて式（９）から作業対象物の座標Ｐ（ｘ、ｙ）が導出される。座標Ｐ（ｘ、ｙ）は、反転起動生成部１３５で導出しても良いし、オブザーバ部１３１で導出しても良い。
【００６１】
次に、本実施形態にかかるロボット制御装置を用いた場合のアームたわみ加速度の推定値と、力覚センサを用いてアーム先端の接触力を計測した場合とを比較する。
【００６２】
図７は、突き当て動作における時間応答波形を示す。図７（ａ）は、突き当て動作時の接触力をアーム先端に装着した力覚センサで計測した波形である。図７（ｂ）は、本実施形態にかかるロボット制御装置を用いて推定した突き当て動作時のアーム先端のたわみ加速度の波形である。
【００６３】
図７（ａ）と（ｂ）の比較から、推定したアーム先端のたわみ加速度は、アーム先端の突き当ての瞬間を精度良く推定できているのが解る。
【００６４】
本実施形態にかかるロボット制御装置を用いることにより、力覚センサ等を特別に設置しなくても、高い精度でアームに加わる外力を推定できる。
【００６５】
また、本実施形態にかかるロボット制御装置を用いることにより、別途力覚センサ等を設置する必要が無い為、低コスト化及び省スペース化が図れる。
【００６６】
（第２の実施形態）
第２の実施形態について図８を参照して説明する。図８は、第２の実施形態にかかるロボット制御装置のオブザーバ部の一例を示す図である。
【００６７】
本実施形態のオブザーバ部は、たわみ加速度推定値の代わりに、たわみ速度推定値を出力ことが第１の実施形態にかかるロボット制御装置と異なる。その他の構成については、第１の実施形態にかかるロボット制御装置と同様である。
【００６８】
図８に示すように、たわみ速度推定値を導出するための軸ねじれ角速度推定値は、各リンクの角速度推定値と各モータの角速度推定値との差分から求められる。
【００６９】
詳しく述べると、このオブザーバ部１３１において、第１リンク３の角速度の推定値、第１リンク３とこの第１リンク３を駆動する第１モータ４との間に設けられる第１減速機５の減速比ｎＧ１、および第１リンク３を駆動する第１モータ４の角速度の推定値に基づいて、第１軸ねじれ角速度の推定値を演算し、この推定値が出力される。同様に、第２リンク８の角速度の推定値、第２リンク８とこの第２リンク８を駆動する第２モータ９との間に設けられる第２減速機１０の減速比ｎＧ２、および第２リンク８を駆動する第２モータ９の角速度の推定値に基づいて、第２軸ねじれ角速度の推定値を演算し、この推定値が出力される。これらのねじれ角速度推定値を式（１１）に基づく座標変換を行うことにより、たわみ速度推定値が得られる。
【００７０】
図９は、ロボットアームの突き当て動作時に、本実施形態にかかるロボット制御装置を用いて推定したリンク先端のたわみ速度推定値の波形である。図７（ｂ）で示した、たわみ加速度推定値の結果の方が、力覚センサで測定した図７（ａ）の波形との類似度が高いことが解る。ただし、たわみ速度の方が波形の安定性が高いため閾値の指定がし易いという利点がある。たわみ加速度とたわみ速度を組合せることにより精度の高い外力の判定ができる。
【００７１】
（第３の実施形態）
第３の実施形態について図１０を参照して説明する。図１０は、第３の実施形態にかかるロボット制御装置のオブザーバ部の一例を示す図である。
【００７２】
本実施形態のオブザーバ部は、たわみ加速度推定値の代わりに、ＰＩ制御出力値を出力することが第１の実施形態にかかるロボット制御装置と異なる。その他の構成については、第１の実施形態にかかるロボット制御装置と同様である。
【００７３】
図１０に示すように、ＰＩ制御１３１１及び１３１２からそれぞれ出力されるΔｕ１、Δｕ２を用いる。ＰＩ制御出力値は、オブザーバ部の推定誤差と外乱トルクを含むので、閾値を設定することによって、外乱トルクだけを抽出することができる。
【００７４】
図１１は、ロボットアームの突き当て動作時に、推定したＰＩ制御出力値の波形である。
【００７５】
図１１中に示すピーク箇所がリンクと作業対象物との接触点を示している。ノイズレベルが比較的大きいため、やや閾値が指定がし難いが、オブザーバ部によるフィードバックがあるため、非線形動力学モデルを開ループで用いる場合より閾値を小さくすることができる。
【００７６】
（第４の実施形態）
第４の実施形態について図１２と図１３を参照して説明する。図１２は、第１乃至３の実施形態にかかるロボット制御装置１３を含むピッキング装置１００の一例を示す側面図と正面図である。図１３は、物品Ｇを載置した棚１１０と対向した場合のピッキング装置１００を示す図である。
【００７７】
ここで、説明の便宜上、＋Ｘ方向、−Ｘ方向、＋Ｙ方向、−Ｙ方向、＋Ｚ方向、および−Ｚ方向について定義する。＋Ｘ方向、−Ｘ方向、＋Ｙ方向、および−Ｙ方向は、例えば、略水平面に沿う方向である。−Ｘ方向は、＋Ｘ方向の反対方向である。実施形態において、−Ｘ方向は「物品Ｇをピッキング装置１００に引き寄せる方向」である。「物品Ｇを引き寄せる方向」とは、図１３に示す、棚１１０からピッキング装置１００への方向である。＋Ｙ方向は、＋Ｘ方向と交差する方向（例えば略直交する方向）である。−Ｙ方向は＋Ｙ方向の反対方向である。＋Ｚ方向は、＋Ｘ方向および＋Ｙ方向と交差する方向（例えば略直交する方向）であり、例えば略鉛直上向き方向である。−Ｚ方向は＋Ｚ方向の反対方向であり、例えば略鉛直下向き方向である。
【００７８】
図１３に示すように、ピッキング装置１００は、例えば床面に設置される棚１１０に対向するように移動し、棚１１０に載置された物品Ｇをピッキングする。棚１１０は、ピッキング装置１００が横方向（＋Ｘ方向、−Ｘ方向、＋Ｙ方向、−Ｙ方向）から物品Ｇを認識し、さらにピッキングできるように、例えば側面に空間を有する。物品Ｇとは、段ボール箱等に入れられた製品、パッケージされた製品、製品単品等を含む。
【００７９】
図１２に示すように、ピッキング装置１００は、台車１２０と、物品Ｇを保持する保持機構１７０と、保持機構１７０を移動する移動機構１４０と、保持機構１７０により保持した物品を入れる荷台１５０を有する。
【００８０】
台車１２０は、保持機構１７０、移動機構１４０と荷台１５０を乗せる基台１２１を有し、基台１２１には例えば４つの車輪１２２が備えられる。台車１２０は、車輪１２２が回転することにより棚１１０が設置される床面上を移動する。移動する手段としては車輪に限定されず、キャタピラのようなものでも良いし、倉庫内等にあらかじめ設置されたレール等を移動しても良い。
【００８１】
台車１２０は、ＡＧＶ（Automatic Guided Vehicle：無人搬送車）であり、車輪１２２の駆動を制御する制御部を有する。制御部は、慣性センサ等の位置センサや画像センサを有し、これらのセンサにより走行路上の自車位置を計測する。制御部は、工場や倉庫内の棚位置や走行路のレイアウト情報を記憶しており、レイアウト情報とセンサ等から取得した自車位置情報から目標経路を走行する。棚位置や走行路のレイアウトが変更された場合は、制御部内のレイアウト情報を変更する。上述した台車１２０は、慣性センサ等により自車位置を把握するいわゆるガイドレス方式について説明したが、それに限定されず、例えば工場や倉庫等の床等に設置された誘導線や磁気テープ等により走行経路を把握するガイド方式であっても良い。
【００８２】
保持機構１７０は、アーム１７１と、アーム１７１の一端に設けられアーム１７１を回転する回転軸１７２と、アーム１７１の他端に設けられ物品Ｇを保持する保持部１７３と、アーム１７１と保持部１７３の間に関節部１７４を有する。
【００８３】
アーム１７１は、保持部１７３を物品Ｇに近づける。アーム１７１は、例えば、アルミニウム合金、プラスチック材料等で形成される。軽く剛性の高いものなら何でもよい。また、アーム１７１は、多関節の構造としても良い。
【００８４】
回転軸１７２は、モータ、減速機、ベアリングとエンコーダで構成される。モータ、減速機、ベアリングは同軸状に配置される。エンコーダは、モータの回転角、回転数、モータの速度、モータの負荷等を検出する。
【００８５】
保持部１７３は、物品Ｇを保持する。
保持部１７３は、複数の吸着パッド（図示しない）を有し、真空ポンプ（図示しない）等に繋がれる。吸着パッドを物品Ｇに接触させ、真空ポンプにより物品Ｇと吸着パッドの間を減圧することにより物品Ｇを吸着保持する。保持部１３３は、吸着パッドにより吸着する場合に限定されず、複数の爪で挟持する保持方式であっても良い。真空ポンプは、台車１２０の中に設置されても良い。
【００８６】
関節部１７４は、保持部１７３をアーム１７１の長手方向を軸に捩じれ方向に回転する関節１７４１と、アーム１７１の長手方向と垂直な方向を軸に保持部１７３を回転する関節１７４２とを有する。関節１７４１と関節１７４２には、それぞれサーボモータが設けられ、保持部１７３を所定の方向へ駆動する。上述したように関節部１７４は、２つの関節を有することを説明したが、それに限定されず、例えばボールジョイント等を用いても良い。第１乃至３にかかるロボット制御装置は、２リンクアーム（２軸アーム）を想定しているため、第１乃至３にかかるロボット制御装置を用いてアームに生じる外力や物品Ｇの位置情報等を推定する場合、捩じれ方向に回転する関節１７４１は、駆動しない。
【００８７】
次に、移動機構１４０について説明する。
【００８８】
移動機構１４０は、第１移動部１４１と、第２移動部１４２と、第３移動部１４３と、を有し、保持機構１７０を３軸方向に移動する直動機構である。具体的には、第１移動部１４１には、保持機構１７０の回転軸１７２が設置され、第１移動部１４１は回転軸１７２を＋Ｘ方向または−Ｘ方向に移動する。Ｘ方向は、保持機構１７０を棚に近づける方向もしくは遠ざける方向である。第２移動部１４２は、台車１２０の基台１２１に対して垂直に設置され、第１移動部１４１と略垂直に接続される。第２移動部１４２は第１移動部１４１を＋Ｚ方向または−Ｚ方向に移動する。つまり、第２移動部１４２は、保持機構１７０を＋Ｚ方向または−Ｚ方向に移動する。第３移動部１４３は、基台１２１上かつ基台１２１の上面と水平に設置される。第３移動部１４３は、第２移動部１４２と略垂直に接続される。別の観点で見ると、第３移動部１４３は、第１移動部１４１と略垂直である。第３移動部１４３は、第２移動部１４２を＋Ｙ方向または−Ｙ方向に移動する。つまり、第３移動部１４３は、保持機構１７０を＋Ｙ方向または−Ｙ方向に移動する。Ｙ方向は、ピッキング装置１００が物品Ｇをピッキングする際に棚に対して平行な方向である。
【００８９】
第１移動部１４１、第２移動部１４２と第３移動部１４３は、移動方向である３軸方向にそれぞれ平行に配置され、軸方向にスライドするように移動する。
【００９０】
移動機構１４０には、例えば、ステッピングモータを搭載した電動スライダの直動機構を用いても良い。また、電動スライダに代わり電動シリンダを用いても良い。
【００９１】
移動機構１４０には、棚１１０までの距離や棚１１０に載置された物品Ｇまでの距離、形状や載置状態を認識する認識部１６０が設置される。認識部１６０は、第１移動部１４１の先端に設置される。
【００９２】
認識部１６０には、距離画像センサまたは赤外線ドットパターン投影方式カメラなどの三次元位置計測可能なカメラを利用することができる。赤外線ドットパターン投影方式カメラは、赤外線のドットパターンを対象物体に投影し、この状態で棚１０に載置された物品Ｇの赤外線画像を撮影する。赤外線画像を解析することで物品Ｇの３次元情報を得ることが可能である。
【００９３】
荷台１５０は、保持機構１７０により保持された物品Ｇを移載する箇所である。荷台１５０は、台車１２０の基台上に設置される。荷台１５０は、移載部１５１と第４移動部１５２と、を有する。移載部１５１は、コンテナ、箱、カゴや板状のステージ等で構成される。第４移動部１５２は、台車１２０の基台１２１上に略垂直に設置され、移載部１５１を＋Ｚ方向または−Ｚ方向に移動可能である。第４移動部１５２は、例えば、ステッピングモータを搭載した電動スライダの直動機構を用いても良い。また、電動スライダに代わり電動シリンダを用いても良い。アーム１７１が駆動する際、荷台１５０は、アーム１７１に接触しないように＋Ｚ方向または−Ｚ方向に移動する。
【００９４】
第１乃至３の実施形態にかかるロボット制御装置１３は、台車１２０の制御部に内蔵される。ロボット制御装置１３は、保持機構１７０の回転軸１７２及び関節部１７４に設置されるモータの電流指令値やエンコーダ値（モータ角速度等）を取得する。そして、電流指令値と角速度の情報に基づいて保持部１７３に生じる外力を推定する。外力を推定するためのオブザーバ部１７１での導出方法や閾値指定部１７２での閾値の設定は、第１乃至３にかかるロボット制御装置と同様である。
【００９５】
本実施形態にかかるピッキング装置１００は、棚１１０や棚１１０に載置された物品Ｇに保持部１７３を突き当てることにより認識部１６０では正確に認識できない棚１１０や物品Ｇの位置情報を正確に取得できる。
【００９６】
また、認識部１６０とロボット制御装置１３を用いた保持部１７３の突き当て動作を組合せることにより物品Ｇの保持を精度良く行うことができる。
【００９７】
また、ロボット制御装置１３により、棚１１０や物品Ｇの位置情報をキャリブレーションすることにより認識部１６０を複数有さなくても正確な位置情報を取得することができる。
【００９８】
また、第１乃至３にかかるロボット制御装置が推定するリンクの角速度及び速度は、第１の推定値とも称され、モータの角速度及び速度は、第２の推定値とも称される。
【００９９】
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【０１００】
１  基台
２  第１軸
３  第１リンク
４  第１モータ
５  第１減速機
６  第１エンコーダ
７  第２軸
８  第２リンク
９  第２モータ
１０  第２減速機
１１  第２エンコーダ
１２  負荷
１３  ロボット制御装置
２０  モータ
２５  減速機
３０  リンク
１００  ピッキング装置
１１０  棚
１２０  台車
１２１  基台
１２２  車輪
１３１  オブザーバ部
１３１１、１３１２  ＰＩ制御
１３１３  非線形動力学モデル
１３１４、１３１５  積分器
１３２  閾値指定部
１３３  外力判定部
１３４  接触位置記憶部
１３５  反転起動生成部
１４０  移動機構
１４１  第１移動部
１４２  第２移動部
１４３  第３移動部
１５０  荷台
１５１  移載部
１５２  第４移動部
１６０  認識部
１７０  保持機構
１７１  アーム
１７２  回転軸
１７３  保持部
１７４  関節部
【図１】
2018083274.tif 000019
【図２】
2018083274.tif 000020
【図３】
2018083274.tif 000021
【図４】
2018083274.tif 000022
【図５】
2018083274.tif 000023
【図６】
2018083274.tif 000024
【図７】
2018083274.tif 000025
【図８】
2018083274.tif 000026
【図９】
2018083274.tif 000027
【図１０】
2018083274.tif 000028
【図１１】
2018083274.tif 000029
【図１２】
2018083274.tif 000030
【図１３】
2018083274.tif 000031