JP2021194751A - 移動体制御装置、移動体制御方法、移動体制御プログラムおよび移動体制御システム - Google Patents

Abstract

【課題】移動体の移動制御処理を高速に実行し、高速で移動する障害物との衝突を適切に回避することができる移動体制御装置などを提供する。【解決手段】ロボットアーム１０と障害物との距離を取得する距離取得部２２と、ロボットアーム１０に対する障害物の相対速度を取得する相対速度取得部２３と、上記距離と距離の関数および上記相対速度と相対速度の関数に基づきポテンシャル場を算出するポテンシャル場算出部２４と、ポテンシャル場に基づき加速度ベクトルを算出する加速度算出部２５と、上記加速度ベクトルに基づいてロボットアーム１０の移動を制御する制御部２６とを備え、ロボットアーム１０が加速度ベクトルに基づいて所定時間移動する毎に、ロボットアーム１０の３次元位置座標の取得、距離の取得、相対速度の取得、ポテンシャル場の算出、加速度ベクトルの算出および移動体の移動の制御を順次行う。【選択図】図２

Description

本発明は、ロボットアームのような移動体の移動経路を制御する移動体制御装置、移動体制御方法、移動体制御プログラムおよび移動体制御システムに関するものである。

従来、工業製品の組み立て工程や商品の分別工程などにおいてロボットアームが用いられている。ロボットアームは、部品や商品などを把持したりするヘッド部を有し、そのヘッド部を所定の移動経路に沿って、目標の部品や商品などまで移動させる。

ロボットアームのヘッド部を目標の部品や商品まで移動させる際、ヘッド部から目標対象物まで直線的にヘッド部を移動させれば最短距離で到達することができ、作業効率が最も良いが、ヘッド部から目標対象物までの間に何らかの障害物が存在する場合がある。

このような障害物が存在する場合、ヘッド部が障害物に衝突しないように障害物を回避して移動する経路を求める必要がある。

たとえば特許文献１においては、ロボットアームのヘッド部が、目的対象物を把持可能な姿勢を維持しつつ、ヘッド部が障害物に衝突しないような移動経路を求めることが提案されている。

また、特許文献２においては、移動体の初期位置から到達目標地点までの初期経路計画を生成し、その初期経路計画と障害物の情報とに基づいて、ＲＲＴ（Rapidly-exploring Random Tree）法に基づく処理により、移動体と各障害物との干渉を回避し得るＲＲＴ経路
計画を生成し、そのＲＲＴ経路計画をポテンシャル法の処理により修正することが提案されている。

また、特許文献３においては、移動体のスタート位置からゴール位置までの間の第１中間地点を設定し、そのスタート位置から第１中間地点までの間に障害物がある場合には、その障害物を回避し得るようなその他の第２中間地点を設定し、その第２中間地点とゴール位置までの間に再び第３中間地点を設定し、第２中間地点と第３中間地点の間に障害物がある場合には、その障害物を回避し得るようなその他の第４中間地点を設定する。そして、特許文献３では、上述した中間地点の設定を繰り返し行うことによって、移動体が障害物に衝突しないような移動経路を生成することが提案されている。

国際公開第２０１８−０９２８６０号公報 特開２０１７−１５１６８７号公報 特開２０００−２０１１７号公報

しかしながら、特許文献１から特許文献３のいずれの方法も、移動体の初期位置から目標到達位置までの移動経路を座標情報として生成する処理を行っている。このように移動経路の座標情報を生成する処理は計算量が膨大となり、高速な処理が困難である。また、移動経路の座標情報に基づいて移動体の制御を行う場合、移動経路の座標情報から逆運動学に基づいて、移動体の速度や加速度を算出する必要があり、この演算処理に多くの時間を要し、高速な処理が困難となる。

特に、移動体が移動している間に、高速で移動体に向かって移動する障害物がある場合には、高速かつリアルタイムに移動制御処理を行わなければ、その障害物を回避することできない。

本発明は、上記の問題に鑑み、移動体の移動制御処理を高速かつリアルタイムに実行することができ、これにより高速で移動する障害物についても適切に衝突を回避することができる移動体制御装置、移動体制御方法、移動体制御プログラムおよび移動体制御システムを提供することを目的とする。

本発明の移動体制御装置は、所定位置における移動体の３次元位置座標を取得する３次元位置取得部と、３次元位置座標の位置の移動体と移動体の目標到達位置との間の所定範囲内に存在する障害物と移動体との距離を取得する距離取得部と、移動体に対する障害物の相対速度を取得する相対速度取得部と、距離取得部によって取得された距離と予め設定された距離に関する関数および相対速度取得部によって取得された相対速度と予め設定された相対速度に関する関数に基づいて、３次元位置座標におけるポテンシャル場を算出するポテンシャル場算出部と、ポテンシャル場に基づいて、３次元位置座標における移動体の加速度ベクトルを算出する加速度算出部と、加速度算出部によって算出された加速度ベクトルに基づいて、移動体の移動を制御する制御部とを備え、移動体が加速度ベクトルに基づいて所定時間移動する毎に、移動体の３次元位置座標の取得、距離の取得、相対速度の取得、ポテンシャル場の算出、加速度ベクトルの算出および移動体の移動の制御を順次行う。

