JP6729773B2 - ロボット制御装置およびこれを用いたロボットシステム - Google Patents

Description

本発明は、ロボットを制御するロボット制御装置およびこれを用いたロボットシステムに関する。

近年、自律的に動作する自律型ロボット、作業者等の作業物体が、安全柵で仕切られることなくロボットと作業空間を共有して作業を行うことができるロボットシステムの開発が進められている。このようなロボットシステムでは、作業物体がロボットの動作範囲に進入する場合があり、このような場合でも作業物体の安全性を担保する必要がある。

ところで、一般的に作業物体とロボットとが近くに位置すればする程、そして両者が高速に移動すればする程、両者の衝突可能性が高まる。よって、作業物体に対する安全性の観点からすると、両者が互いに離れて位置する段階、そして両者が低速で移動している段階で、ロボットの動作を制限することが望ましい。一方で、ロボットの作業効率の観点からすると、できる限りロボットの動作を制限しないことが望ましい。

そこで、作業物体への安全性とロボットの作業効率とを両立させるための取り組みがなされている。例えば特許文献１では、ロボットを極力動作させながらロボットと作業物体との衝突を回避する手法が開示されている。具体的には、ロボットのアームの先端を監視対象部として、この監視対象部が人等の作業物体に向かう方向の速度成分を有していない場合は動作制限を実施しない。一方、監視対象部が当該作業物体に向かう方向の速度成分を有している場合は、その速度成分に応じて設定される安全位置および作業物体との距離に基づいて、監視対象部を減速又は停止させる。

特開２０１１−１２５９７５号公報

しかしながら、従来の技術によれば、作業物体が監視対象部の動作軌道上に侵入した場合、ロボットは、作業物体との衝突を回避するために監視対象部を減速又は停止させるため、作業物体が監視対象部の動作軌道上から離れなければ、ロボット自身では作業を再開することができない。したがってロボットの作業効率が低下するという問題があった。

本発明は、上述のような事情を鑑みてなされたものであって、作業物体の安全性を確保しつつ、高い作業効率が得られるようにロボットの動作を制御するロボット制御装置およびこれを用いたロボットシステムを提供することを目的とする。

本発明に係るロボット制御装置は、ロボットにおいて監視対象として設定された監視対象部が指令軌道に沿って動作するようにロボットを制御するロボット制御装置であって、監視対象部の現在位置および監視対象部の目標位置に基づき、現在位置から目標位置へ向かう第１軌道を演算する第１軌道演算部と、作業物体の位置を含む物***置情報、およびロボットが動作する軌道上の少なくとも１つの点における監視対象部の予測軌道情報に基づき、監視対象部の作業物体に対する衝突可能性を推定する衝突可能性推定部と、推定された衝突可能性に基づき、第１軌道の軌道修正が必要か否かを判断する修正要否判断部と、修正要否判断部の判断結果に基づき、指令軌道を設定する軌道修正部とを備え、軌道修正部は、修正要否判断部にて軌道修正が必要と判断された場合には、複数の修正軌道候補の中から、修正軌道候補を用いたときのそれぞれの目標位置への到達時間と推定された衝突可能性とから選択し、選択した修正軌道候補を指令軌道として設定し、修正要否判断部にて軌道修正が必要でないと判断された場合には、第１軌道を前記指令軌道として設定するものである。

本発明に係るロボット制御装置およびこれを用いたロボットシステムにあっては、作業物体の安全性を確保しつつ、高い作業効率が得られるようにロボットの動作を制御することができる。

図１は本発明の実施の形態１に係るロボット制御装置およびこれを備えたロボットシステムを説明するための上面概略図である。 図１に示すロボット制御装置の構成を示すブロック図である。 軌道修正部にて行われる第１軌道の修正方法の一例示す説明図である。 本発明の実施の形態１に係るロボット制御装置の動作のフローチャートである。 図５は図４のステップＳ２をより詳細に説明するフローチャートである。 本発明の実施の形態１に係る衝突可能性推定部の構成の一例を示すブロック図である。 本発明の実施の形態１に係る接近距離の演算方法の一例に関する説明図である。 本発明の実施の形態１に係る相対速度を演算する方法の一例に関する説明図である。 本発明の実施の形態１に係るロボット制御装置を実現するためのハードウェア構成図である。 本発明の実施の形態１に係るロボット制御装置の変形例を示すブロック図である。 本発明の実施の形態２に係るロボット制御装置の構成例を示すブロック図である。 衝突可能性推定部の構成例を示すブロック図である。 本発明の実施の形態２に係る接近距離演算法の一例を示した図である。 本発明の実施の形態２に係る相対速度演算の一例を示した図である。 本発明の実施の形態３に係るロボット制御装置の構成の一例を示す図である。 本発明の形態３に係るロボット制御装置の動作を説明するフローチャートである。 図１６のステップＳ３Ａの処理を説明するためのフローチャートである。 本発明の実施の形態４に係るロボット制御装置の構成の一例を示す図である。 本発明の実施の形態５に係るロボット制御装置の構成の一例を示す図である。 衝突可能性推定部の構成例を示すブロック図である。

実施の形態１．
図１は本発明の実施の形態１に係るロボット制御装置１およびこれを備えたロボットシステムを説明するための上面概略図である。図中、ロボットシステムは、少なくともロボット制御装置１およびロボット３を備える。ロボットシステムが位置検出装置２を備えていてもよい。

ロボット制御装置１には、位置検出装置２およびロボット３が接続される。ロボット制御装置１は、位置検出装置２が取得した検出情報２０およびロボット３から取得した関節角度等の情報に基づいて、ロボット３に対して制御信号１０を出力する。ロボット制御装置１からの制御信号１０に応じてロボット３が駆動されることにより、ロボット手先３ｂが指令軌道に沿って動作する。

位置検出装置２は、ロボット３の周囲にて作業する作業者５を制御周期ごとに検出し、検出結果を検出情報２０としてロボット制御装置１へ出力する。

