特開 2024-21345 予測システム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024021345

(43)【公開日】2024-02-16

(54)【発明の名称】予測システム

(51)【国際特許分類】

   B25J 19/06 20060101AFI20240208BHJP

【ＦＩ】

B25J19/06

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2022124108

(22)【出願日】2022-08-03

(71)【出願人】

【識別番号】000005197

【氏名又は名称】株式会社不二越

(74)【代理人】

【識別番号】100176072

【弁理士】

【氏名又は名称】小林 功

(74)【代理人】

【識別番号】100169225

【弁理士】

【氏名又は名称】山野 明

(72)【発明者】

【氏名】今岡 誠

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707BS10

3C707KS03

3C707KS04

3C707KS06

3C707KS07

3C707KT03

3C707KT06

3C707KT11

3C707MS08

3C707MT09

(57)【要約】

【課題】低負荷で物体の形状が変化した場合にも物体の衝突を予測できる予測システムを提供する。
【解決手段】予測システムは、画像内の複数の物体を抽出し、物体の重心を算出し、今回及び前回算出した重心のうち最も座標が近い重心同士を同じ物体として特定し、特定された第一物体及び第二物体の間の距離が所定の条件を満たす第一物体の第一座標及び第二物体の第二座標を算出し、第一物体の重心から第一座標に向う第一ベクトル並びに第二物体の重心から第二座標に向う第二ベクトルを算出し、第一ベクトル及び第二ベクトルの変化から第一物体及び第二物体の形状変化を判定し、判定が否定判定である場合に第一座標及び第二座標の変化から第一物体及び第二物体の移動量及び方向を算出し、移動量及び方向から第一物体及び第二物体の移動速度を算出し、移動速度及び方向から第一物体及び第二物体が衝突するか否かを予測する。
【選択図】図２

【特許請求の範囲】

【請求項1】

３Ｄセンサが撮像した画像を取得し前記画像内の複数の物体を抽出する抽出部と、
前記物体の重心の座標を一定時間毎に算出する重心算出部と、
今回算出した前記重心の座標と前回算出した前記重心の座標とを比較し、最も座標が近い前記重心の座標同士を同じ前記物体として特定する特定部と、
特定された複数の前記物体のうちの第一物体及び第二物体に対して、お互いの間の距離が所定の条件を満たすような前記第一物体の外周の座標である第一座標及び前記第二物体の外周の座標である第二座標を算出する座標算出部と、
前記第一物体の前記重心の座標から前記第一座標に向う第一ベクトル及び前記第二物体の前記重心の座標から前記第二座標に向う第二ベクトルを算出するベクトル算出部と、
前記第一ベクトル及び前記第二ベクトルの変化に基づいて、前記第一物体と前記第二物体とのうちのいずれかの形状が変化したか否かを判定する形状変化判定部と、
前記判定が否定判定である場合、前記第一座標及び前記第二座標の前記一定時間における変化に基づいて前記第一物体及び前記第二物体の移動量及び移動方向を算出する移動情報算出部と、
前記移動情報算出部の算出結果に基づいて前記第一物体及び前記第二物体の移動速度を算出し、前記第一物体及び前記第二物体の移動速度及び移動方向に基づいて所定時間の経過後に前記第一物体及び前記第二物体が衝突するか否かを予測する衝突予測部と、
を備えることを特徴とする予測システム。

【請求項2】

可動である前記第一物体を含むロボットと、
前記ロボットの動作を制御するロボット制御装置と、
前記衝突予測部の予測が肯定である場合に、前記ロボットの動作を停止させる命令を前記ロボット制御装置に送信する停止命令部と、
をさらに備えることを特徴とする請求項１に記載の予測システム。

【請求項3】

前記所定の条件は、前記お互いの間の距離が最短となることであることを特徴とする請求項１に記載の予測システム。

【請求項4】

前記衝突予測部は、前記第一物体及び前記第二物体の移動速度及び移動方向に基づいて前記所定時間後の前記第一座標及び前記第二座標を算出し、現在の前記第一座標及び前記所定時間後の前記第一座標を結ぶ線分並びに現在の前記第二座標及び前記所定時間後の前記第二座標を結ぶ線分の間の最短距離が所定距離以下であるか否かを判定し、当該判定が肯定判定である場合に前記所定時間の経過後に前記第一物体及び前記第二物体が衝突すると予測することを特徴とする請求項１に記載の予測システム。

【請求項5】

前記形状変化判定部は、前記第一ベクトルの大きさ及び方向の変化が所定の値又は所定の割合を上回る場合に前記第一物体の形状が変化したと判定するとともに、前記第二ベクトルの大きさ又は方向の変化が所定の値及び所定の割合を上回る場合に前記第二物体の形状が変化したと判定することを特徴とする請求項１に記載の予測システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、物体の衝突を予測する予測システムに関する。

【背景技術】

【0002】

従来より、物体の位置及び速度に基づいて、所定の時間が経過した後に物体が衝突するか否かを予測する予測システム（例えば特許文献１など）が知られている。

【0003】

特許文献１に記載の予測システムでは、物体の位置や速度に関する情報は取得するものの、物体の輪郭や形状などの情報については取得しないため、衝突を予測する精度が不十分である場合があった。

【0004】