また、上記本発明の移動体制御装置においては、移動体の移動開始後に、移動体の３次元位置座標の取得、距離の取得、相対速度の取得、ポテンシャル場の算出、加速度ベクトルの算出および移動体の移動の制御をリアルタイムに行うことができる。

また、上記本発明の移動体制御装置において、相対速度取得部は、移動体が移動中における障害物の相対速度を取得することができる。

また、上記本発明の移動体制御装置において、ポテンシャル場算出部は、移動体の移動開始後に、移動体と移動体の目標到達位置との間の所定範囲内に新たな障害物が現れた場合、その障害物の相対速度が予め設定された閾値以下である場合には、障害物のポテンシャル場を算出しないようにすることができる。

また、上記本発明の移動体制御装置において、ポテンシャル場算出部は、曲率項を有する演算式に基づいてポテンシャル場を算出することができる。

また、上記本発明の移動体制御装置において、移動体は、ロボットアームの可動部分とすることができる。

本発明の移動体制御方法は、所定位置における移動体の３次元位置座標を取得し、移動体の目標到達位置と３次元位置座標の位置の移動体との間の所定範囲内に存在する障害物と移動体との距離を取得し、移動体に対する障害物の相対速度を取得し、上記取得された距離と予め設定された距離に関する関数および上記取得された相対速度と予め設定された相対速度に関する関数に基づいて、３次元位置座標におけるポテンシャル場を算出し、ポテンシャル場に基づいて、３次元位置座標における移動体の加速度ベクトルを算出し、加速度ベクトルに基づいて、移動体の移動を制御する移動体制御方法であって、移動体が加速度ベクトルに基づいて所定時間移動する毎に、移動体の３次元位置座標の取得、距離の取得、相対速度の取得、ポテンシャル場の算出、加速度ベクトルの算出および移動体の移動の制御を順次行う。

本発明の移動体制御プログラムは、所定位置における移動体の３次元位置座標を取得するステップと、移動体の目標到達位置と３次元位置座標の位置の移動体との間の所定範囲内に存在する障害物と移動体との距離を取得するステップと、移動体に対する障害物の相対速度を取得するステップと、上記取得した距離と予め設定された距離に関する関数および上記取得した相対速度と予め設定された相対速度に関する関数に基づいて、３次元位置座標におけるポテンシャル場を算出するステップと、ポテンシャル場に基づいて、３次元位置座標における移動体の加速度ベクトルを算出するステップと、加速度ベクトルに基づいて、移動体の移動を制御するステップとをコンピュータに実行させる移動体制御プログラムであって、移動体が加速度ベクトルに基づいて所定時間移動する毎に、移動体の３次元位置座標の取得、距離の取得、相対速度の取得、ポテンシャル場の算出、加速度ベクトルの算出および移動体の移動の制御を順次実行させる。

本発明の移動体制御システムは、上記本発明の移動体制御装置と、移動体と、移動体と移動体の目標到達位置との間の所定範囲内に存在する障害物を検出する３次元センサとを備える。

本発明の移動体制御装置、方法およびプログラム並びに移動体制御システムによれば、移動体の移動制御処理を高速かつリアルタイムに実行することができ、これにより高速で移動する障害物についても適切に衝突を回避することができる。

本発明の移動体制御装置および移動体制御システムの一実施形態を用いたロボットアーム制御システムの概略構成を示す図 図１に示すロボットアーム制御装置の概略構成を示すブロック図 ロボットアームと障害物との間の距離に関する関数の一例を示す図 ロボットアームに対する障害物の相対速度に関する関数の一例を示す図 図１に示すロボットアーム制御システムの処理の流れを示すフローチャート 従来のロボットアームの移動経路と本発明を適用したロボットアームの移動経路とを比較するための図

以下、本発明の移動体制御装置および移動体制御システムの一実施形態を用いたロボットアーム制御システムについて、図面を参照しながら詳細に説明する。図１は、本実施形態のロボットアーム制御システム１の概略構成図である。

本実施形態のロボットアーム制御システム１は、図１に示すように、ロボットアーム１０と、ロボットアーム制御装置２０と、３次元センサ３０とを備えている。ロボットアーム１０とロボットアーム制御装置２０との間およびロボットアーム制御装置２０と３次元センサ３０との間は、有線または無線によって通信可能に接続されており、種々の信号のやり取りが可能に構成されている。なお、本実施形態では、ロボットアーム制御装置２０が本発明の移動体制御装置に相当する。

