特開 2021-160080 距離場による高速ロボット運動の最適化

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

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

(11)【公開番号】特開2021-160080(P2021-160080A)

(43)【公開日】2021年10月11日

(54)【発明の名称】距離場による高速ロボット運動の最適化

(51)【国際特許分類】

   B25J 19/06 20060101AFI20210913BHJP

   B25J 9/22 20060101ALI20210913BHJP

【ＦＩ】

   B25J19/06

   B25J9/22 Z

【審査請求】未請求

【請求項の数】20

【出願形態】ＯＬ

【外国語出願】

【全頁数】20

(21)【出願番号】特願2021-62532(P2021-62532)

(22)【出願日】2021年4月1日

(31)【優先権主張番号】16/839,642

(32)【優先日】2020年4月3日

(33)【優先権主張国】US

(71)【出願人】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100123582

【弁理士】

【氏名又は名称】三橋 真二

(74)【代理人】

【識別番号】100112357

【弁理士】

【氏名又は名称】廣瀬 繁樹

(74)【代理人】

【識別番号】100169856

【弁理士】

【氏名又は名称】尾山 栄啓

(72)【発明者】

【氏名】シエン−チュン リン

(72)【発明者】

【氏名】加藤 哲朗

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

3C707

【Ｆターム（参考）】

3C707BS12

3C707KS12

3C707KT01

3C707KT06

3C707LS09

3C707LS15

3C707LV19

3C707LW08

3C707MS09

(57)【要約】

【課題】距離場制約関数を使用したロボット衝突回避運動最適化技術を提供する。
【解決手段】ロボット作業空間内の障害物を表すＣＡＤ又はセンサデータをボクセルに変換し、ボクセル占有率の３次元２値行列を作成する。次に、対応する距離マップ行列を計算する。ここで、距離マップ行列の各セルには、最も近い占有セルまでの距離が含まれる。距離マップ行列は、運動計画最適化問題の制約関数として使用される。ここで、最適化問題は凸化され、次に繰り返し解かれて、障害物を回避し、移動距離などの目的関数を最小化するロボット運動プロファイルを生成する。距離場最適化技術は、迅速に計算され、障害物の数に依存しない計算時間を有する。開示した最適化技術は、ロボットと障害物の形状を近似するための幾何学的プリミティブを作成する必要がないため、設定が簡単である。
【選択図】図８

【特許請求の範囲】

【請求項1】

産業用ロボットの経路を計画するための方法であって、前記方法は、
前記ロボットの作業空間内の障害物を定義する障害物データを提供するステップと、
前記障害物データから距離場行列を計算するステップであって、前記距離場行列は、前記作業空間を表す３次元（３Ｄ）行列であり、前記距離場行列の各セルには、前記障害物のいずれかの任意の部分が占有する最も近いセルまでの距離を指定する値が含まれる、ステップと、
プロセッサとメモリとを有するコンピュータを使用してロボット運動最適化問題をモデル化するステップであって、目的関数を定義するステップと、前記経路の開始点及び目標点を等式制約として定義するステップと、前記距離場行列を使用して衝突回避不等式制約を定義するステップとを含む、ステップと、
前記コンピュータによって前記運動最適化問題を解いて、前記ロボットの前記経路を導き出すステップと、を含む方法。

【請求項2】

障害物データを提供するステップが、コンピュータ支援設計（ＣＡＤ）システムから前記障害物データを提供するステップを含む、請求項１に記載の方法。

【請求項3】

障害物データを提供するステップが、前記作業空間内の前記障害物を検出するように構成された１つ又は複数のセンサから前記障害物データを提供するステップを含む、請求項１に記載の方法。

【請求項4】

前記障害物データから前記距離場行列を計算するステップが、前記障害物データをボクセルに変換するステップと、占有データの２値３Ｄ行列を作成するステップと、占有データの前記２値３Ｄ行列から前記距離場行列を計算するステップと、を含む、請求項１に記載の方法。

【請求項5】

前記距離場行列を使用して前記衝突回避不等式制約を定義するステップが、前記ロボットのアーム上の複数の点を定義するステップと、前記ロボットの計算された姿勢ごとに、前記点のそれぞれに対して障害物までの距離を、前記点を含む前記セル内の前記値から前記点に対する所定の半径を減算したものとして決定するステップとを含む、請求項１に記載の方法。

【請求項6】

前記ロボットの計算された姿勢ごとに、ロボットから障害物までの距離が前記点のいずれかについての障害物までの最小距離として決定され、前記衝突回避不等式制約は、前記ロボットから障害物までの距離が、所定の閾値を超えている必要があることである、請求項５に記載の方法。

【請求項7】

前記ロボット運動最適化問題をモデル化するステップはこのほか、前記開始点及び前記目標点に基づいて初期参照経路を生成するステップを含む、請求項１に記載の方法。

【請求項8】

前記ロボット運動最適化問題をモデル化するステップはこのほか、所定の閾値を超えないロボット関節回転速度、加速度及び加加速度と、所定の閾値を超えないロボットツール中心点速度、加速度及び加加速度と、所定の制限を超えない関節位置とをはじめとする運動制限不等式制約を定義するステップを含み、前記ロボットの各関節のためのシステム力学又は運動学方程式に基づいて等式制約を定義するステップをさらに含む、請求項１に記載の方法。

【請求項9】

前記衝突回避不等式制約を一次テーラー級数展開として近似するステップをはじめ、前記ロボット運動最適化問題を解く前に前記ロボット運動最適化問題を凸化するステップをさらに含む、請求項１に記載の方法。

【請求項10】

前記運動最適化問題を解くステップは、解が所定の収束基準に収束するまで、前記運動最適化問題を繰り返し解くステップを含む、請求項９に記載の方法。

【請求項11】

前記運動最適化問題を解くステップにより、前記開始点及び前記目標点並びにいくつかの中間点を含む解ベクトルが生成され、連続ロボット軌道が前記解ベクトルから補間される、請求項１に記載の方法。

【請求項12】

産業用ロボットの経路を計画する方法であって、前記方法は、コンピュータを使用して、ロボット運動最適化問題をモデル化して解くステップを含み、前記最適化問題をモデル化するステップは、目的関数を定義するステップと、前記経路の開始点及び目標点を等式制約として定義するステップと、距離場行列を使用して衝突回避不等式制約を定義するステップとを含み、前記距離場行列は、前記ロボットの作業空間を表す３次元行列であり、前記距離場行列の各セルは、前記作業空間内の任意の障害物の任意の部分によって占有される最も近いセルまでの距離を指定する値を含む、方法。