ロボット３は、アーム３ａおよびアーム３ａの先端に設けられたロボット手先３ｂを有する。アーム３ａの関節には、関節角度を制御する駆動装置（図示省略）が設けられる。なお、駆動装置はサーボモータ、ステッピングモータ等の電動モータにより構成される。ロボット３は各関節の関節角度を駆動装置により変更することで位置および姿勢を自在に変更可能である。駆動装置として、電動モータの代わりに、空気圧又は油圧を利用したシリンダを用いてもよい。

ロボット３は、アーム３ａの関節角度を計測する画像センサ、エンコーダ等の関節角度計測装置（図示省略）を有する。よって、ロボット制御装置１は、ロボット３の関節角度計測装置で得られた関節角度を取得し、この取得した関節角度およびロボット３のアーム３ａのアーム長などの機器情報を用いて、ロボット手先３ｂの位置を演算可能である。

なお、ロボット手先３ｂを監視対象部として設定した場合を例に挙げて以下の説明を行うが、監視対象部はこの例には限定されず、監視対象部をロボット３の任意の場所に設定してもよい。また、監視対象部は一箇所に限らず複数箇所設定してもよい。

本実施の形態に係る作業現場には作業台４が配置される。ロボット３の周囲には作業を行う作業者５が存在し、ロボット３はこの作業者５と共に作業空間を共有して動作する。作業台４上には、ロボット３および作業者５の双方が利用可能な領域として、作業領域６１および６２が設定される。この作業領域６１および６２において、ロボット３は、作業対象物７１および７２に対してロボット手先３ｂで運搬、加工、検査、搬出等の作業を施す。作業領域６１および６２において、作業者５も作業対象物７１および７２に対してロボット３と同様な作業を実施できる。なお、作業領域、作業対象物の個数等の作業現場の構成は上述の例に限定されるものではない。

図中、作業者５が作業対象物７２を保持した状態である。ロボット３が作業領域６２へ向かって（破線矢印）ロボット手先３ｂを移動する前の状態であることがわかる。

なお、作業者５すなわち人がロボット３と共に作業を行う場合を例に挙げて以下の説明を行うが、倉庫内等で作業を行うピッキングロボット等の自律的に動作する装置がロボット３と作業する場合であってもよい。作業物体はロボット３に対して自律的に動作する装置または作業者を意味する。

ここで、位置検出装置２についてより詳細に説明する。位置検出装置２は、レンジセンサ、ＲＧＢ−Ｄ（Ｒｅｄ Ｇｒｅｅｎ Ｂｌｕｅ−Ｄｅｐｔｈ）センサ、超音波センサ、静電容量センサ等のセンサにより構成される。上述のセンサでは検出ができない領域の検出情報２０を取得するために、又は作業者５の検出精度を向上するために、マットスイッチ、ライトカーテン等の追加のセンサを用いてもよい。

また、図１では位置検出装置２が作業現場に設置されているが、位置検出装置２がロボット３に設置されていてもよい。ここで、位置検出装置２を作業現場に固定した場合の座標系での位置は、位置検出装置２をロボット３に固定した場合の座標系での位置と、相互に変換可能である。よって、位置検出装置２の設置場所を変更した場合でも、上述の例と同様に計算できる。

図２は図１に示すロボット制御装置１の構成を示すブロック図である。図２に示すロボット制御装置１は、手先情報取得部１０１、目標点情報取得部１０２、人位置情報取得部１０３、第１軌道演算部１０４、衝突可能性推定部１０５、修正要否判断部１０６、軌道修正部１０７、およびロボット動作制御部１０８を備える。

手先情報取得部１０１は、関節角度計測装置で取得された関節角度に基づき、ロボット３の手先情報１０１０を演算する。その後、手先情報取得部１０１は、この手先情報１０１０を第１軌道演算部１０４へ出力する。

より具体的には、手先情報取得部１０１は、関節角度と関節角速度をロボット３の予め作成された筺体モデルへ適用することにより、ロボット３の手先位置、手先速度を含む手先情報１０１０を演算する。ロボット３の筺体モデルは、ロボット制御装置１の記憶装置（図９に図示）に記憶される。関節角速度はロボット３の関節角度の時間差分を行うことにより演算される。この演算された関節角速度に対して、ローパスフィルタを用いて、ノイズを除去してもよい。

目標点情報取得部１０２は、記憶装置から目標点情報１０２０を取得する。目標点情報１０２０は、ロボット手先３ｂの目標位置および目標姿勢に関する情報を含む。目標点情報１０２０は予めロボット３への教示作業によりロボット制御装置１の記憶装置（図９に後述）に記憶される。なお、予め作成して記憶された目標点情報１０２０を用いる代わりに、目標点情報１０２０がロボット３の動作中に作業者５によって指定されてもよい。

人位置情報取得部１０３は、位置検出装置２からの検出情報２０を用いて、作業者５の代表点の位置座標である人位置情報（物***置情報）１０３０を取得する。人位置情報１０３０は、衝突可能性推定部１０５および軌道修正部１０７へ出力される。

ここで、検出情報２０から人位置情報１０３０を取得する方法の一例を以下に述べる。検出情報２０が作業者５の複数の身体部位を点群とした情報である場合、検出した点群のうちロボット３から最も近い距離にある代表点を人位置情報１０３０として設定できる。

なお、代表点の設定方法は上述の例に限らず、ロボット３の動作条件、作業現場等の構成によって適宜変更してもよい。検出情報２０がＲＧＢ−Ｄセンサによって取得された画像情報である場合、この画像情報に対し画像処理を行うことで作業者５の人体モデルを構築する。その後、この構築された人体モデルの関節およびリンクの座標を計算することで、人位置情報１０３０を取得してもよい。この場合、精密な人体モデルを構築できるため、ロボット３の作業者５に対する衝突可能性を低減することができる。

第１軌道演算部１０４は、手先情報１０１０および目標点情報１０２０を用いて、第１軌道１０４０を計算する。具体的には、第１軌道演算部１０４は、まず手先情報１０１０からロボット手先３ｂの現在位置を取得し、ロボット制御装置１の記憶装置に予め記憶された目標点情報１０２０からロボット手先３ｂの目標位置を取得する。次に、第１軌道演算部１０４は現在位置から目標点へ向かう第１軌道を計算する。その後、第１軌道１０４０が衝突可能性推定部１０５および軌道修正部１０７へ出力される。

