JP2019030930A - 相対的に移動する物体間の最近接点計算方法及び干渉評価方法 - Google Patents

Abstract

【課題】計算コストと、最近接点のジャンプが発生の発生が抑制と、物体形状設定の自由度のバランスをとって再接近距離を計算する。【解決手段】相対的に移動する２つの物体間の最近接距離および最近接点を計算する物体間距離評価方法であって、前記物体の形状を近似するモデル形状を設定するにあたり、前記物体を凸多面体で近似し、前記凸多面体に沿って、前記凸多面体の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む三次元形状モデルを設定し、作成された前記物体の前記三次元形状モデルを動かした際のモデル間の最近接距離および最近接点を計算する。【選択図】図５

Description

本発明は、相対的に移動する物体間の最近接点計算方法及び干渉評価方法に関する。

従来、このような分野の技術として、特開２０１０−１５５３２８号公報がある。例えば、ロボットなどの動作制御において、ロボットアームを動かす際に、物体間の干渉を避けながらロボットアーム先端のエンドエフェクタの軌道を生成する必要がある。物体間の干渉計算には、物体間の最短距離を算出する計算手順であるＧＪＫアルゴリズムを用いて最短距離を算出し、算出結果に基づいて干渉の有無を判断することができる。

特開２０１０−１５５３２８号公報

ロボットの干渉計算は、ロボットのプランニングや運動制御、異常の検出などの様々な領域で行われる。運動制御における干渉計算は、時々刻々と変化する各リンクや環境の情報に基づいて対象同時の距離をリアルタイムに計算し、干渉回避動作を行う必要がある。したがって干渉計算には、高速な距離計算に加えて、物体同士の最近接点の算出と、最近接点が連続的に滑らかに変化することが求められている。

ここで従来の最近接距離計算では、球、円柱、直方体、凸多面体等のプリミティブ形状が用いられており、計算を簡単化することで計算を高速化している。ところが、これらの球を除くプリミティブ形状では、形状同士が互いに平行になる場合があり、図１９（ａ）（ｂ）に示すように、その前後で最接近点が不連続にジャンプする場合があるというデメリットがある。最接近点の不連続なジャンプは、ロボットの制御指令値の急激な変化を意味するため、ロボットの動作が急激に変化し、過剰な制御量の発生や振動の誘発に繋がる場合がある。また、ロボットの姿勢や状況によっては、最近接点のジャンプが繰り返し往復するように発生し、チャタリングのような現象が発生することが考えられる。

そこで、形状同士の平行にならずジャンプが発生しないように、上記のプリミティブ形状に比べて複雑な形状を用いようとすると、計算コストが大幅に増加して、リアルタイム処理ができなくなるという問題が発生する。例えば、球同士の最近接距離計算は互いの中心を結んで両者の半径を引くだけで計算可能だが、球を歪ませた楕円球の形にすると、途端に最近接距離計算が複雑になる。

さらに、干渉検出用のシェープ形状は、できる限りロボットの形状にフィットしていることが望まれる。例えば、干渉検出用のシェープ形状が実際のロボットの形状と異なり大きく膨らんだ部位が存在していた場合に、実際には干渉検出や干渉回避が必要無いにも関わらず、干渉する（もしくは干渉しそうである）という判定の誤りを発生させ、動作を停止させたり変に膨らんだ軌道を描いたりして、作業を円滑に遂行できなくなるおそれがある。ここで、シェープ形状は、球、円柱、直方体、カプセル状などの単純な形状では無く、更に自由度の高い形状設定を可能にすることでロボットの形状に近づけることができるが、前述のように、形状の複雑度が少し上がるだけでも干渉計算が途端に複雑化してしまうデメリットがある。すなわち、自由度の高い形状設定と計算時間はトレードオフの関係であるが、ロボットの制御分野では計算時間の要件を満たすために、プリミティブ形状が主に用いられてきた。

したがって、ロボット等の干渉計算において、計算コストが小さく計算にかかる時間が短いこと、最近接点のジャンプが発生しないこと、ロボットアーム等計算の前提となる物体形状設定の自由度が高くロボットにフィットしたシェープを設定できること、の３つの要件を同時に満たすような干渉検出、距離計算法が望まれている。