【請求項13】

産業用ロボットのための経路計画システムであって、前記システムは、
前記ロボットの作業空間内の障害物を定義する障害物データを提供するための手段と、
前記ロボット及び障害物データを提供するための前記手段と通信するコンピュータであって、前記コンピュータは、プロセッサ及びメモリを有し、
前記障害物データから距離場行列を計算するステップであって、前記距離場行列は、前記作業空間を表す３次元（３Ｄ）行列であり、前記距離場行列の各セルには、前記障害物のいずれかの任意の部分が占有する最も近いセルまでの距離を指定する値が含まれる、ステップと、
ロボット運動最適化問題をモデル化するステップであって、目的関数を定義するステップと、前記経路の開始点と目標点を等式制約として定義するステップと、前記距離場行列を使用して衝突回避不等式制約を定義するステップと、を含むステップと、
前記ロボットの前記経路を生成するために前記運動最適化問題を解くステップと、のために構成された、コンピュータと、を具備する、システム。

【請求項14】

障害物データを提供するステップが、コンピュータ支援設計（ＣＡＤ）システムから前記障害物データを提供するステップ、あるいは前記作業空間内の前記障害物を検出するように構成された１つ又は複数のセンサから前記障害物データを提供するステップであって、前記１つ又は複数のセンサは、１つ又は複数のカメラ、レーダセンサ、ＬｉＤＡＲセンサ、超音波センサ又は赤外線センサを含む、ステップを含む、請求項１３に記載のシステム。

【請求項15】

前記障害物データから前記距離場行列を計算するステップが、前記障害物データをボクセルに変換するステップと、占有データの２値３Ｄ行列を作成するステップと、占有データの前記２値３Ｄ行列から前記距離場行列を計算するステップとを含む、請求項１３に記載のシステム。

【請求項16】

前記距離場行列を使用して前記衝突回避不等式制約を定義するステップが、前記ロボットのアーム上の複数の点を定義するステップと、前記ロボットの計算された姿勢ごとに、前記点のそれぞれについての障害物までの距離を、前記点を含む前記セル内の値から前記点の所定の半径を引いたものとして決定するステップとを含む、請求項１３に記載のシステム。

【請求項17】

前記ロボットの計算された姿勢ごとに、ロボットから障害物までの距離が前記点のいずれかについての障害物までの最小距離として決定され、前記衝突回避不等式制約は、前記ロボットから障害物までの距離が所定の閾値を超えている必要があることである、請求項１６に記載のシステム。

【請求項18】

前記ロボット運動最適化問題をモデル化するステップがこのほか、前記開始点及び前記目標点に基づいて初期参照経路を生成するステップを含む、請求項１３に記載のシステム。

【請求項19】

前記ロボット運動最適化問題をモデル化するステップがこのほか、所定の閾値を超えないロボット関節回転速度、加速度及び加加速度、所定の閾値を超えないロボットツール中心点速度、加速度及び加加速度、所定の制限を超えない関節位置をはじめとする運動制限不等式制約を定義するステップを含み、前記ロボットの各関節のシステム力学又は運動学方程式に基づいて等式制約を定義するステップをさらに含む、請求項１３に記載のシステム。

【請求項20】

前記コンピュータは、前記衝突回避不等式制約を一次テーラー級数展開として近似するステップを含む、前記ロボット運動最適化問題を凸化するステップのためにさらに構成され、前記ロボット運動最適化問題を解くステップは、解が所定の収束基準に収束するまで、前記運動最適化問題を反復的に解くステップを含む、請求項１３に記載のシステム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、産業用ロボット運動制御の分野、さらに具体的には、障害物データをボクセルに変換し、占有ボクセル及び非占有ボクセルの３次元２値行列を作成し、各セルに最も近い占有セルまでの距離が含まれる距離マップを計算し、次に、運動最適化計算の制約として距離マップを使用して、障害物を回避する最も効率的なロボットアーム経路を計算するロボット衝突回避運動計画技術に関する。

【背景技術】

【0002】

産業用ロボットを使用して、製造、組み立て及び材料の移動操作を幅広く実施することは周知である。多くのロボット作業空間環境では、障害物が存在し、その障害物はロボットの運動の経路にある可能性がある。障害物は、機械及び備品などの恒久的な構造物である場合もあれば、一時的なものであるか移動可能なものである場合もある。ロボットが溶接などの操作を実行している間、ロボットがワークピース内又はその周辺を動作する必要があるため、ロボットによって操作されている大きなワークピース自体が障害物になる可能性がある。ロボットと障害物との衝突を絶対に避けなければならない。

【0003】

衝突回避ロボット運動計画のための従来技術には、ロボットの各アーム周りと各障害物周りに、球、円柱などの幾何学的プリミティブを定義することが含まれる。衝突回避運動最適化計算の複雑さを管理可能なレベルに低減するために、幾何学的プリミティブを使用する。しかし、各障害物周りと各ロボットアーム周りに幾何学的プリミティブを定義することは、面倒で時間のかかる一連の作業である。さらに、ロボットによって溶接されているか、塗装されている車体などの一部の物体は、幾何学的プリミティブを使用した近似には適していない。

【0004】

衝突回避ロボット運動計画のための従来技術の別の問題には、最適化問題の計算時間が障害物の数とともに劇的に増大することが挙げられる。ロボットの作業空間に障害物が複数存在する場合、ロボットの動作中に衝突回避の計算をリアルタイムで実行する必要がある環境では、運動計画の計算に時間がかかりすぎて実用的ではない。

【0005】

上記の環境に照らして、設定が容易であり、ロボット作業空間内の障害物の数に関係なく、衝突回避ロボット運動を迅速に計算する、改良されたロボット運動最適化技術が必要である。

【発明の概要】

【0006】

