ロボットの制御装置およびロボット
【課題】力センサを用いることなく、ロボットと障害物との接触を高精度に検出可能で安全性を向上させるロボットの制御装置を提供する。
【解決手段】
ロボットの駆動力を制御するロボットの制御装置であり、制御装置は所望の動作軌跡と速度を出力し、駆動力が所定時間内にロボットに対してした仕事量である制御仕事量を算出し、エネルギー推定値を算出し、所定時間内のエネルギー変化の推定値を算出し、制御仕事量とエネルギー変化推定値とに基づいてロボットへの動作指令速度を調整または停止指令または警報を発生するように構成する。
【解決手段】
ロボットの駆動力を制御するロボットの制御装置であり、制御装置は所望の動作軌跡と速度を出力し、駆動力が所定時間内にロボットに対してした仕事量である制御仕事量を算出し、エネルギー推定値を算出し、所定時間内のエネルギー変化の推定値を算出し、制御仕事量とエネルギー変化推定値とに基づいてロボットへの動作指令速度を調整または停止指令または警報を発生するように構成する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は，ロボットの駆動力を制御して作業を遂行するためのロボットの制御装置およびこれを備えたロボットに関する。
【背景技術】
【０００２】
人間と共存して作業を遂行するロボットの制御装置において、安全のためにはロボットが(人間を含む)外部環境と予期せずして接触した場合、これを確実に検出することが必要である。更には、ロボットが(人間を含む)外部環境に対してする仕事量や手先力を適切な値に制限する必要がある。従来の安全対策としては手先に設けた力センサで手先力を監視し、所定の閾値を超えたらロボットを停止させる技術や関節軸毎にトルク制限を設ける技術などがあった。
あるいは、ロボットが外部環境に接触したことを検出して警告発生や停止などの措置をとることで安全を確保する技術も提案されている。
【０００３】
例えば、特許文献1の技術では、ロボットの過負荷状態を監視し障害の発生を防止するため、所定時間Ｔ内におけるモータの実効トルクの値を監視し、監視値が規定値を越えた場合に警告を発する，関節最高速度を遅く設定する，といった措置をとっていた。
この例では、演算によって実効トルクを求め、結果が閾値よりも大きくなった場合に過負荷状態として警告を発生していた。あるいは、実効トルクが閾値を大きく超過した場合には関節最高速度を減らし、超過量が少ない場合は逆に関節最高速度を増やすことで、過負荷を防止していた。本方法によれば、ロボットが外部環境と接触すると関節負荷が増大して警告発生や減速などの措置がとられるため、安全性が高められる。
【０００４】
特許文献２の技術では、計算量を削減し高精度に衝突判定するため、動作指令からロボットの運動方程式を用いて算出した必要駆動トルク指令要素と、各関節のフィードバック値と、前記動作指令と、から必要駆動トルクを演算し、必要駆動トルクと実際のモータ駆動トルクとの差を、別途演算により求めた閾値と比較するようにしていた。ここで必要駆動トルク指令要素は、位置指令、速度指令、加速度指令をロボットの運動方程式に代入して計算するものである。また、必要駆動トルクは、実際にフィードバックされたロボット動作を実現するために必要な駆動トルクであって、本来ならば位置、速度、加速度のフィードバック値をロボットの運動方程式に代入して求めるものであるが、特許文献２では計算量節約のために近似式が用いられていた。
【０００５】
特許文献２の実施例であるロボット制御装置は、位置、速度、加速度それぞれの指令を生成する指令値生成手段、必要駆動トルク指令要素演算手段、必要駆動トルク演算手段、衝突判別のための閾値を演算する閾値演算手段、閾値とモータ駆動電流に基づいて衝突を判別する衝突判別手段、ロボットの各軸を駆動するモータを制御するモータ制御手段によって構成されていた。ここでは閾値演算手段は、関節毎に予め記憶した閾値をそれぞれ出力するものである。衝突判別手段では、まず、モータ制御手段から入力されるモータ電流から各関節のモータ駆動トルクを演算する。次に、必要駆動トルクと実際のモータ駆動トルクとの差を関節毎に計算する。いずれかの関節における必要駆動トルクとモータ駆動トルクの差がしきい値演算手段から出力された上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成手段に停止命令を送信する。本方法によれば、ロボットと外部環境との接触をある程度精度良く検出できるので、接触検出時に停止するなどの措置によって安全性を向上することができる。
【０００６】
特許文献３では，ロボットの動特性を模擬したモデルとロボット実機の位置および姿勢を比較することで衝突を判定していた。
即ち、特許文献３の技術は多自由度ロボットアーム(実機)と、多自由度ロボットアームのモデルと位置・姿勢を監視する１つのリンクと周辺機器とから構成されており、複数のリンク位置および姿勢について実機とモデルを比較することで，図のようにリンクの途中に周辺機器が接触した場合も検出できる構成となっていた。特許文献３の技術によれば手先以外の接触もある程度精度良く検出できるため、ロボットの作業領域全体で安全性向上を図ることができる。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第２９９２９２０号公報(第４頁，図２)
【特許文献２】特許第３８７８０５４号公報(第４１頁，図１)
【特許文献３】特開２００６−１１６６３５号公報(第１３頁，図８)
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ところが、力センサで手先力を監視する従来技術では、力センサを追加設置する必要があり，コスト高を招く。また，手先以外の接触によって生じる力を検出できないという課題もある。
また、関節軸毎にトルク制限を設ける従来技術では、図７のように障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に大きな力が発生するため、各関節でトルクの増大を検出する前に環境やロボット自身に過大な負荷がかかる虞があった。
即ち、図において、７００はロボットアーム、７０１はロボットアーム７００の第1関節、７０２はロボットアーム７００の第２関節、７０３は第1の障害物、７０４は第２の障害物、矢印７０５は第１関節７０１の回転の向き、矢印７０６は第２関節７０２の回転の向き、矢印７０７は第１の障害物７０３からロボットアーム７００が受ける接触力、矢印７０８は第２の障害物７０４からロボットアーム７００が受ける接触力である。ロボットアーム７００は直線に近い姿勢をとっているため、第1の障害物７０３から受ける接触力７０７が第１関節７０１および第２関節７０２のトルクに与える影響は軽微である。関節７０１および７０２のトルクが制限値を上回らないまま各関節が矢印７０５および７０６の向きに運動を継続すれば、接触力７０７は非常に大きくなってしまう。
【０００９】
また、ロボットアーム７００は第２の障害物７０４と第１関節７０１に近い位置で接触しているため、第２の障害物７０４から受ける接触力７０８が第２関節７０２のトルクに与える影響はなく、第１関節７０１への影響も軽微である。
第1関節７０１のトルクが制限値を上回らないまま第１関節７０１が矢印７０５の向きに運動を継続すれば、接触力７０８が非常に大きくなる虞がある。このように、各関節のトルクを制限するだけでは、ロボットのあらゆる動作部分と外部環境との接触に対応するには限界がある。また、画一的なトルク制限ではロボットの姿勢変化に伴う駆動トルクの大きな変動に対応できず、ロボットの動作性能が制限されてしまう場合もあった。
【００１０】
特許文献１の技術の場合、ロボットの急な加減速によって瞬間的に駆動トルクが増大すると、過負荷による破損の危険性が極めて少ないにもかかわらず警告が誤動作してしまうという問題があった。
これを防ぐためには、実効トルクを求める所定の時間Ｔを十分大きく設定する、警告発生の閾値を十分大きく設定する、といった方法が考えられるが、これらを適用すると過負荷検出の感度が低下してしまう。
また、障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に過大な力が発生することを防止できない虞がある。
【００１１】
特許文献２の場合、関節トルクを監視して衝突有無を判定する限り、障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に大きな力が発生する問題は解決できない。また、必要駆動トルクの算出に用いられる関節加速度のフィードバック値は、実際には関節位置のフィードバック値を二階差分して求める必要があるため、ノイズの影響が増大して衝突判定精度が落ちるという問題もあった。
【００１２】
また、トルクではなくロボットの複数のリンク位置および姿勢を監視する場合には、障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に過大な力が発生することを防止できない虞がある。
更に、上記特許文献を含めた多くの従来技術では、ロボットと障害物との接触や過負荷を検出できても、力センサを用いないでロボットと障害物との接触力を制限することはできていなかった。
本発明はこのような課題に鑑みてなされたものであり、力センサを用いることなく、ロボットと障害物との接触を高精度に検出可能で安全性を向上させるロボットの制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
上記目的を達成するため，本発明は次のような手段をとる。
請求項１にかかるロボットの制御装置は、ロボットの駆動力を制御するロボットの制御装置であって、所望の動作軌跡と速度を出力する動作指令記憶部と、前記駆動力が所定時間内に前記ロボットに対してした仕事量である制御仕事量を算出する制御仕事量算出部と、前記ロボットのエネルギー推定値を算出するエネルギー推定部と、前記ロボットの前記所定時間内のエネルギー変化の推定値を算出するエネルギー変化推定値算出部と、前記制御仕事量と前記エネルギー変化推定値とに基づいてロボットへの動作指令速度を調整または停止指令を発生する動作指令調整部と、を備えたことを特徴としている。
【００１４】
また、前記制御仕事量算出部は、フィードバックした前記ロボットの関節速度と前記駆動力の積である制御仕事率を所定時間にわたって積分することで前記制御仕事量を算出することが好ましい（請求項２）。
また、前記エネルギー推定部は、フィードバックした前記ロボットの関節速度および位置と、内蔵する前記ロボットの動力学モデルと、にもとづいて前記エネルギー推定値を算出することが好ましい（請求項３）。
また、前記エネルギー変化推定値算出部は、所定時間にわたって前記エネルギー推定値を時刻と対応付けて保存するエネルギー推定値履歴保存部を備え、所定時間だけ前のエネルギー推定値を現在のエネルギー推定値から減じたものを前記エネルギー変化推定値として出力することが好ましい（請求項４）。
また、前記動作司令調整部は、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が所定の第一閾値を超えた場合に停止指令を発生することが好ましい（請求項５）。
また、環境との接触を通知する警報発生部を更に備え、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が前記第一閾値を超えた場合に警報を発生することが望ましい(請求項６)。
【００１５】
また、前記第一閾値は、前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボットが接触し得る環境内物体に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値であることが好ましい（請求項７）。
また、前記第一閾値は、前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボット表面に装着した弾性柔軟材料に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値であることが好ましい（請求項８）。
また、前記動作指令調整部は、前記外部出力仕事量推定値が所定の第二閾値を超えた場合に前記動作指令記憶部が出力する動作指令の速度を遅らせたものを出力することが好ましい（請求項９）。
また、前記ロボットはアクチュエータとしてサーボモータを有し、前記制御仕事量算出部における前記駆動力は前記サーボモータを流れる電流値から算出することが好ましい（請求項１０）。
また、前記制御仕事量算出部における駆動力は前記駆動力指令をそのまま用いることが好ましい（請求項１１）。
また、前記駆動力は摩擦補償分を除いたものであることが好ましい（請求項１２）。
【００１６】
また、請求項１３にかかるロボットは、アームと、前記アームに内蔵され、請求項１〜１１のいずれか１項に記載のロボットの制御装置によって制御されるアーム駆動用のサーボモータと、ロボットの制御装置からの前記動作指令速度または前記停止指令に基づいてロボットの関節を駆動制御するサーボ制御部と、を備えたことを特徴としている。
また、前記アームの表面の一部または全体が弾性柔軟材料で覆われていることが好ましい（請求項１４）。
【発明の効果】
【００１７】
請求項１及び請求項１３の発明によれば，力センサなどを追加することなくロボットのあらゆる動作部分と外部環境との接触を検出でき，またロボットの動作指令を適切に調整できるため，ロボットシステムの安全性を高めることができる。
請求項２の発明によれば，極めて簡単な演算によってロボットの駆動力がロボットおよび外部環境に対してした仕事量を求められる。また，数値誤差の蓄積による誤検出を防止できる。
請求項３の発明によれば，ロボットが保持する力学的エネルギーを正確に推定できる。
請求項４の発明によれば，ロボットが保持する力学的エネルギーの変化を正確に検出できる。
請求項５の発明によれば，ロボットのあらゆる動作部分と外部環境との接触が起こった場合にロボットを停止させ，ロボットや外部環境物に過大な力が発生することを防止することができる。
請求項６の発明によれば、ロボットのあらゆる動作部分と外部環境との接触が起こった場合に周囲の作業者や周辺機器に警報で通知することで、迅速な対応を促すことができる。
【００１８】
請求項７の発明によれば，力センサなどを追加することなく，ロボットのあらゆる動作部分と外部環境との接触力を制限でき，ロボットシステムの安全性を向上できる。
請求項８の発明によれば，力センサなどを追加することなく，また外部環境の物理特性によらず，ロボットのあらゆる動作部分と外部環境との接触力を制限でき，ロボットシステムの安全性を向上できる。
請求項９の発明によれば，ロボットのあらゆる動作部分と外部環境との接触が起こった場合にロボットを減速させ，ロボットや外部環境物に過大な力が発生することを防止することができる。
請求項１０の発明によれば，制御仕事量算出に用いる駆動力の値を正確に得られる。
請求項１１の発明によれば，制御仕事量算出に用いる駆動力の値を得る処理負荷を軽減できる。
請求項１２の発明によれば，制御仕事量を正確に算出できる。
【００１９】
請求項１４の発明によれば，外部環境の物理特性によらずロボットのあらゆる動作部分と外部環境との接触を正確に検出でき，またロボットの動作指令を適切に調整できるため，ロボットシステムの安全性を一層高めることができる。
【図面の簡単な説明】
【００２０】
【図１】本発明のロボットの制御装置の構成を示した図
【図２】本発明の第1実施例における動作指令調整部の処理内容を示した流れ図
【図３】弾性エネルギーＵｅｌｉｍ１を実験的に求める方法の例を示した図
【図４】弾性エネルギーＵｅｌｉｍ１を実験的に求める方法の例を示した図
【図５】本発明の第２実施例におけるロボットを示した図
【図６】本発明の第３実施例における動作指令調整部の処理内容を示した流れ図
【図７】従来技術の問題を示した図
【発明を実施するための形態】
【００２１】
本発明の実施の形態を具体的に説明するために，まずロボットの動力学の特徴を概説する。一般に剛体nリンク型ロボットの運動方程式は以下の数式（１）を用いて
【００２２】
【数１】
【００２３】
である。ロボットが作業対象物を把持している場合は上記運動方程式中のパラメータ値が変わるが，基本的な構造は変わらない。したがって以後の説明では，ロボットが作業対象物を把持しているか否かは区別しない。
ある時刻tにおいてロボットが保有する力学的エネルギーE(以下，単にエネルギーと表記)は，運動エネルギーとポテンシャルエネルギーＵ（ｑ）の和として
【００２４】
【数２】
【００２５】
を満足する関節位置ベクトルｑによって変化するスカラー関数である。ロボットが外部環境に対してする仕事率(パワー)である
【００２６】
【数３】
【００２７】
となり，これを時間Ｔにわたって積分すると，該時間中にロボットが外部環境に対してする仕事量ｗｅ（ｔ，Ｔ）は以下の数式（２）を用いて
【００２８】
【数４】
【００２９】
である。数式（２）は，ロボットが外部環境に対してする仕事量は，ロボットが保有する力学的エネルギーの変化量−ΔＥ（ｔ，Ｔ）と摩擦補償を除いた関節駆動制御トルクがした仕事量ｗｃ（ｔ，Ｔ）の和に等しいことを示している。この事実を利用すれば，ロボットと外部環境との接触力が計測できない場合でも，ロボットと外部環境との接触を検出することができる。
【００３０】
以下、本発明の具体的実施例について、図を用いて説明する。
【実施例１】
【００３１】
図１は，本発明の第1実施例におけるロボットの制御装置の構成を示した図である。図において，点線内部１０１が前記ロボットの制御装置，１０２がロボットの制御装置１０１で制御され作業を遂行するロボット，１０３が動作指令記憶部，１０４が制御仕事量算出部，１０５がエネルギー推定部，１０６がエネルギー変化推定値算出部，１０７が動作指令調整部，１０８がサーボ制御部，１０９が速度算出部，１１０が動作指令，１１１が関節速度，１１２が関節トルク，１１３が制御仕事量，１１４がエネルギー推定値，１１５がエネルギー変化推定値，１１６が関節動作指令，１１７が関節トルク指令，１１８が関節位置、１２０が警報部である。ロボット１０２は関節トルク指令１１７にしたがって駆動される。フィードバックされる関節トルク１１２は，関節トルク指令１１７そのものか，関節を駆動するアクチュエータを流れる電流値の計測結果から算出したものである。関節位置１１８はロボット１０２の各関節に取り付けられたロータリーエンコーダによって取得される。本実施例においては，動作指令記憶部１０３，制御仕事量算出部１０４，エネルギー推定部１０５，エネルギー変化推定値算出部１０６，動作指令調整部１０７，サーボ制御部１０８および速度算出部１０９は，図示しない計算機によって構成する。なお，計算機は所定の制御周期δtで動作するディジタル計算機である。また、警報部１２０は前記計算機から制御される図示しないブザーである。
【００３２】
ロボットの制御装置１０１の構成要素について，個別に説明する。
動作指令記憶部１０３はロボットに作業を遂行させるための動作指令を記憶させておく前記計算機の記憶領域に相当し，必要に応じて記憶している動作指令を出力する。本実施例における動作指令は，各制御周期においてロボットの手先が通るべき目標軌跡ベクトルｘｒｅｆ［ｔ］である。
制御仕事量算出部１０４は，以下の数式（３）によって制御仕事量１１３(式ではｗｃ［ｔ，Ｔ］と表記)を計算し出力する。
【００３３】
【数５】
【００３４】
ここでδｔは前記計算機の制御周期である。上記演算は連続時間系における時間積分に相当する。また、τｃは前述のように関節トルクから摩擦補償トルクτｆｃを除いた関節駆動制御トルクである。
【００３５】
エネルギー推定部１０５は、以下の数式（４）に基づいてロボットが現在保有するエネルギーＥ［ｔ］の推定値であるエネルギー推定値１１４(式ではＥ＾［ｔ］と表記)を計算し出力する。
【００３６】
【数６】
【００３７】
ここで(＾)は推定値であることを表す。Ｍ＾やＵ＾に含まれるロボットの物理パラメータには、設計値またはパラメータ同定の結果を用いる。
エネルギー変化推定値算出部１０６は、エネルギー推定値１１４の履歴を時間Ｔ以上にわたって前記計算機のメモリに保持し、以下の数式（５）により時間Tのエネルギー変化推定値１１５(式ではΔＴＥ＾［ｔ］と表記)を計算する。
【００３８】
【数７】
【００３９】
動作指令調整部１０７は、動作指令１１０、制御仕事量１１３およびエネルギー変化推定値１１５を入力とし、ロボットの関節動作指令１１６を出力する処理単位である。本実施例では、関節動作指令１１６は、各制御周期におけるロボットの関節位置指令ｑｃｍｄ［ｔ］である。本実施例における動作指令調整部１０７の処理内容を図２を参照して説明する。図において、２０１が制御仕事量１１３とエネルギー変化推定値１１５との差である外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］を算出する処理、２０２が前記外部出力仕事量推定値を閾値ｗｔｈと比較した結果にもとづいて以後の処理を分岐する処理、２０３は停止指令を出力する処理，２０４は動作指令１１０の時刻をδtだけ進める処理、２０５は動作指令１１０から逆運動学演算によって関節動作指令１１６(すなわちロボットの関節位置指令ｑｃｍｄ［ｔ］を算出する処理である。処理２０１では，以下の数式（６）にもとづいて外部出力仕事量推定値を算出する。
【００４０】
【数８】
【００４１】
また，処理２０２では以下の論理の真偽を調べ，真であれば処理２０３へ，偽であれば処理２０４へと進む。
ｗｅ＾［ｔ，Ｔ］＞ｗｔｈ
ここで閾値ｗｔｈは予め定めた正定数である。その値を決定する方法は後述する。以上に示した一連の処理によって，動作指令調整部１０７は，ロボット１０２が外部環境と接触することによって外部に出力した仕事量の推定値である外部出力仕事量推定値
が閾値ｗｔｈよりも大きい場合は停止指令を出力し，それ以外の場合は指令された動作指令１１０が実現されるように関節動作指令を生成して出力する。
【００４２】
サーボ制御部１０８は，関節動作指令１１６と関節位置１１８を入力とし，関節トルク指令１１７を出力する処理単位である。躯体的には，関節位置１１８が関節動作指令１１６にできるだけ追従するようにフィードバック制御するのであるが，そのための制御ループ構成には多くの公知技術が存在するため割愛する。関節位置１１６を関節動作指令１１６に追従させるような制御ループ構成であれば何でも良く，特に構成を限定する必要はない。なお，サーボ制御部には摩擦補償が含まれることも多い。摩擦補償トルクτｆｃはロボットの関節に存在する摩擦力を打ち消すように設定するのであるが，その算出方法についても公知技術が多く，特に方法は限定しないため割愛する。
【００４３】
速度算出部１０９は，関節位置１１６の時間差分によって関節速度１１１を算出する処理単位である。
以上のような構成によって，ロボット１０２が外部環境と接触することによって外部に出力した仕事量の推定値である外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］が閾値ｗｔｈよりも大きい場合は停止指令が出力されるため，ロボット１０２を安全に停止することができる。それ以外の場合は動作指令１１０を忠実に実行し，作業を遂行することができる。
また、警報部１２０は動作指令調整部が停止指令を発すると同時に警報ブザー音を発し、周囲の人間に異常発生を通知する。警報によって周囲の人間は異常発生を即座に感知でき、素早い対応が可能となる。なお、警報部１２０は本実施例ではブザーとしたが、周囲の人間や周辺機器に異常発生を通知できるものであれば何でもよく、前記計算機に付属のモニタ装置や、異常発生ディジタル信号を発するディジタル出力ポートなども用いることができる。
【００４４】
外部出力仕事量は接触力と接触力によって生じた変位の積でもあり，ロボットと外部環境との接触位置やロボットの姿勢には無関係である。したがって，その推定値である外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］の値によってロボットの動作を調整することにより，ロボットと外部環境との接触位置やロボットの姿勢に影響されずに安全措置を講ずることができるのである。
前記閾値ｗｔｈの決定方法について説明する。まず，ロボットが動作中に接触し得る障害物を抽出する。ここで障害物とは，人間の体やロボット周辺機器などであって，ある程度剛性が低いものとする(障害物の剛性が高い場合の対策については，本発明の第２実施例として後述する)。次いで，前記ロボットが障害物に接触したときに発生する接触力の制限値ｆｅｌｉｍを定める。接触力の制限値ｆｅｌｉｍは，障害物あるいはロボットにかかる負荷の限界値よりも十分小さな値とする。接触力の制限値が定まったら，障害物に力ｆｅｌｉｍを印加したときに障害物に蓄えられる弾性エネルギーＵｅｌｉｍ１を実験的あるいは理論的に算出する。
【００４５】
前記弾性エネルギーＵｅｌｉｍ１を実験的に求める方法の例を図３および図４を用いて説明する。図３において３０１は押し付け用プローブ，３０２は力センサ，３０３は障害物試料，矢印３０４は押し付け用プローブ３０１の押し付け方向，矢印３０５は押し付けによって押し付け用プローブ３０１および力センサ３０２が受ける接触力である。また，図３の左側は押し付け前，右側は押し付け後の様子を示している。
【００４６】
押し付け用プローブ３０１は，障害物試料３０３に力を印加するためのプローブであって，ロボットと同程度の大きさの立体を用い，先端に接触力測定用の力センサ３０２を備える。押し付け用プローブ３０１を押し付け方向３０４に沿って移動させると，やがて障害物試料３０３に接触して接触力３０５を発生する。更に移動を継続すると，障害物試料３０３は変形して弾性エネルギーを内部に蓄えるとともに，接触力３０５が増大して力ｆｅｌｉｍを上回る。このときの押し付け用プローブ３０１の変位量と接触力をグラフ化した例が図４である。図４において，４０１は押し付け用プローブ３０１の押し付け方向３０４に沿った変位量に対応する軸，４０２は接触力３０５に対応する軸，４０３は接触力曲線，４０４は力ｆｅｌｉｍに対応する直線，４０５は接触力が発生したとき(非零の値となったとき)の変位量に対応する直線，４０６は接触力曲線４０３および直線４０４および直線４０５によって囲まれる領域の面積である。弾性エネルギーＵｅｌｉｍ１は，面積４０６を算出することで求められる。
【００４７】
前記弾性エネルギーＵｅｌｉｍ１を理論的に求めるには，障害物の弾性係数Ｅ０を予め推定または調べておき，以下の数式（７）を用いればよい。
【００４８】
【数９】
【００４９】
ここでｌ０は障害物の平均的な厚み，Ａ０は障害物とロボットの平均的な接触面積，であって，抽出された障害物とロボットが接触した場合を想定して設定する量である。
前記弾性エネルギーＵｅｌｉｍ１が求まったら，これに１よりも十分小さい正の安全係数であるαｓａｆｅを乗じたものを閾値ｗｔｈとする。
即ち、ｗｔｈ＝αｓａｆｅＵｅｌｉｍ１
【００５０】
以上のように閾値ｗｔｈを決定することで，ロボットが障害物に接触したときに接触力がｆｅｌｉｍを上回る前に接触を検出し，ロボット動作を安全に停止できるのである。
本実施例は，ロボットが動作中に接触し得る外部環境がある程度の弾性を有する場合には有効であるが，外部環境の剛性が高い場合は接触と同時に接触力が急激に増大するため，停止指令が間に合わず接触力を制限できない可能性がある。そのような場合であっても本発明を適用可能であることを，以下に示す。
【実施例２】
【００５１】
本発明の第２実施例は，ロボットが接触し得る外部環境の剛性が高い場合でも接触力の大きさを安全な範囲内に制限するものである。本実施例の基本構成は図１と同一であるが，外部環境と接触する可能性があるロボット部位の表面を弾性柔軟材料で覆う。
図５は２自由度ロボットアームを弾性柔軟材料で覆った例である。図において５０１が２自由度ロボットアーム，斜線部分５０２が弾性柔軟材料である。弾性柔軟材料５０２には，弾性係数Ｅｍが既知のものを用い，弾性柔軟材料５０２の厚さｌｍは，ロボットが動作中に停止指令を受けたときロボットが停止するまでに動く距離である制動距離よりも十分大きな値とする。
【００５２】
前記閾値ｗｔｈを決定する方法を説明する。まず，弾性柔軟材料５０２に力ｆｅｌｉｍを印加したときに障害物に蓄えられる弾性エネルギーＵｅｌｉｍ２を以下の数式（８）により求める。
【００５３】
【数１０】
【００５４】
ここでＡｍはロボットと障害物との平均的な接触面積であって，ロボットが動作中に接触し得る環境中の障害物を抽出した上で，ロボットと抽出された障害物とが接触した場合を想定して設定する量である。前記弾性エネルギーＵｅｌｉｍ２が求まったら，これに1よりも十分小さい正の安全係数αｓａｆｅを乗じたものを閾値ｗｔｈとする。
即ち、ｗｔｈ＝αｓａｆｅＵｅｌｉｍ１
以上のような構成とすることで，ロボットが接触し得る障害物が剛体であっても，弾性柔軟材料５０２の変形によって外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］が増大するため，大きな接触力が発生する前にこれを検出し，停止させることができる。
【実施例３】
【００５５】
上記実施例１および２では，動作指令調整部１０７において外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］が閾値ｗｔｈを超えたらロボットを即停止させるようにしたが，図６に示した処理によれば，停止指令を発生する前にある程度動作指令の速度を落とすことによって，より安全性を高めることが可能である。図において６０１は閾値ｗｔｈ２と外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］を比較した結果に応じて以後の処理を分岐する処理，６０２は動作指令１１０の時刻をδt’だけ進める処理である。ここで閾値ｗｔｈ２は閾値ｗｔｈよりも小さな正の数，δt’はδtよりも小さな正の数である。
【００５６】
図６の処理は，ロボットと外部環境との接触が発生していなければ所望の動作指令を実行し，ロボットと外部環境との軽微な接触が発生した可能性がある場合は動作指令の速度を遅くし，接触によって比較的大きな接触力が発生した可能性がある場合はロボットを停止させるものである。動作指令の速度を遅くすることでロボット停止までの制動距離を減らすことができるため，実施例１および２より安全性を向上させることができる。
【産業上の利用可能性】
【００５７】
本発明は動作中に人間を含む外部環境と接触する恐れがある作業ロボット全般に適用可能である。
【符号の説明】
【００５８】
１００ 指令生成装置
１０１ サーボモータの制御装置
２０１ 外部出力仕事量推定値を算出する処理
２０２ 分岐処理
２０３ 停止指令出力処理
２０４ 動作指令１１０の時刻をδtだけ進める処理
２０５ 動作指令１１０から関節動作指令１１６を算出する処理
３０１ 押し付け用プローブ
３０２ 力センサ
３０３ 障害物試料
３０４ 押し付け用プローブ３０１の押し付け方向
３０５ 接触力
４０１ 押し付け用プローブ３０１の押し付け方向３０４に沿った変位量に対応する軸
４０２ 接触力３０５に対応する軸
４０３ 接触力曲線
４０４ 力に対応する直線
４０５ 接触力が発生したとき(非零の値となったとき)の変位量に対応する直線
４０６ 接触力曲線４０３および直線４０４および直線４０５によって囲まれる領域の面積
５０１ ２自由度ロボットアーム
５０２ 弾性柔軟材料
６０１ 分岐処理
６０２ 動作指令１１０の時刻をδt’だけ進める処理
7００ ロボットアーム
７０１ ロボットアーム７００の第1関節
７０２ ロボットアーム７００の第２関節
７０３ 第1の障害物
７０４ 第２の障害物
７０５ 第１関節７０１の回転の向き
７０６ 第２関節７０２の回転の向き
７０７ 第１の障害物７０３からロボットアーム７００が受ける接触力
７０８ 第２の障害物７０４からロボットアーム７００が受ける接触力
【技術分野】
【０００１】
本発明は，ロボットの駆動力を制御して作業を遂行するためのロボットの制御装置およびこれを備えたロボットに関する。
【背景技術】
【０００２】
人間と共存して作業を遂行するロボットの制御装置において、安全のためにはロボットが(人間を含む)外部環境と予期せずして接触した場合、これを確実に検出することが必要である。更には、ロボットが(人間を含む)外部環境に対してする仕事量や手先力を適切な値に制限する必要がある。従来の安全対策としては手先に設けた力センサで手先力を監視し、所定の閾値を超えたらロボットを停止させる技術や関節軸毎にトルク制限を設ける技術などがあった。
あるいは、ロボットが外部環境に接触したことを検出して警告発生や停止などの措置をとることで安全を確保する技術も提案されている。
【０００３】
例えば、特許文献1の技術では、ロボットの過負荷状態を監視し障害の発生を防止するため、所定時間Ｔ内におけるモータの実効トルクの値を監視し、監視値が規定値を越えた場合に警告を発する，関節最高速度を遅く設定する，といった措置をとっていた。
この例では、演算によって実効トルクを求め、結果が閾値よりも大きくなった場合に過負荷状態として警告を発生していた。あるいは、実効トルクが閾値を大きく超過した場合には関節最高速度を減らし、超過量が少ない場合は逆に関節最高速度を増やすことで、過負荷を防止していた。本方法によれば、ロボットが外部環境と接触すると関節負荷が増大して警告発生や減速などの措置がとられるため、安全性が高められる。
【０００４】
特許文献２の技術では、計算量を削減し高精度に衝突判定するため、動作指令からロボットの運動方程式を用いて算出した必要駆動トルク指令要素と、各関節のフィードバック値と、前記動作指令と、から必要駆動トルクを演算し、必要駆動トルクと実際のモータ駆動トルクとの差を、別途演算により求めた閾値と比較するようにしていた。ここで必要駆動トルク指令要素は、位置指令、速度指令、加速度指令をロボットの運動方程式に代入して計算するものである。また、必要駆動トルクは、実際にフィードバックされたロボット動作を実現するために必要な駆動トルクであって、本来ならば位置、速度、加速度のフィードバック値をロボットの運動方程式に代入して求めるものであるが、特許文献２では計算量節約のために近似式が用いられていた。
【０００５】
特許文献２の実施例であるロボット制御装置は、位置、速度、加速度それぞれの指令を生成する指令値生成手段、必要駆動トルク指令要素演算手段、必要駆動トルク演算手段、衝突判別のための閾値を演算する閾値演算手段、閾値とモータ駆動電流に基づいて衝突を判別する衝突判別手段、ロボットの各軸を駆動するモータを制御するモータ制御手段によって構成されていた。ここでは閾値演算手段は、関節毎に予め記憶した閾値をそれぞれ出力するものである。衝突判別手段では、まず、モータ制御手段から入力されるモータ電流から各関節のモータ駆動トルクを演算する。次に、必要駆動トルクと実際のモータ駆動トルクとの差を関節毎に計算する。いずれかの関節における必要駆動トルクとモータ駆動トルクの差がしきい値演算手段から出力された上側しきい値以上の場合、もしくは下側しきい値以下となった場合にロボットが衝突したと判別し、指令値生成手段に停止命令を送信する。本方法によれば、ロボットと外部環境との接触をある程度精度良く検出できるので、接触検出時に停止するなどの措置によって安全性を向上することができる。
【０００６】
特許文献３では，ロボットの動特性を模擬したモデルとロボット実機の位置および姿勢を比較することで衝突を判定していた。
即ち、特許文献３の技術は多自由度ロボットアーム(実機)と、多自由度ロボットアームのモデルと位置・姿勢を監視する１つのリンクと周辺機器とから構成されており、複数のリンク位置および姿勢について実機とモデルを比較することで，図のようにリンクの途中に周辺機器が接触した場合も検出できる構成となっていた。特許文献３の技術によれば手先以外の接触もある程度精度良く検出できるため、ロボットの作業領域全体で安全性向上を図ることができる。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第２９９２９２０号公報(第４頁，図２)
【特許文献２】特許第３８７８０５４号公報(第４１頁，図１)
【特許文献３】特開２００６−１１６６３５号公報(第１３頁，図８)
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ところが、力センサで手先力を監視する従来技術では、力センサを追加設置する必要があり，コスト高を招く。また，手先以外の接触によって生じる力を検出できないという課題もある。
また、関節軸毎にトルク制限を設ける従来技術では、図７のように障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に大きな力が発生するため、各関節でトルクの増大を検出する前に環境やロボット自身に過大な負荷がかかる虞があった。
即ち、図において、７００はロボットアーム、７０１はロボットアーム７００の第1関節、７０２はロボットアーム７００の第２関節、７０３は第1の障害物、７０４は第２の障害物、矢印７０５は第１関節７０１の回転の向き、矢印７０６は第２関節７０２の回転の向き、矢印７０７は第１の障害物７０３からロボットアーム７００が受ける接触力、矢印７０８は第２の障害物７０４からロボットアーム７００が受ける接触力である。ロボットアーム７００は直線に近い姿勢をとっているため、第1の障害物７０３から受ける接触力７０７が第１関節７０１および第２関節７０２のトルクに与える影響は軽微である。関節７０１および７０２のトルクが制限値を上回らないまま各関節が矢印７０５および７０６の向きに運動を継続すれば、接触力７０７は非常に大きくなってしまう。
【０００９】
また、ロボットアーム７００は第２の障害物７０４と第１関節７０１に近い位置で接触しているため、第２の障害物７０４から受ける接触力７０８が第２関節７０２のトルクに与える影響はなく、第１関節７０１への影響も軽微である。
第1関節７０１のトルクが制限値を上回らないまま第１関節７０１が矢印７０５の向きに運動を継続すれば、接触力７０８が非常に大きくなる虞がある。このように、各関節のトルクを制限するだけでは、ロボットのあらゆる動作部分と外部環境との接触に対応するには限界がある。また、画一的なトルク制限ではロボットの姿勢変化に伴う駆動トルクの大きな変動に対応できず、ロボットの動作性能が制限されてしまう場合もあった。
【００１０】
特許文献１の技術の場合、ロボットの急な加減速によって瞬間的に駆動トルクが増大すると、過負荷による破損の危険性が極めて少ないにもかかわらず警告が誤動作してしまうという問題があった。
これを防ぐためには、実効トルクを求める所定の時間Ｔを十分大きく設定する、警告発生の閾値を十分大きく設定する、といった方法が考えられるが、これらを適用すると過負荷検出の感度が低下してしまう。
また、障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に過大な力が発生することを防止できない虞がある。
【００１１】
特許文献２の場合、関節トルクを監視して衝突有無を判定する限り、障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に大きな力が発生する問題は解決できない。また、必要駆動トルクの算出に用いられる関節加速度のフィードバック値は、実際には関節位置のフィードバック値を二階差分して求める必要があるため、ノイズの影響が増大して衝突判定精度が落ちるという問題もあった。
【００１２】
また、トルクではなくロボットの複数のリンク位置および姿勢を監視する場合には、障害物と接触する姿勢や障害物が接触する位置によっては小さいトルクでも接触点に過大な力が発生することを防止できない虞がある。
更に、上記特許文献を含めた多くの従来技術では、ロボットと障害物との接触や過負荷を検出できても、力センサを用いないでロボットと障害物との接触力を制限することはできていなかった。
本発明はこのような課題に鑑みてなされたものであり、力センサを用いることなく、ロボットと障害物との接触を高精度に検出可能で安全性を向上させるロボットの制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
上記目的を達成するため，本発明は次のような手段をとる。
請求項１にかかるロボットの制御装置は、ロボットの駆動力を制御するロボットの制御装置であって、所望の動作軌跡と速度を出力する動作指令記憶部と、前記駆動力が所定時間内に前記ロボットに対してした仕事量である制御仕事量を算出する制御仕事量算出部と、前記ロボットのエネルギー推定値を算出するエネルギー推定部と、前記ロボットの前記所定時間内のエネルギー変化の推定値を算出するエネルギー変化推定値算出部と、前記制御仕事量と前記エネルギー変化推定値とに基づいてロボットへの動作指令速度を調整または停止指令を発生する動作指令調整部と、を備えたことを特徴としている。
【００１４】
また、前記制御仕事量算出部は、フィードバックした前記ロボットの関節速度と前記駆動力の積である制御仕事率を所定時間にわたって積分することで前記制御仕事量を算出することが好ましい（請求項２）。
また、前記エネルギー推定部は、フィードバックした前記ロボットの関節速度および位置と、内蔵する前記ロボットの動力学モデルと、にもとづいて前記エネルギー推定値を算出することが好ましい（請求項３）。
また、前記エネルギー変化推定値算出部は、所定時間にわたって前記エネルギー推定値を時刻と対応付けて保存するエネルギー推定値履歴保存部を備え、所定時間だけ前のエネルギー推定値を現在のエネルギー推定値から減じたものを前記エネルギー変化推定値として出力することが好ましい（請求項４）。
また、前記動作司令調整部は、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が所定の第一閾値を超えた場合に停止指令を発生することが好ましい（請求項５）。
また、環境との接触を通知する警報発生部を更に備え、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が前記第一閾値を超えた場合に警報を発生することが望ましい(請求項６)。
【００１５】
また、前記第一閾値は、前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボットが接触し得る環境内物体に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値であることが好ましい（請求項７）。
また、前記第一閾値は、前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボット表面に装着した弾性柔軟材料に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値であることが好ましい（請求項８）。
また、前記動作指令調整部は、前記外部出力仕事量推定値が所定の第二閾値を超えた場合に前記動作指令記憶部が出力する動作指令の速度を遅らせたものを出力することが好ましい（請求項９）。
また、前記ロボットはアクチュエータとしてサーボモータを有し、前記制御仕事量算出部における前記駆動力は前記サーボモータを流れる電流値から算出することが好ましい（請求項１０）。
また、前記制御仕事量算出部における駆動力は前記駆動力指令をそのまま用いることが好ましい（請求項１１）。
また、前記駆動力は摩擦補償分を除いたものであることが好ましい（請求項１２）。
【００１６】
また、請求項１３にかかるロボットは、アームと、前記アームに内蔵され、請求項１〜１１のいずれか１項に記載のロボットの制御装置によって制御されるアーム駆動用のサーボモータと、ロボットの制御装置からの前記動作指令速度または前記停止指令に基づいてロボットの関節を駆動制御するサーボ制御部と、を備えたことを特徴としている。
また、前記アームの表面の一部または全体が弾性柔軟材料で覆われていることが好ましい（請求項１４）。
【発明の効果】
【００１７】
請求項１及び請求項１３の発明によれば，力センサなどを追加することなくロボットのあらゆる動作部分と外部環境との接触を検出でき，またロボットの動作指令を適切に調整できるため，ロボットシステムの安全性を高めることができる。
請求項２の発明によれば，極めて簡単な演算によってロボットの駆動力がロボットおよび外部環境に対してした仕事量を求められる。また，数値誤差の蓄積による誤検出を防止できる。
請求項３の発明によれば，ロボットが保持する力学的エネルギーを正確に推定できる。
請求項４の発明によれば，ロボットが保持する力学的エネルギーの変化を正確に検出できる。
請求項５の発明によれば，ロボットのあらゆる動作部分と外部環境との接触が起こった場合にロボットを停止させ，ロボットや外部環境物に過大な力が発生することを防止することができる。
請求項６の発明によれば、ロボットのあらゆる動作部分と外部環境との接触が起こった場合に周囲の作業者や周辺機器に警報で通知することで、迅速な対応を促すことができる。
【００１８】
請求項７の発明によれば，力センサなどを追加することなく，ロボットのあらゆる動作部分と外部環境との接触力を制限でき，ロボットシステムの安全性を向上できる。
請求項８の発明によれば，力センサなどを追加することなく，また外部環境の物理特性によらず，ロボットのあらゆる動作部分と外部環境との接触力を制限でき，ロボットシステムの安全性を向上できる。
請求項９の発明によれば，ロボットのあらゆる動作部分と外部環境との接触が起こった場合にロボットを減速させ，ロボットや外部環境物に過大な力が発生することを防止することができる。
請求項１０の発明によれば，制御仕事量算出に用いる駆動力の値を正確に得られる。
請求項１１の発明によれば，制御仕事量算出に用いる駆動力の値を得る処理負荷を軽減できる。
請求項１２の発明によれば，制御仕事量を正確に算出できる。
【００１９】
請求項１４の発明によれば，外部環境の物理特性によらずロボットのあらゆる動作部分と外部環境との接触を正確に検出でき，またロボットの動作指令を適切に調整できるため，ロボットシステムの安全性を一層高めることができる。
【図面の簡単な説明】
【００２０】
【図１】本発明のロボットの制御装置の構成を示した図
【図２】本発明の第1実施例における動作指令調整部の処理内容を示した流れ図
【図３】弾性エネルギーＵｅｌｉｍ１を実験的に求める方法の例を示した図
【図４】弾性エネルギーＵｅｌｉｍ１を実験的に求める方法の例を示した図
【図５】本発明の第２実施例におけるロボットを示した図
【図６】本発明の第３実施例における動作指令調整部の処理内容を示した流れ図
【図７】従来技術の問題を示した図
【発明を実施するための形態】
【００２１】
本発明の実施の形態を具体的に説明するために，まずロボットの動力学の特徴を概説する。一般に剛体nリンク型ロボットの運動方程式は以下の数式（１）を用いて
【００２２】
【数１】
【００２３】
である。ロボットが作業対象物を把持している場合は上記運動方程式中のパラメータ値が変わるが，基本的な構造は変わらない。したがって以後の説明では，ロボットが作業対象物を把持しているか否かは区別しない。
ある時刻tにおいてロボットが保有する力学的エネルギーE(以下，単にエネルギーと表記)は，運動エネルギーとポテンシャルエネルギーＵ（ｑ）の和として
【００２４】
【数２】
【００２５】
を満足する関節位置ベクトルｑによって変化するスカラー関数である。ロボットが外部環境に対してする仕事率(パワー)である
【００２６】
【数３】
【００２７】
となり，これを時間Ｔにわたって積分すると，該時間中にロボットが外部環境に対してする仕事量ｗｅ（ｔ，Ｔ）は以下の数式（２）を用いて
【００２８】
【数４】
【００２９】
である。数式（２）は，ロボットが外部環境に対してする仕事量は，ロボットが保有する力学的エネルギーの変化量−ΔＥ（ｔ，Ｔ）と摩擦補償を除いた関節駆動制御トルクがした仕事量ｗｃ（ｔ，Ｔ）の和に等しいことを示している。この事実を利用すれば，ロボットと外部環境との接触力が計測できない場合でも，ロボットと外部環境との接触を検出することができる。
【００３０】
以下、本発明の具体的実施例について、図を用いて説明する。
【実施例１】
【００３１】
図１は，本発明の第1実施例におけるロボットの制御装置の構成を示した図である。図において，点線内部１０１が前記ロボットの制御装置，１０２がロボットの制御装置１０１で制御され作業を遂行するロボット，１０３が動作指令記憶部，１０４が制御仕事量算出部，１０５がエネルギー推定部，１０６がエネルギー変化推定値算出部，１０７が動作指令調整部，１０８がサーボ制御部，１０９が速度算出部，１１０が動作指令，１１１が関節速度，１１２が関節トルク，１１３が制御仕事量，１１４がエネルギー推定値，１１５がエネルギー変化推定値，１１６が関節動作指令，１１７が関節トルク指令，１１８が関節位置、１２０が警報部である。ロボット１０２は関節トルク指令１１７にしたがって駆動される。フィードバックされる関節トルク１１２は，関節トルク指令１１７そのものか，関節を駆動するアクチュエータを流れる電流値の計測結果から算出したものである。関節位置１１８はロボット１０２の各関節に取り付けられたロータリーエンコーダによって取得される。本実施例においては，動作指令記憶部１０３，制御仕事量算出部１０４，エネルギー推定部１０５，エネルギー変化推定値算出部１０６，動作指令調整部１０７，サーボ制御部１０８および速度算出部１０９は，図示しない計算機によって構成する。なお，計算機は所定の制御周期δtで動作するディジタル計算機である。また、警報部１２０は前記計算機から制御される図示しないブザーである。
【００３２】
ロボットの制御装置１０１の構成要素について，個別に説明する。
動作指令記憶部１０３はロボットに作業を遂行させるための動作指令を記憶させておく前記計算機の記憶領域に相当し，必要に応じて記憶している動作指令を出力する。本実施例における動作指令は，各制御周期においてロボットの手先が通るべき目標軌跡ベクトルｘｒｅｆ［ｔ］である。
制御仕事量算出部１０４は，以下の数式（３）によって制御仕事量１１３(式ではｗｃ［ｔ，Ｔ］と表記)を計算し出力する。
【００３３】
【数５】
【００３４】
ここでδｔは前記計算機の制御周期である。上記演算は連続時間系における時間積分に相当する。また、τｃは前述のように関節トルクから摩擦補償トルクτｆｃを除いた関節駆動制御トルクである。
【００３５】
エネルギー推定部１０５は、以下の数式（４）に基づいてロボットが現在保有するエネルギーＥ［ｔ］の推定値であるエネルギー推定値１１４(式ではＥ＾［ｔ］と表記)を計算し出力する。
【００３６】
【数６】
【００３７】
ここで(＾)は推定値であることを表す。Ｍ＾やＵ＾に含まれるロボットの物理パラメータには、設計値またはパラメータ同定の結果を用いる。
エネルギー変化推定値算出部１０６は、エネルギー推定値１１４の履歴を時間Ｔ以上にわたって前記計算機のメモリに保持し、以下の数式（５）により時間Tのエネルギー変化推定値１１５(式ではΔＴＥ＾［ｔ］と表記)を計算する。
【００３８】
【数７】
【００３９】
動作指令調整部１０７は、動作指令１１０、制御仕事量１１３およびエネルギー変化推定値１１５を入力とし、ロボットの関節動作指令１１６を出力する処理単位である。本実施例では、関節動作指令１１６は、各制御周期におけるロボットの関節位置指令ｑｃｍｄ［ｔ］である。本実施例における動作指令調整部１０７の処理内容を図２を参照して説明する。図において、２０１が制御仕事量１１３とエネルギー変化推定値１１５との差である外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］を算出する処理、２０２が前記外部出力仕事量推定値を閾値ｗｔｈと比較した結果にもとづいて以後の処理を分岐する処理、２０３は停止指令を出力する処理，２０４は動作指令１１０の時刻をδtだけ進める処理、２０５は動作指令１１０から逆運動学演算によって関節動作指令１１６(すなわちロボットの関節位置指令ｑｃｍｄ［ｔ］を算出する処理である。処理２０１では，以下の数式（６）にもとづいて外部出力仕事量推定値を算出する。
【００４０】
【数８】
【００４１】
また，処理２０２では以下の論理の真偽を調べ，真であれば処理２０３へ，偽であれば処理２０４へと進む。
ｗｅ＾［ｔ，Ｔ］＞ｗｔｈ
ここで閾値ｗｔｈは予め定めた正定数である。その値を決定する方法は後述する。以上に示した一連の処理によって，動作指令調整部１０７は，ロボット１０２が外部環境と接触することによって外部に出力した仕事量の推定値である外部出力仕事量推定値
が閾値ｗｔｈよりも大きい場合は停止指令を出力し，それ以外の場合は指令された動作指令１１０が実現されるように関節動作指令を生成して出力する。
【００４２】
サーボ制御部１０８は，関節動作指令１１６と関節位置１１８を入力とし，関節トルク指令１１７を出力する処理単位である。躯体的には，関節位置１１８が関節動作指令１１６にできるだけ追従するようにフィードバック制御するのであるが，そのための制御ループ構成には多くの公知技術が存在するため割愛する。関節位置１１６を関節動作指令１１６に追従させるような制御ループ構成であれば何でも良く，特に構成を限定する必要はない。なお，サーボ制御部には摩擦補償が含まれることも多い。摩擦補償トルクτｆｃはロボットの関節に存在する摩擦力を打ち消すように設定するのであるが，その算出方法についても公知技術が多く，特に方法は限定しないため割愛する。
【００４３】
速度算出部１０９は，関節位置１１６の時間差分によって関節速度１１１を算出する処理単位である。
以上のような構成によって，ロボット１０２が外部環境と接触することによって外部に出力した仕事量の推定値である外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］が閾値ｗｔｈよりも大きい場合は停止指令が出力されるため，ロボット１０２を安全に停止することができる。それ以外の場合は動作指令１１０を忠実に実行し，作業を遂行することができる。
また、警報部１２０は動作指令調整部が停止指令を発すると同時に警報ブザー音を発し、周囲の人間に異常発生を通知する。警報によって周囲の人間は異常発生を即座に感知でき、素早い対応が可能となる。なお、警報部１２０は本実施例ではブザーとしたが、周囲の人間や周辺機器に異常発生を通知できるものであれば何でもよく、前記計算機に付属のモニタ装置や、異常発生ディジタル信号を発するディジタル出力ポートなども用いることができる。
【００４４】
外部出力仕事量は接触力と接触力によって生じた変位の積でもあり，ロボットと外部環境との接触位置やロボットの姿勢には無関係である。したがって，その推定値である外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］の値によってロボットの動作を調整することにより，ロボットと外部環境との接触位置やロボットの姿勢に影響されずに安全措置を講ずることができるのである。
前記閾値ｗｔｈの決定方法について説明する。まず，ロボットが動作中に接触し得る障害物を抽出する。ここで障害物とは，人間の体やロボット周辺機器などであって，ある程度剛性が低いものとする(障害物の剛性が高い場合の対策については，本発明の第２実施例として後述する)。次いで，前記ロボットが障害物に接触したときに発生する接触力の制限値ｆｅｌｉｍを定める。接触力の制限値ｆｅｌｉｍは，障害物あるいはロボットにかかる負荷の限界値よりも十分小さな値とする。接触力の制限値が定まったら，障害物に力ｆｅｌｉｍを印加したときに障害物に蓄えられる弾性エネルギーＵｅｌｉｍ１を実験的あるいは理論的に算出する。
【００４５】
前記弾性エネルギーＵｅｌｉｍ１を実験的に求める方法の例を図３および図４を用いて説明する。図３において３０１は押し付け用プローブ，３０２は力センサ，３０３は障害物試料，矢印３０４は押し付け用プローブ３０１の押し付け方向，矢印３０５は押し付けによって押し付け用プローブ３０１および力センサ３０２が受ける接触力である。また，図３の左側は押し付け前，右側は押し付け後の様子を示している。
【００４６】
押し付け用プローブ３０１は，障害物試料３０３に力を印加するためのプローブであって，ロボットと同程度の大きさの立体を用い，先端に接触力測定用の力センサ３０２を備える。押し付け用プローブ３０１を押し付け方向３０４に沿って移動させると，やがて障害物試料３０３に接触して接触力３０５を発生する。更に移動を継続すると，障害物試料３０３は変形して弾性エネルギーを内部に蓄えるとともに，接触力３０５が増大して力ｆｅｌｉｍを上回る。このときの押し付け用プローブ３０１の変位量と接触力をグラフ化した例が図４である。図４において，４０１は押し付け用プローブ３０１の押し付け方向３０４に沿った変位量に対応する軸，４０２は接触力３０５に対応する軸，４０３は接触力曲線，４０４は力ｆｅｌｉｍに対応する直線，４０５は接触力が発生したとき(非零の値となったとき)の変位量に対応する直線，４０６は接触力曲線４０３および直線４０４および直線４０５によって囲まれる領域の面積である。弾性エネルギーＵｅｌｉｍ１は，面積４０６を算出することで求められる。
【００４７】
前記弾性エネルギーＵｅｌｉｍ１を理論的に求めるには，障害物の弾性係数Ｅ０を予め推定または調べておき，以下の数式（７）を用いればよい。
【００４８】
【数９】
【００４９】
ここでｌ０は障害物の平均的な厚み，Ａ０は障害物とロボットの平均的な接触面積，であって，抽出された障害物とロボットが接触した場合を想定して設定する量である。
前記弾性エネルギーＵｅｌｉｍ１が求まったら，これに１よりも十分小さい正の安全係数であるαｓａｆｅを乗じたものを閾値ｗｔｈとする。
即ち、ｗｔｈ＝αｓａｆｅＵｅｌｉｍ１
【００５０】
以上のように閾値ｗｔｈを決定することで，ロボットが障害物に接触したときに接触力がｆｅｌｉｍを上回る前に接触を検出し，ロボット動作を安全に停止できるのである。
本実施例は，ロボットが動作中に接触し得る外部環境がある程度の弾性を有する場合には有効であるが，外部環境の剛性が高い場合は接触と同時に接触力が急激に増大するため，停止指令が間に合わず接触力を制限できない可能性がある。そのような場合であっても本発明を適用可能であることを，以下に示す。
【実施例２】
【００５１】
本発明の第２実施例は，ロボットが接触し得る外部環境の剛性が高い場合でも接触力の大きさを安全な範囲内に制限するものである。本実施例の基本構成は図１と同一であるが，外部環境と接触する可能性があるロボット部位の表面を弾性柔軟材料で覆う。
図５は２自由度ロボットアームを弾性柔軟材料で覆った例である。図において５０１が２自由度ロボットアーム，斜線部分５０２が弾性柔軟材料である。弾性柔軟材料５０２には，弾性係数Ｅｍが既知のものを用い，弾性柔軟材料５０２の厚さｌｍは，ロボットが動作中に停止指令を受けたときロボットが停止するまでに動く距離である制動距離よりも十分大きな値とする。
【００５２】
前記閾値ｗｔｈを決定する方法を説明する。まず，弾性柔軟材料５０２に力ｆｅｌｉｍを印加したときに障害物に蓄えられる弾性エネルギーＵｅｌｉｍ２を以下の数式（８）により求める。
【００５３】
【数１０】
【００５４】
ここでＡｍはロボットと障害物との平均的な接触面積であって，ロボットが動作中に接触し得る環境中の障害物を抽出した上で，ロボットと抽出された障害物とが接触した場合を想定して設定する量である。前記弾性エネルギーＵｅｌｉｍ２が求まったら，これに1よりも十分小さい正の安全係数αｓａｆｅを乗じたものを閾値ｗｔｈとする。
即ち、ｗｔｈ＝αｓａｆｅＵｅｌｉｍ１
以上のような構成とすることで，ロボットが接触し得る障害物が剛体であっても，弾性柔軟材料５０２の変形によって外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］が増大するため，大きな接触力が発生する前にこれを検出し，停止させることができる。
【実施例３】
【００５５】
上記実施例１および２では，動作指令調整部１０７において外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］が閾値ｗｔｈを超えたらロボットを即停止させるようにしたが，図６に示した処理によれば，停止指令を発生する前にある程度動作指令の速度を落とすことによって，より安全性を高めることが可能である。図において６０１は閾値ｗｔｈ２と外部出力仕事量推定値ｗｅ＾［ｔ，Ｔ］を比較した結果に応じて以後の処理を分岐する処理，６０２は動作指令１１０の時刻をδt’だけ進める処理である。ここで閾値ｗｔｈ２は閾値ｗｔｈよりも小さな正の数，δt’はδtよりも小さな正の数である。
【００５６】
図６の処理は，ロボットと外部環境との接触が発生していなければ所望の動作指令を実行し，ロボットと外部環境との軽微な接触が発生した可能性がある場合は動作指令の速度を遅くし，接触によって比較的大きな接触力が発生した可能性がある場合はロボットを停止させるものである。動作指令の速度を遅くすることでロボット停止までの制動距離を減らすことができるため，実施例１および２より安全性を向上させることができる。
【産業上の利用可能性】
【００５７】
本発明は動作中に人間を含む外部環境と接触する恐れがある作業ロボット全般に適用可能である。
【符号の説明】
【００５８】
１００ 指令生成装置
１０１ サーボモータの制御装置
２０１ 外部出力仕事量推定値を算出する処理
２０２ 分岐処理
２０３ 停止指令出力処理
２０４ 動作指令１１０の時刻をδtだけ進める処理
２０５ 動作指令１１０から関節動作指令１１６を算出する処理
３０１ 押し付け用プローブ
３０２ 力センサ
３０３ 障害物試料
３０４ 押し付け用プローブ３０１の押し付け方向
３０５ 接触力
４０１ 押し付け用プローブ３０１の押し付け方向３０４に沿った変位量に対応する軸
４０２ 接触力３０５に対応する軸
４０３ 接触力曲線
４０４ 力に対応する直線
４０５ 接触力が発生したとき(非零の値となったとき)の変位量に対応する直線
４０６ 接触力曲線４０３および直線４０４および直線４０５によって囲まれる領域の面積
５０１ ２自由度ロボットアーム
５０２ 弾性柔軟材料
６０１ 分岐処理
６０２ 動作指令１１０の時刻をδt’だけ進める処理
7００ ロボットアーム
７０１ ロボットアーム７００の第1関節
７０２ ロボットアーム７００の第２関節
７０３ 第1の障害物
７０４ 第２の障害物
７０５ 第１関節７０１の回転の向き
７０６ 第２関節７０２の回転の向き
７０７ 第１の障害物７０３からロボットアーム７００が受ける接触力
７０８ 第２の障害物７０４からロボットアーム７００が受ける接触力
【特許請求の範囲】
【請求項１】
ロボットの駆動力を制御するロボットの制御装置であって、
所望の動作軌跡と速度を出力する動作指令記憶部と、
前記駆動力が所定時間内に前記ロボットに対してした仕事量である制御仕事量を算出する制御仕事量算出部と、
前記ロボットのエネルギー推定値を算出するエネルギー推定部と、
前記ロボットの前記所定時間内のエネルギー変化の推定値を算出するエネルギー変化推定値算出部と、
前記制御仕事量と前記エネルギー変化推定値とに基づいてロボットへの動作指令速度を調整または停止指令を発生する動作指令調整部と、
を備えたことを特徴とするロボットの制御装置。
【請求項２】
前記制御仕事量算出部は、フィードバックした前記ロボットの関節速度と前記駆動力の積である制御仕事率を所定時間にわたって積分することで前記制御仕事量を算出する
ことを特徴とする、請求項１記載のロボットの制御装置。
【請求項３】
前記エネルギー推定部は、フィードバックした前記ロボットの関節速度および位置と、内蔵する前記ロボットの動力学モデルと、にもとづいて前記エネルギー推定値を算出することを特徴とする、請求項１又は２記載のロボットの制御装置。
【請求項４】
前記エネルギー変化推定値算出部は、所定時間にわたって前記エネルギー推定値を時刻と対応付けて保存するエネルギー推定値履歴保存部を備え、所定時間だけ前のエネルギー推定値を現在のエネルギー推定値から減じたものを前記エネルギー変化推定値として出力する
ことを特徴とする、請求項１〜３のいずれか１項に記載のロボットの制御装置。
【請求項５】
前記動作司令調整部は、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が所定の第一閾値を超えた場合に停止指令を発生する
ことを特徴とする、請求項１〜４のいずれか１項に記載のロボットの制御装置。
【請求項６】
環境との接触を通知する警報発生部を更に備え、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が前記第一閾値を超えた場合に警報を発生する
ことを特徴とする、請求項１〜５のいずれか１項に記載のロボットの制御装置。
【請求項７】
前記第一閾値は、
前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボットが接触し得る環境内物体に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値である
ことを特徴とする、請求項５または６記載のロボットの制御装置。
【請求項８】
前記第一閾値は、
前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボット表面に装着した弾性柔軟材料に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値である
ことを特徴とする、請求項５または６記載のロボットの制御装置。
【請求項９】
前記動作指令調整部は、前記外部出力仕事量推定値が所定の第二閾値を超えた場合に前記動作指令記憶部が出力する動作指令の速度を遅らせたものを出力する
ことを特徴とする、請求項１〜８のいずれか１項に記載のロボットの制御装置。
【請求項１０】
前記ロボットはアクチュエータとしてサーボモータを有し、
前記制御仕事量算出部における前記駆動力は前記サーボモータを流れる電流値から算出する
ことを特徴とする、請求項１〜９のいずれか１項に記載のロボットの制御装置。
【請求項１１】
前記制御仕事量算出部における駆動力は前記駆動力指令をそのまま用いることを特徴とする、請求項１〜１０のいずれか１項に記載のロボットの制御装置。
【請求項１２】
前記駆動力は摩擦補償分を除いたものである
ことを特徴とする、請求項１〜１１のいずれか１項に記載のロボットの制御装置。
【請求項１３】
アームと、
前記アームに内蔵され、請求項１〜１２のいずれか１項に記載のロボットの制御装置によって制御されるアーム駆動用のサーボモータと、
ロボットの制御装置からの前記動作指令速度または前記停止指令に基づいてロボットの関節を駆動制御するサーボ制御部と、
を備えたことを特徴とする、ロボット。
【請求項１４】
前記アームの表面の一部または全体が弾性柔軟材料で覆われている
ことを特徴とする、請求項１３記載のロボット。
【請求項１】
ロボットの駆動力を制御するロボットの制御装置であって、
所望の動作軌跡と速度を出力する動作指令記憶部と、
前記駆動力が所定時間内に前記ロボットに対してした仕事量である制御仕事量を算出する制御仕事量算出部と、
前記ロボットのエネルギー推定値を算出するエネルギー推定部と、
前記ロボットの前記所定時間内のエネルギー変化の推定値を算出するエネルギー変化推定値算出部と、
前記制御仕事量と前記エネルギー変化推定値とに基づいてロボットへの動作指令速度を調整または停止指令を発生する動作指令調整部と、
を備えたことを特徴とするロボットの制御装置。
【請求項２】
前記制御仕事量算出部は、フィードバックした前記ロボットの関節速度と前記駆動力の積である制御仕事率を所定時間にわたって積分することで前記制御仕事量を算出する
ことを特徴とする、請求項１記載のロボットの制御装置。
【請求項３】
前記エネルギー推定部は、フィードバックした前記ロボットの関節速度および位置と、内蔵する前記ロボットの動力学モデルと、にもとづいて前記エネルギー推定値を算出することを特徴とする、請求項１又は２記載のロボットの制御装置。
【請求項４】
前記エネルギー変化推定値算出部は、所定時間にわたって前記エネルギー推定値を時刻と対応付けて保存するエネルギー推定値履歴保存部を備え、所定時間だけ前のエネルギー推定値を現在のエネルギー推定値から減じたものを前記エネルギー変化推定値として出力する
ことを特徴とする、請求項１〜３のいずれか１項に記載のロボットの制御装置。
【請求項５】
前記動作司令調整部は、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が所定の第一閾値を超えた場合に停止指令を発生する
ことを特徴とする、請求項１〜４のいずれか１項に記載のロボットの制御装置。
【請求項６】
環境との接触を通知する警報発生部を更に備え、前記制御仕事量と前記エネルギー変化推定値との差である外部出力仕事量推定値が前記第一閾値を超えた場合に警報を発生する
ことを特徴とする、請求項１〜５のいずれか１項に記載のロボットの制御装置。
【請求項７】
前記第一閾値は、
前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボットが接触し得る環境内物体に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値である
ことを特徴とする、請求項５または６記載のロボットの制御装置。
【請求項８】
前記第一閾値は、
前記ロボットが環境に接触したときに発生する接触力の制限値を前記ロボット表面に装着した弾性柔軟材料に印加したときに蓄えられる弾性エネルギーの大きさよりも小さな値である
ことを特徴とする、請求項５または６記載のロボットの制御装置。
【請求項９】
前記動作指令調整部は、前記外部出力仕事量推定値が所定の第二閾値を超えた場合に前記動作指令記憶部が出力する動作指令の速度を遅らせたものを出力する
ことを特徴とする、請求項１〜８のいずれか１項に記載のロボットの制御装置。
【請求項１０】
前記ロボットはアクチュエータとしてサーボモータを有し、
前記制御仕事量算出部における前記駆動力は前記サーボモータを流れる電流値から算出する
ことを特徴とする、請求項１〜９のいずれか１項に記載のロボットの制御装置。
【請求項１１】
前記制御仕事量算出部における駆動力は前記駆動力指令をそのまま用いることを特徴とする、請求項１〜１０のいずれか１項に記載のロボットの制御装置。
【請求項１２】
前記駆動力は摩擦補償分を除いたものである
ことを特徴とする、請求項１〜１１のいずれか１項に記載のロボットの制御装置。
【請求項１３】
アームと、
前記アームに内蔵され、請求項１〜１２のいずれか１項に記載のロボットの制御装置によって制御されるアーム駆動用のサーボモータと、
ロボットの制御装置からの前記動作指令速度または前記停止指令に基づいてロボットの関節を駆動制御するサーボ制御部と、
を備えたことを特徴とする、ロボット。
【請求項１４】
前記アームの表面の一部または全体が弾性柔軟材料で覆われている
ことを特徴とする、請求項１３記載のロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１０−１８８５０４（Ｐ２０１０−１８８５０４Ａ）
【公開日】平成２２年９月２日（２０１０．９．２）
【国際特許分類】
【出願番号】特願２００９−３８１３１（Ｐ２００９−３８１３１）
【出願日】平成２１年２月２０日（２００９．２．２０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２２年９月２日（２０１０．９．２）
【国際特許分類】
【出願日】平成２１年２月２０日（２００９．２．２０）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]