ここで、第１軌道１０４０は、制御周期に対応したそれぞれの時刻（ｔ＝ｔ１，t２，…，ｔｎ−１，ｔｎ，…）におけるロボット手先３ｂの手先位置（予測位置）、予測姿勢、および手先速度（予測速度）に関する予測軌道情報を含む。なお、ロボット手先３ｂの速度は、例えば以下のとおり導出される。第１軌道１０４０上の時刻（ｔ＝ｔ１，t２，…，ｔｎ−１，ｔｎ，…）に対応する手先位置（Ｐ（０），Ｐ（ｔ１），Ｐ（ｔ２），…，Ｐ（ｔｎ−１），Ｐ（ｔｎ），…）のそれぞれについて、１制御周期前の時刻における位置との差分を取り（例えば、Ｐ（ｔｎ）−Ｐ（ｔｎ−１））、この差分値を（ｔｎ―ｔｎ−１）で割ることで、各時刻におけるロボット手先３ｂの手先速度が演算できる。

なお、第１軌道１０４０の軌道の導出方法は、滑らかな軌道となるように始点と終点を複数の点で結ぶＰＴＰ（Ｐｏｉｎｔ Ｔｏ Ｐｏｉｎｔ）軌道として導出する。この滑らかな軌道を生成する方法として関節空間補間軌道を用いるがこれに限定されるものではない。またＰＴＰ軌道の代わりにＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐａｔｈ）軌道を用いてもよく、このＣＰ軌道では始点と終点までの軌道が連続関数として導出される。

衝突可能性推定部１０５は、第１軌道１０４０上の少なくとも一点におけるロボット手先３ｂの予測軌道情報と作業者５の人位置情報１０３０とに基づき、作業者５とロボット手先３ｂとの衝突の可能性を示す衝突可能性１０５０を推定する。衝突可能性１０５０の具体的な推定方法については図６の衝突可能性演算部１０５５と共に詳細に説明する。

修正要否判断部１０６は、衝突可能性１０５０に基づいて、第１軌道１０４０の修正が必要か否かを判断する。例えば、修正要否判断部１０６は算出された衝突可能性１０５０が予め設定された閾値を超えた場合に、第１軌道１０４０の修正が必要であると判断し、修正要否判断部１０６は軌道修正部１０７へ修正指示信号１０６０を出力する。なお、修正要否判断部１０６は、修正指示信号１０６０と共に衝突可能性１０５０を軌道修正部１０７へ出力する。

修正要否判断部１０６から修正指示信号１０６０が入力された場合には、軌道修正部１０７は、人位置情報１０３０および第１軌道１０４０に基づいて、第１軌道１０４０よりも衝突可能性が低い第２軌道を生成する。軌道修正部１０７は、生成された第２軌道を指令軌道１０７０として設定する。修正要否判断部１０６から修正指示信号１０６０が入力されない場合には、軌道修正部１０７は第１軌道１０４０を指令軌道１０７０として出力する。

図３は、軌道修正部１０７による第１軌道１０４０の修正方法を示す説明図である。図中、第２軌道Ｑは、第１軌道Ｐを修正した後の軌道の一例である。図中、Ｐ０およびＰｆはそれぞれロボット３の位置および目標位置を示す。また、Ｌ０は人位置Ｈ１とＰ０とを結ぶ線分を示し、Ｌｆは目標位置Ｐｆと人位置Ｈ１とを結ぶ線分を示す。線Ｃ０は中心Ｈ１であり、半径が線分Ｌ０の長さである円を示す。

第２軌道Ｑは、例えば、人位置Ｈ１から見たときに、第２軌道Ｑの全ての点において第１軌道Ｐよりも外側を通過するように設定される。図３に示すように第２軌道Ｑは人位置Ｈ１から見たときに外側に凸の曲線となるように設定されていてもよい。上述のように第２軌道を設定することにより、第１軌道Ｐと比較して、作業者５とロボット手先３ｂとの接近距離が長くなっているため、作業者５に衝突しにくい軌道とすることができ、衝突可能性を低くすることができる。

図３に示す第２軌道Ｑは、人位置Ｈ１を中心とし、線分Ｌ０を半径とする円弧状の軌道（Ｃ０）から、人位置Ｈ１を中心とし、線分Ｌｆを半径とする円弧状の軌道へと滑らかに移行する軌道となっている。言い換えると、図３において、ロボット手先３ｂが軌道修正時の位置Ｐ０から目標位置Ｐｆへと向かうにつれて、ロボット手先３ｂから作業者５までの距離が単調に変化（単調減少または単調増加）するように、第２軌道Ｑが生成される。このように第２軌道Ｑが生成されることによって、第２軌道Ｑの各点において、ロボット手先３ｂから作業者５までの最短距離は、線分Ｌ０又は線分Ｌｆの長さとなる。したがって、ロボット手先３ｂが作業者５に近付き過ぎることを防止でき、衝突可能性を低くすることができる。

また、人位置Ｈ１から見たときに外側に凸の曲線となるように第２軌道Ｑを設定することで、ロボット手先３ｂの移動速度（手先速度）のうち、人位置Ｈ１に向かう速度成分が小さくなる。この結果、ロボット手先３ｂの衝突可能性を低くすることができる。また、人位置Ｈ１に向かう速度成分が小さくなると、手先速度を速くして目標位置Ｐｆへの到達時間を短縮することも可能となる。なお、図３は第２軌道Ｑの一例を示しており、第２軌道Ｑとしては他の軌道であっても良い。

上述のように第１軌道１０４０を修正する代わりに、軌道修正部１０７は予め作成された複数の修正軌道から、第１軌道１０４０よりも衝突可能性１０５０が小さな修正軌道、例えば、衝突可能性１０５０が最も小さな修正軌道を選択し、選択した修正軌道を第２軌道として用いてもよい。また、軌道修正部１０７は、第１軌道１０４０よりも衝突可能性１０５０が小さく、衝突可能性１０５０と目標位置Ｐｆへの到達時間とのバランスの良い修正軌道を選択し、選択した修正軌道を第２軌道として用いてもよい。なお、複数の修正軌道はロボット制御装置１の記憶装置（図９に図示）に保存される。