本発明にかかる最近接点計算方法は、相対的に移動する２つの物体間の最近接距離および最近接点を計算する物体間距離評価方法であって、前記物体の形状を近似するモデル形状を設定するにあたり、前記物体を凸多面体で近似し、前記凸多面体に沿って、前記凸多面体の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む三次元形状モデルを設定し、作成された前記物体の前記三次元形状モデルを動かした際のモデル間の最近接距離および最近接点を計算する。
これにより、計算コストと、最近接点のジャンプが発生の発生が抑制と、物体形状設定の自由度のバランスをとった状態で、物体間の最近接距離の計算が可能である。

また、本発明にかかる干渉評価方法は、前記最近接点計算方法による最近接距離計算結果に基づいて、相対的に移動する前記物体間に干渉が発生するか否かを評価する。

これにより、計算コストが小さく、最近接点のジャンプが発生の発生が抑制されるとともに、物体形状設定の自由度が高い移動物体と他の物体の間の干渉評価方法を提供することができる。

サポート写像の一例を示す図である。 ＧＪＫアルゴリズムの概念図である。 ＧＪＫアルゴリズムのフローチャートである。 プリミティブ形状の例を示す図である。 サポート写像計算フローを示す図である。 サポート写像計算例を示す図である。 他のサポート写像計算例を示す図である。 ステップＳ２１で求めるパラメータを示す図である。 ステップＳ２１で求めるパラメータを示す図である。 法線ベクトルと方向ベクトルの方向がより一致する平面を選択する状態を示す図である。 方向ベクトルに沿って曲面上で最大値をとる点を探索する状態を示す図である。 平面内の座標点を示す図である。 スウィープ球の存在条件を満たす点の計算を示す図である。 ステップＳ２３のフローチャートを示した図である。 ステップＳ２３における写像球中心を求める計算の流れを示した図である。 サポート写像球中心の領域制限による移動を示した図である。 ＶＲＳＳＶによる丸みを帯びた凸形状の物体を示す図である。 ロボットに適用して干渉検出および距離計算を行う様子を示した図である。 関連する最接近点のジャンプの様子を示した図である。

以下、図面を参照して本発明の実施の形態について説明する。

２つの物体１１，１２は、例えばロボットの部位の一部であって、夫々異なる剛体である。例えば、物体１１は移動物体であり、物体１２に対して相対的に位置が変動する。なお、物体１１，１２はロボットの一部に限られず、相対的に位置が変動するものであればよい。なお、物体１１，１２は、ユーザーによって凸多面体として設定される。

演算装置１３は、メモリやＣＰＵ、記憶媒体等を備えており、後述するように物体１１，１２の三次元形状モデルの生成と、作成された物体１１，１２の三次元形状モデルを動かした際のモデル間の最近接距離を計算と、計算された最近接距離に基づいて物体１１，１２に干渉が発生するか否かの判定を実行する。なお典型的には、演算装置１３では、これらの三次元形状モデル作成、最近接距離の計算、干渉の発生の判定をそれぞれ行うためのソフトウェアを実行する。

ここで、ＧＪＫアルゴリズムについて説明する。ＧＪＫアルゴリズムは、２つの凸形状同士の干渉検出、最近接点および最近接距離を少ない計算量で高速に求められるアルゴリズムである。ＧＪＰアルゴリズムに、サポート写像というテクニックが提案されることによって、２つの凸形状同士の距離計算に用いられている。

後述するように、本発明のアルゴリズムは、ＧＪＫアルゴリズムの計算フローの中で用いるものであり、計算フロー入力は方向ベクトルｈとなる。本発明である移動物体と他の物体の間の干渉評価方法は、物体１１，１２をプリミティブ形状による三次元形状とすること、及び、そのサポート写像を求める方法を提案するものであって、ＧＪＫアルゴリズムと共に用いることを前提としている。

ここで、サポート写像の基本的な概念を説明する。図１は、凸形状の物体Ａに対して、ある方向ベクトルｈの方向にサポート写像を適用した状態を示している。すなわち、ある凸形状の物体Ａに対して、ある方向ベクトルｈが与えられたとき、凸形状内で方向ベクトルの方向に最大となる点、すなわち凸形状であればその点において接線や接平面が方向ベクトルと直交するような点を求める関数がサポート写像である。

