(57)【要約】【課題】計算コストと、最近接点のジャンプが発生の発生が抑制と、物体形状設定の自由度のバランスをとって再接近距離を計算する。【解決手段】相対的に移動する２つの物体間の最近接距離および最近接点を計算する物体間距離評価方法であって、前記物体の形状を近似するモデル形状を設定するにあたり、前記物体を凸多面体で近似し、前記凸多面体に沿って、前記凸多面体の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む三次元形状モデルを設定し、作成された前記物体の前記三次元形状モデルを動かした際のモデル間の最近接距離および最近接点を計算する。【選択図】図５
2019030930.tif 000002
【特許請求の範囲】
【請求項１】
相対的に移動する２つの物体間の最近接距離および最近接点を計算する物体間距離評価方法であって、
前記物体の形状を近似するモデル形状を設定するにあたり、前記物体を凸多面体で近似し、
前記凸多面体に沿って、前記凸多面体の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む三次元形状モデルを設定し、
作成された前記物体の前記三次元形状モデルを動かした際のモデル間の最近接距離および最近接点を計算する、
相対的に移動する物体間の最近接点計算方法。
【請求項２】
請求項１に記載の最近接距離計算結果に基づいて、相対的に移動する前記物体間に干渉が発生するか否かを評価する、
相対的に移動する物体間の干渉評価方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、相対的に移動する物体間の最近接点計算方法及び干渉評価方法に関する。
【背景技術】
【０００２】
従来、このような分野の技術として、特開２０１０−１５５３２８号公報がある。例えば、ロボットなどの動作制御において、ロボットアームを動かす際に、物体間の干渉を避けながらロボットアーム先端のエンドエフェクタの軌道を生成する必要がある。物体間の干渉計算には、物体間の最短距離を算出する計算手順であるＧＪＫアルゴリズムを用いて最短距離を算出し、算出結果に基づいて干渉の有無を判断することができる。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−１５５３２８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ロボットの干渉計算は、ロボットのプランニングや運動制御、異常の検出などの様々な領域で行われる。運動制御における干渉計算は、時々刻々と変化する各リンクや環境の情報に基づいて対象同時の距離をリアルタイムに計算し、干渉回避動作を行う必要がある。したがって干渉計算には、高速な距離計算に加えて、物体同士の最近接点の算出と、最近接点が連続的に滑らかに変化することが求められている。
【０００５】
ここで従来の最近接距離計算では、球、円柱、直方体、凸多面体等のプリミティブ形状が用いられており、計算を簡単化することで計算を高速化している。ところが、これらの球を除くプリミティブ形状では、形状同士が互いに平行になる場合があり、図１９（ａ）（ｂ）に示すように、その前後で最接近点が不連続にジャンプする場合があるというデメリットがある。最接近点の不連続なジャンプは、ロボットの制御指令値の急激な変化を意味するため、ロボットの動作が急激に変化し、過剰な制御量の発生や振動の誘発に繋がる場合がある。また、ロボットの姿勢や状況によっては、最近接点のジャンプが繰り返し往復するように発生し、チャタリングのような現象が発生することが考えられる。
【０００６】
そこで、形状同士の平行にならずジャンプが発生しないように、上記のプリミティブ形状に比べて複雑な形状を用いようとすると、計算コストが大幅に増加して、リアルタイム処理ができなくなるという問題が発生する。例えば、球同士の最近接距離計算は互いの中心を結んで両者の半径を引くだけで計算可能だが、球を歪ませた楕円球の形にすると、途端に最近接距離計算が複雑になる。
【０００７】
さらに、干渉検出用のシェープ形状は、できる限りロボットの形状にフィットしていることが望まれる。例えば、干渉検出用のシェープ形状が実際のロボットの形状と異なり大きく膨らんだ部位が存在していた場合に、実際には干渉検出や干渉回避が必要無いにも関わらず、干渉する（もしくは干渉しそうである）という判定の誤りを発生させ、動作を停止させたり変に膨らんだ軌道を描いたりして、作業を円滑に遂行できなくなるおそれがある。ここで、シェープ形状は、球、円柱、直方体、カプセル状などの単純な形状では無く、更に自由度の高い形状設定を可能にすることでロボットの形状に近づけることができるが、前述のように、形状の複雑度が少し上がるだけでも干渉計算が途端に複雑化してしまうデメリットがある。すなわち、自由度の高い形状設定と計算時間はトレードオフの関係であるが、ロボットの制御分野では計算時間の要件を満たすために、プリミティブ形状が主に用いられてきた。
【０００８】
したがって、ロボット等の干渉計算において、計算コストが小さく計算にかかる時間が短いこと、最近接点のジャンプが発生しないこと、ロボットアーム等計算の前提となる物体形状設定の自由度が高くロボットにフィットしたシェープを設定できること、の３つの要件を同時に満たすような干渉検出、距離計算法が望まれている。
【課題を解決するための手段】
【０００９】
本発明にかかる最近接点計算方法は、相対的に移動する２つの物体間の最近接距離および最近接点を計算する物体間距離評価方法であって、前記物体の形状を近似するモデル形状を設定するにあたり、前記物体を凸多面体で近似し、前記凸多面体に沿って、前記凸多面体の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む三次元形状モデルを設定し、作成された前記物体の前記三次元形状モデルを動かした際のモデル間の最近接距離および最近接点を計算する。
これにより、計算コストと、最近接点のジャンプが発生の発生が抑制と、物体形状設定の自由度のバランスをとった状態で、物体間の最近接距離の計算が可能である。
【００１０】
また、本発明にかかる干渉評価方法は、前記最近接点計算方法による最近接距離計算結果に基づいて、相対的に移動する前記物体間に干渉が発生するか否かを評価する。
【発明の効果】
【００１１】
これにより、計算コストが小さく、最近接点のジャンプが発生の発生が抑制されるとともに、物体形状設定の自由度が高い移動物体と他の物体の間の干渉評価方法を提供することができる。
【図面の簡単な説明】
【００１２】
【図１】サポート写像の一例を示す図である。
【図２】ＧＪＫアルゴリズムの概念図である。
【図３】ＧＪＫアルゴリズムのフローチャートである。
【図４】プリミティブ形状の例を示す図である。
【図５】サポート写像計算フローを示す図である。
【図６】サポート写像計算例を示す図である。
【図７】他のサポート写像計算例を示す図である。
【図８】ステップＳ２１で求めるパラメータを示す図である。
【図９】ステップＳ２１で求めるパラメータを示す図である。
【図１０】法線ベクトルと方向ベクトルの方向がより一致する平面を選択する状態を示す図である。
【図１１】方向ベクトルに沿って曲面上で最大値をとる点を探索する状態を示す図である。
【図１２】平面内の座標点を示す図である。
【図１３】スウィープ球の存在条件を満たす点の計算を示す図である。
【図１４】ステップＳ２３のフローチャートを示した図である。
【図１５】ステップＳ２３における写像球中心を求める計算の流れを示した図である。
【図１６】サポート写像球中心の領域制限による移動を示した図である。
【図１７】ＶＲＳＳＶによる丸みを帯びた凸形状の物体を示す図である。
【図１８】ロボットに適用して干渉検出および距離計算を行う様子を示した図である。
【図１９】関連する最接近点のジャンプの様子を示した図である。
【発明を実施するための形態】
【００１３】
以下、図面を参照して本発明の実施の形態について説明する。
【００１４】
２つの物体１１，１２は、例えばロボットの部位の一部であって、夫々異なる剛体である。例えば、物体１１は移動物体であり、物体１２に対して相対的に位置が変動する。なお、物体１１，１２はロボットの一部に限られず、相対的に位置が変動するものであればよい。なお、物体１１，１２は、ユーザーによって凸多面体として設定される。
【００１５】
演算装置１３は、メモリやＣＰＵ、記憶媒体等を備えており、後述するように物体１１，１２の三次元形状モデルの生成と、作成された物体１１，１２の三次元形状モデルを動かした際のモデル間の最近接距離を計算と、計算された最近接距離に基づいて物体１１，１２に干渉が発生するか否かの判定を実行する。なお典型的には、演算装置１３では、これらの三次元形状モデル作成、最近接距離の計算、干渉の発生の判定をそれぞれ行うためのソフトウェアを実行する。
【００１６】
ここで、ＧＪＫアルゴリズムについて説明する。ＧＪＫアルゴリズムは、２つの凸形状同士の干渉検出、最近接点および最近接距離を少ない計算量で高速に求められるアルゴリズムである。ＧＪＰアルゴリズムに、サポート写像というテクニックが提案されることによって、２つの凸形状同士の距離計算に用いられている。
【００１７】
後述するように、本発明のアルゴリズムは、ＧＪＫアルゴリズムの計算フローの中で用いるものであり、計算フロー入力は方向ベクトルｈとなる。本発明である移動物体と他の物体の間の干渉評価方法は、物体１１，１２をプリミティブ形状による三次元形状とすること、及び、そのサポート写像を求める方法を提案するものであって、ＧＪＫアルゴリズムと共に用いることを前提としている。
【００１８】
ここで、サポート写像の基本的な概念を説明する。図１は、凸形状の物体Ａに対して、ある方向ベクトルｈの方向にサポート写像を適用した状態を示している。すなわち、ある凸形状の物体Ａに対して、ある方向ベクトルｈが与えられたとき、凸形状内で方向ベクトルの方向に最大となる点、すなわち凸形状であればその点において接線や接平面が方向ベクトルと直交するような点を求める関数がサポート写像である。
【００１９】
次に、サポート写像を用いたＧＪＫアルゴリズムの計算プロセスについて説明する。図２には、２つの凸形状の最近接距離を求める場合のＧＪＫアルゴリズムの概念図を示している。なお、ロボットで実際に用いる際には３次元空間であるが、簡単のために２次元で示している。
【００２０】
ＧＪＫアルゴリズムでは、図３に示すように、始めに任意の方向にベクトルｈを設定し（ステップＳ１１）、サポート写像を用いて各凸形状の対向する２点（Ｓｕｐｐｏｒｔ  Ｐｏｉｎｔ）を求め（ステップＳ１２）、求めた２点を結ぶ方向にベクトルｈを再設定する（ステップＳ１３）。その後ステップＳ１２に戻り、ステップＳ１２とステップＳ１３の処理を繰り返す。これにより、Ｓｕｐｐｏｒｔ  Ｐｏｉｎｔの２点は徐々に各々がある点に収束していき、得られた収束点は２つの凸形状の最接近点となる。
【００２１】
このようにＧＪＫアルゴリズムでは、いかなる凸形状であっても、サポート写像さえ得られていれば、簡単に最接近点の計算を行うことができる。またＧＪＫ法自体は非常に簡単なロジックであり、サポート写像を求める計算コストが、ＧＪＫアルゴリズムにおける全体の計算処理コストに大きく寄与する。
【００２２】
したがってＧＪＫアルゴリズムでは、サポート写像として、円、多角形、楕円などの形状が用いられることが多い。円のサポート写像は非常に簡単に求めることが可能であり、
【数１】
2019030930.tif 000003
となる。すなわち、方向ベクトルを単位ベクトルに変換し、半径ｒを掛けた点がＳｕｐｐｏｒｔ  Ｐｏｉｎｔになる。３次元における球でも同様である。また、２次元における多角形（３次元における多面体）においても簡単にサポート写像を定義することができ
【数２】
2019030930.tif 000004
となる。すなわち、方向ベクトルと頂点の内積が最も大きい値となる頂点がＳｕｐｐｏｔ  Ｐｏｉｎｔとなる。したがって、全ての頂点について方向ベクトルと内積の計算をするだけで、Ｓｕｐｐｏｒｔ  ｐｏｉｎｔを計算することが出来る。
【００２３】
なお、楕円のサポート写像は簡単に求めることができず、３次元における楕円球についても更に困難となるという問題がある。このような理由から、ＧＪＫアルゴリズムでは、サポート写像として、円、円柱、カプセル状、直方体が用いられる。
【００２４】
次に、図４を参照し、球体の半径を変化させながら線分に沿ってスウィープさせることにより、プリミティブ形状（以後ＶＲＳＳＶ）を形成することについて、先に説明する。図４（ａ）はプリミティブ形状の参考図であり、図４（ｂ）は球体の半径を変化させずに移動させて形成されるカプセル形状の参考図である。なお、球体について、
【数３】
2019030930.tif 000005
【数４】
2019030930.tif 000006
とする。ここで形状中心は、頂点の平均として定義される。すなわち、
【数５】
2019030930.tif 000007
ただし、
【数６】
2019030930.tif 000008
【数７】
2019030930.tif 000009
とする。これにより、球体スウィープ時に、球体の半径を球体中心からの距離の２次関数で定義することができる。
【００２５】
次に、サポート写像計算フローについて、図５を参照しながら説明する。なお、ステップＳ２１は事前準備として行うものであり、ステップＳ２２〜ステップＳ２５は実際にロボットの稼働中に実行される計算フローである。図６（ａ）〜（ｄ）及び図７（ａ）〜（ｄ）は、夫々ステップＳ２２〜ステップＳ２５に対応する写像計算の概念図である。以下では、凸多面体をベースとした例について説明を行う。なお、線分や凸多角形に関しても、対応するＶＲＳＳＶを計算可能である。
【００２６】
ユーザーにより物体１１，１２を凸多面体として設定し、凸多面体の頂点と最大・最小半径のパラメータに基づいて、形状中心の各平面の法線ベクトル、平面の姿勢行列、平面行列、スウィープ球の半径を変化させる２次関数の係数を求める。（ステップＳ２１）。
【００２７】
図３に示すように凸多面体の頂点が与えられたとすると、これらの頂点群に対して、クイックハル等の公知のアルゴリズムを用いることで、凸多角形を構成する平面ｋを求めることが出来る。そして、平面を構成する頂点間のベクトルの外積を計算することにより、法線ベクトル
【数８】
2019030930.tif 000010
が求められ、３次元空間における平面の姿勢（傾き）を表現する姿勢行列（回転行列）
【数９】
2019030930.tif 000011
も公知の手法により容易に計算可能である。また前述したように、全頂点の平均を計算することにより、形状中心
【数１０】
2019030930.tif 000012
を計算し形状中心から各平面に垂線を下した際の黒点を平面基準点
【数１１】
2019030930.tif 000013
とし、図８に示すように、形状中心と平面基準点の距離
【数１２】
2019030930.tif 000014
を求めておく。さらに図９に示すように、平面基準点を原点として、平面内に任意の直交する２つの基底を用意して平面座標系[u,v]を定義し、平面内で多角形を構成する頂点のｕｖ座標系における座標値
【数１３】
2019030930.tif 000015
を求めておく。
【００２８】
最後に、形状中心から最も遠い頂点と最も近い平面を探索し、
【数１４】
2019030930.tif 000016
に基づいて、
【数１５】
2019030930.tif 000017
【数１６】
2019030930.tif 000018
【数１７】
2019030930.tif 000019
【数１８】
2019030930.tif 000020
として、係数α、βを求める。
【００２９】
これにより、スウィープさせる球の半径は、
【数１９】
2019030930.tif 000021
の範囲に制限されることが保証される。すなわち、頂点によって構成された凸多面体に対して、
【数２０】
2019030930.tif 000022
の範囲のふくらみを持った滑らかな凸形状が形成されることになる。
【００３０】
次に、方向ベクトルｈと法線の内積を計算して、計算結果が最大となる平面Ｐを探索する（ステップＳ２２）。すなわち、
【数２１】
2019030930.tif 000023
により、平面Ｐを探索する。この計算ステップにより、図１０に示すように、方向ベクトルと法線ベクトルの方向がより一致している平面が選択される。
【００３１】
ステップＳ２２において選択した平面内において、図１１、図１２のように方向ベクトルに沿って曲面上で最大値をとる点（Ｓｐｐｏｒｔｉｎｇ  Ｐｏｉｎｔ）を探索し、そのときのスウィープ球の平面内における位置（座標）を求める（ステップＳ２３）。これは、図１３に示すように、スウィープ球と、スウィープ球によって描かれる干渉検出のためのシェープの接点が、スウィープ球の中心から方向ベクトル一致する方向に存在している、という条件を満たす点を計算することにより求めることが出来る。
【００３２】
図１４はステップＳ２３において実行する計算を、フローチャートとして纏めたものである。また、図１５は、ステップＳ２３においてサポート写像球中心を求める計算の流れを纏めたものである。なお、ステップＳ２３、Ｓ２４では、平面内座標系を用いて計算を行うことにより大幅に計算量を削減する。
【００３３】
始めに、方向ベクトルｈを次のように平面ベクトルに変換する
【数２２】
2019030930.tif 000024
ただし、
【数２３】
2019030930.tif 000025
は、ステップＳ２１で求めた平面Ｐの傾きを表す回転行列である。Ｓｐｐｏｒｔｉｎｇ  Ｐｏｉｎｔ  ｐを求めるために、ステップＳ２３ではこのｈを用いて、ｕｖ座標系で表した
【数２４】
2019030930.tif 000026
に対応するスウィープ球の中心
【数２５】
2019030930.tif 000027
を求める。なお、ステップＳ２３、Ｓ４はｕｖ座標系で計算が行われ、ステップＳ２５にて、
【数２６】
2019030930.tif 000028
と順番に変換することでｐを得る。なお、このｑを「ＶＲＳＳＶサポート写像球中心」と呼ぶ。
【００３４】
サポート写像球中心の求解は、最適化問題として定式化できる。すなわち、Ｓｕｐｐｏｒｔｉｎｇ  Ｐｏｉｎｔが、
【数２７】
2019030930.tif 000029
を中心としたスウィープ球面状に存在するための条件
【数２８】
2019030930.tif 000030
と、スウィープ球とＶＲＳＳＶの曲面が
【数２９】
2019030930.tif 000031
で接するための条件
【数３０】
2019030930.tif 000032
を拘束条件として、方向ベクトルに沿って最も大きい点を求める。すなわち、
【数３１】
2019030930.tif 000033
となるような解を求める問題になる。
【００３５】
ここで計算量を減らすために、新しい変数
【数３２】
2019030930.tif 000034
を導入して
【数３３】
2019030930.tif 000035
【数３４】
2019030930.tif 000036
と定義する。このとき次の関係が成り立つ
【数３５】
2019030930.tif 000037
【数３６】
2019030930.tif 000038
【００３６】
また、幾何学的に明らかなように
【数３７】
2019030930.tif 000039
なので、
【数３８】
2019030930.tif 000040
と表すことができる。なお上記において、
【数３９】
2019030930.tif 000041
と置いている。また式（１８）により
【数４０】
2019030930.tif 000042
なので、式（１１）は
【数４１】
2019030930.tif 000043
となり、これを式（１０）に代入することにより、
【数４２】
2019030930.tif 000044
を得ることができる。さらに評価関数の項、式（１２）に関しても同様に変換を掛けると
【数４３】
2019030930.tif 000045
を得ることができる。
【００３７】
ここで、式（２２）（２３）により最終的に解くべき最適化問題は、
【数４４】
2019030930.tif 000046
となる。この最適化問題に対して、ラグランジュ法を適用し
【数４５】
2019030930.tif 000047
を消去すると、
【数４６】
2019030930.tif 000048
という非線形方程式が得られる。これは１変数の非線形方程式であり、かつ、唯一解を持つことが判っているので、残差
【数４７】
2019030930.tif 000049
を定義して、微分情報
【数４８】
2019030930.tif 000050
を用いてニュートン法を適用すると、反復計算によりｔを求めることができる。
【００３８】
さらに、式（２５）を近似して解析解を求め、ニュートン法の初期値に用いることが反復回数を大幅に減少させることができる。すなわち式（２５）の第３項を
【数４９】
2019030930.tif 000051
のように、多項式で近似することで
【数５０】
2019030930.tif 000052
として４次方程式が得られる。この４次方程式は公知のフェラーリ法を用いて解析的に解を求めることが可能である。また、得られた式（２９）の解をニュートン法の初期値に用いることで、式（２６）の残差が十分に小さくなり、多くのケースで反復計算が必要なく、又は、数回の反復で十分な精度の解が得られる。
【００３９】
最後に、得られた解ｔを、式（１４）を用いて
【数５１】
2019030930.tif 000053
に戻すことで、ステップＳ２３を完了する。
【００４０】
次に、ステップＳ２３で求めた
【数５２】
2019030930.tif 000054
が、平面内の頂点
【数５３】
2019030930.tif 000055
で構成される多角形内に入っているかを調べる（ステップＳ２４）。これは対象とする頂点について、多角形に対する内外判定を行うことであるため、公知の技術を用いて行うことができる。
【００４１】
ここで、
【数５４】
2019030930.tif 000056
が、多角形の外側にあると判定された場合には、図１６（ａ）に示すように、多角形の最近傍点に
【数５５】
2019030930.tif 000057
を移動させる。なお図１６（ｂ）は、図１６（ａ）に示したＶＲＳＳＶサポート写像球中心に対する領域制限により移動する様子を、ｕｖ平面正面から見た状態を示した図である。
【００４２】
修正済みの
【数５６】
2019030930.tif 000058
を用いて、ｕｖ座標系におけるＳｕｐｐｏｒｔｉｎｇ  Ｐｏｉｎｔの計算を行い、更にグローバル座標系に戻す変換を掛けることにより、サポート写像関数出力
【数５７】
2019030930.tif 000059
を得る（ステップＳ２５）。
【００４３】
これにより、従来の多角形を、本発明のＶＲＳＳＶにより膨らませた形状とした三次元形状モデルを得ることができる。図１７に示すように、ほぼ元の形状を再現しつつ連続的に丸みを帯びた形状を設定することができるため、最接近点のジャンプが発生せず、連続的に移動することが保証される。なお図１８は、本発明をロボットに適用し、三次元形状モデルが丸みを帯びた形状となるように設定しつつ、ＧＪＫ法により、最接近点の干渉検出および距離計算を行う様子を示した図である。
【００４４】
これにより、物体の形状に近似し、物体の骨格となりえる線分を設定した物体の形状を近似するモデル形状の設定が完了するとともに、線分に沿って、前記線分の中心から離れるに従って半径が小さくなる球を移動させて形成した三次元形状を含む、三次元形状モデルを設定することができる。
【００４５】
その後、演算装置１３はＧＪＫアルゴリズムを用いて、作成された前記物体の三次元形状モデルを動かした際のモデル間の最近接距離を計算し、計算された最近接距離に基づいて、相対的に移動する物体間に干渉が発生するか否かを評価することができる。
【００４６】
上述のステップＳ２３、Ｓ２４では、平面内座標系を用いて計算を行うことにより大幅に計算量を削減している。特に、ステップＳ２３では、平面座標系を後で変数返還することで１次原の方程式にまで縮退しており、1次原の非線形方程式の解を求める際には４次の方程式への近似と、その解析解を初期値とすることで、ほぼ反復することなく１回の計算で解を得られる。これにより、曲面構成の自由度の高さと、連続的な最近接点の遷移と、高速な干渉検出および距離計算の全てを満たすことが出来る。言い換えると、ＶＲＳＳＶの仕組みによって、曲面構成の自由度の高さと、連続的な最近接点の遷移の条件を満たしながら、ステップＳ２３、Ｓ２４において高速なＶＲＳＳＶのサポート写像計算（Ｓｐｐｏｒｔｉｎｇ  Ｐｏｉｎｔを求める計算）が可能となる。
【００４７】
したがって、ロボットに適用した場合には、干渉回避動作が安定して行えるようになる。そのため、干渉回避において最近接点の不連続性に起因する急激な動作や振動を防止しつつ、干渉回避のための演算処理にかかる時間を十分に低減できる。
【符号の説明】
【００４８】
１１、１２  移動物体
１３  演算装置
【図１】
2019030930.tif 000060
【図２】
2019030930.tif 000061
【図３】
2019030930.tif 000062
【図４】
2019030930.tif 000063
【図５】
2019030930.tif 000064
【図６】
2019030930.tif 000065
【図７】
2019030930.tif 000066
【図８】
2019030930.tif 000067
【図９】
2019030930.tif 000068
【図１０】
2019030930.tif 000069
【図１１】
2019030930.tif 000070
【図１２】
2019030930.tif 000071
【図１３】
2019030930.tif 000072
【図１４】
2019030930.tif 000073
【図１５】
2019030930.tif 000074
【図１６】
2019030930.tif 000075
【図１７】
2019030930.tif 000076
【図１８】
2019030930.tif 000077
【図１９】
2019030930.tif 000078