本開示の教示に従って、距離場制約関数を使用するロボット衝突回避運動最適化技術を開示する。ロボット作業空間内の障害物を表すＣＡＤ又はセンサデータをボクセルに変換し、ボクセル占有の３次元（３Ｄ）２値行列を作成する。次に、対応する３Ｄ距離場行列を計算する。ここで、距離場行列の各セルには、障害物ボクセルが占有する最も近いセルまでの距離が含まれる。距離場行列は、運動計画最適化問題の制約関数として使用される。ここで、最適化問題は凸化され、次に繰り返し解かれて、障害物を回避し、移動距離などの目的関数を最小化するロボット運動プロファイルが生成される。距離場最適化技術は迅速に計算され、障害物の数に依存しない計算時間を有する。開示した最適化技術は、ロボットと障害物の形状を近似するための幾何学的プリミティブを作成する必要がないため、設定が簡単である。

【0007】

現在開示している装置及び方法の追加の特徴が、添付の図面と併せて、以下の説明及び添付の特許請求の範囲から明らかになるであろう。

【図面の簡単な説明】

【0008】

【図1】障害物の近くで動作する産業用ロボットであって、ロボット及び障害物は、当技術分野で知られている技術で使用されるように、衝突回避計算のために定義され使用される、対応する幾何学的プリミティブを有する、産業用ロボットの図。

【図2】車体の近くで動作する産業用ロボットであって、ロボットは、図１に示すように対応する幾何学的プリミティブを有するが、当技術分野で知られている技術で使用されるように、車体は、幾何学的プリミティブに関して容易にモデル化されない、産業用ロボットの図。

【図3】（３Ａ）は、本開示の一実施形態による、ＣＡＤデータとして定義されたロボット作業空間内の障害物の図であり、（３Ｂ）は、本開示の一実施形態による、体積要素（ボクセル）に変換された（３Ａ）の障害物の図である。

【図4】本開示の一実施形態による、障害物ＣＡＤ又はセンサデータをボクセルに変換し、次にボクセル占有の２値行列に変換し、最後に距離場行列に変換するためのデータフローを示すブロック図。

【図5】本開示の一実施形態による、図４の距離場行列によって表される作業空間で動作するロボットの図。

【図6A】運動計画に含まれる衝突回避制約なしに定義された経路をたどり、結果として障害物の衝突をもたらすロボットの多段階図。

【図6B】本開示の一実施形態による、衝突回避運動計画によって定義された経路をたどるロボットの多段階図。

【図7】本開示の一実施形態による、幾何学的プリミティブを使用する従来の距離計算技術及び距離場行列を使用する距離計算についての距離計算時間対障害物の数のグラフ。

【図8】本開示の実施形態による、距離場制約関数を使用するロボット衝突回避運動最適化のためのシステムの図。

【図9】本開示の一実施形態による、距離場制約関数を使用するロボット衝突回避運動最適化のための方法のフローチャート図。

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

【0009】

距離場制約関数を使用するロボット衝突回避運動最適化技術に向けられた本開示の実施形態に関する以下の考察は、本質的に単なる例示であり、開示した装置及び技術又はその用途又は使用を制限することを意図するものでは決してない。

【0010】

産業用ロボットをさまざまな製造、組み立て、材料移動操作に使用することは周知である。多くのロボット作業空間環境では、障害物が存在し、その障害物はロボットの運動の経路にある可能性がある。即ち、障害物は、ロボットが現在位置決めされている場所とロボットの目的地の位置との間に配置されている可能性がある。障害物は、機械及び備品などの恒久的な構造物である場合もあれば、一時的なものであるか移動可能なものである場合もある。ロボットが溶接などの操作を実行している間、ロボットがワークピース内又はその周辺を動作する必要があるため、ロボットによって操作されている大きなワークピース自体が障害物になる可能性がある。ツールが、定義された障害物とのロボットの衝突を回避する経路をたどるように、ロボットの運動を計算するための技術が当技術分野で開発されてきた。

【0011】

衝突回避ロボット運動計画のための従来技術には、ロボットの各アーム周りと各障害物周りとに、球、円柱などの幾何学的プリミティブを定義することが含まれる。実際の物体形状を単純化された形状で近似することにより、衝突回避運動最適化計算の複雑さを管理可能なレベルに低減するために、幾何学的プリミティブを使用する。

【0012】

図１は、障害物の近くで動作する産業用ロボットであって、ロボット及び障害物は、当技術分野で知られている技術で使用されるように、衝突回避計算のために定義され使用される、対応する幾何学的プリミティブを有する、産業用ロボットの図である。ロボット１００が、作業空間でプログラムされたいくつかの操作を実行している。ここで、この操作は、例えば、コンベヤからパレット又は貯蔵容器への部品の移動、あるいはワークピースの溶接又は吹き付け塗装である可能性も否定できない。ロボット１００は、とりわけ、内側アーム１１０、外側アーム１２０及び手首／ツール１３０を備える複数の多関節アームから構成される。ロボット１００が回避しなければならない多くの障害物が作業空間に存在する。障害物１４０、１５０及び１６０が、存在する可能性のあるさまざまなタイプの障害物の例である。多くの場合、障害物１４０、１５０及び１６０は静止している。ただし、移動する障害物も作業空間に存在する可能性があり、以下で説明する本開示の技術にて適応することができる。

【0013】

上記のように、衝突回避計算を単純化するために、各ロボットアーム及び各障害物の周りの安全ゾーン幾何学的プリミティブを定義する従来技術が知られている。ロボットの内側アーム１１０はプリミティブ１１２を有し、外側アーム１２０はプリミティブ１２２を有し、手首／ツール１３０はプリミティブ１３２を有し、プリミティブ１１２、１２２及び１３２は、半球形の端部を有する円筒として定義される。ロボットの他の部分には、球として定義された安全ゾーンプリミティブがある。プリミティブ１１２は、ロボット運動及び衝突回避の計算にて内側アーム１１０と共に移動し、同じことが他のプリミティブにも当てはまる。同じように、障害物１４０は、その周りに定義されたプリミティブ１４２を有し、障害物１５０はプリミティブ１５２を有し、障害物１６０はプリミティブ１６２を有する。

【0014】