これに関し、特許文献２には、測定対象物を載置させる載置面を有し該載置面を回転移動させるための回転ステージ及び載置面を平行移動させるための並進ステージを有する載置部と、測定対象物の立体形状を表す点群データを生成する生成部と、載置面の移動により載置された測定対象物が衝突する事態を回避するための衝突回避情報の入力を受け付ける入力部と、入力部により入力された衝突回避情報に基づいて、載置部の移動による衝突の回避を指示する指示部とを備える測定装置が開示されている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特許７００４４７５号公報

【特許文献2】特開２０２１－０２５９１１号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

しかしながら、特許文献２に記載の技術では、物体の衝突の予測を行う度に、物体の特定の為に点群データ同士を比較するマッチング処理を行う必要がある。点群データ同士を比較するマッチング処理では、大量のデータの処理を行う必要がある。したがって、上述した技術では、予測システムに高い負荷がかかってしまうという問題があった。

【0007】

また、特許文献２に記載の技術では、物体の形状が変化した場合に、物体の特定が行えない可能性があるという問題があった。

【0008】

本発明はこのような問題に鑑みてなされたものであり、その目的は、低負荷で、物体の形状が一時的に変化する場合にも物体の衝突を予測できる予測システムを提供することにある。

【課題を解決するための手段】

【0009】

上記課題を解決するために、本発明の予測システムは、３Ｄセンサが撮像した画像を取得し前記画像内の複数の物体を抽出する抽出部と、前記物体の重心の座標を一定時間毎に算出する重心算出部と、今回算出した前記重心の座標と前回算出した前記重心の座標とを比較し、最も座標が近い前記重心の座標同士を同じ前記物体として特定する特定部と、特定された複数の前記物体のうちの第一物体及び第二物体に対して、お互いの間の距離が所定の条件を満たすような前記第一物体の外周の座標である第一座標及び前記第二物体の外周の座標である第二座標を算出する座標算出部と、前記第一物体の前記重心の座標から前記第一座標に向う第一ベクトル及び前記第二物体の前記重心の座標から前記第二座標に向う第二ベクトルを算出するベクトル算出部と、前記第一ベクトル及び前記第二ベクトルの変化に基づいて、前記第一物体と前記第二物体とのうちのいずれかの形状が変化したか否かを判定する形状変化判定部と、前記判定が否定判定である場合、前記第一座標及び前記第二座標の前記一定時間における変化に基づいて前記第一物体及び前記第二物体の移動量及び移動方向を算出する移動情報算出部と、前記移動情報算出部の算出結果に基づいて前記第一物体及び前記第二物体の移動速度を算出し、前記第一物体及び前記第二物体の移動速度及び移動方向に基づいて所定時間の経過後に前記第一物体及び前記第二物体が衝突するか否かを予測する衝突予測部と、を備える。

【0010】

また、本発明のシステムは、可動である前記第一物体を含むロボットと、前記ロボットの動作を制御するロボット制御装置と、前記衝突予測部の予測が肯定である場合に、前記ロボットの動作を停止させる命令を前記ロボット制御装置に送信する停止命令部と、をさらに備える。

【0011】

また、前記所定の条件は、前記お互いの間の距離が最短となることである。

【0012】

また、前記衝突予測部は、前記第一物体及び前記第二物体の移動速度及び移動方向に基づいて前記所定時間後の前記第一座標及び前記第二座標を算出し、現在の前記第一座標及び前記所定時間後の前記第一座標を結ぶ線分並びに現在の前記第二座標及び前記所定時間後の前記第二座標を結ぶ線分の間の最短距離が所定距離以下であるか否かを判定し、当該判定が肯定判定である場合に前記所定時間の経過後に前記第一物体及び前記第二物体が衝突すると予測する。

【0013】

また、前記形状変化判定部は、前記第一ベクトルの大きさ及び方向の変化が所定の値又は所定の割合を上回る場合に前記第一物体の形状が変化したと判定し、前記第二ベクトルの大きさ及び方向の変化が所定の値又は所定の割合を上回る場合に前記第二物体の形状が変化したと判定する。

【発明の効果】

【0014】

本発明によれば、予測システムは、低負荷で、物体の形状が一時的に変化する場合にも物体の衝突を予測できる。

【図面の簡単な説明】

【0015】

【図1】第一実施形態に係る予測システムの構成を概略的に示す図である。

【図2】図１に示す予測システムの回路構成を示す図である。

【図3A】図１に示す予測システムにおける３Ｄセンサが撮像した画像の一例を示す図である。

【図3B】図１に示す予測システムにおける抽出部が画像から抽出した物体の点群データの一例を示す図である。

【図3C】図１に示す予測システムにおける座標算出部が算出した第一座標及び第二座標の一例を示す図である。

【図3D】図１に示す予測システムにおけるベクトル算出部が算出した第一ベクトル及び第二ベクトルの一例を示す図である。

【図4A】図１に示す予測システムにおいて物体の形状が変化していない場合の第一ベクトル及び第二ベクトルの一例を示す図である。

【図4B】図１に示す予測システムにおいて物体の形状が変化した場合の第一ベクトル及び第二ベクトルの一例を示す図である。

【図5】図１に示す予測システムにおける各物体の移動量及び移動方向の一例を示す図である。

【図6】図１に示す予測システムにおける衝突予測部の動作を説明するための図である。

【図7】図１に示す予測システムの一連の処理の流れを示すフローチャートである。

【発明を実施するための形態】

【0016】

以下、添付図面を参照しながら本発明の実施形態（以下、「本実施形態」という。）について説明する。説明の理解を容易にするため、各図面において同一の構成要素及びステップに対しては可能な限り同一の符号を付して、重複する説明は省略する。