ロボット動作制御部１０８は、指令軌道１０７０に基づき、ロボット３に対して制御周期ごとに制御信号１０を出力する。

なお、ロボット動作制御部１０８は、指令軌道１０７０に加えて手先情報１０１０と人位置情報１０３０を用いて、ロボット手先３ｂの動作中に、制御周期ごとに算出される衝突可能性に応じて、その速度を変更する構成としてもよい。具体的には、ロボット動作制御部１０８は、人位置情報１０３０および手先情報１０１０から制御周期ごとに算出される衝突可能性について、その衝突可能性の値が大きく（小さく）なるにつれてロボット手先３ｂの速度を低下（増加）させる制御信号１０を作成してもよい。

図４は本実施の形態のロボット制御装置１に係る動作のフローチャートである。まず、ステップＳ１にて、ロボット制御装置１は、位置検出装置２から検出情報２０を取得し、ロボット３から関節角等のデータを取得する。

ステップＳ２にて、ロボット制御装置１は得られたデータに基づいて指令軌道１０７０を設定する。ステップＳ３にて、ロボット制御装置１は、指令軌道１０７０に基づき、制御周期ごとに制御信号１０を出力する。ステップＳ４にてロボット手先３ｂが目標点に到達すれば（ＹＥＳ）、ロボット３は動作を終了する。ステップＳ４にてロボット手先３ｂが目標点に到達していなければ（ＮＯ）、ステップＳ３に戻る。

図５は図４のステップＳ２をより詳細に説明するフローチャートである。ステップＳ２１にて、ロボット３の制御に必要なデータを準備する。具体的には、まず手先情報取得部１０１、目標点情報取得部１０２、および人位置情報取得部１０３は、それぞれロボット３の現在の手先情報１０１０、目標点情報１０２０、および人位置情報１０３０を取得する。

ステップＳ２２にて、第１軌道演算部１０４がロボット３の手先情報１０１０および目標点情報１０２０を用いて、第１軌道１０４０を演算する。

ステップＳ２３にて、衝突可能性推定部１０５は、人位置情報１０３０および第１軌道１０４０における予測軌道情報を用いて、衝突可能性１０５０を推定する。

ステップＳ２４にて、修正要否判断部１０６は、衝突可能性１０５０の値に基づき、第１軌道１０４０の修正が必要か否かを判断する。具体的には、ステップＳ２４にて、衝突可能性１０５０が閾値以上であれば（ＹＥＳ）、修正指示信号１０６０を生成してステップＳ２５に進むが、衝突可能性１０５０が閾値より小さければ（ＮＯ）、修正指示信号１０６０を生成せずステップＳ２６に進む。

ステップＳ２５にて、軌道修正部１０７は、修正指示信号１０６０の入力を受けて、第１軌道１０４０に比べて、衝突可能性が低い第２軌道へ修正し、第２軌道を指令軌道１０７０として設定する。

ステップＳ２６では、軌道修正部１０７は、修正指示信号１０６０の入力がないため、第１軌道１０４０を指令軌道１０７０として設定する。

図６は、衝突可能性推定部１０５の構成の一例を示すブロック図である。衝突可能性推定部１０５は、手先位置取得部１０５１、接近距離演算部１０５２、手先速度取得部１０５３、相対速度演算部１０５４、および衝突可能性演算部１０５５を備える。

手先位置取得部１０５１は、第１軌道１０４０の予測軌道情報から、ロボット３の手先位置１０５１０を取得する。その後、この取得された手先位置１０５１０は、後述する接近距離演算部１０５２および相対速度演算部１０５４へ出力される。

手先速度取得部１０５３は、第１軌道１０４０上の少なくとも一点における予測軌道情報から手先速度１０５３０を取得する。この取得されたロボット３の手先速度１０５３０は、相対速度演算部１０５４へ入力される。

接近距離演算部１０５２は、手先位置１０５１０および人位置情報１０３０を用いて、ロボット手先３ｂと作業者５との接近距離１０５２０を演算する。

ここで、図７を用いて接近距離１０５２０を演算する方法を詳細に説明する。図７は接近距離１０５２０の演算方法の一例に関する説明図である。

まず、接近距離演算部１０５２は、第１軌道Ｐ上に設定されたｎ点の手先位置Ｐ１，Ｐ２，Ｐ３，〜，Ｐｎについて、このｎ点の手先位置Ｐ１〜Ｐｎのそれぞれと人位置Ｈ１とを結ぶ線分を、それぞれ線分Ｌ１，Ｌ２，Ｌ３，〜，Ｌｎとして設定する。具体的には、線分Ｌ１、Ｌ２、およびＬ３は、それぞれ手先位置Ｐ１と人位置Ｈ１とを結ぶ線分、手先位置Ｐ２と人位置Ｈ１とを結ぶ線分、および手先位置Ｐ３と人位置Ｈ１とを結ぶ線分である。また、以下の説明では、ｎ＝３すなわち３つの手先位置を設定した場合を例に挙げて説明を行うが、少なくとも１つの手先位置を設定していればよい。

次に、接近距離演算部１０５２は、線分Ｌ１〜Ｌ３の長さに基づいて、接近距離１０５２０を導出する。上述の例のように複数の線分が導出される場合には、例えば、線分Ｌ１〜Ｌ３のうちその長さが最も小さい線分を選択し、その選択された線分の長さを接近距離１０５２０として選択する。例えば、図７では、線分Ｌ１〜Ｌ３のうち線分の長さが最小値となる線分Ｌ２の長さを接近距離１０５２０としている。

なお、図７および図８では、各手先位置からその手先位置に隣接する手先位置までの移動距離が予め設定された距離となるように手先位置Ｐ１〜Ｐ３を設定している。一方で、各手先位置からその手先位置に隣接する手先位置までの移動時間が予め設定された時間となるように手先位置Ｐ１〜Ｐ３を設定してもよい。

ここで、図６および図８を用いて相対速度演算部１０５４の処理を説明する。図６に示すとおり、相対速度演算部１０５４は、人位置情報１０３０、手先位置１０５１０、および手先速度１０５３０に基づき、ロボット手先３ｂの作業者５に対する相対速度１０５４０を演算する。