次に、干渉を回避するロボット運動を計算するために、ロボットアームプリミティブから障害物プリミティブまでの距離が計算される。これは、実際の詳細な形状のアームと障害物との間の距離を計算するよりもはるかに簡単である。ただし、各障害物と各ロボットアームの周囲に幾何学的プリミティブを定義することは、面倒で時間のかかる一連の作業である。さらに、実際のロボットアーム部品と実際の障害物が典型的には不規則な形状であるため、幾何学的プリミティブは、プリミティブの体積内にかなりの空き空間がある状態で定義されることが多い。これにより、結果として得られるロボット運動プログラムは、衝突を回避するために実際に必要な距離よりもロボットを移動させる。さらに、一部のオブジェクトが、幾何学的プリミティブを使用した近似に適していない。

【0015】

図２は、車体２００の近くで動作する産業用ロボット１００の図である。ここで、ロボット１００は、図１に示すように、対応する幾何学的プリミティブを有するが、車体２００は、当技術分野で知られている技術で使用されるように、幾何学的プリミティブに関して容易にモデル化されない。図２に示す場合では、ロボット１００による衝突を防ぐために、車体２００周りに六面体（「レンガ形状」）形状プリミティブを定義することが可能であろうが、六面体は、車体２００周りに大きな自由空間を含むであろう。ここで、自由空間は、ロボット１００が車体２００を溶接するか塗装することができないようにするであろう。さらに、六面体は、車体２００の空洞又は開口部内でのロボット１００の運動を妨げるであろう。代替案は、車体２００の形状に近似する多数の幾何学的プリミティブ（例えば、フロントガラス「Ａ」ピラー用の円筒形プリミティブ、ドア「Ｂ」ピラー用の別の円筒形プリミティブなど）を定義することになるであろう。これは、非常に面倒で時間のかかる一連の作業になるであろう。

【0016】

衝突回避ロボット運動計画に関する従来技術の別の問題は、複雑なロボットと障害物の形状が幾何学的プリミティブによって近似される場合でも、運動最適化問題の計算時間が障害物の数とともに劇的に増大することである。ロボットの作業空間に複数の障害物が存在する場合、運動計画の計算を継続的かつ迅速に実施する必要があるリアルタイム環境では、運動計画の計算に時間がかかりすぎて実用的ではない。

【0017】

上記の問題を克服するために、距離場制約関数を使用する新たなロボット衝突回避運動最適化技術を本明細書に開示する。本開示の衝突回避運動最適化技術は、障害物を定義するためにコンピュータ支援設計（ＣＡＤ）データ又はセンサデータを使用し、ロボットアーム及び障害物の周りの幾何学的プリミティブを手動で定義する必要性を回避する、容易な設定を提供する。開示した技術はこのほか、高速最適化計算を提供し、計算時間は、障害物の数が増加しても増大しない。

【0018】

図３（３Ａ）は、本開示の一実施形態による、ＣＡＤデータとして定義されたロボット作業空間３０２内の障害物の図である。本開示の技術を使用して、ロボット作業空間３０２内の障害物を、ＣＡＤデータ又は物体センサからのデータによって定義することができる。図３（３Ａ）の例では、ＣＡＤデータを使用して障害物を記述している。ロボットの操作（溶接又は塗装など）の設計段階で、障害物を含む作業空間３０２の配置が事前にわかっている場合、ＣＡＤデータが障害物を定義するための好ましい選択肢であってもよい。障害物ＣＡＤデータが利用できない場合、あるいは作業空間３０２内で障害物を移動することが可能である場合、物体センサからのデータを使用して障害物を定義してもよい。

【0019】

図３（３Ａ）には、障害物３１０、３２０、３３０、３４０、３５０及び３６０が存在し、それぞれサイズと形状が異なる。障害物３１０−３６０は、任意のタイプの実部品、備品又は他の物体である場合があり、障害物３１０−３６０は、開示した技術では幾何学的プリミティブによって囲まれておらず、あるいはモデル化されていない。障害物３１０−３６０のほとんどを単純な幾何学的形状として示しているが、これは単に説明を単純化して明確にするためである。ＣＡＤデータによって定義された図３（３Ａ）の障害物３１０〜３６０は、滑らかで明確な表面を有する。障害物３１０−３６０が物体センサデータによって定義された場合、障害物は、物体センサデータに典型的な不規則性及び近似を含むであろう。障害物３１０−３６０を判定するために使用される物体センサには、デジタルカメラ、レーダセンサ、ＬｉＤＡＲセンサ、超音波センサ及び／又は赤外線センサのうちの１つ又は複数が含まれることがあり得る。

【0020】

図３（３Ｂ）は、本開示の一実施形態による、体積要素（ボクセル）に変換された図３（３Ａ）の障害物の図である。図３（３Ｂ）は、図３（３Ａ）と同じ作業空間３０２を示しているが、唯一の違いは、図３（３Ｂ）の障害物のボクセル化である。ボクセルとは、当技術分野で知られているように、３次元空間内の規則的なグリッド内のセルを表す。ボクセルは、不均一に充填され、定期的にサンプリングされた空間、例えば、ほとんどが開放空間であるが、場所によっては固い障害物がある作業空間３０２を表すのに特に役立つ。障害物３１０−３６０は、六面体、あるいはさらに具体的には立方体などの多面体に分割されており、各多面体は１つのボクセルである。障害物３１０−３６０はそれぞれ、ボクセルセット３１２−３６２として表される。即ち、ボクセルセット３１２が障害物３１０を表し、ボクセルセット３６２が障害物３６０を表す、等々。このほか、フロア３７０のボクセル表現が提供される。データを巨視的物体から（ＣＡＤ又はセンサデータから）個別のボクセルに変換することは、当技術分野で知られている一連の作業である。この一連の作業は、本開示の技術での準備段階として使用される。

【0021】

図４は、本開示の一実施形態による、障害物ＣＡＤ又はセンサデータをボクセルに変換し、次にボクセル占有の２値行列に変換し、そして最後に距離マップ行列に変換するためのデータフローを示すブロック図である。障害物を定義するＣＡＤ又はセンサデータを提供するブロック４１０と、障害物をボクセル化するブロック４２０は、図３（３Ａ）及び図３（３Ｂ）を参照して先に考察した。次に、３次元２値行列４３０をブロック４２０のボクセルから作成する。２値行列４３０は、作業空間３０２を表す３次元（３Ｄ）行列である。ここで、行列４３０内の各セルは、０又は１のいずれかの２進値を有する。セルが図３（３Ｂ）のボクセルのいずれかによって占有されていない場合、行列４３０内のセルが０の値を有する。図３（３Ｂ）のボクセルのいずれかが行列４３０内のセルの一部又は全部を占有する場合、そのセルは１の値を有する。