【0017】

＜構成＞
図１は、第一実施形態に係る予測システム１の構成を概略的に示す図である。図１に示すように、予測システム１は、例えば、３Ｄセンサ１０と、予測装置２０と、ロボット３０と、ロボット制御装置４０とを含んで主要部が構成される。

【0018】

３Ｄセンサ１０は、例えば３Ｄカメラなどの撮像装置であり、第一物体ＯＢＪ１を有するロボット３０が設置されている空間ＡＲＥＡを撮像し、撮像した空間ＡＲＥＡの画像を予測装置２０に送信する。また、３Ｄセンサ１０は、空間ＡＲＥＡの上方に設置されている。なお、空間ＡＲＥＡには、ロボット３０の他に、例えば、第二物体ＯＢＪ２が配置されていたり、さらにその他の物体（図示せず）が配置されていたりする。なお、第二物体ＯＢＪ２やその他の物体は、例えば、ロボット３０とは異なるロボットや人間、工具、電子機器、治具などであるがこれに限られず、少なくとも一部が空間ＡＲＥＡ内に配置される物体であれば何であっても良い。

【0019】

予測装置２０は、例えば、デスクトップパソコンやノートパソコン、タブレットなどである。予測装置２０は、３Ｄセンサ１０から送信される画像から画像に映っている物体を定期的（例えば、サンプリング時間など一定時間毎に）にデータとして抽出し、抽出したデータがいずれの物体であるかを特定し、所定時間が経過した後に特定した物体同士が衝突するか否かを予測する。また、予測装置２０は、所定時間が経過した後に物体同士が衝突すると予測する場合、ロボット３０の動作を停止させるようにロボット制御装置４０に停止命令を送信する。

【0020】

ロボット３０は、ロボット制御装置４０の動作制御に従って、ワーク（図示せず）などに対して所定の処理作業を行うロボットである。また、ロボット３０は、空間ＡＲＥＡ内に設置される。ロボット３０は、例えば、基台３１と、多関節アーム３２とを含んで主要部が構成され、多関節アーム３２を動作させる。なお、多関節アーム３２の先端付近の一部は、３Ｄセンサ１０に撮像された画像から予測装置２０が物体を特定する際に、第一物体ＯＢＪ１として特定される。

【0021】

ロボット制御装置４０は、ロボット３０の動作を制御するための装置である。ロボット制御装置４０は、作業者によって入力される命令や内部に記憶しているプログラム、外部の装置から送信される命令などに従って、ロボット３０の動作を制御する。また、ロボット制御装置４０は、予測装置２０から送信される停止命令を受け付けて、当該停止命令に従ってロボット３０の動作を停止させる。

【0022】

図２は、図１に示す予測システム１の回路構成を示す図である。図２に示すように、予測装置２０は、例えば、抽出部２００と、重心算出部２０１と、特定部２０２と、座標算出部２０３と、ベクトル算出部２０４と、形状変化判定部２０５と、移動情報算出部２０６と、衝突予測部２０７と、停止命令部２０８と、出力部２０９と、記憶部２１０とを含んで主要部が構成される。

【0023】

抽出部２００は、３Ｄセンサ１０が撮像した画像を取得し、取得した画像内の複数の物体を抽出する。抽出部２００は、抽出した物体をデータとして記憶部２１０に記憶するとともに、重心算出部２０１に送信する。

【0024】

図３Ａ及び図３Ｂを参照しつつ、抽出部２００による物体の抽出の具体例について説明する。図３Ａは、図１に示す予測システム１における３Ｄセンサ１０が撮像した画像ＩＭＧの一例を示す図である。また、図３Ｂは、図１に示す予測システム１における抽出部２００が画像ＩＭＧから抽出した物体ＯＢＪα及びＯＢＪβの点群データの一例を示す図である。なお、本実施形態において、空間ＡＲＥＡを上方から見た場合に、横方向を示す軸をＸ軸、Ｘ軸に対して垂直であり縦方向を示す軸をＹ軸、Ｘ軸及びＹ軸に対して水浴であり高さ方向を示す軸をＺ軸とする。図３Ａに示すように、３Ｄセンサ１０によって撮像された画像ＩＭＧには、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が映っている。なお、本実施形態では、第一物体ＯＢＪ１はロボット３０の多関節アーム３２の先端付近の一部であるが、説明のために形状が直方体であるものと仮定している。

【0025】

抽出部２００は、画像ＩＭＧを取得し、取得した画像ＩＭＧに対して定期的に（例えばサンプリング時間などの一定時間毎に）画像処理を行うことによって、図３Ｂに示すような点群データを生成する。また、抽出部２００は、生成した点群データに対してグループ分けを行う。さらに、抽出部２００は、グループ分けを行った点群データをそれぞれ物体ＯＢＪα及びＯＢＪβとして抽出する。そして、抽出部２００は、今回抽出した物体ＯＢＪα及びＯＢＪβに対応する点群データを記憶部２１０に記憶するとともに、当該点群データを重心算出部２０１に送信する。ここで、点群データは、例えば、空間ＡＲＥＡ内における位置情報（Ｘ軸方向、Ｙ軸方向及びＺ軸方向の座標）や色に関する情報及び生成された時刻などを含む点データの集合である。

【0026】