次に、サポート写像を用いたＧＪＫアルゴリズムの計算プロセスについて説明する。図２には、２つの凸形状の最近接距離を求める場合のＧＪＫアルゴリズムの概念図を示している。なお、ロボットで実際に用いる際には３次元空間であるが、簡単のために２次元で示している。

ＧＪＫアルゴリズムでは、図３に示すように、始めに任意の方向にベクトルｈを設定し（ステップＳ１１）、サポート写像を用いて各凸形状の対向する２点（Ｓｕｐｐｏｒｔ Ｐｏｉｎｔ）を求め（ステップＳ１２）、求めた２点を結ぶ方向にベクトルｈを再設定する（ステップＳ１３）。その後ステップＳ１２に戻り、ステップＳ１２とステップＳ１３の処理を繰り返す。これにより、Ｓｕｐｐｏｒｔ Ｐｏｉｎｔの２点は徐々に各々がある点に収束していき、得られた収束点は２つの凸形状の最接近点となる。

このようにＧＪＫアルゴリズムでは、いかなる凸形状であっても、サポート写像さえ得られていれば、簡単に最接近点の計算を行うことができる。またＧＪＫ法自体は非常に簡単なロジックであり、サポート写像を求める計算コストが、ＧＪＫアルゴリズムにおける全体の計算処理コストに大きく寄与する。

したがってＧＪＫアルゴリズムでは、サポート写像として、円、多角形、楕円などの形状が用いられることが多い。円のサポート写像は非常に簡単に求めることが可能であり、
となる。すなわち、方向ベクトルを単位ベクトルに変換し、半径ｒを掛けた点がＳｕｐｐｏｒｔ Ｐｏｉｎｔになる。３次元における球でも同様である。また、２次元における多角形（３次元における多面体）においても簡単にサポート写像を定義することができ
となる。すなわち、方向ベクトルと頂点の内積が最も大きい値となる頂点がＳｕｐｐｏｔ Ｐｏｉｎｔとなる。したがって、全ての頂点について方向ベクトルと内積の計算をするだけで、Ｓｕｐｐｏｒｔ ｐｏｉｎｔを計算することが出来る。

なお、楕円のサポート写像は簡単に求めることができず、３次元における楕円球についても更に困難となるという問題がある。このような理由から、ＧＪＫアルゴリズムでは、サポート写像として、円、円柱、カプセル状、直方体が用いられる。

次に、図４を参照し、球体の半径を変化させながら線分に沿ってスウィープさせることにより、プリミティブ形状（以後ＶＲＳＳＶ）を形成することについて、先に説明する。図４（ａ）はプリミティブ形状の参考図であり、図４（ｂ）は球体の半径を変化させずに移動させて形成されるカプセル形状の参考図である。なお、球体について、
とする。ここで形状中心は、頂点の平均として定義される。すなわち、
ただし、
とする。これにより、球体スウィープ時に、球体の半径を球体中心からの距離の２次関数で定義することができる。

次に、サポート写像計算フローについて、図５を参照しながら説明する。なお、ステップＳ２１は事前準備として行うものであり、ステップＳ２２〜ステップＳ２５は実際にロボットの稼働中に実行される計算フローである。図６（ａ）〜（ｄ）及び図７（ａ）〜（ｄ）は、夫々ステップＳ２２〜ステップＳ２５に対応する写像計算の概念図である。以下では、凸多面体をベースとした例について説明を行う。なお、線分や凸多角形に関しても、対応するＶＲＳＳＶを計算可能である。

ユーザーにより物体１１，１２を凸多面体として設定し、凸多面体の頂点と最大・最小半径のパラメータに基づいて、形状中心の各平面の法線ベクトル、平面の姿勢行列、平面行列、スウィープ球の半径を変化させる２次関数の係数を求める。（ステップＳ２１）。