ロボットアーム１０は、ロボットアーム制御装置２０から出力された制御信号に応じて動作する。ロボットアーム１０としては、既に公知のものを使用することができ、垂直多関節、水平多関節および関節数などの構成、並びに工業用、医薬・医療用および搬送用などの用途は特に限定されない。たとえば３軸以上のロボットアーム１０や、６軸または７軸のロボットアーム１０を用いることができる。

本実施形態のロボットアーム１０は、物体を把持するなどの所定の作業を実行する機能を有するヘッド部１０ａと、ヘッド部１０ａに一端が接続され、ヘッド部１０ａを移動させるアーム部１０ｂとを備えている。本実施形態においては、ロボットアーム１０全体が、すなわちヘッド部１０ａとアーム部１０ｂとを含む可動部分全体が、本発明の移動体に相当する。

ロボットアーム制御装置２０は、ロボットアーム１０に対して制御信号を出力し、ロボットアーム１０のアーム部１０ｂの動作を制御することによって、ヘッド部１０ａの移動経路を制御する。

本実施形態のロボットアーム制御装置２０は、ロボットアーム１０が所定時間移動する毎に、その移動後のロボットアーム１０の位置での加速度ベクトルを順次算出することによってリアルタイムにロボットアーム１０の移動経路を生成する。従来のように、移動経路の座標情報を演算することなく、ロボットアーム１０の制御に直結する加速度ベクトルの演算を行うようにしたので、逆運動学に基づくロボットアーム１０の速度や加速度の演算処理を行う必要がない。したがって、ロボットアーム１０の制御処理を高速かつリアルタイムに実行することができるので、たとえば障害物の移動速度が速い場合でも、その障害物との衝突を適切に回避することができる。以下、本実施形態のロボットアーム制御装置２０の構成について説明する。

図２は、本実施形態のロボットアーム制御装置２０の概略構成を示すブロック図である。

ロボットアーム制御装置２０は、図２に示すように、３次元位置取得部２１と、距離取得部２２と、相対速度取得部２３と、ポテンシャル場算出部２４と、加速度算出部２５と、制御部２６とを備えている。

ロボットアーム制御装置２０は、たとえばコンピュータから構成されるものであり、ＣＰＵ（Central Processing Unit）、半導体メモリおよびハードディスクなどの記憶媒体並びに通信Ｉ／Ｆなどを備えている。

ロボットアーム制御装置２０の半導体メモリまたはハードディスクには、本発明の移動体制御プログラムの一実施形態を含むロボットアーム制御プログラムがインストールされている。そして、このロボットアーム制御プログラムが、制御部２６が有するＣＰＵによって実行されることによって、３次元位置取得部２１、距離取得部２２、相対速度取得部２３、ポテンシャル場算出部２４および加速度算出部２５が機能する。

なお、本実施形態においては、上述した各部の機能を全てロボットアーム制御プログラムによって実行するようにしたが、これに限らず、一部または全部の機能をＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field-Programmable Gate Array）、その他の電気回路などのハードウェアから構成するようにしてもよい。

３次元位置取得部２１は、ロボットアーム１０全体の３次元位置座標を取得する。具体的には、本実施形態の３次元位置取得部２１は、３次元センサ３０から出力された３次元の点群に基づいて、ロボットアーム１０全体、すなわちロボットアーム１０のヘッド部１０ａおよびアーム部１０ｂの表面上の多数の点群の３次元位置座標を取得する。ロボットアーム１０の３次元位置座標は、ロボットアーム１０が制御される３次元空間上における座標である。３次元位置取得部２１は、所定の時間間隔でロボットアーム１０の３次元位置座標を取得するものであり、本実施形態では、３０回／ｓの間隔でロボットアーム１０の３次元位置座標をリアルタイムに取得する。

距離取得部２２は、ロボットアーム１０の目標到達位置と現在位置のロボットアーム１０との間の所定範囲内に存在する障害物と、ロボットアーム１０との距離を取得する。なお、本実施形態においては、上記所定範囲内に存在する障害物には、ロボットアーム１０の移動開始以降の所定の時点では上記所定範囲内に存在していなかったが、所定の時点以降において上記所定範囲外から上記所定範囲内に進入してロボットアーム１０に接近してくる障害物も含むものとする。

そして、本実施形態の距離取得部２２は、３次元センサ３０から出力される３次元の点群に基づいて、現在位置のロボットアーム１０と上述した障害物との距離をリアルタイムに取得する。本実施形態において、ロボットアーム１０と障害物との距離とは、ロボットアーム１０と障害物との間の最短距離である。具体的には、本実施形態の距離取得部２２は、たとえば３次元位置取得部２１によって取得されたロボットアーム１０全体の３次元位置座標と、障害物の表面上の多数の点群の３次元位置座標との組み合わせのうち、距離が最短となる点同士間の距離を取得する。

なお、障害物は１つに限らず、複数存在する場合もある。距離取得部２２は、各障害物とロボットアーム１０との距離をそれぞれ取得する。