図８は、相対速度１０５４０を演算する方法の一例に関する説明図である。図８を用いて相対速度演算部１０５４が相対速度１０５４０を演算する方法を詳細に説明する。図中Ｖ１〜Ｖ３は手先速度取得部１０５３にて演算された手先位置Ｐ１〜Ｐ３における手先速度１０５３０を示す。

最初に、相対速度演算部１０５４は、手先速度Ｖ１〜Ｖ３を線分Ｌ１〜Ｌ３方向へ射影することにより、作業者５に対するロボット手先３ｂの相対速度ベクトルＲＶ１〜ＲＶ３を演算する。例えば、図８ではＲＶ１、ＲＶ２およびＲＶ３は、それぞれ手先速度Ｖ１を線分Ｌ１へ射影した相対速度ベクトル、手先速度Ｖ２を線分Ｌ２へ射影した相対速度ベクトル、および手先速度Ｖ３に対して線分Ｌ３へ射影した相対速度ベクトルである。

次に、相対速度演算部１０５４は相対速度ベクトルＲＶ１〜ＲＶ３の値に基づき、相対速度１０５４０を導出する。上述の例のように複数の相対速度ベクトルが演算された場合には、例えば、相対速度ベクトルＲＶ１〜ＲＶ３のうち、作業者５へ向かう相対速度ベクトルであってその絶対値が最大となる相対速度ベクトルを相対速度１０５４０とする。具体的には、相対速度ベクトルＲＶ１〜ＲＶ３のうち、作業者５に向かう相対速度ベクトルであるＲＶ１、ＲＶ２を選択し、選択された両ベクトルの絶対値を比較して絶対値が大きい相対速度ベクトルＲＶ１を相対速度１０５４０とする。

図６に戻り、衝突可能性推定部１０５の残りの構成の説明を行う。衝突可能性演算部１０５５は、例えば、入力された接近距離１０５２０および相対速度１０５４０に基づき、衝突可能性１０５０を演算する。衝突可能性１０５０を演算する方法の一例としては、予測軌道情報と作業者５の位置情報について予め作成された衝突評価指標を参照することで算出される。衝突評価指標はロボット制御装置１が有する記憶装置（図９に図示）に保存される。衝突評価指標は、例えば接近距離１０５２０が小さくなるにつれて、また相対速度１０５４０が大きくなるにつれて、衝突可能性１０５０が大きな値となるように設定される。

また、衝突評価指標の別の例として、人位置情報１０３０の経時変化から算出された作業者５の移動速度を用い、作業者５とロボット３との仮想的な衝突予測時間を算出する。次に、算出された衝突予測時間が短くなるにつれて衝突可能性が高くなるように作成された衝突評価指標を用いる。さらに、作業者５が侵入した場合にロボット３との衝突が予想される範囲を、ロボット３が設置された位置の周囲に予め設定し、この範囲内に人位置情報１０３０が存在するか否かにより、衝突可能性を推定してもよい。

なお、接近距離１０５２０および相対速度１０５４０の両方に基づき衝突可能性１０５０を演算する例を説明したが、接近距離１０５２０および相対速度１０５４０のいずれか一方のみに基づき、衝突可能性１０５０を演算する構成としてもよい。この場合、衝突評価指標は、接近距離１０５２０および相対速度１０５４０のいずれか一方のみに応じて衝突可能性１０５０が変化するように作成される。このとき、予測軌道情報は手先位置１０５１０又は手先速度１０５３０のいずれか一方を含んでいればよい。

本実施の形態によれば、ロボット３の指令軌道上で予め衝突可能性が高いと判定した場合には、ロボット３と作業者５との衝突可能性１０５０を低下させるように第１軌道１０４０を修正するため、ロボット３の第１軌道１０４０に作業者５の侵入が予想される場合においてもロボット３の作業効率を維持したロボット動作が可能となる。

図９はロボット制御装置１を実現するためのハードウェア構成図である。図９に示すとおり、上記ロボット制御装置１の一部又は全部は、具体的には、ＣＰＵ１５１（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、記憶装置１５２、ＩＯ（ＩＮＰＵＴ ＯＵＴＰＵＴ：入出力）インターフェース１５３、およびシステムバス１５４等により構成される。記憶装置１５２はＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等から構成される。ロボット制御装置１のＩＯインターフェース１５３には、入力装置２０１および出力装置２０２がケーブル１５５を介して接続される。なお、記憶装置１５２には、先述したロボット３の筺体モデル、目標点情報１０２０、衝突評価指標、軌道修正部１０７で用いられる第２軌道の候補となる修正軌道等が保存されている。

ロボット制御装置１の各処理はＣＰＵ１５１により実行される。手先情報取得部１０１、目標点情報取得部１０２、人位置情報取得部１０３、およびロボット動作制御部１０８については、ＩＯインターフェース１５３を介して、ロボット制御装置１の外部構成（位置検出装置２およびロボット３等）との入力および出力を行う。

図１０は、本発明の実施の形態１に係るロボット制御装置１の変形例を示すブロック図である。図２では手先情報取得部１０１がロボット３の関節角度計測装置からの情報を用いて手先情報１０１０を取得していたが、図１０に示したロボット制御装置１Ａのように、第１軌道演算部１０４Ａが、制御信号１０を用いて現在の手先情報１０１０を導出する構成としてもよい。この場合、ロボット制御装置１Ａにおいて手先情報取得部１０１を省略することができる。

実施の形態２．
図１１は本発明の実施の形態２に係るロボット制御装置１Ｂの構成例を示すブロック図である。衝突可能性推定部１０５の代わりに衝突可能性推定部１０５Ａを備える点が上述の実施の形態とは異なる。

図１２は衝突可能性推定部１０５Ａの構成例を示すブロック図である。図１２に示す衝突可能性推定部１０５Ａは、人移動速度演算部（移動速度演算部）１０５６をさらに備え、作業者５の移動速度を考慮して衝突可能性１０５０を演算する点が、図６に示した衝突可能性推定部１０５と異なる。図１３は本発明の実施の形態２に係る接近距離１０５２０を演算する方法の一例を示した図である。図中、手先位置を３点設定した場合を示す。図中、人位置Ｈ１、ロボット３の第１軌道Ｐにおける手先位置Ｐ１〜Ｐ３は、図７と同様である。