【0022】

行列４３０は、視覚化を容易にするための２次元グリッドとして図４に示されるが、行列４３０は実際には３次元であり、作業空間３０２の体積を表す。例えば、図４に示す５×５グリッドは、行列４３０の最下層であることがあり得る。セル４３２が１の値を有し、このため、障害物３１０−３６０のうちの１つの一部によって占有されるボクセルを表す。このほか、セル４３４が１の値を有する。セル４３６が０の値を有し、このため、障害物３１０−３６０のいずれかの一部によって占有されていないボクセルを表す。このほか、セル４３８が０の値を有する。ブロック４２０のボクセル化された障害物データを再び参照すると、障害物３１０−３６０を構成するボクセルの全部が、行列４３０内で１の値を有し、（障害物３１０−３６０によって占有されていない）空き空間又は自由空間の全部が、行列４３０内で０の値を有するであろう。床３７０は、行列４３０内の（占有された）ものの層として表されることがあり得、あるいは行列４３０は、床３７０が行列４３０の下にあり、このため行列４３０に含まれないように、定義されることがあり得る。

【0023】

２値行列４３０は、開示したロボット運動最適化の目的に適した任意のサイズ及び寸法を有することができる。以下でさらに考察する一例では、作業空間３０２は、１．５メートル×１．５メートル（床面積）×１．０メートルの高さのサイズを有する。この１．５×１．５×１．０メートルの体積は、３００×３００×２００セルの寸法を有する２値行列４３０に分割されてもよい。ここで、各セルは、５×５ｍｍの辺を有する立方体である。このような寸法は単なる例示であるが、実際のロボット作業空間に対応し、すぐに利用できるプロセッサハードウェアを使用して迅速に計算することができる例を表す。

【0024】

上記で考察したように、２値行列４３０は、作業空間３０２を表す１及び０の３Ｄ行列である。ここで、１は、障害物３１０−３６０によって占有されている行列４３０のセルである。一連の作業の次のステップは、距離マップ、すなわち距離場行列４４０を計算することである。距離場行列４４０は、２値行列４３０と同じサイズ及び寸法を有する３Ｄ行列である。距離場行列４４０はこのほか、視覚化を容易にするために２次元グリッドとして図４に示される。

【0025】

距離場行列４４０の各セルには、その特定のセルから最も近い占有セルまでの距離を表す値が投入される。例えば、２値行列４３０の自由空間セル４３６を考えてみよう。最も近い占有セルはセル４３４であり、これは左側に２セル単位である。このため、距離場行列４４０内のセル４４６の値は２．０であり、これは、最も近い占有セルが２．０単位離れていることを意味する。２値行列４３０の自由空間セル４３８を考えてみよう。最も近い占有セルはセル４３２であり、これは右側に２セル単位、上方に１単位である。このため、距離場行列４４０内のセル４４８の値は、２．２３（５の平方根、これは、２及び１の辺を有する三角形の斜辺の長さ）であり、これは、最も近い占有セルが２．２３セル単位離れていることを意味する。この距離場値の例は、図４の２次元グリッドの図と一致するように、２次元（２方向の距離の２乗の合計の平方根）で計算される。実際には、距離場行列４４０が３Ｄ行列である場合、距離値は、３次元（３方向の距離の２乗の合計の平方根）で計算される。距離場マップ４４０が多数のセルを表す値を含む場合、真の距離は、セルサイズを乗算することによって得ることができる。例えば、距離場マップ４４０内のセルが、最も近い占有セルに対して１０．０セルの値を有し、セルサイズが５ｍｍである場合、最も近い障害物までの実際の距離は５０．０ｍｍである。これとは別に、距離場マップ４４０内のセルには、セル単位の数ではなく、実際の距離値が投入されることがあり得る。

【0026】

距離場行列４４０内のセル４４２が、占有セル４３２に対応するため、０．０の距離値を有する。距離場行列４４０内のセル４４４が、占有セル４３４のすぐ隣（１セル離れている）にあるため、１．０の距離値を有する。距離場行列４４０が３次元であることを認識すると、距離場行列４４０内のいくつかのセルは、それ自体が占有され、占有セルによって囲まれることを理解することができる。障害物３１０−３６０の「内側」にあるこのようなセルの場合、距離場には負の値が与えられ、距離場は、これまでに考察したのと同じ３次元の方法で計算された最も近い非占有セルまでの距離を表す（３方向の距離の２乗の合計の平方根）。

【0027】

図５は、本開示の一実施形態による、図４の距離場行列４４０によって表される作業空間５０２にて動作するロボット５００の図である。ロボット５００は、多アーム多関節ロボットであり、各アームは、その周りに１つ又は複数の球形の安全ゾーンバブルが定義されている。バブル５１０がロボット基部周りに定義され、バブル５１２が股関節周りに定義され、バブル５１４〜５１８が内側アームを囲み、バブル５２０が肘関節を囲み、バブル５２２〜５２８が外側アーム及び手首／ツールを囲む。安全ゾーンバブル５１０−５２８は、ロボット５００のアームを包む空間体積を定義し、以下で考察する衝突回避計算にて使用されることになる。バブルのそれぞれには、既知の中心点及び半径がある。例えば、バブル５２０は、中心点５４０及び半径５４２を有する。

【0028】

図３（３Ａ）に示す障害物３１０〜３６０は、図５の作業空間５０２に存在することを理解されたい。障害物３１０−３６０に基づいて計算された距離場マップ４４０は、ロボット５００が距離場マップ４４０の空間体積内でその作業を実施するように、作業空間５０２上に重ね合わされる。ロボット５００がその関節可動域を介して関節運動するとき、バブル５１０−５２８の中心点のそれぞれは、距離場行列４４０のセルのうちの１つを占有してもよい。ロボット５００の任意の特定の姿勢では、各バブルから距離場マップ４４０の最も近い占有セルまでの距離を、式（１）（数１）のように計算することができる。

【0029】

【数1】

【0030】

ここで、p_iは、バブル５１０−５２８のうちの特定のもの（i）の中心点であり、r_iは、中心点p_iを有するバブルの半径であり、ＤＦは、中心点p_iの場所である座標(x_i,y_i,z_i)でのセルの距離場マップ４４０の値である。