また、距離取得部２２は、３次元位置取得部２１と同様に、所定の時間間隔で距離を取得するものであり、本実施形態では、３０回／ｓの間隔で距離を取得する。

相対速度取得部２３は、ロボットアーム１０に対する上述した障害物の相対速度をリアルタイムに取得する。本実施形態の相対速度取得部２３は、３次元センサ３０から出力された３次元の点群に基づいて、ロボットアーム１０の移動速度Ｖａおよび障害物の移動速度Ｖｂを算出し、その障害物の移動速度Ｖｂからロボットアーム１０の移動速度Ｖａを減算することによって障害物の相対速度を算出する。本実施形態では、ロボットアーム１０の移動速度Ｖａの方向を正とする。すなわち、ロボットアーム１０に対して障害物が近づいている場合には、障害物の移動速度Ｖｂは負となり、ロボットアーム１０から障害物が離れている場合には、障害物の移動速度Ｖｂは正となる。

具体的には、本実施形態の相対速度取得部２３は、ロボットアーム１０の移動速度Ｖａについては、距離取得部２２において最短距離を取得する際に使用したロボットアーム１０の表面上の点（以下、アーム側最近接点という）の移動距離に基づいて、ロボットアーム１０の移動速度Ｖａを算出する。具体的には、現時点と現時点から１／３０秒前の時点との間における上記アーム側最近接点の移動距離を求め、その移動距離を１／３０秒で除算することによって、ロボットアーム１０の移動速度Ｖａを算出する。

また、相対速度取得部２３は、障害物の移動速度Ｖｂについては、距離取得部２２において最短距離を取得する際に使用した障害物の表面上の点（以下、障害物側最近接点という）の移動距離に基づいて、障害物の移動速度Ｖｂを算出する。具体的には、現時点と現時点から１／３０秒前の時点との間における上記障害物側最近接点の移動距離を求め、その移動距離を１／３０秒で除算することによって、障害物の移動速度Ｖｂを算出する。

相対速度取得部２３も、距離取得部２２と同様に、障害物が複数存在する場合には、各障害物の相対速度をそれぞれ取得する。また、障害物が静止している場合には、移動速度Ｖｂはゼロとする。

また、相対速度取得部２３も、距離取得部２２と同様に、所定の時間間隔で相対速度を取得するものであり、本実施形態では、３０回／ｓの間隔で相対速度を取得する。

ポテンシャル場算出部２４は、距離取得部２２によって取得された距離と上記距離に関する予め設定された関数および相対速度取得部２３によって取得された相対速度と上記相対速度に関する予め設定された関数に基づいて、現時点のロボットアーム１０の３次元位置座標におけるポテンシャル場を算出する。以下、本実施形態におけるポテンシャル場の具体的な算出方法について説明する。

本実施形態のポテンシャル場ｍ（ｓ，ｖ）は、たとえば下式（１）によって算出される。下式（１）は、３次元空間の関数であり、ポテンシャル場ｍ（ｓ，ｖ）は、３次元空間での３次元ポテンシャル場である。下式（１）におけるｓは、距離取得部２２によって取得された距離であり、ｗ（ｓ）は、距離ｓに関する予め設定された関数であり、ｖは、相対速度取得部２３によって取得された相対速度であり、ｕ（ｖ）は、相対速度に関する予め設定された関数である。

上式（１）におけるｗ（ｓ）は、距離ｓに対して単調減少する関数（ｄｗ／ｄｓ≦０）であれば如何なる関数でも良いが、たとえば下式（２）で表される。図３は、下式（２）で表されるｗ（ｓ）の一例を示す図である。具体的には、ｗ（ｓ）は、ロボットアーム１０と障害物との距離ｓが大きくなるほどゼロに近づく関数であって、ｓがｒｗ以上となったときにゼロとなる。すなわち、ｗ（ｓ）は、ロボットアーム１０と障害物との距離ｓが大きくなるほどポテンシャル場が小さくなり、ロボットアーム１０と障害物との距離ｓが小さくなるほどポテンシャル場が大きくなるように設定された関数である。

また、上式（１）におけるｕ（ｖ）は、たとえば下式（３）で表される。図４は、下式（３）で表されるｕ（ｖ）の一例を示す図である。具体的には、ｕ（ｖ）は、ロボットアーム１０に対する障害物の相対速度ｖが正の場合には、すなわちロボットアーム１０に対して障害物が離れる方向に移動している場合にはゼロである。一方、ｕ（ｖ）は、ロボットアーム１０に対する障害物の相対速度ｖが負の場合には、すなわちロボットアーム１０に対して障害物が近づく方向に移動している場合には、ｖの大きさに応じて単調減少する。すなわち、ｕ（ｖ）は、ロボットアーム１０に対して障害物が離れる方向に移動している場合には、ポテンシャル場がゼロとなり、ロボットアーム１０に対して障害物が近づく方向に移動している場合には、相対速度の絶対値が大きくなるほどポテンシャル場が大きくなるように設定された関数である。なお、下式（３）におけるσは、図４に示すグラフのスロープの勾配を決定するものであり、すなわち各相対速度に対する重み付けを決定するものである。σは、用途などに応じて任意に設定される。