最初に、接近距離演算部１０５２Ａは、現在の人位置情報１０３０および人移動速度１０５６０を用いて、各手先位置に対応する時刻（制御周期）における人位置の移動先１０５２１を求める。具体的には、図１３における人位置Ｈ２および人位置Ｈ３は、人位置Ｈ１および人移動速度ＨＶを用いて演算される。言い換えると、接近距離演算部１０５２Ａは、取得された人位置情報１０３０および人移動速度１０５６０を用いて、人位置情報１０３０が取得された時刻より後の時刻における人位置の移動先１０５２１を推定する。人位置の移動先１０５２１は相対速度演算部１０５４Ａに出力される。人位置Ｈ１〜Ｈ３は、それぞれ手先位置Ｐ１〜Ｐ３に対応した制御周期における人位置を示す。人位置の移動先１０５２１は、Ｈ２およびＨ３に対応する。ここで、人位置Ｈ１はロボット手先３ｂの動作開始前に、手先位置取得部１０５１で取得される。人移動速度ＨＶは人移動速度演算部１０５６により演算される。

次に、図１２に示す接近距離演算部１０５２Ａは、人位置情報１０３０、人位置の移動先１０５２１、および手先位置１０５１０に基づいて、ロボット手先３ｂと作業者５との接近距離１０５２０を演算する。その後、接近距離１０５２０は、衝突可能性演算部１０５５へ出力される。

より詳細には、図１３に示した人位置Ｈ１〜Ｈ３と手先位置Ｐ１〜Ｐ３との間の線分として、それぞれＬａ１，Ｌａ２，Ｌａ３を作成する。例えば図１３では、手先位置Ｐ１と人位置Ｈ１との間の線分がＬａ１であり、手先位置Ｐ２と人位置Ｈ２との間の線分がＬａ２であり、手先位置Ｐ３と人位置Ｈ３との間の線分がＬａ３である。さらに、線分Ｌａ１〜Ｌａ３を用いて接近距離１０５２０を演算する。具体的には、線分Ｌａ１〜Ｌａ３の長さのうちの最小値を接近距離１０５２０とする。なお、上述の例では手先位置を３点設定した場合を例示しているが、手先位置は一点以上設けられていればよい。

図１２示す相対速度演算部１０５４Ａは、手先位置１０５１０、人位置の移動先１０５２１、手先速度１０５３０、および人移動速度１０５６０に基づいて、ロボット手先３ｂの作業者５に対する相対速度１０５４０を出力する。

図１４は本発明の実施の形態２に係る相対速度１０５４０の演算方法の一例を示した図である。図１４において、図１３と同様の構成は同符号を付し説明を省略する。図８と同様に、図中Ｖ１〜Ｖ３は手先速度取得部１０５３にて演算された手先位置Ｐ１〜Ｐ３における手先速度１０５３０を示す。

相対速度演算部１０５４Ａは、まず、手先速度Ｖ１〜Ｖ３を線分Ｌａ１〜Ｌａ３方向へ射影することにより、作業者５に対するロボット手先３ｂの相対速度ベクトルＲＶａ１〜ＲＶａ３を演算する。さらに相対速度演算部１０５４Ａは、相対速度ベクトルＲＶａ１〜ＲａＶ３に基づき、相対速度１０５４０を導出する。

上述の例のように複数の相対速度ベクトルが演算された場合には、例えば、相対速度ベクトルＲＶａ１〜ＲＶａ３のうち、作業者５へ向かう相対速度ベクトルであってその絶対値が最大となる相対速度ベクトルを、相対速度１０５４０とする。例えば、相対速度ベクトルＲＶａ１〜ＲＶａ３のうち、作業者５に向かう相対速度ベクトルであるＲＶａ１、ＲＶａ２を選択し、選択された両ベクトルの絶対値を比較して絶対値が大きい相対速度ベクトルＲＶａ１を相対速度１０５４０とする。

本実施の形態によれば、作業者５の人移動速度を考慮して衝突可能性を判定するために仮に作業者５がロボット３の指令軌道に接近するように動作していた場合では、実施の形態１に比べ接近距離が小さくなることでより衝突可能性が高く算出され、結果として第１軌道の修正が行われやすくなる。一方で、作業者５がロボット３の指令軌道から遠ざかるように動作していた場合では、実施の形態１に比べ接近距離が大きくなることでより衝突可能性が低く算出され、結果として不必要な第１軌道の修正が行われない。したがって、実施の形態１のロボット制御装置と比較して、本実施の形態のロボット制御装置では、作業者５への安全性とロボットの動作効率を向上させることができる。

実施の形態３．
図１５は本発明の実施の形態３に係るロボット制御装置１Ｃの構成の一例を示す図である。図１５において、図２と同様の構成は同符号を付し説明を省略する。ただし、図１５に示す衝突可能性推定部１０５は、図１１および図１２に示す衝突可能性推定部１０５Ａであってもよく、後述する図１９および図２０に示す衝突可能性推定部１０５Ｅであってもよい。

ロボット制御装置１Ｃは、軌道修正部１０７に代わり軌道修正部１０７Ｃを備え、ロボット動作制御部１０８に代わりロボット動作制御部１０８Ａを備える。

図１５〜図１７を用いて本実施の形態に係るロボット制御装置１Ｃの構成および動作を説明する。図１６は、本発明の実施の形態３に係るロボット制御装置１Ｃの動作を説明するフローチャートである。図１７は、図１６のステップＳ３Ａの処理を説明するためのフローチャートである。なお、図１６では、ステップＳ１は図４のステップＳ１と同様である。また、図中、ステップＳ２（ステップＳ２１〜ステップＳ２６）は、図４のステップＳ２つまり図５のステップＳ２１〜Ｓ２６と同様である。よって、それらのステップの説明は繰り返し行わない。

ステップＳ３Ａにて、図１６に示すようにロボット動作制御部１０８Ａはロボット３の駆動制御を行う。ここで図１７を用いて、ステップＳ３Ａを詳細に説明する。