図２に戻って、重心算出部２０１は、物体ＯＢＪα及びＯＢＪβの重心の座標を算出する。重心算出部２０１は、今回算出した物体ＯＢＪα及びＯＢＪ２の重心の座標を特定部２０２に送信するとともに記憶部２１０に記憶する。

【0027】

図３Ｂを参照して、重心算出部２０１による物体ＯＢＪα及びＯＢＪβの重心の座標の算出について説明する。図３Ｂに示すように、重心算出部２０１は、物体ＯＢＪα及びＯＢＪβに対応する点群データに対して、各点データの座標（Ｘ軸方向、Ｙ軸方向及びＺ軸方向の座標）の平均値を算出する。重心算出部２０１は、物体ＯＢＪαの各点データの座標の平均値を重心の座標ｃｇａ１として、物体ＯＢＪβの各点データの座標の平均値を重心の座標ｃｇｂ１とする。

【0028】

図２に戻って、特定部２０２は、重心算出部２０１が今回算出した重心の座標及び前回算出した重心の座標を比較し、最も座標が近い重心の座標同士を同じ物体として特定する。具体的には、特定部２０２は、記憶部２１０から重心算出部２０１が前回算出した各物体の重心の座標を取得し、取得した重心の座標（本例では、第一物体ＯＢＪ１及び第二物体ＯＢＪ２の座標とする）と、重心算出部２０１から送信される重心算出部２０１が今回算出した物体ＯＢＪα及びＯＢＪβの重心の座標とを比較する。続いて、特定部２０２は、重心算出部２０１から送信された今回重心算出部２０１が算出した重心の座標のそれぞれについて、記憶部２１０から取得した重心算出部２０１が前回算出した重心の座標のいずれが最も近いかを判定し、最も近い座標を重心算出部２０１が前回算出した重心の座標に対応付けられている物体として特定する。そして、特定部２０２は、今回算出した重心の座標に対応する特定された物体の情報を記憶部２１０に記憶するとともに、物体の特定を終えたことを示す信号を座標算出部２０３に送信する。一例を挙げると、特定部２０２は、重心算出部２０１から送信された物体ＯＢＪαの重心の座標に対して、記憶部２１０から取得した重心の座標のうちの第一物体ＯＢＪ１の重心の座標に最も近い場合、物体ＯＢＪαを第一物体ＯＢＪ１として特定し、物体ＯＢＪαが第一物体ＯＢＪ１であるという情報を記憶部２１０に記憶する。

【0029】

座標算出部２０３は、特定された複数の物体のうちの第一物体ＯＢＪ１及び第二物体ＯＢＪ２に対して、お互いの間の距離が所定の条件を満たすような第一物体ＯＢＪ１の外周の座標である第一座標ａ１及び第二物体ＯＢＪ２の外周の座標である第二座標ｂ１を算出する。ここで、所定の条件は、例えば、第一物体ＯＢＪ１及びＯＢＪ２のお互いの間の距離が最短となることである。また、座標算出部２０３は、今回算出した第一座標ａ１及び第二座標ｂ１を記憶部２１０に記憶するとともに、第一座標ａ１及び第二座標ｂ１を算出した旨を示す信号をベクトル算出部２０４及び移動情報算出部２０６に送信する。

【0030】

図３Ｃを参照しつつ、座標算出部２０３による第一座標ａ１及び第二座標ｂ１の算出の具体例について説明する。図３Ｃは、図１に示す予測システム１における座標算出部２０３が算出した第一座標ａ１及び第二座標ｂ１の一例を示す図である。図３Ｃに示すように、空間ＡＲＥＡ内には、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が配置されている。座標算出部２０３は、特定部２０２から送信される信号を受け付けて、特定された複数の物体のうち、第一物体ＯＢＪ１に対応する点群データに含まれる各点データと、特定された第二物体ＯＢＪ２に対応する点群データに含まれる各点データとを比較し、お互いの距離が最短となる点データのセットを検出する。座標算出部２０３は、検出した点データのセットのうち第一物体ＯＢＪ１の点データの座標を第一座標ａ１として算出し、第二物体ＯＢＪ２の点データの座標を第二座標ｂ１として算出する。この時、第一座標ａ１及び第二座標ｂ１の間の距離は、距離ｄｏｂｊ１となる。

【0031】

図２に戻って、ベクトル算出部２０４は、第一物体ＯＢＪ１の重心の座標ｃｇａ１から第一座標ａ１に向う第一ベクトルｖｅｃａ１及び第二物体ＯＢＪ２の重心の座標ｃｇｂ１から第二座標ｂ１に向う第二ベクトルｖｅｃｂ１を算出する。ベクトル算出部２０４は、今回算出した第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１を形状変化判定部２０５に送信するとともに、記憶部２１０に記憶する。

【0032】

図３Ｄを参照しつつ、ベクトル算出部２０４による第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１の算出の具体例について説明する。図３Ｄは、図１に示す予測システム１におけるベクトル算出部２０４が算出した第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１の一例を示す図である。図３Ｄに示すように、ベクトル算出部２０４は、座標算出部２０３から送信される信号を受け付けて、第一座標ａ１の座標から第一物体ＯＢＪ１の重心の座標ｃｇａ１の座標を減算することによって、第一ベクトルｖｅｃａ１を算出する。また、ベクトル算出部２０４は、座標算出部２０３から送信される信号に基づいて、第二座標ｂ１の座標から第二物体ＯＢＪ２の重心の座標ｃｇｂ１の座標を減算することによって、第二ベクトルｖｅｃｂ１を算出する。