上式（１）における第１項であるｗ（ｓ）・ｕ（ｖ）は、上述したように距離ｓおよび相対速度ｖに応じたポテンシャル場を算出するための項である。本実施形態では、相対速度も考慮してポテンシャル場を算出することによって、ロボットアーム１０が、移動中の障害物を適切に回避することができる。

また、第２項は、曲率項である。この曲率項を設けることによって、ロボットアーム１０が障害物を回り込んで避けつつ、目標到達位置に向かっていくようなポテンシャル場を算出することができる。

ポテンシャル場ｍ（ｓ，ｖ）は、上式（１）に示すように、ｗ（ｓ）と、相対速度ｖの関数であるδ（ｖ）で表すことができる。したがって、実際は、関数ｗ（ｓ）と関数δ（ｖ）とが予め設定されており、距離の関数と相対速度の関数の掛け合わせにより、簡易かつ高速に演算することができる。

また、ポテンシャル場算出部２４も、距離取得部２２および相対速度取得部２３と同様に、所定の時間間隔でポテンシャル場を算出するものであり、本実施形態では、３０回／ｓの間隔でポテンシャル場を算出する。すなわち、ポテンシャル場算出部２４は、ロボットアーム１０の移動に応じて、３０回／ｓの間隔でポテンシャル場を順次算出する。

加速度算出部２５は、ポテンシャル場算出部２４によって算出されたポテンシャル場に基づいて、各３次元位置座標におけるロボットアーム１０の加速度ベクトルを算出する。本実施形態の加速度算出部２５は、ポテンシャル場算出部２４によって算出されるポテンシャル場ｍ（ｓ，ｖ）を偏微分することによって加速度ベクトルａを算出する。下式（４）は、運動方程式に基づく加速度ベクトルａの算出式である。加速度ベクトルａは、３次元空間上における３次元の加速度ベクトルである。下式（４）におけるФは、微分可能な関数であり、ξは、非線形減速項であり、ロボットアーム１０が障害物に近づくと減速させ、障害物から離れると消滅する項である。

加速度算出部２５は、ポテンシャル場算出部２４においてポテンシャル場が算出される毎に加速度ベクトルを算出する。すなわち、加速度算出部２５も、本実施形態では、３０回／ｓの時間間隔で加速度ベクトルを算出する。加速度算出部２５によって算出された加速度ベクトルは、制御部２６に順次出力される。

制御部２６は、加速度算出部２５によって算出された加速度ベクトルに基づく制御信号を生成し、その制御信号をロボットアーム１０に出力する。ロボットアーム１０は、入力された制御信号に応じてアーム部１０ｂを駆動させ、これによりヘッド部１０ａが上記加速度ベクトルに応じた加速度および方向で移動する。

そして、制御部２６の制御によってロボットアーム１０が加速度ベクトルに応じて移動する毎に、その移動後のロボットアーム１０の３次元位置座標でのポテンシャル場の算出およびそのポテンシャル場に基づく加速度ベクトルの算出が行われる。

すなわち、ロボットアーム１０が移動する毎に、そのロボットアーム１０の３次元位置座標でのポテンシャル場および加速度ベクトルの算出が行われ、その順次算出された加速度ベクトルに基づいて、ロボットアーム１０が順次移動する。

本実施形態では、このようにロボットアーム１０の所定時間の移動毎に加速度ベクトルを算出することによってロボットアーム１０の移動経路を制御するようにしたので、従来のように、移動経路の座標情報を演算することなく、また逆運動学に基づくロボットアーム１０の速度や加速度の演算処理を行う必要がない。したがって、上述したようにロボットアーム１０の制御処理を高速に実行することができる。そして、ロボットアーム１０の経路生成を従来のようにロボットアーム１０の移動開始前に全て行うのではなく、ロボットアーム１０が移動しながら１/３０毎にリアルタイムで経路を生成しているので、ロボットアーム１０の移動の途中において障害物がロボットアームに向かってきても、それを臨機応変に避けることができる。

また、本実施形態においては、ロボットアーム１０全体の表面上の点群を用いてポテンシャル場および加速度ベクトルを算出するようにしたので、ロボットアーム１０のヘッド部１０ａに限らず、たとえばアーム部１０ｂに対して接近してくる障害物との衝突もより確実に回避することができる。

本実施形態では、上述したように多関節（たとえば３軸以上）のロボットアーム１０を用いることができ、この場合でも、ロボットアーム１０全体の表面上の点群を用いてポテンシャル場および加速度ベクトルを算出するようにしたので、障害物をリアルタイムに回避することができる。