図３に示すように凸多面体の頂点が与えられたとすると、これらの頂点群に対して、クイックハル等の公知のアルゴリズムを用いることで、凸多角形を構成する平面ｋを求めることが出来る。そして、平面を構成する頂点間のベクトルの外積を計算することにより、法線ベクトル
が求められ、３次元空間における平面の姿勢（傾き）を表現する姿勢行列（回転行列）
も公知の手法により容易に計算可能である。また前述したように、全頂点の平均を計算することにより、形状中心
を計算し形状中心から各平面に垂線を下した際の黒点を平面基準点
とし、図８に示すように、形状中心と平面基準点の距離
を求めておく。さらに図９に示すように、平面基準点を原点として、平面内に任意の直交する２つの基底を用意して平面座標系[u,v]を定義し、平面内で多角形を構成する頂点のｕｖ座標系における座標値
を求めておく。

最後に、形状中心から最も遠い頂点と最も近い平面を探索し、
に基づいて、
として、係数α、βを求める。

これにより、スウィープさせる球の半径は、
の範囲に制限されることが保証される。すなわち、頂点によって構成された凸多面体に対して、
の範囲のふくらみを持った滑らかな凸形状が形成されることになる。

次に、方向ベクトルｈと法線の内積を計算して、計算結果が最大となる平面Ｐを探索する（ステップＳ２２）。すなわち、
により、平面Ｐを探索する。この計算ステップにより、図１０に示すように、方向ベクトルと法線ベクトルの方向がより一致している平面が選択される。

ステップＳ２２において選択した平面内において、図１１、図１２のように方向ベクトルに沿って曲面上で最大値をとる点（Ｓｐｐｏｒｔｉｎｇ Ｐｏｉｎｔ）を探索し、そのときのスウィープ球の平面内における位置（座標）を求める（ステップＳ２３）。これは、図１３に示すように、スウィープ球と、スウィープ球によって描かれる干渉検出のためのシェープの接点が、スウィープ球の中心から方向ベクトル一致する方向に存在している、という条件を満たす点を計算することにより求めることが出来る。

図１４はステップＳ２３において実行する計算を、フローチャートとして纏めたものである。また、図１５は、ステップＳ２３においてサポート写像球中心を求める計算の流れを纏めたものである。なお、ステップＳ２３、Ｓ２４では、平面内座標系を用いて計算を行うことにより大幅に計算量を削減する。

始めに、方向ベクトルｈを次のように平面ベクトルに変換する
ただし、
は、ステップＳ２１で求めた平面Ｐの傾きを表す回転行列である。Ｓｐｐｏｒｔｉｎｇ Ｐｏｉｎｔ ｐを求めるために、ステップＳ２３ではこのｈを用いて、ｕｖ座標系で表した
に対応するスウィープ球の中心
を求める。なお、ステップＳ２３、Ｓ４はｕｖ座標系で計算が行われ、ステップＳ２５にて、
と順番に変換することでｐを得る。なお、このｑを「ＶＲＳＳＶサポート写像球中心」と呼ぶ。

サポート写像球中心の求解は、最適化問題として定式化できる。すなわち、Ｓｕｐｐｏｒｔｉｎｇ Ｐｏｉｎｔが、
を中心としたスウィープ球面状に存在するための条件
と、スウィープ球とＶＲＳＳＶの曲面が
で接するための条件
を拘束条件として、方向ベクトルに沿って最も大きい点を求める。すなわち、
となるような解を求める問題になる。

ここで計算量を減らすために、新しい変数
を導入して
と定義する。このとき次の関係が成り立つ

また、幾何学的に明らかなように
なので、
と表すことができる。なお上記において、
と置いている。また式（１８）により
なので、式（１１）は
となり、これを式（１０）に代入することにより、
を得ることができる。さらに評価関数の項、式（１２）に関しても同様に変換を掛けると
を得ることができる。

ここで、式（２２）（２３）により最終的に解くべき最適化問題は、
となる。この最適化問題に対して、ラグランジュ法を適用し
を消去すると、
という非線形方程式が得られる。これは１変数の非線形方程式であり、かつ、唯一解を持つことが判っているので、残差
を定義して、微分情報
を用いてニュートン法を適用すると、反復計算によりｔを求めることができる。