【0031】

言い換えれば、例えば、バブル５２０の中心点５４０が（ある固定された作業空間座標フレーム内の）場所(x_i,y_i,z_i)にある場合、バブル５２０から障害物３１０−３６０のいずれかまでの最小距離は、中心点５４０によって現在占有されている距離場マップ４４０内のセルの値から、バブル５２０の半径を引いたものである。この計算は、運動最適化計算中に評価されるロボット５００の各姿勢でのバブル５１０−５２８のそれぞれの中心点及び半径に対して実施される。距離場マップ４４０は特定の障害物の場に対して事前に計算されているため、式（１）は非常に単純な減算問題であり、１０個のバブル５１０−５２８のそれぞれについて容易に解かれる。

【0032】

次に、ロボット５００の特定の姿勢について、ロボット５００から障害物３１０−３６０のいずれかまでの最小距離Dist(Robot,Obs)は、１０個のバブル(i)のそれぞれについて式（１）から計算された値の最小値である。即ち、式（２）（数２）となる。

【0033】

【数2】

【0034】

例えば、ロボット５００が障害物３１０−３６０に向かって伸びている姿勢では、バブル５２８は、その中心点が、距離場マップ４４０内のセルを非常に低い値で占有するため、障害物までの最小距離を有してもよい。

【0035】

中心点のうちの１つ又は複数が実際に障害物内にある（障害物に干渉している）ロボット５００の姿勢が評価される場合、最小距離はその姿勢の負の値として計算されることになる。最小距離値は、（以下で考察する）運動最適化計算でのいくらかの閾値安全値よりも大きくなければならない制約関数として使用されるため、干渉のある姿勢には罰則が課せられることになり、運動最適化計算が干渉のない別の解を見つけ出すことになる。

【0036】

図６Ａは、運動計画で考慮される衝突回避制約なしで定義された経路６１０をたどるロボット６００の多段階図である。経路６１０は、開始位置６１２(q_start)から目標位置６１４(q_goal)に進むものとして定義される。開始位置６１２は、ロボット６００のツール６２０が第１の貯蔵容器から部品を取り出す場所である可能性も否定できず、目標位置６１４は、例えば、ロボット６００が部品を第２の貯蔵容器に配置する場所である可能性も否定できない。図６Ａでは、ロボット６００は３つの姿勢で示される。ここで、ツール６２０は、開始位置６１２、中間位置及び目標位置６１４に配置されている。

【0037】

経路６１０などの経路を計算するために使用される運動最適化計算では、多くの関数を考慮することができる。もちろん、開始位置及び目標位置（６１２、６１４）を満たす必要があるため、このような関数は等式制約として定義される。システム力学又は運動学方程式を含む別の等式制約を定義してもよい。これにより、計算された運動をロボットが実際に物理的に実施することができるようになる。不等式拘束を、関節の回転速度と加速度及びツールの中心点のデカルト加速度と加加速度が閾値を超えないように定義してもよい。また、最適化目的関数を定義する必要があり、例えば、移動距離、経路の曲率、移動時間、ロボットが消費するエネルギーなどのパラメータを最小化することによって、経路の効率を最適化するために最適化目的関数を使用してもよい。

【0038】

衝突回避が制約関数として定義されていない場合、運動最適化により、ロボット６００が、経路６１０とほぼ同じ経路をたどる可能性が高い。ここで、ロボット６００のツール６２０は、壁６３０又は貯蔵容器６４０などの障害物に衝突する。そのような障害物との衝突は明らかに許容できるものではないため、衝突回避条件を制約関数として追加する必要がある。本開示の技術では、衝突回避制約関数は、式（１）及び式（２）の計算を使用して、距離場マップ４４０に関して定義される。

【0039】

図６Ｂは、本開示の一実施形態による、衝突回避運動計画によって定義された経路をたどるロボットの多段階図である。図６Ｂでは、経路６５０をロボット６００のツール６２０について計算する。経路６５０は、経路６１０と同じ開始位置６１２及び同じ目標位置６１４を使用する。しかし、経路６５０を計算する際に、運動最適化では衝突回避制約関数を使用する。

【0040】

具体的には、運動最適化計算では衝突回避不等式制約関数を使用する。ここで、衝突回避制約は、（上記で考察したように安全ゾーンバブルによって表される）ロボット６００から障害物（壁６３０及び貯蔵容器６４０）のいずれかまでの最小距離であって、式（１）及び式（２）を使用して壁６３０及び貯蔵容器６４０の距離場マップから決定された最小距離が、いくらかの閾値安全値（例えば、５０ｍｍなど）よりも大きくなければならないように定義される。運動最適化計算に、この衝突回避不等式制約が、図６Ａに関してこれまでに考察した他の制約関数及び目的関数とともに含まれている場合、経路６５０が結果である。図６Ｂから、経路６５０により、ツール６２０とロボット６００の全部品とが壁６３０及び貯蔵容器６４０を回避することがわかる。

【0041】

図７は、本開示の実施形態による、幾何学的プリミティブを使用する従来の距離計算技術と、距離場行列を使用する距離計算とについての距離計算時間対障害物の数のグラフ７００である。グラフ７００は、曲線７１０及び曲線７２０を含み、それぞれが、垂直軸７０２上に距離計算時間を描画し、水平軸７０４上に障害物の数を描画する。曲線７１０は、障害物及びロボットアームが幾何学的プリミティブによって表される従来の距離計算技術では、計算時間が障害物の数とともに劇的に増大することを示す。対照的に、曲線７２０は、障害物が距離場マップによって表され、ロボットアームが多数の点及び球形のバブルによって表される本開示の距離計算技術では、計算時間が短く、障害物の数から独立していることを示す。

【0042】

ロボットの動作中に運動最適化計算をリアルタイムで実施する必要がある場合、距離場マップ技術を使用した高速距離計算が特に有利であることは容易に理解することができる。例えば、入庫コンベヤの部品をロボットが拾い上げて、輸送コンテナ内の次に利用可能な場所に配置する必要がある用途について考えてみよう。この例示的用途では、開始位置(q_start)と目標位置(q_goal)の両方がピックアンドプレース操作ごとに固有のものである。これは、運動最適化を使用して新たな経路を計算する必要があることを意味する。距離場マップ技術を使用して、（衝突回避制約機能のための）ロボットから障害物までの距離計算を実施すると、リアルタイムのロボット操作を支援するのに充分な速さで反復最適化計算を完了することができる。