【0033】

図２に戻って、形状変化判定部２０５は、第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１の変化に基づいて、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちのいずれかの形状が変化したか否かを判定する。形状変化判定部２０５は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したか否かの判定結果を移動情報算出部２０６に送信する。また、形状変化判定部２０５は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したと判定する場合、記憶部２１０に記憶されている今回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する点群データ、重心の座標、第一座標、第二座標の情報を削除する。なお、形状変化判定部２０５は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２の形状の変化を判定するにあたって、第一ベクトルｖｅｃａ１の大きさ及び方向の変化が所定の値を上回る場合に第一物体ＯＢＪ１の形状が変化したと判定し、第二ベクトルｖｅｃｂ１の大きさ及び方向の変化が所定の値を上回る場合に第二物体ＯＢＪ２の形状が変化したと判定する。

【0034】

図４Ａ及び図４Ｂを参照しつつ、形状変化判定部２０５による第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したか否かの判定の具体例について説明する。図４Ａは、図１に示す予測システム１において物体の形状が変化していない場合の第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１の一例を示す図である。また、図４Ｂは、図１に示す予測システム１において物体の形状が変化した場合の第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１の一例を示す図である。なお、図４Ａ及び図４Ｂにおいて、特定部２０２によって前回特定された第一物体ＯＢＪ１には、重心の座標ｃｇａ１、第一座標ａ１及び第一ベクトルｖｅｃａ１の情報が関連付けられており、前回特定された第二物体ＯＢＪ２には、重心の座標ｃｇｂ１、第二座標ｂ１及び第二ベクトルｖｅｃｂ１の情報が関連付けられているものとする。また、第一座標ａ１及び第二座標ｂ１の間の距離は、距離ｄｏｂｊ１であるものとする。さらに、図４Ａ及び図４Ｂにおいて、特定部２０２によって今回特定された第一物体ＯＢＪ１には、重心の座標ｃｇａ２、第一座標ａ２及び第一ベクトルｖｅｃａ２の情報が関連付けられており、今回特定された第二物体ＯＢＪ２には、重心の座標ｃｇｂ２、第二座標ｂ２及び第二ベクトルｖｅｃｂ２の情報が関連付けられているものとする。また、第一座標ａ２及び第二座標ｂ２の間の距離は、距離ｄｏｂｊ２であるものとする。

【0035】

図４Ａ及び図４Ｂを参照して、形状変化判定部２０５は、ベクトル算出部２０４から送信される今回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する第一ベクトルｖｅｃａ２及び第二ベクトルｖｅｃｂ２を受け付け、記憶部２１０から前回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１を取得する。形状変化判定部２０５は、第一ベクトルｖｅｃａ２から第一ベクトルｖｅｃａ１を減算して、第一物体ＯＢＪ１に関する差ベクトルを算出する。形状変化判定部２０５は、第一物体ＯＢＪ１に関する差ベクトルの大きさが予め定められている所定の値を上回るか否かを判定する。一方、形状変化判定部２０５は、第一物体ＯＢＪ１に関する差ベクトルの大きさが所定の値を上回ると判定する場合、第一物体ＯＢＪ１の形状が変化したと判定する。他方、形状変化判定部２０５は、第一物体ＯＢＪ１に関する差ベクトルの大きさが所定の値以下であると判定する場合、第一物体ＯＢＪ１の形状が変化してないと判定する。そして、形状変化判定部２０５は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したか否かの判定結果を移動情報算出部２０６に送信する。

【0036】

より具体的には、図４Ａにおいては、第一物体ＯＢＪ１に関する差ベクトルの大きさは、第一ベクトルｖｅｃａ２及び第一ベクトルｖｅｃａ１の向き及び大きさの差が小さいため、所定の値以下である。したがって、形状変化判定部２０５は、第一物体ＯＢＪ１の形状が変化していないと判定する。また、図４Ｂにおいては、第一物体ＯＢＪ１に関する差ベクトルの大きさは、第一ベクトルｖｅｃａ２及び第一ベクトルｖｅｃａ１の向き及び大きさの差が大きいため、所定の値を上回る。したがって、形状変化判定部２０５は、第一物体ＯＢＪ１の形状が変化したと判定する。なお、形状変化判定部２０５が第二物体ＯＢＪ２に関して行う処理については、第一物体ＯＢＪ１と同様であるため、その説明を省略する。

【0037】

図２に戻って、移動情報算出部２０６は、形状変化判定部２０５の判定が否定判定である場合、第一座標ａ１及び第二座標ｂ１の一定時間（例えばサンプリング時間）毎の変化に基づいて第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動量及び移動方向を算出する。また、移動情報算出部２０６は、算出した第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動量及び移動方向を衝突予測部２０７に送信する。

【0038】

図５を参照しつつ、移動情報算出部２０６による第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動量及び移動方向の算出の具体例について説明する。図５は、図１に示す予測システム１における各物体の移動量及び移動方向の一例を示す図である。なお、図５において、特定部２０２によって前回特定された第一物体ＯＢＪ１には、第一座標ａ１の情報が関連付けられており、前回特定された第二物体ＯＢＪ２には、第二座標ｂ１の情報が関連付けられているものとする。また、特定部２０２によって今回特定された第一物体ＯＢＪ１には、第一座標ａ２の情報が関連付けられており、今回特定された第二物体ＯＢＪ２には、第二座標ｂ２の情報が関連付けられているものとする。