まず、ステップＳ３１Ａでは、ロボット動作制御部１０８Ａが現在の手先情報１０１０および人位置情報１０３０を取得する。次に、ステップＳ３２Ａにて、ロボット動作制御部１０８Ａは、現在の手先情報１０１０および人位置情報１０３０に基づき、上述した衝突評価指標を用いて、予め設定された周期で衝突可能性を算出する。ここで、現在の手先情報１０１０および人位置情報１０３０とは、取得されている最新の手先情報１０１０および人位置情報１０３０を指す。ステップＳ３３Ａにて、ロボット動作制御部１０８Ａは、算出された衝突可能性が閾値以下であるか否かを判断する。ステップＳ３３Ａにて、衝突可能性が閾値以下（ＹＥＳ）であれば、ロボット動作制御部１０８Ａは制御信号１０を生成する。一方で、ステップＳ３３Ａにて、衝突可能性が閾値より大きければ（ＮＯ）、ステップＳ３５Ａに進み、ロボット動作制御部１０８Ａは再計画指令１０８０を生成する。

図１６に戻り、ロボット制御装置１ＣのステップＳ３Ｂ以降の動作を説明する。ステップＳ３Ｂにて、再計画指令１０８０の有無を判断する。ステップＳ３Ｂにて、ロボット動作制御部１０８Ａにより再計画指令１０８０が生成されていない場合（ＹＥＳ）、ステップＳ４に進む。一方で、ステップＳ３Ｂにて、ロボット動作制御部１０８Ａにより再計画指令１０８０が生成されている場合は（ＮＯ）ステップＳ２５に戻り、軌道修正部１０７Ｃにより指令軌道１０７０を修正する。

なお、再計画指令１０８０が入力されたとき、軌道修正部１０７Ｃは、第１軌道１０４０の代わりに指令軌道１０７０を用いて、第２軌道を生成する（ステップＳ２５）。軌道修正部１０７Ｃは、この生成された第２軌道を新たな指令軌道１０７０として設定する。この新たに設定された指令軌道は、予め設定された指令軌道１０７０に比べて衝突可能性が低くなるように軌道修正部１０７Ｃにより生成される。

ステップＳ４にて、ロボット動作制御部１０８Ａはロボット手先３ｂが目標点に到達したか否かを判断し、目標点に到達している場合は（ＹＥＳ）動作を終了し、そうでない場合は（ＮＯ）ステップＳ３Ａに戻る。

図１６のフローチャートでは、再計画指令が入力された場合に、指令軌道１０７０の衝突可能性が閾値以下となるまでステップＳ２５〜ステップＳ３Ｂを繰り返す。

本実施の形態によれば、ロボット３と作業者５との衝突可能性が高まる場合において、ロボット３の指令軌道１０７０を再計画することで作業者５への安全性とロボット３の動作効率が向上する。

実施の形態４．
図１８は本発明の実施の形態４に係るロボット制御装置１Ｄの構成の一例を示す図である。図２と同じ又は対応する構成については同符号を付しその説明を繰り返さない。

本実施の形態のロボット制御装置１Ｄでは、ロボット制御装置１（実施の形態１）と比較して、ロボット制御装置１Ｄがロボット手先３ｂの周囲の部材の情報を取得するセンサ６に接続されている点、および、ロボット制御装置１Ｄが動作環境取得部１０９と干渉判定部１１０とを有する点が異なる。センサ６はレンジセンサ、ＲＧＢ−Ｄ（Ｒｅｄ Ｇｒｅｅｎ Ｂｌｕｅ−Ｄｅｐｔｈ）センサ、超音波センサ、静電容量センサを例示することができ、図１における位置検出装置２と併用してもよい。

動作環境取得部１０９は、センサ６からの情報を用いて、ロボット手先３ｂの動作範囲に配置されている部材の位置情報１０９０を取得する。その後、この部材に関する位置情報１０９０は動作環境取得部１０９により干渉判定部１１０へ出力される。

なお、ロボット手先３ｂの動作範囲に配置された部材とは、ロボット３以外の部材に加えて、ロボット手先３ｂ以外のロボット３の構成（ロボット３の土台等）を意味する。

干渉判定部１１０は、部材の位置情報１０９０および第１軌道１０４０の軌道予測情報を用いてロボット手先３ｂがこの部材へ干渉するか否かを判断する。干渉判定部１１０により軌道の修正が必要と判定された場合、軌道修正指示信号１１００が軌道修正部１０７Ｄへ出力される。これにより軌道修正部１０７Ｄは、第１軌道１０４０の予測軌道情報および部材の位置情報１０９０に基づき、軌道の修正を行う。一方で、干渉判定部１１０により軌道の修正が必要でない判定された場合、軌道修正指示信号１１００が軌道修正部１０７Ｄへ入力されないため、軌道修正部１０７Ｄによる軌道修正が行われない。

本実施の形態によれば、ロボット手先３ｂの動作範囲に存在する部材に衝突しないように修正された指令軌道を得ることが可能である。なお、本実施の形態は、上述の実施の形態のロボット制御装置１Ａ〜１Ｃ、後述する実施の形態のロボット制御装置１Ｅのいずれとも組み合わせることができる。

実施の形態５．
図１９は本発明の実施の形態５に係るロボット制御装置１Ｅの構成の一例を示す図である。図１９に示すロボット制御装置１Ｅは、実施の形態２に係るロボット制御装置１Ｂの変形例となる。図１９において、図１１と同様の構成は同符号を付し説明を省略する。

図１１では、軌道修正部１０７が、人位置情報取得部１０３から出力される人位置情報１０３０を用いて第２軌道を生成していた。一方、図１９に示したロボット制御装置１Ｅのように、衝突可能性推定部１０５Ｅが、第２軌道を生成するための基準位置１２００を出力し、軌道修正部１０７が人位置情報１０３０の代わりに基準位置１２００を用いて第２軌道を生成する構成としてもよい。軌道修正部１０７の構成は、第２軌道を生成する際に、人位置情報１０３０の代わりに基準位置１２００を用いる他は、図１１に示されるものと同一である。