【0043】

図８は、本開示の実施形態による、距離場制約関数を使用するロボット衝突回避運動最適化のためのシステムの図である。ロボット８００が、１つ又は複数の障害物８１０を含む作業空間８０２内で動作する。障害物８１０は、単に一貫性を保つために、図３（３Ａ）の障害物３１０−３６０として図示される。コントローラ８２０が、典型的には、配線ケーブル接続を介してロボット８００と通信する。コントローラ８２０は、当技術分野で知られているように、関節運動コマンドをロボット８００に送信し、ロボット８００内の関節エンコーダから関節運動位置データを受信することによって、ロボット８００の運動を制御する。

【0044】

コントローラ８２０と通信するコンピュータ８３０が、ＣＡＤ立体又は表面モデルの形態で障害物形状データを提供することを含む、いくつかの異なるタスクのために使用されてもよい。コンピュータ８３０は、使用される場合、任意の適切な無線又は配線ネットワーク接続を介してコントローラ８２０と通信する。障害物８１０を定義するためにＣＡＤデータを使用する代わりに、センサ８４０などの１つ又は複数のセンサを使用してもよい。センサ８４０は、作業空間８０２内の障害物８１０の３Ｄ幾何学を提供することができるカメラ又は任意のタイプの物体センサであってもよい。センサ８４０は、１つ又は複数の３Ｄカメラ、あるいは複数の２Ｄカメラであって、そのデータが３Ｄ障害物データに結合される２Ｄカメラであることがあり得る。センサ８４０はこのほか、レーダ、ＬｉＤＡＲ及び／又は超音波などの他のタイプの装置を含むことがあり得る。センサ８４０はこのほか、任意の無線又は配線ネットワーク接続を介して、コントローラ８２０及び／又はコンピュータ８３０と通信する。

【0045】

一実施形態では、コントローラ８２０は、コンピュータ８３０又はセンサ８４０のいずれかから障害物形状データを受信する。障害物形状データは、作業空間８０２に存在する障害物８１０の全部の３Ｄ形状を定義する。コントローラ８２０は、障害物形状データをボクセル化し、各セルが占有されている場合は１、占有されていない場合は０を含む作業空間８０２の２値３Ｄ行列を作成し、２値行列から距離場マップを計算することによって、障害物形状データを距離場マップ、すなわち行列に変換する。距離場行列、すなわちマップは、コンピュータ８３０によって交替で計算され、コントローラ８２０に提供されることがあり得る。コントローラ８２０は、ロボット８００をロボットアーム上の点のセットとしてモデル化する。ここで、各点は、所定の半径の対応する球形の安全ゾーンバブルを有する。ロボット８００のバブルモデルと、障害物８１０を表す距離場マップとを使用して、コントローラ８２０は、式（１）及び式（２）を使用して、ロボット８００の任意の姿勢について、ロボットから障害物までの最小距離をきわめて迅速に計算することができる。計算されたロボットから障害物までの最小距離を制約関数として使用して、コントローラ８２０は、ロボット８００の動作を制御するために必要に応じて運動最適化計算を実施する。

【0046】

別の実施形態では、コンピュータ８３０は、距離場行列の作成、距離場制約及び他の制約を使用した運動最適化問題のモデル化及び最適化問題の解決をはじめとする計算のほとんどを実施する。この実施形態では、コンピュータ８３０は、収束最適化解からコントローラ８２０にロボット運動プログラムを提供する。計算の責任は、コンピュータ８３０とコントローラ８２０との間で任意の適切な方法で分割されてもよい。

【0047】

例示的用途では、ロボット８００は、連続的に変化する開始位置及び目標位置を伴ってタスクを実施し、このため、運動最適化計算は、ロボットタスクごとに計算される必要がある。別の用途では、１つ又は複数の障害物８１０が動いている可能性がある。その場合、運動最適化計算の前に、距離場マップを再計算する必要がある。どちらの用途でも、距離場マップ技術を使用したロボットから障害物までの最小距離の迅速な計算は、リアルタイムのロボット運動制御を支援する上できわめて有益である。

【0048】

図９は、本開示の一実施形態による、距離場制約関数を使用するロボット衝突回避運動最適化のための方法のフローチャート図９００である。ボックス９０２では、ロボット作業空間内の障害物は、これまでに記載したように、ＣＡＤデータ又はセンサデータによって定義される。ボックス９０４では、作業空間内の障害物を表す距離場行列が作成される。距離場行列は、障害物データをボクセルモデルに変換し、占有セルすなわちボクセルに１が含まれ、非占有セルすなわちボクセルに０が含まれる作業空間環境の２値３Ｄ行列を作成し、次に、最も近い占有セルすなわちボクセルまでの距離である値が各セルに含まれる距離場行列を計算することによって作成される。占有され、他の占有セルに囲まれているセルが、距離場行列で負の値を有し、最も近い非占有セルまでの距離を表す。

【0049】

ボックス９０６では、ロボットが実施することになるタスクに基づいて、ロボット経路の開始点及び目標点が定義される。ボックス９０８では、初期参照経路が生成される。初期参照経路は、開始点から目標点までの直線である場合もあれば、ほぼ同じ開始点と目標点を有し、以前に計算された経路に基づく近似された経路である場合もある。初期参照経路は、運動最適化計算の開始点（初期解）を提供する。ボックス９１０では、運動最適化問題がロボットコントローラにてモデル化される。運動最適化問題のモデル化には、これまでに考察したように目的関数と制約関数の定義が含まれる。特に、これには、ロボットから任意の障害物までの最小距離が、ロボットアーム上の点及び球形バブルのセットとともにボックス９０４の距離場行列を使用してモデル化され、式（１）及び式（２）を使用して計算される衝突回避不等式制約が含まれる。

【0050】

このため、運動最適化問題は式（３）（数３）のように定義することができる。

【0051】

【数3】

【0052】

このとき、次のようにする。

【0053】

【数4】

【0054】

【数5】

【0055】

【数6】

【0056】

