(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024154872
(43)【公開日】2024-10-31
(54)【発明の名称】干渉検知装置および干渉検知方法
(51)【国際特許分類】
G08G 1/16 20060101AFI20241024BHJP
G05D 1/43 20240101ALI20241024BHJP
【ＦＩ】
G08G1/16 C
G05D1/02 S
【審査請求】有
【請求項の数】7
【出願形態】ＯＬ
(21)【出願番号】P 2023069074
(22)【出願日】2023-04-20
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成３１年度、国立研究開発法人新エネルギー・産業技術総合開発機構「次世代人工知能・ロボットの中核となるインテグレート技術開発／人工知能技術の社会実装に向けた研究開発・実証／機械学習による生産支援ロボットの現場導入期間削減と多能化」委託事業、産業技術力強化法第１７条の適用を受ける特許出願
(71)【出願人】
【識別番号】000003458
【氏名又は名称】芝浦機械株式会社
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】中村 陽一郎
【テーマコード（参考）】
5H181
5H301
【Ｆターム（参考）】
5H181AA01
5H181AA07
5H181AA26
5H181AA27
5H181CC03
5H181CC04
5H181LL01
5H181LL02
5H181LL09
5H301CC03
5H301CC04
5H301LL01
5H301LL02
5H301LL03
5H301LL06
5H301LL14
(57)【要約】
【課題】干渉有無を検知する干渉検知領域に少ないデータ量で多様な形状を与え、周囲に存在する物体と干渉検知領域との干渉有無と、同物体と同干渉検知領域との間の最短距離を、容易かつ短時間に求めることができる干渉検知の実現手段を提供する。
【解決手段】本実施形態に係る干渉検知装置は、計測した周辺物体との距離に基づき、設定した３次元または２次元領域と前記周辺物体との干渉の有無を検知する干渉検知装置であって、干渉有無を検知する干渉検知領域を、超２次関数形式で保持する干渉検知領域の形状表現手段と、を備えることを特徴とする。
【選択図】図１
【解決手段】本実施形態に係る干渉検知装置は、計測した周辺物体との距離に基づき、設定した３次元または２次元領域と前記周辺物体との干渉の有無を検知する干渉検知装置であって、干渉有無を検知する干渉検知領域を、超２次関数形式で保持する干渉検知領域の形状表現手段と、を備えることを特徴とする。
【選択図】図１
【特許請求の範囲】
【請求項1】
計測した周辺物体との距離に基づき、設定した３次元または２次元領域と前記周辺物体との干渉の有無を検知する干渉検知装置であって、
干渉有無を検知する干渉検知領域を、超２次関数形式で保持する干渉検知領域の形状表現手段と、
を備えることを特徴とする、干渉検知装置。
干渉有無を検知する干渉検知領域を、超２次関数形式で保持する干渉検知領域の形状表現手段と、
を備えることを特徴とする、干渉検知装置。
【請求項2】
干渉有無を検知する前記干渉検知領域を表す超２次関数曲面の少数の設定パラメータを記憶し、手動または外部からの信号または通信により設定または切り替える干渉検知領域形状の設定切替手段と、
を備えることを特徴とする請求項１に記載の干渉検知装置。
を備えることを特徴とする請求項１に記載の干渉検知装置。
【請求項3】
前記干渉検知領域を表す超２次関数曲面上の点をサンプルすることにより、前記干渉検知装置の周囲に存在する周囲物体上の距離計測点と前記干渉検知領域との間の最短距離を算出する、干渉検知領域との最短距離算出手段と、
を備えることを特徴とする、請求項１または２に記載の干渉検知装置。
を備えることを特徴とする、請求項１または２に記載の干渉検知装置。
【請求項4】
前記干渉検知装置の周辺空間内に接近禁止領域に相当する前記超２次関数形式で表される干渉検知領域を設定し、
移動体の制御手段から与えられる前記移動体の移動軌道のサンプル点が前記干渉検知領域に干渉するかどうかを、前記干渉検知領域を表す超２次関数曲面に対する各軌道サンプル点の内外判定計算により判断する干渉検知手段と、
を備えることを特徴とする、請求項３に記載の干渉検知装置。
移動体の制御手段から与えられる前記移動体の移動軌道のサンプル点が前記干渉検知領域に干渉するかどうかを、前記干渉検知領域を表す超２次関数曲面に対する各軌道サンプル点の内外判定計算により判断する干渉検知手段と、
を備えることを特徴とする、請求項３に記載の干渉検知装置。
【請求項5】
前記干渉検知装置の周辺空間内に接近禁止領域に相当する前記超２次関数形式で表される干渉検知領域を設定し、
前記干渉検知装置を制御装置の一部として内蔵する移動体が所定の軌道に沿って移動する場合に、当該移動体の構成部分の一部または全体の形状を点群データで表すとともに、前記構成部分の一部または全体が前記干渉検知領域に干渉するかどうかを、前記干渉検知領域を表す超２次関数曲面に対する点群各点の内外判定計算により判断する干渉検知手段と、
を備えることを特徴とする、請求項３に記載の干渉検知装置。
前記干渉検知装置を制御装置の一部として内蔵する移動体が所定の軌道に沿って移動する場合に、当該移動体の構成部分の一部または全体の形状を点群データで表すとともに、前記構成部分の一部または全体が前記干渉検知領域に干渉するかどうかを、前記干渉検知領域を表す超２次関数曲面に対する点群各点の内外判定計算により判断する干渉検知手段と、
を備えることを特徴とする、請求項３に記載の干渉検知装置。
【請求項6】
前記干渉検知領域は、前記干渉検知装置を制御装置の一部として内蔵する移動体のボディ概形状またはボディ形状を一定距離オフセットさせた形状を、一つ以上の超２次関数で表す、
請求項３に記載の干渉検知装置。
請求項３に記載の干渉検知装置。
【請求項7】
計測した周辺物体との距離に基づき、設定した３次元または２次元領域と前記周辺物体との干渉の有無を検知する干渉検知方法であって、
干渉有無を検知する干渉検知領域を、超２次関数形式で保持するステップと、
を含むことを特徴とする、干渉検知方法。
干渉有無を検知する干渉検知領域を、超２次関数形式で保持するステップと、
を含むことを特徴とする、干渉検知方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、自動車や建設や農業用の車輌、ドローンや無人搬送車を含む移動ロボット、多関節型ロボットの可動するマニピュレータ部分など、有人運転走行または無人自律走行のいずれをも問わない様々な移動体と、その周辺の物体との干渉検出や干渉回避の技術に関する干渉検知装置および干渉検知方法である。さらに本発明は、ロボットや、セイフティスキャナなどのセンサにおける、当該ロボットやセンサ周辺に設定された干渉検知領域への人や物体の侵入検知、およびロボットやセンサ自体の干渉検知領域への侵入検知の技術に関わる干渉検知装置および干渉検知方法である。
【背景技術】
【0002】
自動車分野では現在、周囲環境を認識するカメラや、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉоｎ ａｎｄ Ｒａｎｇｉｎｇ）を備え、人や他車との衝突を回避したり、自動的に駐車をしたり、さらに自動走行をする、自律または半自律走行機能を持つ自動車が実用化されてきている。建設車輌や農業車輌の分野においても同様に、それぞれ無人で施工や耕うんが可能な車輌も実現されている。
【0003】
また、ロボット分野では現在、工場や物流施設内で工作物や材料、各種製品等の搬送を行うロボットや無人搬送車、家庭内で活動する自走式の掃除ロボット、屋内外を飛行し撮像や物資を運搬するドローンなど、自律移動型のロボットの実用化が進んでいる。自律移動型ロボットの中には、走行移動部の上に多関節型のマニピュレータを搭載した、移動マニピュレータ型のロボットもある。基部を床面や設備上の台座に据付固定する多関節型のマニピュレータも、基部から先の腕に相当するリンク部分は、一種の移動体とみなせる。
【0004】
さらに、センサ分野では現在、セイフティスキャナと呼ばれる安全用のセンサがあり、多関節型マニピュレータを利用した設備を含む各種の自動化設備において、人や物流用車輌などの設備への接近を検知するために用いられている。セイフティスキャナは、固定された設備に設置されるほか、自律移動するロボットや無人搬送車に搭載されることもある。こうした様々な車輌やロボットなど移動体の移動中における障害物との衝突や干渉の検知と回避、あるいは自動化設備におけるセンサによる設備に接近する人や車輌との衝突や干渉の検知と回避は、重要な技術になっている。
【0005】
例えば、自動車では近年、道路上での自動運転だけではなく、より狭いスペースでの自動駐車も実現されている。また、移動ロボットや無人搬送車は、専用に設けられた走行レーンを走行移動する事例よりも、人と同じ通路を走行する事例が多くなってきている。このように、自動車や移動ロボット、農業や建設用の車輌等も含む移動体が、有人か無人にかかわらず、できるだけ止まらずにより狭い空間内でより緻密な移動と、移動を伴う作業をするようになると、移動体に搭載されたＬｉＤＡＲ等の距離計測手段が、進行方向の一定距離内に障害物を検知した場合に停止するといった粗い衝突回避では不十分で、移動を止めずに進行方向に対し直交方向にある障害物をすり抜けつつ、進行方向にある障害物にぎりぎりまで接近するといった、より緻密な衝突回避が必要になる。
【0006】
移動ロボットについては、走行部の上にマニピュレータを備えた形式のものも増加してきている。こうしたマニピュレータを備えた移動ロボットについては、人の腕に相当するマニピュレータ部分が作業中に作業対象以外の周囲の物体と衝突することを防ぐ必要があるほか、マニピュレータが移動ロボットのボディ自体に衝突することも防ぐ必要がある。後者の場合、マニピュレータの肘に相当する部分が、ボディのくびれにより、周長が小さい腰部分とは離れているものの、周長が大きい胸部分には接触しそうになっているような場合がある。
【0007】
また、移動ロボットがマニピュレータでドアノブを握ってドアを開け、ドアを通り抜けるような動作を行う場合、自らが開けているドアがボディの左前側から近づいているから、右後側へ少し後退するといった動作も必要になる。
【0008】
移動ロボットに含まれるドローンについては今後、行政機関や事業者が設けた接近禁止空域の情報をドローンの管理者が手動で、またはドローンの制御装置自体が自動で更新し、操縦者がそうした接近禁止空域にドローンを故意または誤って近づけようとした場合も、接近禁止空域には進入できないような制御がなされていくことも想定される。このような場合には接近禁止空域を、様々な形状の建物をおおまかに包絡するような形状や、鉄塔や煙突の周囲一定範囲を覆う円筒状や直方体状、ある地点を中心にした半球状など、多様に設定できることが望まれる。
【0009】
この場合、これら多様な接近禁止空域形状を同一の枠組みかつ少ないデータ量で表現できると、飛行の回避処理も同一の枠組み、かつ少ない計算資源で実行できるため、好ましい。ドローンについてはさらに、例えば特定の田畑のみに手動操作で農薬を散布するような場合、逆に散布対象の田畑上に干渉検知領域を設定しておき散布飛行させ、散布対象の田畑からドローンが外れると干渉検知装置の出力が干渉状態から非干渉状態に変わることを人またはドローンの制御装置が察知して、ドローンが再び対象田畑に入るよう逆方向にドローンを戻すというような使い方もできる。田畑が広く、地表の操作地点からではドローンが操作地点から離れた地点で対象田畑から外れたかどうかが把握しづらい場合に有用である。
【0010】
農業や建設用の車輌についても無人運転や無人施工の実用化が進んできているが、今後は無人化を前提に広くかつ単純な区画形状の圃場や、周囲の樹木等を伐採し整備したような工事現場だけでなく、現在人が巧妙かつ注意深く作業や施工を行っているような圃場や工事現場にも、無人化が求められていく。このような場合、単に地面上にある、他の車輌や人との衝突回避だけではなく、高い位置に突き出た、例えば、撤去を省いた樹木の枝やその他構造物との接触や衝突を避けるような必要も生じる。この場合には、同じ枝に対しても搭乗部とエンジンボンネット部では最短距離や最短距離方向が異なり、その結果接触の危険性も両者で異なるといったことも想定される。
【0011】
衝突や干渉を防止する安全センサについては、２次元平面内で物体を検出する形式に加え、３次元空間内で物体を検出する形式も実用化されている。したがって、３次元空間に多様な形状の干渉検知領域をできるだけ簡単に設定可能にすることが求められてくる。
【0012】
産業用の多関節型マニピュレータについては従来から、マニピュレータ周辺の空間に直方体状の領域を仮想的に設け、当該領域に手先が入った場合に制御装置から外部に信号を出力する機能が備えられていることが多い。この機能は当該領域への接近を禁止しその領域内の設備類を保護したり、他の協調するロボットや設備とのインタロックをとったりするような安全上の目的のほか、他の協調するロボットや設備に対し動作開始指令を出すように、干渉検知を積極的に制御に用いる目的でも使用されている。しかし、当該領域形状は、設定が容易な直方体形状程度に限られている。
【0013】
以上をまとめると、各分野における移動体やセンサの運用形態が高度化するにしたがい、移動体と周囲の物体との衝突の検知と回避、あるいはセンサが設定する干渉検知領域と物体や人との干渉を、移動体やセンサそれぞれが検知するには、単に移動体やセンサが備える距離計測手段と移動体周囲の物体との距離を把握するだけでは不十分である。それだけではなく、移動体のボディを含む多様な形状の干渉検知領域に対し、容易に人や物体との干渉の有無が確認できることに加え、移動体のボディを含む多様な形状の干渉検知領域各部と周囲の物体との距離を、できるだけ正確かつ短時間に把握することが必要になる。
【0014】
干渉検知領域の形状については、特に自動車や農機や建機、人型を含む移動ロボットなど、移動体のボディ形状自体が複雑になってきている。このため、こうした複雑なボディ形状をも扱うことができ、しかもできるだけ少ないデータ量で扱える干渉検知と干渉検知領域に対する距離算出の手段が求められる。
【0015】
移動体とセンサに関する衝突や干渉の検知と回避については、従来から様々な実現方法が提案されている。しかし、それらの仕様を上記に記した背景と要件に照らした場合、必ずしも十分とは言えない。
【0016】
特許文献１には、ロボットやその環境を３角形または４角形の多角形で構成される多面体でモデル化し、ポリゴンすなわち多角形を構成する稜線が相手側の多面体を構成する多角形と交差するかどうかで干渉を判断する方法が開示されている。しかし、用途はロボットの動作を事前に確認するシミュレータであり、ロボットの運転中にリアルタイムに干渉を検知または回避する技術ではない。シミュレーションにとどまっている大きな理由は、ロボットやその外環境を多面体でモデル化するため、多面体のデータ量とデータ処理量が膨大になることである。
【0017】
この干渉検知の計算処理量を具体的にみると、ロボット側の多面体と環境側の多面体との干渉を判定する場合、ロボット側の多面体上の多角形を構成する全稜線ｍ本と、環境側の全多面体ｎ個に対して、ｍ×ｎ回の稜線と多角形との交差判定が必要になる。
【0018】
また、一方の多面体上の多角形に対し、他方の多面体上のどの多角形が近接しているかを即時に逆算することが難しいため、処理プログラムに特別な工夫をしない限り、ｍ×ｎ回の組み合わせ全探索を避け、計算量を減らすことも困難である。また、ロボット側のみが移動するとしても、ロボット側または環境側いずれかの多面体については、多角形を構成する全頂点の座標値を、ロボットの移動量に応じて他方の座標系に座標変換する計算も加わる。干渉検知の精度が、多角形の分解能に依存することも問題である。精度を上げるために分解能を細かくすると、データ量と干渉検知の計算量がともに増大してしまうからである。このように、干渉検知領域の形状を多角形でモデル化することは、一般的な手法ではあるが、リアルタイムで干渉を検知する方法としては得策ではない。
【0019】
特許文献２には、主に道路状の走行路を進むような移動ロボットを対象に、障害物センサを備えた移動ロボットの制御装置が該進行方向に位置する障害物に対して移動速度を調整する形式の、衝突回避法が開示されている。しかし、センサ位置を基準とするセンサ座標系での障害物との距離に応じて衝突回避処理がなされ、直方体や円筒状などの単純形状とは限らないボディ各部と障害物との距離は、衝突回避処理に考慮されていない。このため、衝突回避の移動経路は障害物に対し大回りにならざるを得ず、狭い空間をすり抜けるような移動は難しい。
【0020】
特許文献３には、ロボットのボディと障害物それぞれの３Ｄモデルをもとに、ロボットのボディと障害物との間の最短距離を算出し、障害物を回避する技術が開示されている。このため、衝突回避の移動経路はロボットのボディや障害物の形状に応じた小回りがきき、狭い空間をすり抜けるような移動もできる。しかし、３Ｄモデルを凸多面体モデルに変換して最短距離計算がなされているためボディの凹部に対しては負値の距離誤差が発生する。
【0021】
したがって、腕としてマニピュレータを備えた人型に近いボディ形状の移動ロボットを想定し、この移動ロボットがテーブル上の奥にある物品を把持することを想定した場合、テーブルの天板とくびれた腰部との間にはまだ距離余裕があるのに、前記凸多面体変換に伴う最短距離誤差により腰部と天板が干渉状態を判別され、テーブル奥の物品に手先を近接させられない、というような状況も生じ得る。
【0022】
また、特許文献３には前記３Ｄモデルの具体的な形式は明記されていないが、前記凸多面体モデルを求めるには、少なくともロボットのボディや障害物の構成面上の点群データが必要になる。これは特許文献１同様、干渉検知手段が多くのデータ量を記憶する必要があることを意味する。また、２つの凸多面体の間の最短距離の計算法には一例としてＧＪＫアルゴリズムが使われている。
【0023】
この方法は点と凸多面体を構成する多角形面との最短距離を計算する手法であり、特別な工夫をしない限りは一方の凸多面体上の頂点と、相手側多面体を構成する多角形との組み合わせ全探索が必要になる。最短距離の計算精度が凸多面体を構成する多角形の解像度に依存する点、ロボットの移動に伴いロボットボディもしくは障害物どちらかの点群データの座標変換が必要になる点も特許文献１と同じである。以上から３Ｄモデルの形式については、特許文献１とまったく同じ問題が生じる。
【0024】
特許文献４には、機械に対する接近体の接近を検出するためのセンサの検出条件設定方法に関する技術が開示されているが、センサそのものに関する技術ではない。特許文献４が想定しているセンサの干渉検知領域は２次元で、３次元ではない。センサを中心に干渉検知領域が設定されており、センサから離れた位置に干渉検知領域を設定することはできない。
【先行技術文献】
【特許文献】
【0025】
【特許文献1】特開平８－２７８９９０号公報
【特許文献2】特開２０１９－２１２０２号公報
【特許文献3】特表２０１９－５１６１４６号公報
【特許文献4】特開２０２１－１８６９４６号公報
【発明の概要】
【発明が解決しようとする課題】
【0026】
以上の先行技術文献に対し、本発明が対応する干渉検知に関する要件は、次のような内容である。干渉を検知する領域は、ロボットやセンサを中心とした周囲の概円筒形や概部分円筒形の領域であったり、ロボットやセンサから離れた空間内に固定的に設けられた、概直方体状や球状の接近禁止領域であったり、自動車やロボットのボディそのものの概形状であったり、またはボディ概形状を一定距離オフセットした形状であったりする。
【0027】
このとき、同じ干渉検知の手順をとりながらも、干渉検知領域の形状は、前記のようにあるときは概円筒状、またあるときは概直方体状、さらにまたあるときは自動車やロボットのボディの概形状と、多様に形状を変えられると便利である。
【0028】
また、少ないデータ量でこうした多様な形状を表されることが望ましい。さらに、距離計測手段でその一部または全てを距離計測した周囲物体と、前記干渉検知領域との干渉が少ない計算量で容易に確認でき、干渉がない場合でも、一定時間後の干渉の可能性を予測したり、大回りしすぎないように周囲物体を避けたりするために、周囲物体上の距離計測点と前記干渉検知領域との間の最短距離を、容易かつ短時間に求めることができると望ましい。
【0029】
本発明は上記に鑑みてなされたものであって、干渉有無を検知する干渉検知領域に少ないデータ量で多様な形状を与え、周囲に存在する物体と干渉検知領域との干渉有無と、同物体と同干渉検知領域との間の最短距離を、容易かつ短時間に求めることができる干渉検知の実現手段を提供することを目的とする。
【課題を解決するための手段】
【0030】
本発明に係る干渉検知装置は、計測した周辺物体との距離に基づき、設定した３次元または２次元領域と物体との干渉の有無を検知する干渉検知装置であって、干渉有無を検知する干渉検知領域を、超２次関数形式で保持する干渉検知領域の形状表現手段と、を備える。
【発明の効果】
【0031】
本発明に係る干渉検知装置によれば、干渉有無を検知する干渉検知領域に少ないデータ量で多様な形状を与え、周囲に存在する物体と干渉検知領域との干渉有無と、同物体と同干渉検知領域との間の最短距離を、容易かつ短時間に求めることができる。
【図面の簡単な説明】
【0032】
【発明を実施するための形態】
【0033】
本実施形態では、干渉検知領域の３次元立体形状を簡潔でありながら多彩な形状表現性を有する超２次関数で表すことにより、その干渉検知領域の形状を、単純な概円筒形状からより複雑な自動車やロボットのボディ形状まで、少ないデータ量で表現可能とするとともに、容易かつ柔軟に変更可能とする。
【0034】
そして、超２次関数式に点（ｘ，ｙ，ｚ）の座標値を代入すれば、計算値が１を超えるか否かで容易に超２次関数曲面に対する内外判定ができる特性を利用し、物体と干渉検知領域との干渉の有無を、簡単かつ短時間で判断可能にする。
【0035】
さらに、この超２次関数曲面の内外判定による干渉確認で干渉が生じていないと判断された場合であっても、将来の衝突の可能性を予測したり、本干渉検知装置を搭載した移動体が物体との衝突を回避する軌道を生成したりするために必要な物体と干渉検知領域境界との距離を、効率よく算出可能とする。
【0036】
この目的のために、超２次関数が与える曲面形状はＸ・Ｙ・Ｚの直交３次元空間内で立体をなすにもかかわらず、曲面上の任意の点を方位角と仰角の２変数でサンプルする（＝指し示す）ことができる特性を利用する。
【0037】
以下に、本開示に係る干渉検知装置および干渉検知方法の実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、下記実施形態における構成要素には、当業者が置換可能、且つ、容易に想到できるもの、あるいは実質的に同一のものが含まれる。
【0038】
本実施形態に係る干渉検知装置は、計測した周辺物体との距離に基づき、設定した３次元または２次元領域と前記周辺物体との干渉の有無を検知する装置である。また、干渉検知装置は、自動車、ドローンや無人搬送車を含む移動ロボット、農機や建機等の移動体の制御系に組み込まれて機能する。また、それら移動体に搭載されて移動体の制御系を構成する、安全用センサに組み込まれて機能する。さらにまた、各種自動化設備など固定設備の制御系に組み込まれて機能する。
【0039】
図１は、本実施形態の干渉検知装置を移動体の移動体制御装置に組み込んだ場合の構成とデータの受け渡し関係を表したブロック図である。移動体１の移動体制御装置３において本実施形態の干渉検知装置４は、干渉検知の対象領域との干渉の有無を検出したり、当該干渉検知領域と距離計測点との最短距離を算出したりする。それらの結果は、移動体制御装置３を構成する実行制御手段に渡され同実行制御手段が移動体１の走行部駆動手段を停止させたり、衝突回避手段に渡され同衝突回避手段はそれらをもとに移動体１が障害物を回避するように走行部駆動手段を制御したりする。
【0040】
また、図２は、本実施形態の干渉検知装置をセンサに組み込む場合の構成とデータの受け渡し関係を表したブロック図である。センサ２に組み込まれた場合に本実施形態の干渉検知装置４は、距離計測手段が距離を計測した周囲環境上の距離計測点が干渉検知領域に干渉しているかどうか、外部の、図２では移動体制御装置３に、干渉検知結果を返す。外部の同じく移動体制御装置３から別途指令があった場合には、干渉検知領域と距離計測点との最短距離も移動体制御装置３に返す。
【0041】
図１および図２において、干渉検知装置４の構成手段である、干渉検知領域の形状表現手段４１、干渉検知手段４２、干渉検知領域形状の設定切替手段４３および干渉検知領域との最短距離算出手段４４の、それぞれについて説明する。
【0042】
まず、干渉検知領域の形状表現手段４１について説明する。干渉検知領域の形状表現手段４１は、干渉有無を検知する干渉検知領域を、超２次関数形式で保持する。本実施形態の干渉検知装置４が対象とする干渉検知領域には、装置の利用形式に対応して３つの種類がある。まず１つめは、移動体１またはセンサ２の周辺環境上の座標系の任意の位置姿勢にあらかじめ設定され、移動体１またはセンサ２自体が能動的な接近を回避すべき領域である。
【0043】
次に２つめは、移動体１またはセンサ２の外部かつ移動体１またはセンサ２上の座標系の任意の位置姿勢に設定され、他の物体や人などの対象物の受動的な接近を回避すべき領域である。次に３つめは、移動体１のボディの全体または一部の近似形状に相当する干渉検知領域である。
【0044】
２つめの対象物の受動的な接近には、移動体１またはセンサ２の能動的な移動に伴い、対象物が新たに検知されてくることを含む。そして、以上の干渉検知領域の３次元形状を、次の式（１）に示す超２次関数で規定する。
【0045】
【数1】
【0046】
ここで言う超２次関数は、英語表記でＳｕｐｅｒｑｕａｄｒｉｃｓと表される関数である。超２次関数は３次元形状をなすが、式（１）においてパラメータε１＝ε２＝１とした場合、ａ１とａ２とａ３をいずれも等しい値にすれば、その形状は球となり、ａ１とａ２とａ３をそれぞれ異なる値にすれば、楕円体になる。
【0047】
このように、ａ１、ａ２、ａ３は、超２次関数が与える３次元形状の、Ｘ、Ｙ，Ｚ各方向の大きさまたは広がりを規定する寸法パラメータである。一方、ε１とε２は、ともに形状の丸みを制御する形状パラメータである。本実施形態では、移動体１や障害物について、超２次関数を用いて３Ｄモデル化を行う。例えば、概円筒や概直方体、円錐台を含む概円錐体、球体を含む概楕円体、概トーラス、概ピラミッド型など、一般的な形状については、予めパラメータを設定したものを多数用意しておき、ユーザはその中から近い形状を選び、パラメータを微調整して、より実際の形状に近づける。複雑な形状については、移動体の製造元が形状を表す数式を与えたり、センサ２のユーザが数式中の前記パラメータを調整可能にしたりしておく。
【0048】
図３から図７に、式（１）に示す基本形の超２次関数で与えられる３次元形状の点群メッシュを、設定数値とともに示す。図３は、球の一例である。図４は、直方体の一例である。図５は、円筒の一例である。図６は、直方体と楕円体の中間形状の一例である。図７は、上半分のみを図示した円錐体の一例である。これらの図示には、超２次関数曲面上の点をサンプルすることが必要である。超２次関数の中心を基準とする超２次関数座標系（ＸＳ，ＹＳ，ＺＳ）において、超２次関数曲面上の点ＰＳ＝（ｘＳ，ｙＳ，ｚＳ）Ｔは、楕円体座標と直交３次元座標との変換式を拡張した次の式（２）にしたがいサンプルされる。
【0049】
【数2】
【0050】
ｓｇｎ（ ）は符号関数である。式（２）に示す、ωは超２次関数座標系（ＸＳ，ＹＳ，ＺＳ）の原点と点ＰＳを結ぶベクトルがＸＳ軸に対してなす角で地球では経度に相当し、本実施形態では方位角と呼ぶ。ηは前記ベクトルがＸＳ－ＹＳ平面に対してなす角で地球では緯度に相当し、本実施形態では仰角と呼ぶ。
【0051】
なお、干渉検知領域は平面上の２次元領域にもできる。２次元領域は式（１）左辺の小括弧（ ）のｚの項を０と置いた場合に等しく、式（２）では仰角ηを０とすればよい。このように２次元領域は３次元領域に包含されるため、以下では引き続き干渉検知領域を３次元領域として説明を進める。
【0052】
次に、超２次関数の形状表現性を高める方法を示す。超２次関数には、複数の拡張系がある。その一つはＤｅｆｏｒｍａｂｌｅ ｓｕｐｅｒｑｕａｄｒｉｃｓと呼ばれ、次の式（３）に示すように式（１）内のｘ、ｙ、ｚを単なる変数ではなく、変形関数と呼ばれる関数にする形式である。
【0053】
【数3】
【0054】
式（３）中、ｆｘ
－１（ｙ，ｚ）、ｆｙ
－１（ｘ，ｚ）、ｆｚ
－１（ｘ，ｙ）はそれぞれ、変形関数ｆｘ（ｙ，ｚ）、ｆｙ（ｘ，ｚ）、ｆｚ（ｘ，ｙ）の逆関数である。
【0055】
変形関数については、文献１に述べられているように、Ｔａｐｅｒｉｎｇ、Ｂｅｎｄｉｎｇ、Ｃａｖｉｔｙ ｄｅｆｏｒｍａｔｉｏｎ等が提案されており、これらの合成変換も可能である。
【0056】
文献１： Ｆ． Ｓｏｌｉｎａ， Ｒ． Ｂａｊｃｓｙ， Ｒｅｃｏｖｅｒｙ ｏｆ Ｐａｒａｍｅｔｒｉｃ Ｍｏｄｅｌｓ ｆｒｏｍ Ｒａｎｇｅ Ｉｍａｇｅｓ： Ｔｈｅ Ｃａｓｅ ｆｏｒ Ｓｕｐｅｒｑｕａｄｒｉｃｓ ｗｉｔｈ Ｇｌｏｂａｌ Ｄｅｆｏｒｍａｔｉｏｎｓ， ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ， ｐｐ．１３１－１４６， Ｖｏｌ．１２， Ｎｏ．２， （１９９０）．
【0057】
図８は、図５に示した円筒形状に、底面と上面の径を中央に対しそれぞれ３０％増減させるＴａｐｅｒｉｎｇと、２０°の仰角で曲げるＢｅｎｄｉｎｇを与えた円筒の変形例である。また、図９は、円筒に１８０°の仰角で曲げるＢｅｎｄｉｎｇを与え、円筒をトーラス状に変形した例である。基本的には任意に変形関数を定義できるが、式（３）への座標値の代入による座標点の内外判定を行う必要がある場合には、順方向変換とともに逆方向変換を定められなければならない。
【0058】
図１０は、Ｔａｐｅｒｉｎｇ変形に加え、逆関数を持つシグモイド関数を用いた変形を施し、移動体（一例として、ミニバン自動車のボディ）を模した形状である。もし、式（３）への座標値の代入による内外判定が必要ない場合には、逆変換を考えずに回帰などの各種関数近似手法や曲線補間を用いて、より複雑な形状変形を施すこともできる。
【0059】
超２次関数曲面上の点のサンプルは、干渉検知手段４２と、干渉検知領域との最短距離算出手段４４でも用いられる。前者において対象形状を偏りなく干渉検知したり、後者において外部の点に対し最も近接した干渉検知領域上の点を正確に求めたりするには、図３から図１０、図１２から図１３、図１５から図１６に示した曲面メッシュのように、干渉検知領域上の点をできるだけ一様にサンプルできることが望ましい。
【0060】
一様サンプルの方法は複数あり、文献２や文献３などに詳しいが、適用する方法と変形関数の形や近似したい曲面形状によっては、アレンジが必要な場合がある。
【0061】
文献２： Ｅ． Ｂａｒｄｉｎｅｔ， Ｌ． Ｄ． Ｃｏｈｅｎ， Ｎ． Ａｙａｃｈｅ， Ａ ｐａｒａｍｅｔｒｉｃ ｄｅｆｏｒｍａｂｌｅ ｍｏｄｅｌ ｔｏ ｆｉｔ
ｕｎｓｔｒｕｃｔｕｒｅｄ ３Ｄ ｄａｔａ， Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｉｍａｇｅ Ｕｎｄｅｒｓｔａｎｄｉｎｇ， ７１（１）， ｐｐ．３９－５４， １９９８．
ｕｎｓｔｒｕｃｔｕｒｅｄ ３Ｄ ｄａｔａ， Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ａｎｄ Ｉｍａｇｅ Ｕｎｄｅｒｓｔａｎｄｉｎｇ， ７１（１）， ｐｐ．３９－５４， １９９８．
【0062】
文献３： Ｂ． Ｃ． Ｖｅｍｕｒｉ， Ａ． Ｒａｄｉｓａｖｗｅｖｉｃ， Ｍｕｌｔｉｒｅｓｏｌｕｔｉｏｎ Ｓｔｏｃｈａｓｔｉｃ Ｈｙｂｒｉｄ Ｓｈａｐｅ Ｍｏｄｅｌｓ ｗｉｔｈ Ｆｒａｃｔａｌ Ｐｒｉｏｒｓ， ＡＣＭ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｇｒａｐｈｉｃｓ， Ｖｏｌ． １３， Ｎｏ． ２， ｐｐ．１７７－２０７， １９９４．
【0063】
【0064】
【数4】
【0065】
【数5】
【0066】
式（２）と式（４）いずれにおいても重要なのは、超２次関数曲面は立体をなすにもかかわらず、方位角ωと仰角ηの角度２つのみを指定すれば、それら角度に対応する３次元の座標値点（ｘＳ，ｙＳ，ｚＳ）が、いつでもサンプルできる点である。そして、式（２）と式（４）に示したこれら２つの角度の定義範囲を、それぞれ任意の分解能で変化させることで、曲面全体から曲面上の点をサンプルすることができる。
【0067】
これは、超２次関数は幾何学的には球面と同じ２次元多様体であり、地球表面の位置が経度と緯度で定まるのと同じく、超２次関数曲面上の点が方位角と仰角で定まるからである。これにより、後述するように干渉検知と最短距離計算を効率的に実行することができる。
【0068】
この特性は、干渉検知領域を構成する点群データや多面体の頂点の３次元座標値（ｘ，ｙ，ｚ）を特定の粗さの分解能で全て保持する必要があり、なおかつ領域の特定の部位にある点に検索手段なしで直接アクセスする手段を持たない従来技術の特性と、大きく異なる。
【0069】
なお、ここに述べた式（１）のｘ、ｙ、ｚを変形関数にする拡張系のほかにも、式（１）においてパラメータε１とε２を関数にするＥｘｔｅｎｄｅｄ Ｓｕｐｅｒｑｕａｄｒｉｃｓや、それをさらに一般化したＨｙｐｅｒｑｕａｄｒｉｃｓと称する形式、式（１）のａ１、ａ２、ａ３を関数にする形式などもあるが、本実施形態はこれらの拡張系も総合して超２次関数と呼び、本発明の対象に含める。
【0070】
図３から図１０に示したような３次元形状の例のほか、これら拡張系を用いると、例えば、くさび状、バナナ状、帽子や灰皿状、靴状など、少数かつ有限個のパラメータで多様な３次元形状を表現することが可能である。逆に、最適化計算により様々な物体の形状計測データに超２次関数をあてはめることも可能で、飲料缶や菓子袋、心臓の心室等の形状に超２次関数を適合させている例がある。
【0071】
このように、大きさと形状を規定する式（１）の５つのパラメータと、各種の変形を規定する少数かつ有限個のパラメータで多様な３次元形状を表現することができる超２次関数は、干渉検知領域の形状表現手段４１に好適である。
【0072】
次に、干渉検知手段４２について説明する。超２次関数の座標系（ＸＳ，ＹＳ，ＺＳ）において、ある点Ｐ（ｘ，ｙ，ｚ）が超２次関数で表される曲面内に位置するか否かのいわゆる内外判定は、内外関数（ｉｎｓｉｄｅ－ｏｕｔｓｉｄｅ ｆｕｎｃｔｉｏｎ）と呼ばれる式（１）または式（３）の左辺に（ｘ，ｙ，ｚ）の座標値を代入するだけで、容易に判定できる。
【0073】
すなわち、式（１）の左辺をＦ（ｘ，ｙ，ｚ）と表したとき、Ｆ（ｘ，ｙ，ｚ）＜１であれば、点Ｐは超２次関数で表される曲面内に位置し、Ｆ（ｘ，ｙ，ｚ）＝１であれば点Ｐは超２次関数曲面上に位置し、１＜Ｆ（ｘ，ｙ，ｚ）であれば点Ｐは超２次関数曲面の外側に位置する。式（３）でも同様である。この特性を用いて、本実施形態における干渉検知手段４２は、以下の３種類を対象に干渉を検知する。
【0074】
まず、１つめに、移動体１の移動軌道からサンプルした移動軌道上の点が、超２次関数で表された干渉検知領域に干渉するか否かを判定する。図１１は、移動体１の移動軌道と干渉検知領域との干渉検知の一例を説明するための図である。図１１には、干渉検知領域１１１と、移動軌道１１２を示す。図１１に示す干渉検知領域１１１は、移動体１の外部周辺に設定した接近禁止領域に相当する場合と、移動体１のボディそのものの場合の両方がある。
【0075】
マニピュレータ手先やドローンなど移動体１の移動軌道１１２の上の点が移動体１を代表するとみなし、次に示す２つめの詳細な干渉検出よりも粗く干渉を検知しようとする処理であり、移動体１の動作前の実行、または動作中に軌道上の点を先読みして実動作よりも先に計算する形での実行を意図している。
【0076】
つまり、干渉検知手段４２は、干渉検知装置４の周辺空間内に接近禁止領域に相当する超２次関数形式で表される干渉検知領域を設定し、移動体１の制御手段から与えられる移動体１の移動軌道のサンプル点が干渉検知領域に干渉するかどうかを、干渉検知領域を表す超２次関数曲面に対する各軌道サンプル点の内外判定計算により判断する。なお、干渉検知領域１１１は複数の超２次関数曲面を組み合わせ配置して構成してもよく、複数の超２次関数曲面は、それぞれ形状が異なっていてもよい。また、この処理に必要な軌道データは、移動体制御装置３の軌道記憶手段または軌道生成手段から干渉検知手段４２に渡される。
【0077】
次に、２つめに、移動体１の全体形状、あるいは干渉を検出または回避したい一部の形状を近似した点群データが、超２次関数で表された干渉検知領域に干渉するか否かを判定する。本形状近似点群データは、移動体１のＣＡＤ（Ｃｏｍｐｕｔｅｒ Ａｉｄｅｄ Ｄｅｓｉｇｎ）データをもとに作成したデータを記憶する形式でもよいが、該当部分の形状を近似した超２次関数から式（２）や式（４）に基づき干渉検出時に適時データをサンプルするのが、データ記憶量削減の観点からは適当である。
【0078】
また、この場合の干渉検知領域も、移動体１の周辺に設定した接近禁止領域に相当する場合と、移動体１のボディそのものの場合の両方がある。図１２は、マニピュレータを備えた移動ロボットにおけるマニピュレータおよびボディと周辺の接近禁止領域との干渉検知の一例を説明するための図である。図１３は、マニピュレータを備えた移動ロボットにおけるマニピュレータとボディの干渉検知の一例を説明するための図である。図１２および図１３には、マニピュレータ１２１、移動ロボット１２２、干渉検知領域１２３および干渉発生地点１２４を示す。なお、図１２および図１３に示すボディ１３３とマニピュレータ１２１は、ともに、外形の点群メッシュは超２次関数をサンプルして生成している。
【0079】
マニピュレータ１２１を備えた移動ロボット１２２を考えた場合（図１２、図１３）、マニピュレータ部分を超２次関数で近似し、そこから粗くサンプルした点群が、移動ロボット１２２の外部の接近禁止領域に干渉しないかどうかを、上述した内外関数に基づく方法によって判断することができる（図１２）。同様に、干渉検知領域１２３を移動ロボット１２２のボディ１３３としておき、マニピュレータ１２１が動作時にボディ１３３と干渉しないかどうかを判断することもできる（図１３）。本検知処理も１つめと同様に、移動体１の動作前の実行または、動作中に軌道上の点を先読みする形での実行を意図している。
【0080】
つまり、干渉検知手段４２は、干渉検知装置４の周辺空間内に接近禁止領域に相当する超２次関数形式で表される干渉検知領域１２３を設定し、干渉検知装置４を移動体制御装置３の一部として内蔵する移動体１（移動ロボット１２２）が所定の軌道に沿って移動する場合に、当該移動体１の構成部分の一部または全体の形状を点群データで表すとともに、構成部分の一部または全体が干渉検知領域１２３に干渉するかどうかを、干渉検知領域１２３を表す超２次関数曲面に対する点群各点の内外判定計算により判断する。
【0081】
次に、３つめに、移動体１またはセンサ２上に備えられた距離計測手段が計測した距離に対応する周囲の物体や環境上の距離計測点が、前記超２次関数で表された干渉検知領域に干渉するか否かを判定する。例えば、移動体１やセンサ２の周囲に接近禁止領域に相当する干渉検知領域を設定し、移動体１の進行方向の特定範囲への人や車輌の進入と障害物の接近、あるいは危険箇所近くに固定設置したセンサ２の監視エリアへの人や車輌の進入を検知するような場合に相当する。図１４はこの例である。図１４は、センサ２による距離計測点の干渉検知を示す。図１４には、超２次関数で表現した干渉検知領域１４１、センサ２の一例である距離センサ１４２および距離計測点（距離検出点）１４３を示す。
【0082】
なお、超２次関数は座標変換をすることで、３次元空間内の任意の位置に任意の姿勢で配置することができる。これにより、上記１つめの干渉検知において空間内に傾けて接近禁止領域に相当する干渉検知領域を設置したり、上記２つめの干渉検知においてマニピュレータ１２１のリンク形状を超２次関数で近似して（図１２、図１３）、６自由度で運動するそれらリンクが任意の座標系空間内に設置した接近禁止領域相当の干渉検知領域１２３に干渉するか否かを、判断したりすることができる（図１２）。このことを具体的に、最も基本的な座標変換の例で示す。
【0083】
超２次関数曲面がなす３次元形状の中心－これは超２次関数の基本形態である球または楕円体の中心に等しい－に原点を置いた超２次関数座標系（ＸＳ，ＹＳ，ＺＳ）上の点ｐＳ＝（ｘＳ，ｙＳ，ｚＳ）Ｔと、距離データを干渉検知装置４に提供する距離計測手段に設けた距離計測手段座標系（ＸＭ，ＹＭ，ＺＭ）上の点ｐＭ＝（ｘＭ，ｙＭ，ｚＭ）Ｔとの関係は、次の式（６）および式（７）で表される。１
【0084】
【数6】
【0085】
【数7】
【0086】
これらの式は、座標変換を表す一般式である。式（６）中の座標変換行列Ｔの構成要素のうち、ｄは距離計測手段の座標系（ＸＭ，ＹＭ，ＺＭ）の原点から超２次関数座標系（ＸＳ，ＹＳ，ＺＳ）の原点までの並進量を表すベクトル（３×１）、Ｒは（ＸＭ，ＹＭ，ＺＭ）に対する（ＸＳ，ＹＳ，ＺＳ）の回転を表す３×３の回転行列を表す。
【0087】
式（２）や式（４）でサンプルした超２次関数曲面上の点は、式（６）により、距離計測手段座標系上の任意の位置に任意の回転姿勢で配置できる。同様に、距離計測手段座標系から例えば距離計測手段を保持する移動体座標系、移動体座標系から移動体１が走行するワールド座標系へと、座標変換することも当然可能である。
【0088】
逆に、距離計測手段が計測した距離に対応する距離計測手段座標系上の距離計測点１４３を、式（７）にしたがい超２次関数座標系に座標変換することで、干渉検知手段４２における内外関数による内外判定により、同距離計測点が超２次関数で表される干渉検知領域１４１内にあるか否かを判断することができる。
【0089】
なお、ロボット工学の一般知識により、マニピュレータを構成する各リンクのマニピュレータ運動中の位置と姿勢角は運動学の概念により算出でき、求められた位置と姿勢角はそれぞれ式（６）のｄとＲにあてはめられる。これにより、２つめの形式の干渉検知が可能になる。姿勢角の表現法にはオイラー角や角軸ベクトル等の表現があるが、いずれも式（６）の回転行列形式に変換できる。
【0090】
次に、干渉検知領域形状の設定切替手段４３について説明する。干渉検知領域形状の設定切替手段４３は、干渉有無を検知する干渉検知領域を表す超２次関数曲面の形状や場所に関するパラメータを複数記憶する。そして、干渉検知装置４が移動体に組み込まれる場合(図１)、干渉検知装置４がセンサ２に組み込まれる場合に(図２)、いずれにおいても干渉検知装置４の外部からの信号または通信または手動設定により、それらパラメータを設定または切り替えられるようにする。また、移動体１やセンサ２の外部からもユーザあるいは製造元により、外部インタフェースを通じて新しい干渉検知領域形状に対応した超２次関数のパラメータが追加されたり処理対象として設定されたりすることもある。
【0091】
例えば、干渉検知領域形状の設定切替手段４３は、移動体制御装置３の実行制御手段から、移動体１の動作モードに応じた干渉検知領域の切り替え指令を受ける。例えば、切り替え指令は、所定軌道上を移動する移動体１の特定の周囲物体との干渉検知のために、移動体外部の干渉検知領域に指定番号の超２次関数のパラメータを切り替える、あるいは干渉検知領域を指定番号の移動体のボディ形状に切り替える、等である。
【0092】
本実施形態の干渉検知装置４が対象とする干渉検知領域には、装置の利用形式に対応して３つの種類がある。
【0093】
まず、１つめの種類は、移動体１またはセンサ２の周辺環境上の座標系の任意の位置姿勢にあらかじめ設定され、移動体１または移動体１に搭載されたセンサ２が、能動的な接近を回避すべき領域である。次に、２つめの種類は、移動体１またはセンサ２の外部かつ移動体１またはセンサ２上の座標系の任意の位置姿勢に設定され、他の物体や人などの対象物の受動的な接近を回避すべき領域である。なお、対象物の受動的な接近には、移動体１またはセンサ２の能動的な移動に伴い、対象物が新たに検知されてくることを含む。続いて、３つめの種類は、移動体１のボディの全体または一部の近似形状に相当する干渉検知領域である。
【0094】
上記３つの干渉検知領域の種類の違いに対応して、干渉検知領域形状の設定切替手段４３は、移動体１やセンサ２の運用の変更時に干渉検知領域形状の手動設定を受け付けるとともに、移動体１の活動状況に応じて移動体制御装置３の実行制御手段から指令を受け、干渉検知領域を切り替える。これら両方のはたらきをする。
【0095】
はじめに、１つめおよび２つめの種類に対応した場合のはたらきについて述べる。概円筒や概直方体など一般的な代表的形状については、外部インタフェースからユーザが手動、または外部からの信号や通信などにより形状を選択可能としたうえ、超２次関数曲面が選択された形状になるように式（１）の丸みのパラメータε１とε２をあらかじめ与えておき、寸法パラメータａ１とａ２とａ３をユーザが変えられるようにしておく。
【0096】
また、超２次関数曲面で与えられる干渉検知領域の設定位置と姿勢について、式（６）内の座標変換行列Ｔを構成する並進量ｄと回転行列Ｒを設定および保持できるようにしておく。その結果、図３に示したような概円筒や概直方体、円錐台を含む概円錐体、球体を含む概楕円体、概トーラス、概ピラミッド型など、一般的な代表的形状については形状を選択可能としたうえ、超２次関数曲面が選択された形状になるようなパラメータをあらかじめ記憶しておき、寸法と配置位置を手動で変えられるようにしておくことで、ユーザは詳細設定の手間を省きつつ、多様な３次元形状を干渉検知領域に選ぶことができる。
【0097】
次に、３つめの種類の移動体１のボディ形状の指定と切り替えについて述べる。移動体１のボディ形状は複雑であるため、複数の超２次関数の組み合わせや、当該ボディ形状に特化した超２次関数の変形関数によって表現されると考えるのが妥当である。また、ボディ形状については通常、移動体１を運用するユーザが干渉検知装置４に変形関数を含む超２次関数のパラメータを設定することはない。干渉検知装置４を組み込む移動体１の製造元が、特定機種のボディに関する超２次関数のパラメータを干渉検知装置４に設定するのが、通常の運用形態であると想定される。
【0098】
この場合、移動体１のボディ形状が一般に複雑かつ機種固有であることや、干渉検知の処理時間を短縮したいことを考えると、複数の超２次関数の組み合わせや輪郭を近似した変形関数等を含むボディ形状を表す数式を、移動体１の製造元が干渉検知手段４２の処理実行プログラムに直接記述するのが妥当である。そして、干渉検知領域形状の設定切替手段４３が干渉検知装置４の外部からの指定を受けた場合にこの関数を呼び出したり、番号を通知して干渉検知手段４２が処理を実行する関数を切り替えられるようにしたりしておく。
【0099】
個別の数式で定義される超２次関数の変形関数についても同様に、種別ごとに干渉検知手段４２の処理実行プログラムに変形関数を実装しておく。そして、干渉検知領域形状の設定切替手段４３が指定した形式の変形関数が干渉検知手段４２の処理実行プログラムで呼び出される際、干渉検知領域形状の設定切替手段４３は、超２次関数の形状や位置を定めるパラメータとともに、記憶していた当該変形関数のパラメータも干渉検知手段４２に対して渡す。
【0100】
次に、干渉検知領域との最短距離算出手段４４について説明する。干渉検知領域との最短距離算出手段４４は、干渉検知領域を表す超２次関数曲面上の点をサンプルすることにより、干渉検知装置４の周囲に存在する周囲物体上の距離計測点と、干渉検知領域と、の間の最短距離を算出する。干渉検知領域に対し最短距離を算出する対象点は、干渉検知手段の干渉検知対象と同じく、以下の３つの種類である。
【0101】
まず、１つめの種類は、距離検出手段が計測した距離に対応する、移動体１やセンサ２周囲の物体上の距離計測点である。次に、２つめの種類は、移動体１の全体形状、あるいは干渉を検出または回避したい一部の形状を近似した点群データ各点である。続いて、３つめの種類は、移動体１の軌道上の点である。
【0102】
干渉検知手段４２により干渉がないと判断された場合でも、衝突回避手段３６が一定時間後の衝突の可能性を予測したり、大回りしすぎないように周囲物体を避けたりするために、周囲物体上の距離計測点、あるいは移動体１の軌道上の点と、干渉検知領域との間の最短距離を知ることは非常に有用である。
【0103】
そのため、式（２）または式（４）にしたがい方位角と仰角を変化させて超２次関数曲面上の点を繰り返しサンプルし、これら各点と対象点との距離を探索的に計算することで、対象点と干渉検知領域との最短距離を算出する。その手順の一例を具体的に示す。
【0104】
最初に、最短距離を算出する対象点の位置から、最短距離の探索計算を開始する方位角と仰角を逆算する。式（１）や式（３）に示した超２次関数で与えられる曲面は、球や楕円体の表面をパラメータε１とε２や変形関数でスケーリングした曲面、あるいは歪める形で変形した曲面ととらえられる。
【0105】
この認識に基づいて、球や楕円体を利用して逆算する方法が複数考えられる。超２次関数曲面の形によって、どの方法が最も真値に近い角度を与えるかは変わり得るが、ここでは簡単かつ広範に利用できる方法について一例を示す。
【0106】
この方法は、超２次関数曲面の広がりを規定するパラメータである式（１）のａ１、ａ２、ａ３のみに着目し、これら３パラメータで決まる楕円体に対して、対象点に最も近接した楕円体上の点を与える方位角と仰角を求める。最短距離を算出すべき対象点（ｘ，ｙ，ｚ）が与えられたとき、この点に対応する超２次関数への変形前、すなわちε１＝ε２＝１の楕円体上の点の方位角ωと仰角ηは、楕円体に関する一般的な幾何学的知識から、次式の式（８）により容易に計算できる。
【0107】
【数8】
【0108】
続いて、このようにして得られる方位角と仰角の２つの角度値を探索初期値として、最短距離の探索を開始する。以下に、３次元空間内の１つの点Ａが与えられた場合、点Ａに対して超２次関数で与えられる干渉検知領域との最短距離および最短距離点を算出する全体の手順を以下に要約する。なお、干渉検知領域の形状が球でそれを表す超２次関数においてａ１＝ａ２＝ａ３の場合、これら方位角と仰角が最短距離点を与える角度値であり、以下に述べる探索処理が必要ないことは言うまでもない。
【0109】
（最短距離点の算出手順）
手順１：例えば、式（８）にしたがい、干渉検知領域における点Ａの方位角と仰角を求める。これら方位角と仰角を探索の初期角度とする。また、暫定最短距離値に大きな値をセットする。
手順２：手順１で求めた探索の初期角度近傍の一定角度範囲を対象に、最短距離を以下のように探索する。
手順２－１：方位角と仰角をそれぞれ探索初期角度の周辺で一定の角度増分を与え、変化させる。
手順２－２：変化させた方位角と仰角を式（４）に代入して超２次関数で与えられる曲面上の点をサンプルして、これを点Ｂとする。
手順２－３：点Ａと点Ｂとの間の距離を求め、距離値が暫定最短距離値より小さければ、暫定最短距離値を当該距離値に更新する。同時に、点Ｂの座標を暫定最短距離点として記憶する。
手順３：方位角と仰角それぞれ所定角度範囲分探索が終了したら、その時点の暫定最短距離と暫定最短距離点を、それぞれ最終的な暫定最短距離と暫定最短距離点とする。
手順１：例えば、式（８）にしたがい、干渉検知領域における点Ａの方位角と仰角を求める。これら方位角と仰角を探索の初期角度とする。また、暫定最短距離値に大きな値をセットする。
手順２：手順１で求めた探索の初期角度近傍の一定角度範囲を対象に、最短距離を以下のように探索する。
手順２－１：方位角と仰角をそれぞれ探索初期角度の周辺で一定の角度増分を与え、変化させる。
手順２－２：変化させた方位角と仰角を式（４）に代入して超２次関数で与えられる曲面上の点をサンプルして、これを点Ｂとする。
手順２－３：点Ａと点Ｂとの間の距離を求め、距離値が暫定最短距離値より小さければ、暫定最短距離値を当該距離値に更新する。同時に、点Ｂの座標を暫定最短距離点として記憶する。
手順３：方位角と仰角それぞれ所定角度範囲分探索が終了したら、その時点の暫定最短距離と暫定最短距離点を、それぞれ最終的な暫定最短距離と暫定最短距離点とする。
【0110】
手順３で最短距離点を与えた方位角と仰角を探索初期角度として、その近傍を手順２にしたがいさらに細かい角度増分で探索し、より正確な最短距離を算出することもできる。
【0111】
図１５と図１６は、複雑な形状の干渉検知領域に対し、その外部の点に対し最短距離を上記の方法に基づいて算出する過程を示している。図１５は、ボディ上最近傍点の探索過程を示す。図１６は、ボディ外部の点とボディとの最短距離の算出例を示す。この例で干渉検知領域の対象としているのは、一例として、マニピュレータを備えた芝浦機械製の移動ロボットＣＯＮＯＩＤ－３のボディ形状である。なお、ＣＯＮＯＩＤは登録商標である。式（３）の変形関数ｆｘ、ｆｙに輪郭形状の近似式を設定し、ボディ形状を超２次関数で表している。
【0112】
図１５に示す点１５１は最短距離の算出対象点である。また、点１５２は、式（８）に基づき求められた方位角と仰角の探索初期角度に対応するボディ上の探索初期点を示す。線１５３は、探索過程で探索初期角度の周辺に方位角と仰角を変化させて算出した、距離に対応するベクトルを示す。
【0113】
以上の探索過程の中から、最短距離とそれに対応する最短距離点を求める。ここで、超２次関数曲面はＸ・Ｙ・Ｚの直交３次元空間内で立体をなすにもかかわらず、式（４）や式（２）に基づき、上述した干渉検知手段４２に示したように、曲面上の任意の点（ｘＳ，ｙＳ，ｚＳ）を、方位角ωと仰角ηの２変数でサンプルすることができる。
【0114】
そのため、Ｘ・Ｙ・Ｚの３方向を探索することなく、しかも式（８）のようにして絞り込んだ探索の初期角度近傍に限定して２つの角度を変化させて超２次関数曲面上の点をサンプルすることで、効率よく最短距離点と最短距離を算出できる。
【0115】
図１６は、図１６に示す点１６１に対し、上述の探索過程により最短距離点と最短距離を算出した例である。点１６２は方位角と仰角の探索初期角度に対応する探索初期点である。線１６３は、点１６１に対し算出された最短距離に対応するベクトルである。
【0116】
このボディ形状の表現に必要なデータは、式（１）の５つのパラメータと、左右側面の輪郭形状を近似するｆｙの関数式、および前面と後面の輪郭形状を近似する２つのｆｘの関数式のみであり、形状の複雑さに比べて少ない。なお、図１２および図１３も同ロボットを示している。
【0117】
このようにボディを干渉検知領域とし、それに対し自身に備わるマニピュレータや周囲の物体上の代表点との最短距離と、最短距離を与えるボディ上の最短距離点がどこかを把握し、それら最短距離と最短距離点の情報を図１に示す衝突回避手段３６に伝えることで、ボディとマニピュレータ、またはボディと周囲の物体との衝突を回避させられる。
【0118】
以上説明したように、本実施形態の干渉検知装置４は、計測した周辺物体との距離に基づき、設定した３次元または２次元領域と物体との干渉の有無を検知し、干渉有無を検知する干渉検知領域を、超２次関数形式で保持する。
【0119】
例えば、周辺物体との干渉の有無を検知する干渉検知領域を、超２次関数を使用することが好適であることは、図１に示した多様な３次元形状を簡単な式（１）により表現可能であることからも、明らかである。本実施形態の干渉検知装置４が干渉検知領域の形状を記憶するにあたり、既存技術のように干渉検知領域境界の全点群データや全多角形ポリゴンデータを保持する必要はなく、位置姿勢以外の形状情報に関しては超２次関数の大きさと形状を規定する５つのパラメータ、および形状変形を与える少数かつ有限個のパラメータのみ保持すればよい。
【0120】
また、本実施形態の干渉検知装置４は、移動体１の制御手段から与えられる移動体１の移動軌道のサンプル点が干渉検知領域に干渉するかどうかを、干渉検知領域を表す超２次関数曲面に対する各軌道サンプル点の内外判定計算により判断する。
【0121】
例えば、干渉検知領域に干渉するかどうかを、干渉の有無を判断したい物体上の点や軌道上の点の（ｘ，ｙ，ｚ）座標値を超２次関数式に入力し、当該超２次関数式が１を超えるかどうかだけを確認すればよいので、ごく短時間で実行できる。したがって、例えば、ロボットの実機上で運転中リアルタイムに、もしくは運転中の動作直前に、干渉を検知することができる。
【0122】
具体的には、干渉検知領域に対し干渉を判断すべき点がｎ点ある場合、本実施形態における干渉判断の回数は、前記の単純な内外判定計算がｎ回でよい。これは、例えば、多角形多面体で構成される干渉検知領域がｍ個の稜線(多角形の)を有し、その多面体とｎ個の多角形で構成される別の多面体との干渉を判断する場合に、稜線と多角形とのｍ×ｎ回の組み合わせ交差判定が必要になる従来手法とは、処理計算回数が大きく異なる。
【0123】
さらに、本実施形態の干渉検知装置４は、干渉有無を検知する干渉検知領域を表す超２次関数曲面の少数の設定パラメータを干渉検知領域形状の設定切替手段に記憶しておいて、それらを手動または外部からの信号または通信により設定または切り替える。
【0124】
例えば、干渉検知領域形状の設定切替について、超２次関数の特性を活かし多様な３次元形状を干渉検知領域として設定可能とするため、手動や外部からの通信または信号などにより、超２次関数を規定する少数のパラメータを、切り替えまたは調整可能とした。その結果、概円筒や概直方体など一般的な代表的形状については形状を選択可能としたうえ、超２次関数曲面が選択された形状になるようなパラメータをあらかじめ与えておき、寸法と配置位置を手動で変えられるようにしておくことで、ユーザは詳細設定の手間を省きつつ、多様な３次元形状を干渉検知領域に選ぶことができる。
【0125】
一方、マニピュレータを備えた移動ロボットで例示すると、マニピュレータを使用しない広域走行時は、移動ロボットの前方に走行速度によって自動的に大きさが変化する概直方体状の干渉検知領域を設け、走行路上の障害物を検知する。次に、局所でのマニピュレータを使った作業時には、自動で移動ロボットのボディを干渉検知領域に切り替え、周辺の物体の距離を計測した距離計測点や、ボディと同じく超２次関数で近似したマニピュレータの複数のリンクが、ボディに干渉しないかを監視する。それに加えて、移動ロボットの外部にも接近禁止領域相当の干渉検知領域を自動追加設定し、そこにマニピュレータ手先の軌道が干渉しないか判断する。以上のようなことが可能になる。
【0126】
また、人や物体の接近を検出するセンサを例にとると、あるときは３次元空間内にセンサを中心とする外円筒状の干渉検知領域をその直径と高さをユーザが変更したうえで設定し、別のあるときには手動で切り替えて、センサから離れた空間に概直方体状の接近禁止領域に相当する干渉検知領域を設ける、といったことが可能になる。
【0127】
また、本実施形態の干渉検知装置４は、干渉検知領域を表す超２次関数曲面上の点をサンプルすることにより、衝突回避処理に必要となる干渉検知装置４の周囲に存在する周囲物体上の距離計測点と、干渉検知領域と、の間の最短距離を算出する。
【0128】
例えば、干渉検知領域との最短距離算出について、一方の多面体上の多角形に対し、他方の多面体上のどの多角形が近接しているかを即時に逆算することが難しい従来技術と異なり、最短距離を算出する対象点に対して、それに対応する超２次関数の方位角と仰角を、干渉検知領域によっては近似ながら、即座に逆算できる。
【0129】
さらに、超２次関数が与える曲面形状はＸ・Ｙ・Ｚの直交３次元空間内で立体をなすにもかかわらず、曲面上の任意の点（ｘＳ，ｙＳ，ｚＳ）を方位角と仰角の２変数でサンプルすることができるため、Ｘ・Ｙ・Ｚの３方向を探索することなく、逆算した方位角と仰角を基準にその周辺に限定して２つの角度を変化させて超２次関数曲面上の点をサンプルし、その点と対象点との距離を探索的に計算することで、効率よく最短距離を算出できる。
【0130】
３次元形状を点群や多面体で近似する従来技術の場合、点群を構成する点や多面体を構成する多角形の頂点のデータはメモリ上に記憶されるが、隣接した点と点や隣接した頂点と頂点がメモリ上で隣接しているとは限らないため、ある部分に絞って最短距離を探索することも容易ではない。一方、超２次関数の場合は、逆算した最短距離近傍の方位角と仰角それぞれに対し、単純に、例えば、１０°から＋１０°まで０．１°ずつ角度を変化させるだけで、それら角度の周辺を連続的に探索することができ、最短距離計算を効率よく実行できる。
【0131】
さらに、凹部がある３次元形状であっても、従来技術のように凸体近似をすることはないため、精度よく距離を求めることができる。また、距離精度を上げようとすると点群や多面体の分解能を細かくする必要があり、結果的に点群や多面体の記憶量が増大してしまう従来技術と異なり、最短距離を探索する際の方位角と仰角の変化の分解能を、干渉検知領域形状のパラメータ記憶量を変えることなく、粗くすることも細かくすることもできる。したがって、粗い分解能の探索で最短距離の点を与える方位角と仰角を絞り込んだ後に、細かい分解能の探索をすることで、距離算出時間を短縮しつつ距離の精度を上げることができる。
【0132】
なお、上述した実施形態は、干渉検知装置４が有する構成または機能の一部を変更することで、適宜に変形して実施することも可能である。そこで、以下では、上述した実施形態に係るいくつかの変形例を説明する。なお、以下では、上述した実施形態と異なる点を主に説明することとし、既に説明した内容と共通する点については詳細な説明を省略する。また、以下で説明する変形例は、個別に実施されてもよいし、適宜組み合わせて実施されてもよい。
【0133】
（変形例１）
例えば、干渉検知領域の形状表現手段４１は、超２次関数曲面上に一様分布する点群の生成が難しい、あるいは変形関数による輪郭の近似が著しく難しいなど、干渉検知領域の３次元形状を超２次関数による曲面に近似しづらいような場合、干渉検知領域の３次元形状を、各高さの断面を積層した形に近似してもよい。また、各断面では、断面の輪郭形状を、２次元の超２次関数で近似してもよい。
例えば、干渉検知領域の形状表現手段４１は、超２次関数曲面上に一様分布する点群の生成が難しい、あるいは変形関数による輪郭の近似が著しく難しいなど、干渉検知領域の３次元形状を超２次関数による曲面に近似しづらいような場合、干渉検知領域の３次元形状を、各高さの断面を積層した形に近似してもよい。また、各断面では、断面の輪郭形状を、２次元の超２次関数で近似してもよい。
【0134】
（変形例２）
例えば、干渉検知手段４２は、移動体１のボディ形状そのものではなく、ボディ形状を一定距離オフセットさせた形状を一つ以上の超２次関数で表し、干渉検知領域としてもよい。これにより、移動体１のボディに他の物体や移動ロボットのマニピュレータなどが干渉するおそれがあることを、オフセット量相当分だけ早期に、把握することができる。
例えば、干渉検知手段４２は、移動体１のボディ形状そのものではなく、ボディ形状を一定距離オフセットさせた形状を一つ以上の超２次関数で表し、干渉検知領域としてもよい。これにより、移動体１のボディに他の物体や移動ロボットのマニピュレータなどが干渉するおそれがあることを、オフセット量相当分だけ早期に、把握することができる。
【0135】
（変形例３）
例えば、干渉検知領域との最短距離算出手段４４は、本実施形態においては、図１５と図１６に示すような、超２次関数の勾配計算が簡単ではない曲面形状に対して、勾配を使わずに外部の点との最短距離を効率よく算出する方法を与えている。ここではその算出法を具体的かつ直接的に説明するために、いわゆるグリッドサーチの手法を用いて説明した。しかしながら、手順２については、このようなグリッドサーチではなく、ネルダーミード法のような、評価関数の勾配を使わない非線形最適化法を使ってもよい。
例えば、干渉検知領域との最短距離算出手段４４は、本実施形態においては、図１５と図１６に示すような、超２次関数の勾配計算が簡単ではない曲面形状に対して、勾配を使わずに外部の点との最短距離を効率よく算出する方法を与えている。ここではその算出法を具体的かつ直接的に説明するために、いわゆるグリッドサーチの手法を用いて説明した。しかしながら、手順２については、このようなグリッドサーチではなく、ネルダーミード法のような、評価関数の勾配を使わない非線形最適化法を使ってもよい。
【0136】
なお、上述した実施の形態における、上記処理を実行するためのプログラムは、上記各機能部を含むモジュール構成となっており、実際のハードウェアとしては、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）がＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）またはＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）から処理プログラムを読み出して実行することにより、上述した各機能手段がＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）上にロードされ、上述した各機能手段がＲＡＭ上に生成されるようになっている。なお、上述した各機能手段の一部または全部を、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）などの専用のハードウェアを用いて実現することも可能である。
【0137】
以上、本発明の実施形態について説明したが、上述した実施形態は、例として提示したものであり、本発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能である。また、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。また、この実施形態は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【0138】
１ 移動体
２ センサ
３ 移動体制御装置
４ 干渉検知装置
４１ 干渉検知領域の形状表現手段
４２ 干渉検知手段
４３ 干渉検知領域形状の設定切替手段
４４ 干渉検知領域との最短距離算出手段
２ センサ
３ 移動体制御装置
４ 干渉検知装置
４１ 干渉検知領域の形状表現手段
４２ 干渉検知手段
４３ 干渉検知領域形状の設定切替手段
４４ 干渉検知領域との最短距離算出手段
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