図２０は、衝突可能性推定部１０５Ｅの構成例を示すブロック図である。図２０に示す衝突可能性推定部１０５Ｅは、衝突可能性演算部１０５５Ｅが、人位置情報１０３０に含まれる人位置、及び人位置の移動先１０５２１の中から、基準位置１２００を選択して出力する点が、図１２に示した衝突可能性推定部１０５Ａと異なる。

衝突可能性演算部１０５５Ｅは、図１３又は図１４に示す人位置Ｈ１、Ｈ２、Ｈ３の中から、接近距離１０５２０又は相対速度１０５４０を参照して基準位置１２００を決定する。例えば、衝突可能性演算部１０５５Ｅは、人位置Ｈ１、Ｈ２、Ｈ３のそれぞれに対応する接近距離又は相対速度を参照して、最も衝突の可能性が高い人位置を選択して基準位置１２００とする。したがって、基準位置１２００は、現在の作業者５の位置、又は推定される作業者５の移動先を表す。作業者５の移動先は、将来における作業者５の予測位置と言い換えることもできる。

本実施の形態のロボット制御装置１Ｅによれば、作業者５への衝突の可能性がより低い、ロボット手先３ｂの移動軌道を生成することが可能となる。

１ ロボット制御装置
１０ 制御信号
２ 位置検出装置
２０ 検出情報
３ ロボット
４ 作業台
５ 作業者（作業物体）
６１、６２ 作業領域
７１、７２ 作業対象物
１０１ 手先情報取得部
１０１０ 手先情報
１０２ 目標点情報取得部
１０２０ 目標点情報
１０３ 人位置情報取得部
１０３０ 人位置情報（物***置情報）
１０４ 第１軌道演算部
１０４０ 第１軌道
１０５ 衝突可能性推定部
１０６ 修正要否判断部
１０７ 軌道修正部
１０８ ロボット動作制御部
１０９ 動作環境取得部
１０５６ 人移動速度演算部（移動速度演算部）

Claims (11)

1. ロボットにおいて監視対象として設定された監視対象部が指令軌道に沿って動作するように前記ロボットを制御するロボット制御装置であって、
   前記監視対象部の現在位置および前記監視対象部の目標位置に基づき、前記現在位置から前記目標位置へ向かう第１軌道を演算する第１軌道演算部と、
   作業物体の位置を含む物***置情報、および前記ロボットが動作する軌道上の少なくとも１つの点における前記監視対象部の予測軌道情報に基づき、前記監視対象部の前記作業物体に対する衝突可能性を推定する衝突可能性推定部と、
   前記推定された衝突可能性に基づき、前記第１軌道の軌道修正が必要か否かを判断する修正要否判断部と、
   前記修正要否判断部の判断結果に基づき、前記指令軌道を設定する軌道修正部と
   を備え、
   前記軌道修正部は、
   前記修正要否判断部にて前記軌道修正が必要と判断された場合には、複数の修正軌道候補の中から、前記修正軌道候補を用いたときのそれぞれの前記目標位置への到達時間と推定された前記衝突可能性とから選択し、選択した前記修正軌道候補を前記指令軌道として設定し、
   前記修正要否判断部にて前記軌道修正が必要でないと判断された場合には、前記第１軌道を前記指令軌道として設定する
   ロボット制御装置。
2. 前記軌道修正部は、前記複数の修正軌道候補のうち、少なくとも前記第１軌道で推定された前記衝突可能性よりも低い衝突可能性をもつ前記修正軌道候補を選択する
   ことを特徴とする請求項１に記載のロボット制御装置。
3. 前記複数の修正軌道候補は、前記作業物体の位置から見たときに、全ての点において前記第１軌道よりも外側を通過するように設定された修正軌道候補を含む
   ことを特徴とする請求項１または請求項２に記載のロボット制御装置。
4. 前記軌道修正部は、前記監視対象部が前記現在位置から前記目標位置へと向かうにつれて、前記監視対象部から前記作業物体までの距離が、前記現在位置と前記作業物体との間の距離から前記目標位置と前記作業物体との間の距離へと単調に変化する前記修正軌道候補を生成する
   ことを特徴とする請求項１から請求項３のいずれか１項に記載のロボット制御装置。
5. 作成された前記複数の修正軌道候補を保存する記憶装置を備える
   ことを特徴とする請求項１から請求項４のいずれか１項に記載のロボット制御装置。
6. 前記衝突可能性推定部は、前記作業物体の移動速度を演算する移動速度演算部を備え、前記作業物体の前記移動速度に基づいて前記作業物体に対する衝突可能性を推定する
   請求項１から請求項５のいずれか一項に記載のロボット制御装置。
7. 前記衝突可能性推定部は、前記作業物体の前記移動速度に基づいて前記作業物体の移動先を推定し、推定された前記作業物体の前記移動先に基づいて衝突可能性を推定する
   請求項６に記載のロボット制御装置。
8. 前記監視対象部の動作範囲に配置された部材の位置情報および前記予測軌道情報を用いて、前記監視対象部が前記部材に衝突するか否かを判断する干渉判定部と
   を備え、
   前記軌道修正部は、前記干渉判定部の判定結果に応じて、前記指令軌道を修正する
   請求項１から請求項７のいずれか一項に記載のロボット制御装置。
9. 前記指令軌道に基づき前記監視対象部の移動を制御するロボット動作制御部をさらに備え、
   前記ロボット動作制御部は、
   前記監視対象部の前記現在位置および前記物***置情報に基づき、予め設定された制御周期ごとに衝突可能性を算出すると共に、前記算出された衝突可能性が予め設定された閾値を越えた場合には、再計画指令を生成し、
   前記軌道修正部は、前記再計画指令が入力された場合には、前記指令軌道を修正する
   請求項１から請求項８のいずれか一項に記載のロボット制御装置。
10. 前記指令軌道に基づき前記監視対象部の移動を制御するロボット動作制御部をさらに備え、
    前記監視対象部の前記現在位置は、前記ロボット動作制御部から出力された制御信号から導出されたものである
    請求項１から請求項８のいずれか一項に記載のロボット制御装置。
11. 請求項１から請求項１０のいずれか一項に記載のロボット制御装置と、
    前記ロボット制御装置により制御されて動作する前記ロボットと
    を備えたロボットシステム。

Images