３次元センサ３０は、ロボットアーム１０を含む所定の３次元空間を、たとえばステレオカメラによって撮影し、その撮影画像に基づいて、上記３次元空間内の３次元の点群を生成して出力する。３次元センサ３０としては、たとえば撮像素子、フォーカスレンズ等の光学レンズおよび画像処理プロセッサ（たとえばＧＰＵ（Graphics Processing Unit））などを有する３次元視覚センサを用いることができる。

なお、３次元センサ３０は、その撮影範囲内において障害物による死角が生じないような位置に設置される。３次元センサ３０は、たとえばロボットアーム１０の上方に配置されるが、その数は１つでなくてもよく、複数設けるようにしてもよい。また、３次元センサ３０の撮影範囲は、ロボットアーム１０が移動可能な最大の範囲に設定することが好ましいが、これに限らず、ロボットアーム１０の目標到達位置を含み、上記最大の範囲よりも狭い範囲に設定するようにしてもよい。

次に、本実施形態のロボットアーム制御システム１の処理の流れについて、図５に示すフローチャートを参照しながら、より詳細に説明する。

まず、３次元センサ３０によって上述した所定の撮影範囲が撮影され（Ｓ１０）、撮影範囲の３次元の点群が、３次元センサ３０からロボットアーム制御装置２０に出力される。

３次元センサ３０から出力された３次元の点群は、距離取得部２２によって取得される（Ｓ１２）。距離取得部２２は、入力された３次元の点群を、上記撮影範囲内に含まれる障害物などの物体毎にグルーピングする（Ｓ１４）。

一方、３次元位置取得部２１が、３次元センサ３０から出力された３次元の点群に基づいて、現時点のロボットアーム１０の３次元位置座標を取得する（Ｓ１６）。

また、相対速度取得部２３においてロボットアーム１０の移動速度Ｖａが算出される（Ｓ１８）。本実施形態の相対速度取得部２３は、上述したように３次元位置取得部２１によって取得されたロボットアーム１０の３次元位置座標に基づいて、ロボットアーム１０の移動速度Ｖａを算出する。

次に、距離取得部２２によってロボットアーム１０全体と障害物との距離が取得される（Ｓ２０）。具体的には、距離取得部２２は、３次元位置取得部２１によって取得されたロボットアーム１０全体の３次元位置座標と、Ｓ１４にてグルーピングされた各障害物の３次元の点群とを用いて、ロボットアーム１０と障害物との距離を取得する。

また、相対速度取得部２３が、ロボットアーム１０に対する障害物の相対速度を算出する（Ｓ２２）。相対速度取得部２３は、３次元センサ３０の撮影範囲内に含まれる障害物の移動速度Ｖｂから、Ｓ１８で算出されたロボットアーム１０の移動速度Ｖａを減算することによって相対速度を算出する。

次に、距離取得部２２によって取得された距離と相対速度取得部２３によって取得された相対速度がポテンシャル場算出部２４に入力され、ポテンシャル場算出部２４は、入力された距離および相対速度と上式（１）に基づいて、ロボットアーム１０の現在の３次元位置座標におけるポテンシャル場を算出する。なお、ロボットアーム１０と目標到達位置との間の所定範囲内に複数の障害物が存在する場合には、各障害物についてポテンシャル場が算出される。

また、ポテンシャル場算出部２４は、ロボットアーム１０と目標到達位置との距離と上式（１）に基づいて、ポテンシャル場を算出する。そして、ポテンシャル場算出部２４は、各障害物に対して算出されたポテンシャル場と、目標到達位置に対して算出されたポテンシャル場とを加算して、最終的なポテンシャル場を算出する（Ｓ２４）。

ポテンシャル場算出部２４によって算出された最終的なポテンシャル場は加速度算出部２５に入力され、加速度算出部２５は、上式（３）に示すように、入力されたポテンシャル場を偏微分することによって加速度ベクトルを算出する（Ｓ２６）。

加速度算出部２５によって算出された加速度ベクトルは制御部２６に入力され、制御部２６は、入力された加速度ベクトルに基づいて制御信号を生成し、その制御信号をロボットアーム１０に出力する（Ｓ２８）。

ロボットアーム１０は、入力された制御信号に基づいてアーム部１０ｂを駆動させることによってヘッド部１０ａを移動させる（Ｓ３０）。そして、制御部２６は、所定時間移動後（本実施形態では１／３０秒移動後）のロボットアーム１０の３次元位置座標を算出する（Ｓ３２）。