ここで、f(q)は、姿勢q=｛q₁,・・・,q_T｝を含むロボット運動全体にわたって最小化されることになる最適化目的関数（ツールの中心点の経路長など）であり、数７は、満たす必要のある不等式制約（制限を下回った状態を維持する、関節の制限、関節の速度、加速度及び加加速度、ツールの中心点の速度、加速度、加加速度など）であり、h(q)は、満たす必要のある等式の制約（開始位置と目標位置の場所、システム力学又は運動学方程式など）であり、DF(q_t)は別の不等式制約（衝突回避）であり、ロボットから障害物までの最小距離は、上記で詳細に考察したように、ロボットの姿勢q_tのすべてに対する閾値d_safeよりも大きくなければならない。

【0057】

【数7】

【0058】

システム力学又は運動学に関連する等式制約は、計算された運動をロボットが実際に物理的に実施できるようにするために、含まれている。この制約は、数８の形式のシステム力学方程式を含めることによってモデル化されてもよい。ここで、qは関節の角位置（及びその通常の１次及び２次微分）であり、Mは質量慣性モーメントであり、Cはコリオリ係数であり、Kは剛性であり、Gは重力による荷重であり、τは関節トルクである。

【0059】

【数8】

【0060】

このタイプの方程式が、ロボットの関節ごとに評価されるであろう。

【0061】

これとは別に、等式制約は、二重積分器形式の運動方程式（数９）としてモデル化されてもよい。ここでは、数１０、数１１である。

【0062】

【数9】

【0063】

【数10】

【0064】

【数11】

【0065】

この場合も、ロボットの各関節は運動学方程式によってモデル化されるであろう。

【0066】

ボックス９１２では、運動最適化問題は、解決の準備のために凸化されている。最適化問題の凸化には、制約のうちの１つ又は複数を多項式時間関数として近似することが含まれ、結果として得られる計算はさらに簡単に解くことができ、１つの最適解のみが得られる。一実施形態では、衝突回避不等式制約は、最適化問題の収束挙動を改善するために線形化される。この実施形態では、式（６）は、以下の式（７）（数１２）のように一次テーラー展開として近似される。

【0067】

【数12】

【0068】

ここで、数１３は時間ステップtでの参照位置（参照位置は入力から与えられた初期参照、あるいは前の最適化反復）であり、q_tは最適化で解かれる時間ステップtでの計画位置であり、∇DFは、位置（数１３）での距離関数DFの勾配である。

【0069】

【数13】

【0070】

線形化された制約の勾配項は、ロボットアーム上の点を距離場マップの障害物から「押しのける」のに役立つ。

【0071】

ボックス９１４では、凸化最適化問題が解かれ、決定ダイアモンド９１６でのチェックにより、解が所定の許容範囲内に収束したかどうかを判定する。凸化最適化問題を解くための技術が当技術分野で知られており、衝突回避距離場の制約が線形化されると、解は迅速に収束する。ボックス９１８では、収束最適化解が補間され、全関節運動と、衝突回避制約を含む制約関数を満たすツール運動とを含む完全なロボット軌道を定義する。

【0072】

ＣＡＤ又はセンサからの障害物データ定義を考慮すると、フローチャート図９００の方法のステップのいずれもが、コントローラ８２０及び／又はコンピュータ８３０などのロボットコントローラにて実施されてもよい。即ち、一実施形態では、コントローラ８２０は、障害物を表す距離場行列を計算し、考察したように、距離場行列及びロボットの多点モデルを使用して、任意のロボット姿勢のロボットから障害物までの距離を計算する。この実施形態では、ロボットコントローラ８２０はこのほか、開始点及び目標点並びに初期参照経路、距離場行列モデルをはじめとする制約に基づいて最適化問題をモデル化し、次にコントローラ８２０は最適化問題を解き、ロボット８００の運動をリアルタイムで制御する。

【0073】

別の実施形態では、ロボットコントローラ８２０は、コンピュータ８３０によって計算される動作プログラムを使用して、ロボット８００の運動のみを制御する。この実施形態では、コンピュータ８３０は、障害物を表す距離場行列を計算し、距離場行列及びロボットの多点モデルを使用して任意のロボット姿勢のためのロボットから障害物までの距離を計算し、開始点と目標点並びに初期参照経路、距離場行列モデルをはじめとする制約に基づいて最適化問題をモデル化し、次に最適化問題を解く。当業者によって理解されるであろうように、計算の責任はこのほか、他の方法で分割されてもよい。例えば、コンピュータ８３０は、距離場行列を提供することがあり得、コントローラ８２０は、残りの最適化問題のモデル化及び解を実施することがあり得る。いくつかの実施形態では、コンピュータ８３０は必要とされない。

【0074】

これまでの考察を通じて、さまざまなコンピュータとコントローラについて説明し、暗示している。このようなコンピュータ及びコントローラのソフトウェアアプリケーション及びモジュールは、プロセッサ及びメモリモジュールを有する１つ又は複数の計算装置上で実行されることを理解されたい。特に、これは、上記で考察したロボットコントローラ８２０及びコンピュータ８３０のそれぞれにプロセッサを含む。具体的には、コントローラ８２０及び／又はコンピュータ８３０内のプロセッサは、前述の開示全体を通して説明した方法で、衝突回避最適化経路計画計算での制約関数として距離場行列を使用するように構成される。

【0075】

上記で概説したように、距離場制約関数を使用するロボット衝突回避運動最適化技術のための開示技術は、衝突回避のためのロボット経路計画の速度及び信頼性を改善する。開示技術は、障害物を幾何学的プリミティブとしてモデル化するという先行作業を回避し、複雑な形状をボクセルとして簡単かつ自動的に表現し、距離場行列の計算に使用することができるようにし、ロボットから障害物までの距離を、多数の障害物が存在する場合でも迅速に計算することができるようにする。

【0076】

距離場制約関数を使用するロボット衝突回避運動最適化技術のうちのいくつかの例示的な態様及び実施形態を上記で考察してきたが、当業者は、その修正、置換、追加及び副次的組み合わせを認識するであろう。このため、以下の添付の特許請求の範囲及び以下に導入される請求項は、その真の精神及び範囲内にあるようなそのような修正、置換、追加及び副次的組み合わせのいずれも含むと解釈されることが意図される。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6A】

【図6B】

【図7】

【図8】

【図9】

【外国語明細書】

2021160080000001.pdf