【0039】

図５を参照して、移動情報算出部２０６は、形状変化判定部２０５の判定が第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれの形状も変化していないという判定である場合、座標算出部２０３から送信される今回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する第一座標ａ１及びｂ１を受け付けるとともに、記憶部２１０から前回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する第一座標ａ２及びｂ２を取得する。移動情報算出部２０６は、第一座標ａ２から第一座標ａ１を減算することによって、第一物体ＯＢＪ１の移動量及び移動方向に関する情報を含む移動ベクトルｍｖａを算出する。ここで、第一物体ＯＢＪ１の移動量は、移動ベクトルｍｖａの大きさである。また、第一物体ＯＢＪ１の移動方向は、Ｘ軸方向に対する移動ベクトルｍｖａの角度である。移動情報算出部２０６は、第二座標ｂ２から第二座標ｂ１を減算することによって、第二物体ＯＢＪ２の移動量及び移動方向に関する情報を含む移動ベクトルｍｖｂを算出する。ここで、第二物体ＯＢＪ２の移動量は、移動ベクトルｍｖｂの大きさである。また、第二物体ＯＢＪ２の移動方向は、Ｘ軸方向に対する移動ベクトルｍｖｂの角度である。そして、移動情報算出部２０６は、算出した移動ベクトルｍｖａ及びｍｖｂを衝突予測部２０７に送信する。

【0040】

図２に戻って、衝突予測部２０７は、移動情報算出部２０６の算出結果に基づいて、所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するか否かを予測する。衝突予測部２０７は、例えば、移動情報算出部２０６の算出結果に基づいて第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動速度を算出し、第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動速度及び移動方向に基づいて所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するか否かを予測する。

【0041】

図６を参照しつつ、衝突予測部２０７による第一物体ＯＢＪ１及び第二物体ＯＢＪ２が所定時間の経過後に衝突するか否かの予測の具体例について説明する。図６は、図１に示す予測システム１における衝突予測部２０７の動作を説明するための図である。なお、図６において、特定部２０２によって前回特定された第一物体ＯＢＪ１には、第一座標ａ１の情報が関連付けられており、前回特定された第二物体ＯＢＪ２には、第二座標ｂ１の情報が関連付けられているものとする。また、特定部２０２によって今回特定された第一物体ＯＢＪ１には、第一座標ａ２の情報が関連付けられており、今回特定された第二物体ＯＢＪ２には、第二座標ｂ２の情報が関連付けられているものとする。

【0042】

図６を参照して、衝突予測部２０７は、移動情報算出部２０６から移動ベクトルｍｖａ及びｍｖｂを取得する。衝突予測部２０７は、移動ベクトルｍｖａが含む第一物体ＯＢＪ１の移動量をサンプリング時間で除算することによって、第一物体ＯＢＪ１の移動速度を算出する。衝突予測部２０７は、算出した第一物体ＯＢＪ１の移動速度に対して所定時間を乗算することによって、第一物体ＯＢＪ１が所定時間で移動する移動距離を算出する。そして、衝突予測部２０７は、第一座標ａ２から移動ベクトルｍｖａが示す第一物体ＯＢＪ１の移動方向に向かって、算出した移動距離だけ進んだ場合における第一物体ＯＢＪ１の座標である第一座標ａ３を算出する。第一座標ａ３は、第一物体ＯＢＪ１の所定時間後の位置を示す座標となる。

【0043】

また、衝突予測部２０７は、移動ベクトルｍｖｂが含む第二物体ＯＢＪ２の移動量をサンプリング時間で除算することによって、第二物体ＯＢＪ２の移動速度を算出する。衝突予測部２０７は、算出した第二物体ＯＢＪ２の移動速度に対して所定時間を乗算することによって、第二物体ＯＢＪ２が所定時間で移動する移動距離を算出する。そして、衝突予測部２０７は、第二座標ｂ２から、移動ベクトルｍｖｂが示す第二物体ＯＢＪ２の移動方向に向かって、算出した移動距離だけ進んだ場合における第二物体ＯＢＪ２の座標である第二座標ｂ３を算出する。第二座標ｂ３は、第二物体ＯＢＪ２の所定時間後の位置を示す座標となる。

【0044】

衝突予測部２０７は、第一座標ａ２及びａ３を結ぶ線分ｌｉｎｅａと、第二座標ｂ２及びｂ３を結ぶ線分ｌｉｎｅｂとの間の最短距離である距離ｄｏｂｊ３を算出する。衝突予測部２０７は、算出した距離ｄｏｂｊ３が所定距離以下であるか否かを判定し、当該判定が肯定判定である場合に所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突すると予測する。そして、衝突予測部２０７は、予測結果を停止命令部２０８及び出力部２０９に送信する。

【0045】

停止命令部２０８は、衝突予測部２０７の予測が肯定である場合に、ロボット３０の動作を停止させる命令をロボット制御装置４０に送信する。具体的には、停止命令部２０８は、衝突予測部２０７から所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するか否かを予測した結果を受け付ける。停止命令部２０８は、受け付けた予測が、所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するという予測である場合に、ロボット３０の動作を停止させる命令をロボット制御装置４０に送信する。

【0046】

出力部２０９は、例えば、ディスプレイやランプ、アラームなどであり、衝突予測部２０７の予測結果を予測装置２０の外部に出力する。具体的には、出力部２０９は、衝突予測部２０７から所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するか否かを予測した結果を受け付ける。出力部２０９は、受け付けた予測結果を画面表示や光の点滅、アラームの音などで外部に出力する。