さらに、式（２５）を近似して解析解を求め、ニュートン法の初期値に用いることが反復回数を大幅に減少させることができる。すなわち式（２５）の第３項を
のように、多項式で近似することで
として４次方程式が得られる。この４次方程式は公知のフェラーリ法を用いて解析的に解を求めることが可能である。また、得られた式（２９）の解をニュートン法の初期値に用いることで、式（２６）の残差が十分に小さくなり、多くのケースで反復計算が必要なく、又は、数回の反復で十分な精度の解が得られる。

最後に、得られた解ｔを、式（１４）を用いて
に戻すことで、ステップＳ２３を完了する。

次に、ステップＳ２３で求めた
が、平面内の頂点
で構成される多角形内に入っているかを調べる（ステップＳ２４）。これは対象とする頂点について、多角形に対する内外判定を行うことであるため、公知の技術を用いて行うことができる。

ここで、
が、多角形の外側にあると判定された場合には、図１６（ａ）に示すように、多角形の最近傍点に
を移動させる。なお図１６（ｂ）は、図１６（ａ）に示したＶＲＳＳＶサポート写像球中心に対する領域制限により移動する様子を、ｕｖ平面正面から見た状態を示した図である。

修正済みの
を用いて、ｕｖ座標系におけるＳｕｐｐｏｒｔｉｎｇ Ｐｏｉｎｔの計算を行い、更にグローバル座標系に戻す変換を掛けることにより、サポート写像関数出力
を得る（ステップＳ２５）。

これにより、従来の多角形を、本発明のＶＲＳＳＶにより膨らませた形状とした三次元形状モデルを得ることができる。図１７に示すように、ほぼ元の形状を再現しつつ連続的に丸みを帯びた形状を設定することができるため、最接近点のジャンプが発生せず、連続的に移動することが保証される。なお図１８は、本発明をロボットに適用し、三次元形状モデルが丸みを帯びた形状となるように設定しつつ、ＧＪＫ法により、最接近点の干渉検出および距離計算を行う様子を示した図である。

これにより、物体の形状に近似し、物体の骨格となりえる線分を設定した物体の形状を近似するモデル形状の設定が完了するとともに、線分に沿って、前記線分の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む、三次元形状モデルを設定することができる。

その後、演算装置１３はＧＪＫアルゴリズムを用いて、作成された前記物体の三次元形状モデルを動かした際のモデル間の最近接距離を計算し、計算された最近接距離に基づいて、相対的に移動する物体間に干渉が発生するか否かを評価することができる。

上述のステップＳ２３、Ｓ２４では、平面内座標系を用いて計算を行うことにより大幅に計算量を削減している。特に、ステップＳ２３では、平面座標系を後で変数返還することで１次原の方程式にまで縮退しており、1次原の非線形方程式の解を求める際には４次の方程式への近似と、その解析解を初期値とすることで、ほぼ反復することなく１回の計算で解を得られる。これにより、曲面構成の自由度の高さと、連続的な最近接点の遷移と、高速な干渉検出および距離計算の全てを満たすことが出来る。言い換えると、ＶＲＳＳＶの仕組みによって、曲面構成の自由度の高さと、連続的な最近接点の遷移の条件を満たしながら、ステップＳ２３、Ｓ２４において高速なＶＲＳＳＶのサポート写像計算（Ｓｐｐｏｒｔｉｎｇ Ｐｏｉｎｔを求める計算）が可能となる。

したがって、ロボットに適用した場合には、干渉回避動作が安定して行えるようになる。そのため、干渉回避において最近接点の不連続性に起因する急激な動作や振動を防止しつつ、干渉回避のための演算処理にかかる時間を十分に低減できる。

１１、１２ 移動物体
１３ 演算装置

Claims (2)

1. 相対的に移動する２つの物体間の最近接距離および最近接点を計算する物体間距離評価方法であって、
   前記物体の形状を近似するモデル形状を設定するにあたり、前記物体を凸多面体で近似し、
   前記凸多面体に沿って、前記凸多面体の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む三次元形状モデルを設定し、
   作成された前記物体の前記三次元形状モデルを動かした際のモデル間の最近接距離および最近接点を計算する、
   相対的に移動する物体間の最近接点計算方法。
2. 請求項１に記載の最近接距離計算結果に基づいて、相対的に移動する前記物体間に干渉が発生するか否かを評価する、
   相対的に移動する物体間の干渉評価方法。