次いで、Ｓ３２において算出されたロボットアーム１０の３次元位置座標に基づいて、制御部２６によってロボットアーム１０が目標到達位置に到達したか否かが判定され（Ｓ３４）、目標到達位置に到達していない場合には（Ｓ３４，ＮＯ）、Ｓ１０〜Ｓ３２までの処理が繰り返して行われる。すなわち、ロボットアーム１０の３次元位置座標の取得、ロボットアーム１０と各障害物との距離の取得、ロボットアーム１０に対する各障害物の相対速度の取得、ポテンシャル場の算出、加速度ベクトルの算出、加速度ベクトルに基づくロボットアーム１０の移動およびその移動後のロボットアーム１０の３次元位置座標の算出が繰り返して行われる。

一方、ロボットアーム１０が目標到達位置に到達した場合には（Ｓ３４，ＹＥＳ）、ロボットアーム１０の移動経路算出処理を終了する。

図６は、上述したようにロボットアーム１０を制御した場合におけるロボットアーム１０の移動経路の一例を模式的に示す図である。図６における移動経路Ｉが、本実施形態の移動制御による移動経路の一例であり、移動経路ＩＩおよび移動経路ＩＩＩは、従来の移動制御による移動経路の一例である。なお、図６では、２次元的な移動経路を示しているが、本実施形態による移動経路は、３次元空間上の移動経路である。従来の移動経路ＩＩは、障害物の移動（図６に示す矢印方向への移動）を考慮していない場合の移動経路であり、この場合、移動前の障害物の回避だけしか考慮されていないので、ロボットアーム１０と移動後の障害物とが衝突する可能性がある。また、従来の移動経路ＩＩＩは、移動前の障害物と移動後の障害物とを考慮した移動経路であるが、本実施形態のようにロボットアーム１０に対する障害物の相対速度をリアルタイムに反映していないので、無駄に長い移動経路となり、ロボットアーム１０の処理効率の低下を招く。

これに対し、本実施形態の移動経路Ｉは、ロボットアーム１０に対する障害物の相対速度をリアルタイムに反映した移動経路なので、移動する障害物との衝突を回避しつつ、短い移動経路で目標到達位置までロボットアーム１０を移動させることができる。

また、上記実施形態のロボットアーム制御システム１においては、上述したように３次元センサ３０の撮影範囲内に存在する障害物を検出し、その障害物とロボットアーム１０との距離およびロボットアーム１０に対する障害物の相対速度に基づいて、ポテンシャル場を算出するようにしているが、障害物としては、上述したようにロボットアーム１０の移動開始時から３次元センサ３０の撮影範囲内に存在する障害物と、ロボットアーム１０の移動開始後に３次元センサ３０の撮影範囲内に移動して進入してくる障害物とがある。

そして、本実施形態では、上述した種々の障害物を３次元センサ３０によって検出し、ロボットアーム１０が衝突しないようなポテンシャル場および加速度ベクトルを算出する。この際、ロボットアーム１０の移動開始後に３次元センサ３０の撮影範囲内に進入してくる障害物について、ロボットアーム１０に対する相対速度が遅い障害物については、ロボットアーム１０に衝突する可能性が低い。

そこで、たとえばポテンシャル場算出部２４が、ロボットアーム１０の移動開始後に、３次元センサ３０の撮影範囲内に新たな障害物が現れた場合、その障害物の相対速度が予め設定された閾値以下である場合には、障害物のポテンシャル場を算出しないようにしてもよい。これにより、ポテンシャル場の演算処理の負荷を軽減することができるので、より高速な処理が可能となる。

また、上記実施形態においては、ロボットアーム１０全体と障害物との間で最短距離となる２点（アーム側最近接点と障害物側最近接点）の移動速度に基づいて相対速度を算出し、これらの点同士におけるポテンシャル場を算出しているが、たとえば障害物の表面上の多数の点群の中で、障害物側最近接点よりも速い移動速度でロボットアーム１０に近づいてくる点が存在する場合がある。このような場合、ロボットアーム１０への障害物の衝突をより確実に回避するためには、この点についてもポテンシャル場を考慮することが好ましい。

したがって、たとえば障害物側最近接点以外の障害物の表面上の点であって、アーム側最近接点に対する相対速度の絶対値が、予め設定された閾値を超える点が存在する場合には、その点の相対速度とアーム側最近接点との距離に基づいて、上式（１）からポテンシャル場を算出し、そのポテンシャル場をさらに加算して最終的なポテンシャル場を算出するようにしてもよい。

また、上記実施形態のロボットアーム制御システム１は、本発明の移動体制御装置および移動体制御システムにおける制御対象の移動体をロボットアーム１０としたものであるが、移動体としては、ロボットアーム１０に限らない。本発明の移動体制御装置および移動体制御システムは、自律型移動ロボットやドローンにも適用でき、さらに、フォークリフト、並びに自動車および自動二輪車などの車両に適用することでいわゆる運転支援技術、自動運転技術にも適用できる。すなわち、制御対象の移動体は、ロボットアーム１０に限らず、各種の移動体に適用できる。