【0047】

記憶部２１０には、予測装置２０における各種のデータが記憶される。具体的には、記憶部２１０には、抽出部２００によって抽出が行われる各タイミングにおける第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する点群データ、重心の座標、第一座標、第二座標、第一ベクトル及び第二ベクトルが記憶される。また、記憶部２１０には、抽出部２００が抽出した物体ＯＢＪα及びＯＢＪβに対応する点群データが記憶される。また、記憶部２１０には、重心算出部２０１が今回算出した物体ＯＢＪα及びＯＢＪ２の重心の座標ｃｇａ１及びｃｇｂ１が記憶される。また、記憶部２１０には、抽出部２００によって抽出された物体ＯＢＪα及びＯＢＪβが特定部２０２によって前回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちのいずれに対応するかの情報が記憶される。

【0048】

＜一連の処理の流れ＞
以上、予測システム１の回路構成について説明した。次に、予測システム１の一連の処理の流れについて詳しく説明する。図７は、図１に示す予測システム１の一連の処理の流れを示すフローチャートである。

【0049】

（ステップＳＰ１０）
予測システム１は、抽出部２００によって、３Ｄセンサ１０が撮像した画像を取得し、取得した画像内の複数の物体を抽出する。予測システム１は、抽出部２００によって、抽出した物体をデータとして記憶部２１０に記憶するとともに重心算出部２０１に送信する。そして、処理は、ステップＳＰ１２の処理に移行する。

【0050】

（ステップＳＰ１２）
予測システム１は、重心算出部２０１によって、抽出部２００が抽出した物体ＯＢＪα及びＯＢＪβの重心の座標を算出する。予測システム１は、重心算出部２０１によって、今回算出した物体ＯＢＪα及びＯＢＪ２の重心の座標を特定部２０２に送信するとともに記憶部２１０に記憶する。そして、処理は、ステップＳＰ１４の処理に移行する。

【0051】

（ステップＳＰ１４）
予測システム１は、特定部２０２によって、重心算出部２０１が今回算出した重心の座標及び前回算出した重心の座標を比較し、最も座標が近い重心の座標同士を同じ物体として特定する。予測システム１は、特定部２０２によって、特定した物体に関する情報を記憶部２１０に記憶する。そして、処理は、ステップＳＰ１６の処理に移行する。

【0052】

（ステップＳＰ１６）
予測システム１は、座標算出部２０３によって、特定部２０２が特定した第一物体ＯＢＪ１及び第二物体ＯＢＪ２に対して、お互いの間の距離が所定の条件を満たすような第一物体ＯＢＪ１の外周の座標である第一座標ａ１及び第二物体ＯＢＪ２の外周の座標である第二座標ｂ１を算出する。予測システム１は、座標算出部２０３によって、今回算出した第一座標ａ１及び第二座標ｂ１を記憶部２１０に記憶する。そして、処理は、ステップＳＰ１８の処理に移行する。

【0053】

（ステップＳＰ１８）
予測システム１は、ベクトル算出部２０４によって、第一物体ＯＢＪ１の重心の座標ｃｇａ１から第一座標ａ１に向う第一ベクトルｖｅｃａ１及び第二物体ＯＢＪ２の重心の座標ｃｇｂ１から第二座標ｂ１に向う第二ベクトルｖｅｃｂ１を算出する。予測システム１は、ベクトル算出部２０４によって、今回算出した第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１を形状変化判定部２０５に送信するとともに、記憶部２１０に記憶する。そして、処理は、ステップＳＰ２０の処理に移行する。

【0054】

（ステップＳＰ２０）
予測システム１は、形状変化判定部２０５によって、第一ベクトルｖｅｃａ１及び第二ベクトルｖｅｃｂ１の変化に基づいて、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちのいずれかの形状が変化したか否かを判定する。予測システム１は、形状変化判定部２０５によって、第一ベクトルｖｅｃａ１の大きさ及び方向の変化が所定の値を上回る場合に第一物体ＯＢＪ１の形状が変化したと判定し、第二ベクトルｖｅｃｂ１の大きさ及び方向の変化が所定の値を上回る場合に第二物体ＯＢＪ２の形状が変化したと判定する。一方、形状変化判定部２０５による判定が否定判定である場合、処理は、ステップＳＰ２２の処理に移行する。他方、形状変化判定部２０５による判定が肯定判定である場合、図７における一連の処理は、終了する。

【0055】

（ステップＳＰ２２）
予測システム１は、移動情報算出部２０６によって、第一座標ａ２から第一座標ａ１を減算することによって、第一物体ＯＢＪ１の移動情報（移動量及び移動方向）に関する情報を含む移動ベクトルｍｖａを算出する。移動情報算出部２０６は、第二座標ｂ２から第二座標ｂ１を減算することによって、第二物体ＯＢＪ２の移動情報（移動量及び移動方向）に関する情報を含む移動ベクトルｍｖｂを算出する。そして、処理は、ステップＳＰ２４の処理に移行する。

【0056】

（ステップＳＰ２４）
予測システム１は、衝突予測部２０７によって、移動情報算出部２０６の算出結果に基づいて第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動速度を算出し、第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動速度及び移動方向に基づいて所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するか否かを予測する。一方、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が所定時間の経過後に衝突すると衝突予測部２０７が予測する場合、処理は、ステップＳＰ２６の処理に移行する。他方、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が所定時間の経過後に衝突しないと衝突予測部２０７が予測する場合、図７に示す一連の処理は、終了する。

