(57)【特許請求の範囲】
【請求項１】
リンク機構の関節部を駆動するモータトルクを制御するロボットアームの制御装置であって、
前記関節部の関節角度の検出値が目標値に追従するように演算される基本トルクに、安全性を考慮して設定される安全モータトルクプロフィールによる制限を適用することにより、安全モータトルクを演算するモータトルク演算手段と、
前記関節角度の検出値に対する前記安全モータトルクの変化が、前記関節部より先端側に連結される前記リンクの重量を支えるためのトルクに基づいて規定される基準値から乖離したと判断される場合に、隣合う前記リンクの間に物体が挟まれたと判定する挟み検出手段と、
前記挟み検出手段により前記物体の挟み込みが検出された場合に、前記基準値からの乖離が検出された時の関節角度、前記関節角度の検出値、前記物体について予め設定される物性特性値、及び前記モータトルクの検出値から少なくとも前記関節部より先端側に連結される前記リンクの重量を支えるためのトルクを排除した挟みトルクに基づいて、前記関節部から前記物体が挟まれた位置までの距離を示す挟み位置を推定する前記挟み位置推定手段と、
前記挟み位置が小さくなるに伴い前記安全モータトルクの上限値が小さくなるように、且つ前記挟み位置が大きくなるに伴い前記安全モータトルクの上限値が大きくなるように前記安全モータトルクプロフィールを生成する安全モータトルク演算手段と、
を備えるロボットアームの制御装置。
【請求項２】
前記安全モータトルクプロフィールは、前記基本トルクを変数とし、前記安全モータトルクの上限値を最大値とする飽和関数又はシグモイド関数である、
請求項１に記載のロボットアームの制御装置。
【請求項３】
前記基準値は、前記リンクの重量、及び当該リンクが所定の対象物を保持している場合には当該対象物の重量を加算した重量に基づいて規定される前記関節角度の検出値の正弦関数で表わされる、
請求項１又は２に記載のロボットアームの制御装置。
【請求項４】
前記正弦関数は、下記式（１）により表わされる、
請求項３に記載のロボットアームの制御装置。
【数９】
0005556558.tif 000010
ただし、記号の意味は以下の通りである。
Ｔm：２つの隣合う前記リンクが前記物体を挟んでいない場合の前記モータトルク
ｍ：前記２つの隣合うリンクのうち前記ロボットアームを備えたサービスロボットの本体から遠い方のリンクの質量（前記対象物を把持している場合はその質量も含む）
ｇ：重力加速度
ｌ：前記関節から前記２つの隣合うリンクのうち前記サービスロボットの本体から遠い方のリンクの重心までの距離
θ：前記関節角度の検出値
θ0：前記２つの隣合うリンクのうち前記サービスロボットの本体に近い方のリンクの水平となす角度
【請求項５】
前記物性特性値は、人体の一部のヤング率及びポアソン比に基づいて規定される、
請求項１〜４のいずれか１つに記載のロボットアームの制御装置。
【請求項６】
前記挟み位置は、下記式（２）により算出される、
請求項１〜５のいずれか１つに記載のロボットアーム制御装置。
【数１０】
0005556558.tif 000011
ただし、記号の意味は以下の通りである。
θ：前記関節角度の検出値
θc：前記正弦関数からの乖離が検出された時の関節角度
Ｔc：前記挟みトルク
Ｅ：人体のヤング率
ν：人体のポアソン比
【請求項７】
リンク機構の関節部を駆動するモータトルクを制御するロボットアームの制御方法であって、
前記関節部の関節角度の検出値が目標値に追従するように基本トルクを演算するステップと、
前記基本トルクに、安全性を考慮して設定される安全モータトルクプロフィールによる制限を適用することにより、安全モータトルクを演算するステップと、
前記関節角度の検出値に対する前記安全モータトルクの変化が、前記関節部より先端側に連結される前記リンクの重量を支えるためのトルクに基づいて規定される基準値から乖離したと判断される場合に、隣合う前記リンクの間に物体が挟まれたと判定するステップと、
前記物体の挟み込みが検出された場合に、前記基準値からの乖離が検出された時の関節角度、前記関節角度の検出値、前記物体について予め設定される物性特性値、及び前記モータトルクの検出値から少なくとも前記関節部より先端側に連結される前記リンクの重量を支えるためのトルクを排除した挟みトルクに基づいて、前記関節部から前記物体が挟まれた位置までの距離を示す挟み位置を推定するステップと、
前記挟み位置が小さくなるに伴い前記安全モータトルクの上限値が小さくなるように、且つ前記挟み位置が大きくなるに伴い前記安全モータトルクの上限値が大きくなるように前記安全モータトルクプロフィールを生成するステップと、
を備えるロボットアームの制御方法。
【請求項８】
前記安全モータトルクプロフィールは、前記基本トルクを変数とし、前記安全モータトルクの上限値を最大値とする飽和関数又はシグモイド関数である、
請求項７に記載のロボットアームの制御方法。
【請求項９】
前記基準値は、前記リンクの重量、及び当該リンクが所定の対象物を保持している場合には当該対象物の重量を加算した重量に基づいて規定される正弦関数で表わされる、
請求項７又は８に記載のロボットアームの制御方法。
【請求項１０】
前記正弦関数は、下記式（３）により表わされる、
請求項９に記載のロボットアームの制御方法。
【数１１】
0005556558.tif 000012
ただし、記号の意味は以下の通りである。
Ｔm：２つの隣合う前記リンクが前記物体を挟んでいない場合の前記モータトルク
ｍ：前記２つの隣合うリンクのうち前記ロボットアームを備えたサービスロボットの本体から遠い方のリンクの質量（前記対象物を把持している場合はその質量も含む）
ｇ：重力加速度
ｌ：前記関節から前記２つの隣合うリンクのうち前記サービスロボットの本体から遠い方のリンクの重心までの距離
θ：前記関節角度の検出値
θ0：前記２つの隣合うリンクのうち前記サービスロボットの本体に近い方のリンクの水平となす角度
【請求項１１】
前記物性特性値は、人体の一部のヤング率及びポアソン比に基づいて規定される、
請求項７〜１０のいずれか１つに記載のロボットアームの制御方法。
【請求項１２】
前記挟み位置は、下記式（４）により算出される、
請求項７〜１１のいずれか１つに記載のロボットアーム制御方法。
【数１２】
0005556558.tif 000013
ただし、記号の意味は以下の通りである。
θ：前記関節角度の検出値
θc：前記正弦関数からの乖離が検出された時の関節角度
Ｔc：前記挟みトルク
Ｅ：人体のヤング率
ν：人体のポアソン比
【請求項１３】
リンク機構の関節部を駆動するモータトルクを制御するロボットアームの制御用プログラムであって、
前記ロボットアームを制御するコンピュータに、
請求項７〜１２のいずれか１つに記載の方法を実行させる、
ロボットアームの制御用プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアームの制御に関し、特にロボットアームのリンク機構に人体等の物体が挟まれた場合の安全制御に関するものである。
【背景技術】
【０００２】
リンク機構を自律的に制御することにより各種作業を実行するロボットアームは、リンク間に人体等の物体が挟まれた場合の安全制御を必要とする。このような安全制御の重要性は、ロボットアームが人の生活空間で使用されるケースが増加していることに伴い増加している。
【０００３】
図５は、特許文献１に係る関節装置１０１の機能的な構成を示している。関節装置１０１は、比較部１０２、誤差増幅部１０３、モータ１０４、減速機１０５、位置検出部１０６、及びトルク制限値発生部１０７を備える。比較部１０２は、位置検出部１０６が検出する関節の位置（角度）の検出値を位置指令（目標値）から減算した位置追従偏差を出力する。誤差増幅部１０３は、トルク制限値発生部１０７からのトルク制限値の範囲内で位置追従偏差が最も小さくなるようなトルク指令を出力する。モータ１０４は、トルク指令に応じてモータトルクを発生する。減速機１０５は、モータトルクをロボットアームのリンク機構を駆動する助力に変換する。位置検出部１０６は、減速機１０５と同じ回転軸に設置され、関節角度を示す位置検出値を出力する。トルク制限値発生部１０７は、位置検出値と位置追従偏差に応じて、関節角度と関節角度の変化の方向に応じたトルク制限値を出力する。
【０００４】
図６は、特許文献２に係る圧力分布センサ２０１の構成を概略的に示している。圧力分布センサ２０１は、センサユニット２０２、センサメッシュ２０３、及び光ファイバ２０４を備える。センサメッシュ２０３は、複数のセンサユニット２０２がマトリクス状に配列されて構成される。各センサユニット２０２により検出された圧力検出信号は、各センサユニット２０２と接続する複数の光ファイバ２０４を介して外部に出力される。出力された各圧力検出信号により各センサメッシュ２０３の位置における圧力が分かる。そして、このような圧力分布センサ２０１をロボットアームのリンク表面に設置することにより、リンク間に物体が挟まれたこと及び挟まれた位置を検出することができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０００−６０６５
【特許文献２】特開平６−８２３２０
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記特許文献１に係る関節装置１０１は、物体がロボットアームのリンク間に挟まれた場合、挟まれた場所によらず、モータトルクを関節角度のみに依存して制限する。従って、挟まれた位置が２つのリンクを連結する関節に近い場合、挟まれた物体に大きな力が加わる可能性がある。また、挟まれた位置が関節から遠い場合、モータトルクが必要以上に抑制される場合がある。即ち、ロボットアームが人体等に危害を与えることなく作業を続行できる状況であっても、作業が中断される場合がある。
【０００７】
また、上記特許文献２に係る圧力分布センサ２０１を用いる場合、挟まれた位置を正確に検出するためには、リンク表面に多数のセンサユニット２０２を設置しなければならない。そのため、製造コスト・制御装置の演算負荷・重量・故障率等が増加し、電磁干渉が発生する等の問題がある。
【０００８】
そこで、本発明は、リンク上の圧力分布を検出するセンサ等を用いることなく物体が挟まれた位置（挟み位置）を正確に検出し、挟み位置に応じて関節機構のトルクを最適に制御できるようにすることを目的とする。
【課題を解決するための手段】
【０００９】
本発明の第１の態様は、リンク機構の関節部を駆動するモータトルクを制御するロボットアームの制御装置であって、リンク機構の関節部を駆動するモータトルクを制御するロボットアームの制御装置であって、前記関節部の関節角度の検出値が目標値に追従するように演算される基本トルクに、安全性を考慮して設定される安全モータトルクプロフィールによる制限を適用することにより、安全モータトルクを演算するモータトルク演算手段と、前記関節角度の検出値に対する前記安全モータトルクの変化が、前記関節部より先端側に連結される前記リンクの重量を支えるためのトルクに基づいて規定される基準値から乖離したと判断される場合に、隣合う前記リンクの間に物体が挟まれたと判定する挟み検出手段と、前記挟み検出手段により前記物体の挟み込みが検出された場合に、前記基準値からの乖離が検出された時の関節角度、前記関節角度の検出値、前記物体について予め設定される物性特性値、及び前記モータトルクの検出値から少なくとも前記関節部より先端側に連結される前記リンクの重量を支えるためのトルクを排除した挟みトルクに基づいて、前記関節部から前記物体が挟まれた位置までの距離を示す挟み位置を推定する前記挟み位置推定手段と、前記挟み位置が小さくなるに伴い前記安全モータトルクの上限値が小さくなるように、且つ前記挟み位置が大きくなるに伴い前記安全モータトルクの上限値が大きくなるように前記安全モータトルクプロフィールを生成する安全モータトルク演算手段とを備えるものである。
【００１０】
上記態様によれば、高価なセンサ等を用いることなく物体の挟み位置を正確に推定することができると共に、挟み位置に応じて適切なトルク指令を生成することができる。また、挟まれた人体等への危害を確実に防止することができると共に、当該人体等に危害を与えない範囲でロボットアーム本来の動作を続行させることができる。
【００１１】
また、前記安全モータトルクプロフィールは、前記基本トルクを変数とし、前記安全モータトルクの上限値を最大値とする飽和関数又はシグモイド関数であることが好ましい。
【００１２】
また、前記基準値は、前記リンクの重量、及び当該リンクが所定の対象物を保持している場合には当該対象物の重量を加算した重量に基づいて規定される、前記関節角度の検出値の正弦関数で表わされることが好ましい。
【００１３】
また、前記正弦関数は、例えば下記式（１）により表わすことができる。
【００１４】
【数１】
0005556558.tif 000002
ただし、記号の意味は以下の通りである。
Ｔm：２つの隣合う前記リンクが前記物体を挟んでいない場合の前記モータトルク
ｍ：前記２つの隣合うリンクのうち前記ロボットアームを備えたサービスロボットの本体から遠い方のリンクの質量（前記対象物を把持している場合はその質量も含む）
ｇ：重力加速度
ｌ：前記関節から前記２つの隣合うリンクのうち前記サービスロボットの本体から遠い方のリンクの重心までの距離
θ：前記関節角度の検出値
θ0：前記２つの隣合うリンクのうち前記サービスロボットの本体に近い方のリンクの水平となす角度
【００１５】
また、前記物性特性値は、人体の一部のヤング率及びポアソン比に基づいて規定されることが好ましい。
【００１６】
また、前記挟み位置は、例えば下記式（２）により算出することができる。
【００１７】
【数２】
0005556558.tif 000003
ただし、記号の意味は以下の通りである。
θ：前記関節角度の検出値
θc：前記正弦関数からの乖離が検出された時の関節角度
Ｔc：前記挟みトルク
Ｅ：人体のヤング率
ν：人体のポアソン比
【００１８】
また、本発明の第２の態様は、上記制御装置と同様の技術的思想に基づく制御方法である。
【００１９】
更に、本発明の第３の態様は、上記制御装置及び制御方法と同様の技術的思想に基づく制御用プログラムである。
【発明の効果】
【００２０】
本発明によれば、高価なセンサ等を用いることなく物体の挟み位置を正確に推定することができると共に、挟み位置に応じて適切なトルク指令を生成することができる。また、挟まれた人体等への危害を確実に防止することができると共に、当該人体等に危害を与えない範囲でロボットアーム本来の動作を続行させることができる。
【図面の簡単な説明】
【００２１】
【図１】本発明の実施の形態１に係るロボットアームの機能的な構成を示すブロック図である。
【図２Ａ】隣合う第１及び第２のリンクがそれぞれ人体と点接触している状態を概略的に示す図である。
【図２Ｂ】隣合う第１及び第２のリンクがそれぞれ人体と面接触している状態を概略的に示している。
【図３】実施の形態１に係るロボットアームにおける制御の流れを示すフローチャートである。
【図４Ａ】実施の形態１に係る安全モータトルク指令と時間との関係を示すグラフである。
【図４Ｂ】実施の形態１に係る挟み力と時間との関係を示すグラフである。
【図４Ｃ】実施の形態１に係る関節角度検出値と時間との関係を示すグラフである。
【図５】特許文献１に係る関節装置の機能的な構成を示すブロック図である。
【図６】特許文献２に係る圧力分布センサの構成を概略的に示す図である。
【発明を実施するための形態】
【００２２】
実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の実施の形態１に係るロボットアーム１の機能的な構成を示している。ロボットアーム１は、アーム部１１、関節角度指令発生部１２、及び駆動力抑制制御部１３を備える。
【００２３】
アーム部１１は、リンク機構２１、関節モータ２２、及び関節角度検出部２３を備える。リンク機構２１は、基本的に変形しない構造物である複数のリンク、リンク間を接続する関節機構等から構成される。リンク機構２１は、ロボットアーム１の使用目的に応じて設計され、例えば物体の把持、運搬等が可能となるように構成される。関節モータ２２は、電気信号に応じてその駆動軸が制御され、隣合うリンクの相対的な位置関係を変化させる。関節角度検出部２３は、各関節モータ２２の駆動軸の回転量及び回転方向を検出し、その検出結果を電気信号に変換して出力する。
【００２４】
関節角度指令発生部１２は、アーム部１１が所定の動作を行うのに必要な関節角度（関節機構の駆動軸の回転角度）の目標値を示す関節角度指令θrを出力する。この関節角度指令θrは、通常、アーム部１１の本来果たすべき動作を制御するメインの制御プログラムに従って生成される。
【００２５】
駆動力抑制制御部１３は、モータトルク演算部３１、電流制御部３２、挟み検出部３３、挟み位置推定部３４、及び安全モータトルク演算部３５を備える。
【００２６】
モータトルク演算部３１は、関節角度指令θr、関節角度検出値θ、及び安全モータトルク演算部３５が出力する安全モータトルクプロフィールＰsを入力し、安全モータトルク指令Ｔsを出力する。安全モータトルク指令Ｔsは、関節角度検出値θが関節角度指令θrに追従するように生成される基本モータトルク指令Ｔ0に、安全モータトルクプロフィールＰsを適用して生成されたものである。ここでいう適用とは、例えば、ＰsをＴ0を変数とする関数で表わし、Ｔs＝Ｐs（Ｔ0）を演算すること等が相当する。例えば、当該関数Ｐsは、所定値を上限値とし、当該所定値の符号を反転させた値を下限値とする飽和関数、シグモイド関数等であることが好ましい。
【００２７】
電流制御部３２は、モータトルク演算部３１からの安全モータトルク指令Ｔsを入力し、関節モータ２２が発生するモータトルクＴmが安全モータトルク指令Ｔsに一致するようなモータ電流を関節モータ２２に出力する。モータトルクＴmは、周知の機構により検出され、フィードバック制御等に利用される。
【００２８】
挟み検出部３３は、関節角度検出値θ及び安全モータトルク指令Ｔsを入力し、安全モータトルク指令Ｔsの関節角度検出値θに対する変化の特徴から、リンク機構２１に物体が挟まれたことを検出し、挟み検出信号Ｓを出力する。
【００２９】
挟み位置推定部３４は、関節角度検出値θ、安全モータトルク指令Ｔs、及び挟み検出信号Ｓを入力し、物体が挟まれた位置（関節から当該位置までの距離）を示す挟み位置ｘを推定し、この推定結果を出力する。
【００３０】
安全モータトルク演算部３５は、挟み位置ｘを入力し、この挟み位置ｘに応じた安全モータトルクプロフィールＰsを生成し出力する。
【００３１】
上記関節角度指令生成部１２及び駆動力抑制制御部１３は、マイクロプロセッサ等を用いたプログラマブル電子制御系、各種電気回路（論理回路）の組み合わせ等により構成される。
【００３２】
図２Ａは、隣合う第１及び第２のリンク４１，４２がそれぞれ人体４３と点接触している状態を概略的に示している。図２Ｂは、隣合う第１及び第２のリンク４１，４２がそれぞれ人体４３と面接触している状態を概略的に示している。第１及び第２のリンク４１，４２は、関節４４により回動可能に連結されている。例えば、当該ロボットアームが人間を模倣したものの場合、第１のリンク４１は上腕、関節４４は肘、第２のリンク４２は前腕に対応する。
【００３３】
図２Ａにおいて、人体４３の断面の中心と第１又は第２のリンク４１，４２の接点との間の距離Ｒ、関節４４と当該接点との距離（挟み位置）ｘ、第１及び第２のリンク４１，４２が人体４３と点接触している状態の関節角度θc（θc／２）が示されている。図２Ａは、第１のリンク４１に対して第２のリンク４２が時計回りに回転して人体４３を挟み始めた状態を示している。
【００３４】
図２Ａの状態から更に第２のリンク４２を時計回りに回転させると、人体４３が潰されて図２Ｂのような状態となる。図２Ｂの状態において、人体４３は第１及び第２のリンク４１，４２と面接触している。図２Ｂにおいて、接触面の断面の長さの半分ａ、関節角度θ（θ／２）が示されている。本実施の形態においては、関節角度θは第２のリンク４２が第１のリンク４１と重なった状態を０[ｒａｄ]とし、反時計回りに最大π[ｒａｄ]まで変化できるものとする。
【００３５】
図２Ａに示されるように、挟み角度θc、距離Ｒ、及び挟み位置ｘの関係式は、下記式（３）となる。
【数３】
0005556558.tif 000004
【００３６】
人体４３が第１及び第２のリンク４１，４２に挟まれて潰された状態における接触面の断面の長さの半分ａは、Contact Mechanics, Johnson, K,L, Cambridge University Press, Cambridge, United Kingdom, 1985より、下記式（４）で与えられる。
【００３７】
【数４】
0005556558.tif 000005
ただし、Ｅは人体のヤング率、νは人体のポアソン比、Ｔcは挟みトルクである。ここで、挟みトルクＴcとは、モータトルクＴmから第２のリンク４２の重量（第２のリンク４２が物体を把持等している場合は当該物体の重量を含む）、慣性力等に対抗するための成分を除いたものである。
【００３８】
図２Ｂ及び式（４）より下記式（５）が得られる。
【数５】
0005556558.tif 000006
【００３９】
式（３）及び式（５）より挟み位置ｘは、下記式（６）で表わされる。
【数６】
0005556558.tif 000007
【００４０】
このように、挟み位置ｘは、挟み角度θc、検出された関節角度θ、Ｅ，ν等から定まる所定の物性特性値、及び挟みトルクＴcにより定まる。図２Ｂにおいて、第２のリンク４２の質量（上記把持物体等を含む）をｍ、関節４４から第２のリンク４２の重心までの距離をｌ、第１のリンク４１の水平と成す角度をθ0、重力加速度をｇとすると、図２Ｂにおける理論上のモータトルクＴmは、式（５）を用いて下記式（７）となることが導出される。
【００４１】
【数７】
0005556558.tif 000008
【００４２】
式（７）において、上式（０＜θ≦θc）は、人体４３が挟まれている場合に適用され、下式（θc＜θ≦π）は、人体４３が挟まれていない場合に適用されるものである。上式において、右辺第１項は、第２のリンク４２の重量を支えるためのモータトルク成分であり、第２項は、挟みトルクＴcである。利用者の生活補助等に用いられるロボットアームは、一般的にゆっくりした動作しか実行しないため、当該上式においては第２のリンク４２の慣性力が考慮されていない。慣性力を無視できない場合には、当該慣性成分を右辺に加算すればよい。
【００４３】
式（７）において、第１のリンク４１の水平と成す角度θ0は既知であり、関節４４から第２のリンク４２の重心までの距離ｌは設計値より既知である。従って、第１のリンク４１と第２のリンク４２とが人体４３に接触していない時（θc＜θ≦π）のモータトルクＴmから、第２のリンク４２の質量ｍを算出することができる。そして、実際のモータトルクＴmから式（７）下式におけるモータトルクＴmを減算し、その結果の絶対値が閾値（微小値）を超えたら人体４３の挟み込みが発生したと判定される。即ち、人体４３が挟まれていないθc＜θ≦πの範囲においては、理論上のＴmの変化は関節角度検出値θの正弦関数となるため、実際のモータトルクＴm、即ち安全モータトルク指令Ｔsの変化が正弦関数を外れた時点で人体４３の物体の挟み込みが発生したと判定することができる。そして、この時の関節角度検出値θが挟み角度θcとなる。
【００４４】
そして、本実施の形態においては、挟みトルクＴcが、人体４３に危害を与えない限界安全モータトルクＴlimを超えないように制御される。限界安全モータトルクＴlimは、人体４３に危害を与えない外力の最大値である限界安全外力Ｆlim及び挟み位置ｘに基づいて求められる。限界安全外力Ｆlimは、産業安全の分野で広く知られるものである（齋藤剛、池田博康「人間協調型ロボットの機械的刺激に対する人体痛覚耐性限界の測定」、産業安全研究所特別研究報告、NIIS-SRR-NO.３３、２００５年９月；「消費生活製品向けリスクアセスメントのハンドブック、第１版」、経済産業省等参照）。本実施の形態に係る限界安全モータトルクＴlimは、下記式（８）で与えられる。尚、当該数式は一例であり、当該事項の趣旨は、挟み位置ｘが小さい程限界安全モータトルクＴlimが小さくなるように、挟み位置ｘが大きい程限界安全モータトルクＴlimが大きくなるようにすることである。挟み位置ｘが小さい（関節４４に近い）場合には、人体４３への危害を確実に防止するために、ロボットアーム１に求められる本来の動作ができなくなったとしても、Ｔlimを小さく設定する必要がある。一方、挟み位置ｘが大きい（関節４４から遠い）場合には、Ｔlimをある程度大きく設定しても人体４３への危害を防止できる可能性が高い。従って、挟み位置ｘが比較的大きい場合には、Ｔlimを比較的大きく設定し、本来の動作を続行できる可能性を高くすることが有効である。
【００４５】
【数８】
0005556558.tif 000009
【００４６】
安全モータトルクプロフィールＰsは、式（８）の限界安全モータトルクＴlimを最大値、その符号を反転させたものを最小値とする飽和関数又はシグモイド関数として与えられることが好ましい。これにより、安全モータトルク指令Ｔs（＝Ｐs（Ｔ0））は、−Ｔlim＜Ｔs＜Ｔlimの範囲内で連続的に変化する値として与えられる。
【００４７】
図３は、本実施の形態に係るロボットアーム１における制御の流れを示している。先ず、モータトルク演算部３１（図１参照）は、関節角度検出値θが関節角度指令θrに追従するように基本モータトルク指令Ｔ0を算出し（Ｓ１）、安全モータトルク演算部３５より安全モータトルクプロフィールＰsを読み込み（Ｓ２）、安全モータトルク指令Ｔsを算出する（Ｓ３）。次いで、挟み検出部３３は、Ｔsのθに対する変化を表わす曲線が正弦波から乖離しているか否かを判定し（Ｓ４）、乖離していないと判定された場合（Ｎ）には、当該Ｔsに基づいてアーム部１１の関節モータ２２を制御する（Ｓ１０）。
【００４８】
一方、ステップＳ４において、乖離していると判定された場合（Ｙ）には、挟み検出部３３は挟み検出信号Ｓを出力する（Ｓ５）。挟み位置推定部３４は、挟み検出信号Ｓを受信すると、関節角度検出値θ及び安全モータトルク指令Ｔsを入力し、上述のように式（６）、（７）等に基づいて、挟み位置ｘを推定する（Ｓ６）。次いで、安全モータトルク演算部３５は、挟み位置ｘに基づいて、上述のように式（８）等に基づいて限界安全モータトルクＴlimを決定し（Ｓ７）、安全モータトルクプロフィールＰsを生成する（Ｓ８）。そして、モータトルク演算部３１は、ステップＳ８で生成された安全モータトルクプロフィールＰsを適用して安全モータトルク指令Ｔsを演算し（Ｓ９）、当該演算結果に基づいてモータ制御を行う（Ｓ１０）。
【００４９】
上記構成のアームロボット１によれば、高価なセンサ等を用いることなく物体の挟み位置を正確に推定することができると共に、挟み位置に応じて適切なトルク指令を生成することができる。また、挟まれた人体等への危害を確実に防止することができると共に、当該人体等に危害を与えない範囲でロボットアーム１本来の動作を続行させることができる。
【００５０】
以下に、実施の形態１に係るロボットアーム１のシミュレーション結果を示す。シミュレーションに用いた数値は以下の通りである。
ｍ＝２．０[ｋｇ]
ｌ＝０．１３[ｍ]
ｇ＝９．８[ｓ^−２]
Ｅ＝２５×１０^３[ＭＰａ]
ν＝０．２５
θ0＝２００・π／１８０[ｒａｄ]
Ｒ＝２０×１０^−３[ｍ]
ｘ＝５０×１０^−３[ｍ]
Ｆlim＝５０[Ｎ]
上記数値は、人間の前腕の物性値に基づいている。
【００５１】
本シミュレーションは、図２Ａ，Ｂに示すロボットアームの関節角度を制御する際に、挟まれた人体４３に加わる力が限界安全外力Ｆlim以内となるように制御するものである。
【００５２】
図４Ａは、実施の形態１に係るモータトルクＴm（Ｔs）と時間との関係を示している。同図中、実線は本発明に係るモータトルクを示し、一点鎖線は従来技術に係るモータトルクを示している。
【００５３】
図４Ａ中０[ｓ]から２．７３[ｓ]の区間において、モータトルクＴmの関節角度検出値θに対する変化の波形が正弦波となっており、第１及び第２のリンク４１，４２の間に人体４３は挟まれていないと判定された。２．７３[ｓ]において、モータトルクＴmは急激に変化して正弦波から乖離しており、人体４３が挟まれたことが検出された。その後、挟み位置ｘが推定され、限界安全モータトルクＴlimが決定され、安全モータトルクプロフィールＰsが生成され、当該Ｐsが適用されて安全モータトルク指令Ｔsが出力された。即ち、２．７３[ｓ]以降、式（７）の上式右辺第２項で示される挟みトルクＴcが計算され、その絶対値が式（８）の限界安全モータトルクＴlimを超えないように制限された。そして、３．１４[ｓ]において、挟みトルクＴcの絶対値が限界安全モータトルクＴlimに達し、その後モータトルクＴmは一定の値を超えないように制限されている。
【００５４】
一方、従来技術においては、限界安全モータトルクＴlimによる挟みトルクＴcの制限がないので、予め設定したトルク制限値に達するまでモータトルクの絶対値は増加し続ける。
【００５５】
ここで、式（６）を用いて２．７３[ｓ]における挟み位置ｘが推定されたが、この推定結果は５０×１０^−３[ｍ]となり、実測値と一致した。また、限界安全モータトルクＴlimは、式（８）を用いて算出され、モータトルクの成分のうち挟みトルクＴcの絶対値のみが限界安全モータトルクＴlimに制限されるような安全モータトルクプロフィールＰsが出力されている。そして、関節角度検出値θが関節角度指令θrに追従するようなモータトルク指令Ｔ0に安全モータトルクプロフィールＰsを適用することにより、安全モータトルク指令Ｔsが出力されている。
【００５６】
図４Ｂは、実施の形態１に係る挟み力Ｆcと時間との関係を示している。この挟み力Ｆcは、第１のリンク４１と第２のリンク４２とが人体４３を挟む力のことである。挟み力Ｆcは、上記挟みトルクＴcを挟み位置ｘにより除算することにより算出される。図４Ｂにおいて、実線は本発明に係る挟み力Ｆcを示し、一点鎖線は従来技術に係る挟み力を示し、破線は限界安全外力Ｆlimを示している。本発明によると挟み力Ｆcが限界安全外力Ｆlimを超えないのに対し、従来技術によると予め設定したトルク制限値に達するまで挟み力Ｆcが増加し続けることが分かる。
【００５７】
図４Ｃは、実施の形態１に係る関節角度検出値θと時間との関係を示している。同図において、実線は本発明に係る関節角度検出値を示し、一点鎖線は従来技術に係る関節角度検出値を示している。本実施の形態においては、関節角度指令生成部１２は、１．０４[ｒａｄ]から０．５２[ｒａｄ]まで直線的に変化する関節角度指令θrを出力する。従来技術によると関節角度検出値は人体４３が受ける挟み力Ｆcの大きさに関わらず、関節角度指令θrに追従する。一方、本発明によると３．１４[ｓ]以降において挟み力Ｆcが限界安全外力Ｆlimを超えないようにするため、関節角度検出値θが０．７１[ｒａｄ]より小さくならないように抑制される。
【００５８】
以上のように、本発明によれば、高価なセンサ等を用いることなく物体の挟み位置を正確に推定することができると共に、挟み位置に応じて適切なトルク指令を生成することができる。また、挟まれた人体等への危害を確実に防止することができると共に、当該人体等に危害を与えない範囲でロボットアーム１本来の動作を続行させることができる。
【００５９】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【産業上の利用可能性】
【００６０】
本発明は、例えば家事手伝いロボット、接客ロボット、介護ロボット、リハビリロボット等のリンク機構を備えたロボット全般に適用することができる。
【符号の説明】
【００６１】
１  ロボットアーム
１１  アーム部
１２  関節角度指令発生部
１３  駆動力抑制制御部
２１  リンク機構
２２  関節モータ
２３  関節角度検出部
３１  モータトルク演算部
３２  電流制御部
３３  挟み検出部
３４  挟み位置推定部
３５  安全モータトルク演算部
４１  第１のリンク
４２  第２のリンク
４３  人体（物体）
４４  関節
【図１】
0005556558.tif 000014
【図２Ａ】
0005556558.tif 000015
【図２Ｂ】
0005556558.tif 000016
【図３】
0005556558.tif 000017
【図４Ａ】
0005556558.tif 000018
【図４Ｂ】
0005556558.tif 000019
【図４Ｃ】
0005556558.tif 000020
【図５】
0005556558.tif 000021
【図６】
0005556558.tif 000022