１ ロボットアーム制御システム
１０ ロボットアーム
１０ａ ヘッド部
１０ｂ アーム部
２０ ロボットアーム制御装置
２１ ３次元位置取得部
２２ 距離取得部
２３ 相対速度取得部
２４ ポテンシャル場算出部
２５ 加速度算出部
２６ 制御部
３０ ３次元センサ

Claims (9)

1. 所定位置における移動体の３次元位置座標を取得する３次元位置取得部と、
   前記３次元位置座標の位置の前記移動体と前記移動体の目標到達位置との間の所定範囲内に存在する障害物と前記移動体との距離を取得する距離取得部と、
   前記移動体に対する前記障害物の相対速度を取得する相対速度取得部と、
   前記距離取得部によって取得された距離と予め設定された前記距離に関する関数および前記相対速度取得部によって取得された相対速度と予め設定された前記相対速度に関する関数に基づいて、前記３次元位置座標におけるポテンシャル場を算出するポテンシャル場算出部と、
   前記ポテンシャル場に基づいて、前記３次元位置座標における前記移動体の加速度ベクトルを算出する加速度算出部と、
   前記加速度算出部によって算出された加速度ベクトルに基づいて、前記移動体の移動を制御する制御部とを備え、
   前記移動体が前記加速度ベクトルに基づいて所定時間移動する毎に、前記移動体の前記３次元位置座標の取得、前記距離の取得、前記相対速度の取得、前記ポテンシャル場の算出、前記加速度ベクトルの算出および前記移動体の移動の制御を順次行う移動体制御装置。
2. 前記移動体の移動開始後に、前記移動体の前記３次元位置座標の取得、前記距離の取得、前記相対速度の取得、前記ポテンシャル場の算出、前記加速度ベクトルの算出および前記移動体の移動の制御をリアルタイムに行う請求項１記載の移動体制御装置。
3. 前記相対速度取得部が、前記移動体が移動中における前記障害物の相対速度を取得する請求項１または２記載の移動体制御装置。
4. 前記ポテンシャル場算出部が、前記移動体の移動開始後に、前記移動体と前記移動体の目標到達位置との間の所定範囲内に新たな障害物が現れた場合、該障害物の前記相対速度が予め設定された閾値以下である場合には、前記障害物の前記ポテンシャル場を算出しない請求項１から３いずれか１項記載の移動体制御装置。
5. 前記ポテンシャル場算出部が、曲率項を有する演算式に基づいて前記ポテンシャル場を算出する請求項１から４いずれか１項記載の移動体制御装置。
6. 前記移動体が、ロボットアームの可動部分である請求項１から５いずれか１項記載の移動体制御装置。
7. 所定位置における移動体の３次元位置座標を取得し、
   前記移動体の目標到達位置と前記３次元位置座標の位置の前記移動体との間の所定範囲内に存在する障害物と前記移動体との距離を取得し、
   前記移動体に対する前記障害物の相対速度を取得し、
   前記取得した距離と予め設定された前記距離に関する関数および前記取得した相対速度と予め設定された前記相対速度に関する関数に基づいて、前記３次元位置座標におけるポテンシャル場を算出し、
   前記ポテンシャル場に基づいて、前記３次元位置座標における前記移動体の加速度ベクトルを算出し、
   前記加速度ベクトルに基づいて、前記移動体の移動を制御する移動体制御方法であって、
   前記移動体が前記加速度ベクトルに基づいて所定時間移動する毎に、前記移動体の前記３次元位置座標の取得、前記距離の取得、前記相対速度の取得、前記ポテンシャル場の算出、前記加速度ベクトルの算出および前記移動体の移動の制御を順次行う移動体制御方法。
8. 所定位置における移動体の３次元位置座標を取得するステップと、
   前記移動体の目標到達位置と前記３次元位置座標の位置の前記移動体との間の所定範囲内に存在する障害物と前記移動体との距離を取得するステップと、
   前記移動体に対する前記障害物の相対速度を取得するステップと、
   前記取得した距離と予め設定された前記距離に関する関数および前記取得した相対速度と予め設定された前記相対速度に関する関数に基づいて、前記３次元位置座標におけるポテンシャル場を算出するステップと、
   前記ポテンシャル場に基づいて、前記３次元位置座標における前記移動体の加速度ベクトルを算出するステップと、
   前記加速度ベクトルに基づいて、前記移動体の移動を制御するステップとをコンピュータに実行させる移動体制御プログラムであって、
   前記移動体が前記加速度ベクトルに基づいて所定時間移動する毎に、前記移動体の前記３次元位置座標の取得、前記距離の取得、前記相対速度の取得、前記ポテンシャル場の算出、前記加速度ベクトルの算出および前記移動体の移動の制御を順次実行させる移動体制御プログラム。
9. 請求項１から６いずれか１項記載の移動体制御装置と、
   前記移動体と、
   前記移動体と前記移動体の目標到達位置との間の所定範囲内に存在する障害物を検出する３次元センサとを備えた移動体制御システム。

Images