【0057】

（ステップＳＰ２６）
予測システム１は、停止命令部２０８によってロボット３０の動作を停止させる命令をロボット制御装置４０に送信する。そして、図７に示す一連の処理は、終了する。

【0058】

＜効果＞
以上、本実施形態では、予測システム１は、マッチング処理に代えて、重心の座標によって物体を特定し、物体同士の距離が所定の条件を満たすような第一座標ａ１及び第二座標ｂ１を算出し、重心の座標ｃｇａ１から第一座標ａ１に向かう第一ベクトルｖｅｃａ１及び重心の座標ｃｇｂ１から第二座標ｂ１に向かう第二ベクトルｖｅｃｂ１の変化に基づいて物体の形状が変化したか否かを判定し、物体の形状が変化していないと判定する場合に、第一座標ａ１及び第二座標ｂ１の変化に基づいて物体が衝突するか否かを判定する。したがって、予測システム１は、低負荷で、物体の形状が一時的に変化する場合にも物体の衝突を予測できる。

【0059】

また、予測システム１は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が所定時間の経過後に衝突すると予測する場合に、ロボット３０の動作を停止させる命令をロボット制御装置４０に送信する。したがって、予測システム１は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突する前にロボット３０の動作を停止できる。

【0060】

また、予測システム１は、特定された複数の物体のうちの第一物体ＯＢＪ１及び第二物体ＯＢＪ２に対して、お互いの間の距離が最短となるような第一物体ＯＢＪ１の外周の座標である第一座標ａ１及び第二物体ＯＢＪ２の外周の座標である第二座標ｂ１を算出する。したがって、予測システム１は、３Ｄセンサ１０が撮像した画像ＩＭＧに第一物体ＯＢＪ１及び第二物体ＯＢＪ２の全体が映っていなくても、物体の衝突を予測できる。

【0061】

また、予測システム１は、第一物体ＯＢＪ及び第二物体ＯＢＪ２の移動速度を算出し、第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動速度及び移動方向に基づいて所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突するか否かを予測する。したがって、予測システム１は、物体が移動する速度に応じて、物体の衝突を予測できる。

【0062】

また、予測システム１は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２の移動速度及び移動方向に基づいて所定時間が経過後の第一座標ａ３及び第二座標ｂ３を算出し、現在の第一座標ａ２及び所定時間が経過後の第一座標ａ３を結ぶ線分ｌｉｎｅａ並びに現在の第二座標ｂ２及び所定時間が経過後の第二座標ｂ３を結ぶ線分ｌｉｎｅｂの間の最短距離である距離ｄｏｂｊ３が所定距離以下であるか否かを判定し、当該判定が肯定判定である場合に所定時間の経過後に第一物体ＯＢＪ１及び第二物体ＯＢＪ２が衝突すると予測する。したがって、予測システム１は、予め定められた所定距離を基準として、物体が衝突するか否かを予測できる。

【0063】

＜変形例＞
なお、本発明は上記の実施形態に限定されるものではない。すなわち、上記の実施形態に、当業者が適宜設計変更を加えたものも、本発明の特徴を備えている限り、本発明の範囲に包含される。また、上記実施形態及び後述する変形例が備える各要素は、技術的に可能な限りにおいて組み合わせることができ、これらを組み合わせたものも本発明の特徴を含む限り本発明の範囲に包含される。

【0064】

例えば、本実施形態では、予測システム１は、形状変化判定部２０５によって、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したと判定された場合、記憶部２１０に記憶されている今回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する点群データ、重心の座標、第一座標及び第二座標の情報を削除するが、これに限られるものではない。予測システム１は、形状変化判定部２０５によって、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したと判定された場合、記憶部２１０に記憶されている今回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する点群データ、重心の座標、第一座標及び第二座標の情報が抽出部２００、重心算出部２０１、特定部２０２及び移動情報算出部２０６によって参照されないように参照不可フラグを付加しても良い。この構成によれば、予測システム１は、形状が変化した場合の第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する各情報が他の処理によって必要になった場合などのために記憶部２１０に記憶し続けることができる。

【0065】

さらに、予測システム１は、形状変化判定部２０５によって、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化したと判定された後に、記憶部２１０に記憶されている今回特定された第一物体ＯＢＪ１及び第二物体ＯＢＪ２に関する点群データ、重心の座標、第一座標及び第二座標の情報に対して削除も参照不可フラグの不可もせず、そのままにしても良い。この構成によれば、予測システム１は、第一物体ＯＢＪ１及び第二物体ＯＢＪ２のうちいずれかの形状が変化した場合においても、形状が変化した後に続く各データ（第一座標ａ１や第二座標ｂ１、重心の座標ｃｇａ１及びｃｇｂ１など）を比較することによって、第一物体ＯＢＪ１及び第二物体ＯＢＪ２が所定時間の経過後に衝突するか否かを予測できる。

【符号の説明】

【0066】

１…予測システム、１０…３Ｄセンサ、２００…抽出部、２０１…重心算出部、２０２…特定部、２０３…座標算出部、２０４…ベクトル算出部、２０５…形状変化判定部、２０６…移動情報算出部、２０７…衝突予測部

【図1】

【図2】

【図3A】

【図3B】

【図3C】

【図3D】

【図4A】

【図4B】

【図5】

【図6】

【図7】