JP2019504418A - 移動物体の位置を判定するための方法およびシステム - Google Patents

Abstract

本発明は、環境における基準系に基づく３Ｄモデルを形成する１組の幾何学的表面によりモデル化された環境に対して移動物体の相対位置を判定するため方法であって、移動物体の基準系における１組の画定された点を取得するために、少なくとも１つの方向において移動物体から環境までの複数の距離を測定するステップ（１０）と、前述の１組の点と環境の３Ｄメッシュとの間の差異を評価するステップと、前述の差異から、環境の基準系における移動物体の相対位置を判定するステップ（２０）と、を含む、方法に関する。
【選択図】図１

Description

本発明は、環境に対する移動物体の相対位置を判定するための方法に、および係るシステムに、関する。特に、本発明は、認識済みの環境（例えばその表面が検査対象となっている物体を含む格納庫など）における飛行ドローン（例えばマルチローター型またはヘリコプター型の無人空中輸送手段としても知られる飛行ロボット）または表面検査ロボットの位置を判定するための方法に、および係るシステムに、関する。

移動物体（例えば空間内を自律的に移動するドローンまたはロボットなど）は、移動物体が動き回る環境における移動物体自体の位置を持続的に認識することが必要である。

係る移動物体が開放環境（屋外）で動き回る場合、移動物体は通常、位置情報を使用して（例えば、全地球測位システム（ＧＰＳ）により）測位される。係る位置情報の使用により高レベルの精度の達成が可能となる。

しかし全地球測位システムの主要な欠点は、屋内では動作しないことである。したがって屋内における移動物体の測位を可能とする解決策が求められてきた。

例えば、１つの提案される解決策は、移動物体の認識済み位置からスタートして、この認識済みのスタート位置から物体によりなされた移動を測定することにより、物体の位置を判定することであった。しかし、この解決策では、誤差を検出することが、すなわち時間の経過とともに移動の精度が失われることを検出することが、不可能である。係る誤差は時間の経過とともに蓄積され、その結果、測位が不正確になり、測位の修正は不可能である。さらに、この解決策では慣性基準装置が使用される。慣性基準装置は、良好なレベルの精度を有し、上で述べた誤差を最小化することが可能であるにもかかわらず、価格が高く、かつ重い（多くの場合１ｋｇを越える）ため、飛行ドローンの使用に関しては不適合である。

代替的に、ビーコンのシステムを使用することも提案されてきた。係るビーコン・システム（例えば無線送受信器など）は環境内に予め配置される。このように移動物体に送受信器を装備することにより、他のビーコンに対する三角測量に関する技術により、移動物体の位置を推定することが可能である。しかし、ビーコンの配置が必要であり、ビーコンの位置を高い精度で判定する（較正により）必要があるため、この解決策は高額であると同時に実装にあたり長時間を要する。

これらの欠点のうちの少なくともいくつかを克服する、屋内測位を可能にするための解決策が求められてきた。

本発明の目的は、既知の測位方法および測位システムの欠点のうちの少なくともいくつかを克服することである。

特に、本発明の目的は、本発明の少なくとも１つの実施形態において、屋内環境における移動物体の測位を可能にする測位方法を提供することである。

本発明の目的は、少なくとも１つの実施形態において、パワーが低いプロセッサ上での実行が可能である、高速な測位方法を提供することでもある。

本発明の目的は、少なくとも１つの実施形態において、精度が高い測位方法を提供することでもある。

本発明の目的は、少なくとも１つの実施形態において、移動物体が移動する環境に対して変更を加えることがまったく要求されない測位方法を提供することでもある。

本発明の目的は、少なくとも１つの実施形態において、移動物体上に搭載可能な測位システムを提供することでもある。

以上を達成するために、本発明は、
− 移動物体の基準系における１組の画定された点を取得するために、少なくとも１つの方向における移動物体から環境までの複数の距離を測定するステップと、
− 前述の１組の点と環境の３Ｄモデルとの間の差異を評価するステップと、
− 前述の差異から、環境の基準系における移動物体の相対位置を判定するステップと
を含む、環境における基準系に基づく３Ｄモデルを形成する１組の幾何学的表面によりモデル化された環境に対して移動物体の相対位置を判定するための方法に関する。

したがって、本発明に係る方法は、ビーコンの配置を要求することなく、全体的または部分的に認識済みの環境における移動物体の測位を可能にする。「環境」という用語は、移動物体がその中で移動する体積と、前述の体積を構成する要素と、を意味するものと理解される（例えば、表面の検査が必要とされる航空機を収容する格納庫）。大きい寸法を有する要素に対して表面検査を実施するとき、検査対象の要素は、既知であり、コンピュータ支援設計（ＣＡＤ）ソフトウェアにより３Ｄモデル化されうるか、または、他の用途のためにモデル化済みである。

この方法では、環境から移動物体までの距離の測定値が常時使用され、環境に対する移動物体の相対位置を判定することが可能である。例えば、検査目的のために航空機の周囲を飛行するドローンは、ドローン自体の位置を常時認識し、航空機の表面に欠陥が存在した場合には、ドローン自体の位置に対する当該欠陥の位置を判定し、したがって航空機上での当該欠陥の位置を特定し得るであろう。

したがって実装されたこの方法は、高速であり、実行のための資源に関しては費用効果が高く、移動物体に対する統合化された搭載が容易であり、エネルギーをほとんど消費せず、かつ、リアルタイム処理が可能である搭載型プロセッサを使用する。この方法の一部は、特に一度のみの実行を必要とし、移動物体の位置または測定距離に関わらないステップ（例えば、３Ｄモデル化にリンクされた前処理ステップ）は、移動物体の外部での実行が可能であり、それにより、これらのステップの結果は、処理されるステップの実行スピードが改善されるよう、搭載型プロセッサに提供される。

この方法の実行スピードは、実行の頻度を向上させることも可能であり、その結果、移動物体の位置が高速に追跡され、したがって前述の移動物体がより高速に移動することが可能であることが保証される。

この方法は、位置情報がないために特に屋内で有利であるにもかかわらず、認識済みの環境における改善された確度が要求される用途（例えば屋外の航空機、風力タービンの表面、乾ドック内の大型船舶の検査）のために屋外における使用も可能である。

本発明によれば、３Ｄモデルが３Ｄ多角形メッシュであり幾何学的表面が多角形であると有利である。

３Ｄ多角形メッシュを使用して３Ｄ環境をモデル化することにより、簡単なモデル化と、この方法の異なるステップのより高速な処理と、が可能となる。加えて、より複雑な幾何学的表面を含む３Ｄモデルは、当業者に周知である技術にしたがって、３Ｄ多角形メッシュによる近似が可能である。

本発明によれば、複数の距離を測定するステップは、少なくとも１つのレーザスキャナ（レーザ距離計としても知られる）により実行されると有利である。

本発明の以前の態様によれば、複数の距離を測定するステップは、交差する平面においてスキャンするよう構成された少なくとも２つのレーザスキャナにより実行されると有利である。

各追加的レーザスキャナは、他のレーザスキャナのスキャン表面に対して交差する平面においてスキャン動作を実行するよう構成されていると好適である。

本発明によれれば、１組の点と、環境の３Ｄモデルと、の間の差異を評価するステップは、
− 位置の推定値から、および移動物体の姿勢から、移動物体の基準系における１組の点を環境の基準系における点群に変換するステップと、
− 前述の点群における各点に対して、前述の点と環境の３Ｄモデルの表面との間のノルムを計算するステップと
を含むと有利である。

移動物体の姿勢は空間内における移動物体の方位に対応し、当業者に周知であるオイラー角θ、φ、およびψにより表現される。

本発明に係る方法は、前述の点群における各点に対して、前述の点と環境の３Ｄモデルの表面との間のノルムを計算するステップの前に、
− 環境における各点が複数の立方体に含まれるよう、環境を重畳立方体に分解するステップと、
− 各立方体に対して、立方体に対して非ゼロの交点を有する環境の３Ｄモデルの表面を判定するステップと
を含むと有利であり、前述の点群における各点に対して、前述の点と環境の３Ｄモデルの表面との間のノルムを形成するステップは、
− これらの立方体のうちの中心立方体とよばれる１つの立方体であって、点群における点がその中に配置され、かつ、点群における点がその表面の各表面から最も遠位にある、立方体を選択するステップと、
− 中心立方体に対して交点を有する、近接表面と呼ばれる表面のリストを取得するステップと、
− 点と各近接表面との間のノルムを計算するステップと、
− 近接表面から点に対して最も近位にある表面を判定するステップであって、前述の最も近位にある表面は、前述の表面と点との間のノルムが最低である表面であり、前述のノルムは、点と環境の３Ｄモデルとの間のノルムと考えられる、ステップと
を含む。

環境を重畳立方体に分解するステップ、および、各立方体に対して、立方体に対して非ゼロの交点を有する環境の３Ｄモデルの表面を判定するステップは事前に実行され得、これらのステップの結果は、処理の高速化を図るために、移動物体のメモリに保存され得ると有利である。実際、これらのステップは、移動物体の位置にも、または、取得される距離測定値にも、リンクされない。

本発明のこの態様によれば、環境を立方体に分解するステップ、および、各点と近接表面との間のノルムを計算するステップは、各点と環境の３Ｄモデルとの間のノルムが高速に計算されること、および、資源に関して費用効果が高くなること、を可能にする。なぜなら、表面全体に対するノルムの計算よりもむしろ近接表面に対するノルムの計算のみが実行されるためである。

３Ｄモデルから点群までの距離は、点群における各点から３Ｄモデルまでの、特にこの点に対して最も近位にある３Ｄモデルの表面までの、ノルムの合計である。

本発明によれば、その点のノルムが所定閾値よりも大きい点（分離点とよばれる）は点群から抽出され、この方法は、障害物を検出するステップを含む。このステップではこれらの近接した分離点が一緒にグループ化されて、障害物を表す体積が形成され、前述の体積は記録されると有利である。

本発明のこの態様によれば、この方法は、移動物体が前述の障害物に接触することを防止するために、環境の３Ｄモデルに含まれない障害物に対応する点を分離すること、前述の点を処理すること、および、移動物体の測位を実施するときに前述の点を考慮すること、を可能する。障害物を示す体積は記録され、ナビゲーションの際に考慮されることが可能であり、移動物体が動き回るにつれて追跡されることが可能である。

本発明に係る方法は、環境の地面または天井に対応する点を点群から除去するステップを含むと有利である。

本発明のこの態様によれば、この方法は、移動物体の高度または係る高度の推定値が利用可能であるときにはほとんど利用価値がない環境の地面または天井に対応する点を点群から除去することにより、処理すべき点の個数を制限することを可能する。移動物体は、地面および天井が一定高度に対応することを認識し、地面または天井を形成する点を認識する必要がない。高度は、直接測定により、または推定により、認識が可能である。

したがってこの方法は、処理すべき点の個数が低減されているため、より高速である。

本発明に係る方法は、不明瞭または冗長な点を点群から除去するステップを含むと有利である。

本発明のこの態様によれば、この方法は、不明瞭または冗長な距離測定値に対応する点を点群から除去することにより、処理すべき点の個数を制限することを可能にする。不明瞭な点とは、表面の縁部上の点測定に、または、低い入射角を有する表面上の測定に、対応する点であり、これらの不明瞭な点は不正確であり得る。冗長な点とは、表面上に配置された点であって、点群が前述の表面を画成するにあたりすでに十分な点を含む、点である。

したがってこの方法は、処理すべき点の個数が低減されているため、より高速である。

本発明は、移動物体を移動させる少なくとも１つのステップを含む、環境においてナビゲートするための方法であって、移動物体の位置が、移動ステップの間、本発明に係る判定方法により判定されることを特徴とする、方法にも関する。

本発明は、移動物体を移動させる少なくとも１つのステップを含む、環境において移動物体をナビゲートするための方法であって、移動物体の位置および障害物に対応する体積の位置が本発明に係る判定方法により判定されること、および、この方法が障害物に対応する前述の体積を回避する少なくとも１つのステップを含むこと、を特徴とする、方法にも関する。

本発明は、環境における基準系に基づく３Ｄモデルによりモデル化された環境に対して移動物体の相対位置を判定するための、前述の移動物体上に搭載されたシステムであって、
− 移動物体の基準系における１組の画定された点を取得するために、少なくとも１つの方向において移動物体から環境までの複数の距離を測定するための手段と、
− 前述の１組の点と環境の３Ｄモデルとの間の差異を評価するためのモジュールと、
− 前述の差異から、環境の基準系における移動物体の相対位置を判定するためのモジュールと
を含むことを特徴とするシステムにも関する。

この測定手段は例えば、モジュール上に搭載されたセンサであり、係るセンサの例としては、交差する平面においてスキャンするよう構成された少なくとも１つのレーザスキャナ（好適に２つのレーザスキャナ）、深度測定カメラ（赤−緑−青−深度（ＲＧＢＤ）型）、ステレオビジョン装置、ミリ波レーダなど、または前述のセンサの組み合わせが挙げられる。

距離を評価するためのモジュール、および移動物体の相対位置を判定するためのモジュールは、移動物体上に搭載される。しかし、測定距離に、または移動物体の位置に、リンクされないいくつかの計算は、別のコンピュータ上で実行され得、係る計算は、判定システムに含まれる異なる手段の処理スピードを改善するために、事前に判定システムに提供され得る。

本発明は、通信ネットワークからダウンロード可能であり、および／または、コンピュータ可読および／またはプロセッサ実行可能な媒体上に格納され得る、コンピュータ・プログラム製品であって、本発明に係る判定方法を実装するためのプログラム・コード命令を含むことを特徴とする、コンピュータ・プログラム製品にも関する。

この種類のコンピュータ・プログラム製品は、移動物体の位置を判定することを可能にする。ここで、係るコンピュータ・プログラム製品は高速で実行され、処理資源をあまり要求せず、したがって、低いエネルギー消費を有する。

本発明はコンピュータ可読格納手段にも関し、係るコンピュータ可読格納媒体は、完全にまたは部分的に取り外し可能であり、本発明に係る判定方法を実装するための１組のコンピュータ実行可能命令を含むコンピュータ・プログラムを格納する。

本発明に係る判定システムは、本発明に係る判定方法を実装すると有利である。

本発明に係る判定方法は、本発明に係るシステムにより実装されると有利である。

本発明は、通信ネットワークからダウンロード可能であり、および／または、コンピュータ可読および／またはプロセッサ実行可能な媒体上に格納され得る、コンピュータ・プログラム製品であって、本発明に係る判定方法を実装するためのプログラム・コード命令を含むことを特徴とする、コンピュータ・プログラム製品にも関する。

本発明はコンピュータ可読格納手段にも関し、係るコンピュータ可読格納媒体は、完全にまたは部分的に取り外し可能であり、本発明に係る判定方法を実装するための１組のコンピュータ実行可能命令を含むコンピュータ・プログラムを格納する。

本発明は、組み合わせにおいて前述のまたは後述の特徴の一部または全部により特徴付けられる、判定方法に、ナビゲーション方法に、システムに、コンピュータ・プログラム製品に、および、格納手段に、も関する。

本発明の他の目的、特徴、および長所は、以下に添付する図面と併せて、非限定的な事例として与えられる以下の説明を読むことにより、明らかとなるであろう。

本発明の１つの実施形態に係る方法を示す概略図である。 環境の３Ｄメッシュの多角形を示す図である。

以下の実施形態は例示である。以下の説明では、１つまたは複数の実施形態が参照されるが、このことは、それぞれの参照が同一の実施形態に関することを、または、これらの特徴が単一の実施形態のみに当てはまることを、必ずしも意味しない。他の実施形態を提供するために、異なる実施形態の単一の特徴を組み合わせることも可能である。図面では、縮尺および比率は、図示および明瞭化のために、厳格には尊重されない。

図１では、本発明の一実施形態に係る、環境における基準系に基づく３Ｄモデルを形成する１組の幾何学的表面によりモデル化された環境に対して移動物体の位置を判定するための方法が概略的に示されている。この判定方法の全部または一部は、本発明の一実施形態にしたがって、移動物体上に搭載された判定システムにより実装される。この判定方法は、３次元において移動し、かつ、ドローンなどの飛行物体（例えばヘリコプター型またはマルチローター型の）である、移動物体に対して有利に説明される。

この実施形態では、３Ｄモデルは３Ｄ多角形メッシュである。すなわち、３Ｄモデルを形成する幾何学的表面は多角形である。しかし本発明は、任意種類の３Ｄモデルにも、特に、より複雑な幾何学的表面（特に球体、円筒、楕円、および円錐）を使用する３Ｄモデルにも、適用される。

判定方法は、少なくとも１つの方向において移動物体から環境までの複数の距離を測定するための手段を使用して、移動物体の基準系における１組の画定された点を取得するために、少なくとも１つの方向において移動物体から環境までの複数の距離を測定する第１ステップ１０を含む。

前述の測定手段は例えばセンサであり、係るセンサの例としては、交差する平面においてスキャンする少なくとも１つのレーザスキャナ（好適に２つのレーザスキャナ）、深度測定カメラ（赤−緑−青−深度（ＲＧＢＤ）型）、ステレオビジョン装置、ミリ波レーダなど、または前述のセンサの組み合わせが挙げられる。

距離測定値は、移動物体の基準系に基づき、かつ、移動物体上の認識済みの点（通常は、移動物体の重心に対して既知である測定手段の位置）に関する、ペア（立体角、および、この立体角における測定距離）の形態にある。

レーザスキャナは、例えば、低い角度ピッチ（例えば０．２５°）で広い角度範囲（２７０°以上）にわたり、距離測定を実施し得る。それにより多数のペア（立体角、および距離）の取得が可能である。複数の、異なる方位を有するレーザスキャナ、および／またはミラーを使用することにより、異なる平面における測定値を得ることが可能である。

他の実施形態によれば、ＲＧＢＤカメラもしくはステレオビジョン・カメラなどの取得手段、または、レーダ型もしくはソナー型の取得手段は、スキャンの実行を必要とすることなく、いくつかの次元にわたり距離測定を取得することが可能である。

これらのペア（立体角、および距離）は移動物体の基準系における１組の点を形成する。係るペアは、移動物体の基準系における１組の点における点の球面座標に対応する。

次にこの方法は、環境の地面または天井に対応する点を点群から除去するためのモジュールにより、環境の地面または天井に対応する点を点群から除去する、任意選択によるステップ１２を含む。

全測定距離から、地面または天井に対応する測定距離を抽出することにより、判定方法の残りの部分において考慮すべき距離の個数を減少させることが可能である。

この抽出においては、移動物体の姿勢を認識することが必要である。この抽出においては、一実施形態では、移動物体の高度を認識することが必要であり（係る高度は特定のセンサにより測定される）、または、移動物体の高度を推定することが必要である（係る推定は、移動物体の以前の位置から、この最終位置からの近似的な移動を考慮に入れて、導き出される）。

この姿勢から、第１の事例では、移動物体の基準系におけるペア（立体角、距離）に関連付けられた各点の高さを判定すること、すなわち、移動物体から上向きに向けられ、かつ、移動物体から発する、垂直Ｏｚ軸に沿った座標を求めること、が可能である。したがって、正のＺ座標にある点は移動物体の上方にあり、負のＺ座標にある点は移動物体の下方にある。

次に、中心ＺＣおよび振幅２ＤＺを有する区間［ＺＣ−ＤＺ，ＺＣ＋ＤＺ］が、各表面（通常は地面および天井）に対して構築される。値ＤＺは定数（例えば１０ｃｍ）であってもよく、または垂直速さＶＺの関数であってもよい。例えばＤＺ＝ＤＺｍｉｎ＋Ｋ×｜ＶＺ｜である。式中｜ＶＺ｜はＶＺの絶対値を示し、ＤＺｍｉｎおよびＫは係数である。

第１の実施形態によれば、ＺＣは、移動物体の高度を使用して、次のように計算される。
− 地面に対しては、ＺＣ＝−ＺＥである。式中、ＺＥは、地面に対する移動物体の高度である。
− 天井に対しては、ＺＣ＝Ｈ−ＺＥである。式中、Ｈは天井の高さである。

第２の実施形態によれば、ＺＣは点の座標Ｚから、次のように直接的に計算される。
− 地面に対しては、ＺＣ＝（全Ｚ座標の最小値）＋ＤＺである。
− 天井に対しては、ＺＣ＝（全Ｚ座標の最大値）−ＤＺである。

第１の実施形態は、全部の点の最小値または最大値を計算する必要なく区間をただちに知ることができるため、より高速であるという利点を有し、地面または天井に「穴」が存在する場合でさえも動作するという利点を有する。

第２の実施形態は、移動物体の高度を使用しないという利点を有し、特に、高度の推定がもはや動作しない場合でさえも動作することが可能である。

第３の、好適な実施形態では、良好な推定値が利用可能である場合には第１の実施形態が使用され、良好な推定値が利用可能でない場合には第２の実施形態が使用される。

最終的に、これらの点に対する垂直座標がこの区間内に含まれる全部の点が抽出される。

前述の抽出された点は、前述の点の垂直座標に対してＲＡＮＳＡＣ型のアルゴリズムを適用することにより、高度に関する新しい推定を実行するためにも使用され得る。このＲＡＮＳＡＣ型のアルゴリズムでは、全部の垂直座標の平均値が計算され、これらの点に対する垂直座標がその平均値から最も大きく逸脱している点のうちの所定のパーセンテージ（例えば１０％）が除去され、残りの点から平均値が再計算され、前述の平均値が、地面に対応する点に対する地面の高さの推定値、および天井に対応する点に対する天井の高さの推定値となる。

次に、この方法は、不明瞭または冗長な点を点群から除去するための手段を使用して、不明瞭または冗長な点を点群から除去する、任意選択によるステップ１４を含む。

このステップは、いくつかの目的を有し、第１の目的は、あまりに似た測定距離（例えば同一の平面を表す１組の測定距離）が複製されることを防止することである。１つの平坦表面を画定するために数百の点を保持することは不要である。

第２の目的は、特に距離測定手段がスキャンにより測定を実行するレーザスキャナである場合に、信頼性が低い測定距離の個数を減少させることである。例えば、近接した立体角に対して、測定距離は、環境が非常に遠位にある表面を含む場合、顕著に変動し得、前述の長い距離間の中間点は不正確であり得る。さらに、それに対してスキャナのレーザが低い（０°に近い）入射角を有する表面に関する測定値は不正確であり得る。

第３の目的は、一貫した結果を得るために、全部の方向における最小個数の点が存在することを保証することである。

これらの目的を満足するために、距離を除去するステップは、
− 非不明瞭スコアＳ１＝ｆ１（最も近接した縁部までの距離／測定距離）を割り当てるステップ（ｆ１は、例えば、０（縁部に対して非常に近い）〜１（縁部から非常に遠い）の範囲の数値を返すガウス関数）と、
− 非不明瞭スコアＳ２＝ｆ２（入射角）を割り当てるステップ（ｆ２は、例えば、０（０°の見通し角）〜１（９０°の直交角）の範囲の数値を返すガウス関数）と、
− ０〜１の範囲のグローバル・スコアＳ＝Ｓ１×Ｓ２が形成されるよう、これらのスコアを組み合わせるステップと、
− 球の角度区域により、これらの測定値を分解する（例えば３０°×３０°の（３６０／３０）^２＝１４４の角度区域）ステップと、
− 各角度区域に対して、当該角度区域の測定値のグローバル・スコアが事前決定された閾値（例えば０．５）より大きい間、最良のグローバル・スコアを有する測定値を選択し、この点に最も近い近接したＰ点（例えばＰ＝２０）を除去するステップ（好適には、選択された測定値に非常に近い点は、除去される前に、前述の選択された測定値を統合するために使用され得る。この統合は、例えば、３点に対する平均または中央値である）と
を含む。

かくして、距離測定値の低減されたリストが得られる。それにより、不正確であり得るデータが除去され、各角度区域において十分な個数の点が保たれる一方で、以下のステップの処理の高速化が可能となる。

次に、この方法は、前述の１組の点と環境の３Ｄメッシュとの間の差異を評価するステップと、前述の差異から、環境の基準系における移動物体の相対位置を判定するステップ２０と、を含む。これらのステップは、移動物体の位置を精緻化することが可能となるよう、ループにおいて数回実行され得る。

前述の１組の点と環境の３Ｄメッシュとの間の差異を評価するステップは、移動物体の位置の推定から、移動物体の基準系における１組の点を、環境の基準系における点群に変換するステップ１６と、前述の点群の各点に対して、前述の点と環境の３Ｄモデルの表面との間のノルムを計算するステップ１８と、を含む。

この変換は、移動物体の位置および姿勢の推定値に基づいて、環境の基準系に対する移動物体の基準系の基底行列の変化を判定することにより実行される。環境の基準系は、好適には、Ｏｘｙｚ型の正規直交基準系である。

移動物体の位置は、この実施形態にしたがって、いくつかの異なる方法で推定され得る。

移動物体の姿勢は一般に、慣性システム（例えば低コスト慣性測定システムなど）を使用して認識される。

この方法がすでに実装されている場合、この方法により提供される最終位置が位置推定値として使用され得る。この最終位置は、移動物体が例えば加速度計により移動物体の速さおよび移動方向を近似的に認識している場合、精緻化され得る。

この方法がまだ実装されていない（通常、移動物体がスタートし、この方法が最初に起動された）場合、以下の方法が、初期位置（ｘ，ｙ，ｚ，ψ）を判定するために使用されるであろう。

移動物体の初期位置および方位が確立された（移動物体が、正確な既知の位置および方向に配置された）場合、この位置および方位は、移動物体の位置の推定値として提供され、判定方法の以下のステップは固定された回数（例えば５回）実行され、この位置の一貫性が検証される。各点と環境の表面とのノルムの平均を表す精度指標（以下で説明される）が閾値より低く（例えば平均距離が２０ｃｍより小さく）、かつ、その点と環境との間のノルムが最大距離より小さい点のパーセンテージが閾値より大きい（例えば７５％より大きい）場合、位置は一貫性を有するものとみなされる。位置が一貫性を有する場合、位置は、位置の推定値として使用され、位置が一貫性を有さない場合、以下の方法（メッシュ方法と呼ばれる）が使用される。

初期位置および方位が特定されない場合、唯一の情報は、地面に対する移動物体の高さ（すなわち高度）であり、これはｈで表される。したがって、ドローンが平面の方程式ｚ＝ｈにあることが知られる。この平面（環境が有限のサイズを有するため実際には長方形である）は、特定間隔（例えば１ｍ）でメッシュ化され、その各点は、この方法の以下のステップで使用される位置の推定値を構成する。方位の推定値が利用可能である（例えば磁界を有する屋外）場合、前述の推定値が使用される。係る推定値が利用可能でない場合、移動物体の方位ψが、例えば２０°の間隔を使用して離散化される。各ペア（位置、および方位）が、この方法の以下のステップを開始するために提供され、これらのステップは固定された回数（例えば５回）実行される。１つのペアが上記で定義された一貫性基準を満足すると、係るペアはただちに初期位置の推定値として使用される。

いくつかの環境では、移動物体の初期位置を近似的に判定することが可能である。例えば、環境が航空機を含む場合、移動物体の航空機の機体との間の距離、および／または移動物体の航空機の翼との間の距離を抽出することが可能であり、それにより、テストされるべき点の個数を顕著に限定することが可能である。

以前の位置推定値が利用可能である場合、近接した位置が優先的にテストされるであろう。

加えてメッシュ化方法では、移動物体が移動するにつれて移動物体の位置が失われた場合、有効な位置を見出すことが可能である。有効位置を見出すにあたり、検索メッシュは、最後に認識された位置を中心とする球体上の点から構成される（前述の球体は、例えば１ｍの半径を有する）。同様に、最後に認識された位置に最も近位にある点が優先的にテストされるであろう。このアルゴリズムは、一貫性基準を確認する位置が見出されるまで、反復される。当該の球体上で点がまったく見出されない場合、その半径は（例えば１ｍの間隔で）増加され、アルゴリズムは再スタートされる。

さらに高度の推定値は、環境の地面に対応する測定距離を抽出する任意選択によるステップの結果から、当該のステップが実行された場合、生じ得る。

環境の３Ｄメッシュは、例えば、環境の表面の近似を形成するために組み合わされた１組の多角形の形態における環境の表現である。これらの多角形は、例えば、三角形または四角形である。

前述の点群における各点に対して、前述の点と環境の３Ｄメッシュの多角形との間のノルムを計算するステップ１８の目的は、点群における各点に対して、この点と最も近位にある多角形とのノルムを判定すること、および、係る判定から、測定された位置を環境の基準系における点群に変換するステップにおける位置の誤差を表す点群と３Ｄメッシュとのノルムを推定すること、である。

これを実行するために、環境の３Ｄメッシュ（参照符号１００により表される）は、環境全体をカバーする重畳立方体を含む立方体パターンにより事前に分割される。環境内の各点が複数の立方体内に含まれるよう環境を重畳立方体に分割する前述のステップは、以下のステップ、すなわち、
− 環境を、サイズが等しく、重なり合わず、かつ、環境の全体をカバーする隣接する立方体（例えば、立方体の側面の長さが１ｍ）のパターンに分割するステップと、
− 立方体パターンを複製し、立方体の複製を環境の基準系のＯｘ軸に沿って半分の長さだけ並進させるステップ（したがって初期パターンの２倍の個数の立方体が取得される）と、
− 新しい立方体パターンを複製し、立方体の複製を環境の基準系のＯｙ軸に沿って半分の長さだけ並進させるステップ（したがって先行するステップの２倍の個数の立方体が取得され、初期パターンの４倍の個数の立方体が取得される）と、
− 新しい立方体パターンを複製し、立方体の複製を環境の基準系のＯｚ軸に沿って半分の長さだけ並進させるステップ（したがって先行するステップの２倍の個数の立方体が取得され、初期パターンの８倍の個数の立方体が取得される）と
の後に実行される。

この立方体パターンのために、立方体の表面に含まれず、かつ、環境の縁部にない、環境内の任意の点は、正確に８個の立方体内に存在し、これらの８個の立方体のうち、中心立方体と呼ばれる１つの立方体がつねに存在する。この中心立方体では、点は前述の立方体の各表面から最も遠位にあり、すなわち、立方体の各表面の縁部長さの１／４（１ｍ縁部を有する立方体に対して２５ｃｍ）よりも遠位にある。

立方体パターンにおける立方体のリストは、その点に対する座標が環境の基準系において表現される点に対して、簡単な数式により、この点に対する中心立方体が容易に見出されることが可能であり、立方体が環境におけるそれらの位置にリンクされた添え字を有するよう、組織されると有利である。

次にこの方法は、各立方体に対して、この立方体に対して非ゼロの交点を有する環境の３Ｄメッシュの多角形を判定するステップを含む。このステップでは、各立方体に対して、立方体の体積との交点が非ゼロである多角形（すなわち、立方体に含まれる多角形、または立方体の少なくとも１つの表面と交差する多角形）を取得するために、多角形のリストがスキャンされる。このステップの結果は搭載システムに対して高速なアクセスが可能となるよう、格納される。この方法は、どの表面が立方体に対して非ゼロの交点を有するかを定めるために、表面のリストにも同様に当てはまるであろう。

環境を重畳立方体に分割するステップ、および、各立方体に対して、その立方体に対して非ゼロの交点を有する環境の３Ｄメッシュの多角形を判定するステップは、距離測定にまたは移動物体の位置にリンクされず、かつ、環境に対して１度実行され得るステップである。したがって、これらのステップは外部コンピュータにより実行され得、添え字が添付された立方体と、この立方体に対して交点を有する各添え字が添付された立方体に関連付けられた三角形と、のリストを含む結果が、搭載システムにより格納される。

この方法は、前述の点群における各点に対して、前述の点と環境の３Ｄメッシュの多角形との間のノルムを計算するステップ１８において、立方体のうちの、点群における点がその各表面から最も遠位にある中心立方体と呼ばれる１つの立方体を選択するステップを含む。上記の立方体の優先的な添え字添付により、中心立方体は、点の座標により高速に判定される。例えば座標（ｘ_ｐ，ｙ_Ｐ，ｚ_Ｐ）を有する点Ｐに対する立方体の添え字を判定するための計算は次の式のようになる。
添え字＝Ｋ_１＋Ｋ_２×Ｆｌｏｏｒ（ｘ_ｐ／Ｌ）＋Ｋ_３×Ｆｌｏｏｒ（ｙ_ｐ／Ｌ）＋Ｋ_４×Ｆｌｏｏｒ（ｚ_ｐ／Ｌ）
式中、Ｋ_１、Ｋ_２、Ｋ_３、Ｋ_４は、メッシュの位置およびそのサイズの関数としての整数であり、Ｌは立方体の縁部の半分の長さであり、Ｆｌｏｏｒ（）は整数部分数学的関数である。

次に、この方法は、近接多角形と呼ばれる、中心立方体に対して交点を有する多角形のリストを取得するステップを含む。これらの多角形は、各立方体に対して、その立方体が非ゼロの交点を有する多角形をリンクする上記の格納により、中心立方体から高速に取得される。

リストが多角形をまったく含まない場合、この点は分離点と呼ばれ、１組の点と環境の３Ｄメッシュとの間の距離を評価するステップの残りの部分のために、点のリストから除去される。分離点は、分離点のリストにおいて、一緒にグループ化される。それにより障害物を検出することが可能となる。

次にこの方法は、点と各近接多角形との間のノルムを計算するステップを含む。

例えば多角形が三角形である場合、各ノルムは次のように計算される。
すなわち：
− Ｐは、それ自体と三角形との間のノルムが計算される点であり、
− 次の式により構成された頂点Ａ、Ｂ、およびＣを有する三角形Ｔ
Ｔ（ｓ，ｔ）＝Ａ＋ｓＢ＋ｔＣ 式中（ｓ，ｔ）∈Ｄ＝｛（ｓ，ｔ），ｔ∈［０．１］，ｓ＋ｔ≦１｝
− 三角形の点Ｔ（ｓ，ｔ）におけるＰの距離の２乗は、次の式で与えられる。
Ｑ（ｓ，ｔ）＝｜Ｔ（ｓ，ｔ）−Ｐ｜^２

アルゴリズムの目的は、Ｑを最小化する点に対応するペア（ｓ，ｔ）を見出すことである。三角形の点のノルムを高速に計算するためのアルゴリズムは、以下の通りである。
１．Ｑの勾配を最小化し、候補点と呼ばれる点を画定する、ペア（ｓ，ｔ）を計算すること。
２．候補点が三角形に属す（したがってｓおよびｔがＤに属す）場合、最小化するペア（ｓ，ｔ）は見出されず、アルゴリズムは終了される。
３．候補点が三角形内にない場合、Ｑの最小値は三角形の辺において実現される。
この場合、候補点は、三角形により画定される平面上へのＰの正射影であるが、この点は三角形により囲まれる表面に属さない。
したがって三角形により画定される平面内の空間は図２を参照して示されるように複数の領域に区画化され、図２は、三角形ＡＢＣを示し、２Ｄ基準系（Ａ，ｓ，ｔ）上にある。したがって、候補点は、領域１〜６（それぞれ、Ｒ１、Ｒ２、Ｒ３、Ｒ４、Ｒ５、Ｒ６の参照番号が与えられている）、上述の点２に対応する領域０（参照番号Ｒ０で示される）のうちの１つの領域内にある。
４．候補点のｓおよびｔの値にしたがって、対応する領域を判定すること。すなわち、簡単な幾何学的考慮は直接的に候補点に対応する領域を与える。例えばｓ≦０かつｔ≦０である場合、候補点は領域４内にある。
５．候補点までの距離を最小化する三角形の輪郭線上の、最小化点と呼ばれる点を判定すること。
−領域１：Ｑのレベルにおける曲線は、候補点を中心とする楕円である。したがって勾配がこのエリアにおいてそれ自体を無効化するならば最小化点は［ＣＢ］上に配置される。この場合、最小化ペア（ｓ，ｔ）はＣまたはＢにおいて得られる。アルゴリズムは終了する。
−領域２：Ｑのレベルにおける曲線は楕円であり、したがって最小化点は［ＣＡ］上または［ＣＢ］上に配置される。ＣにおけるＱの勾配に関する幾何学的考慮は、最小点が［ＣＡ］上または［ＣＢ］上にあるかどうか判定することを可能にする。領域１と同一の原理にしたがって、その区域における勾配の無効性または非無効性は、最小化ペア（ｓ，ｔ）を得ることを可能にする。アルゴリズムは終了する。
−領域３および５：論拠は領域１に対するものと同様である。
−領域４および６：論拠は領域２に対するものと同様である。

点ＰとエリアＤ内に配置された最小化点を特徴付ける最小化ペア（ｓ，ｔ）との間のノルムであって、点Ｐと三角形との間のノルムに対応するノルムが、最終的に返される。

計算は、三角形に関する特定個数のパラメータが事前計算されることにより、さらに高速化され得、前述の三角形に関する情報を有するメモリに格納され得る。

他の多角形の形状に対して、計算は、多角形の特定的な幾何学的形状への適応により、同様に実行され得る。代替的に各多角形は、１組の三角形により置き換えられ得る。

次に計算ステップ１８は、近接多角形から最も近位にある多角形を判定するステップを含む。前述の最も近位にある多角形は、前述の多角形と点との間のノルムが最低となる多角形であり、前述のノルムは点と環境の３Ｄメッシュとの間のノルムと考えられる。

全部の点のノルムの合計を点（分離点は除く）の個数で除算した値（精度指標と呼ばれる）は、位置推定値の精度を推定することを可能にする。

この方法は、各点と最も近位にある多角形との間の３次元ベクトルの形態におけるベクトル誤差を計算することも含む。

ベクトル誤差は、環境の基準系の３つの軸ｘ、ｙ、ｚに沿った３つの成分を含む。

環境の基準系における移動物体の相対位置を判定するステップ２０において、１組の点と環境の３Ｄメッシュとの間の距離を表す、各点との間のこれら全部のノルムの全部は、この距離を最小化するために、処理される。これを実行するために、ガウス・ニュートン勾配降下アルゴリズムを使用することが可能である。このアルゴリズムの各反復では、位置（ｘ、ｙ、およびｚに沿った）の推定値ＰＯＳ＿ｃａｌｃを計算するために位置（ｘ、ｙ、およびｚに沿った）のＰＯＳ＿ｉｎｉｔ推定値が使用され、以下のステップ、すなわち、
− 各座標において、各点と３Ｄメッシュとの間のノルムを含む、サイズＮのベクトルｒを作るステップと、
− ＰＯＳ＿ｉｎｉｔベクトルに対するｒの勾配に対応するＭ×Ｎのサイズの行列Ｊ（ヤコビ行列）を作るステップと、
− ０〜１（一般に１、または移動物体が動いていないとき、もしくは、わずかにしか動いていないときには、より小さい値をとる）の実数βの値を選択するステップと、
− （Ｊ^Ｔ＊Ｊが可逆行列である場合）次のガウス・ニュートン勾配降下、すなわち、
ＰＯＳ＿ｃａｌｃ＝ＰＯＳ＿ｉｎｉｔ−β×（Ｊ^Ｔ×Ｊ）^−１×Ｊ×γ
を適用するステップと
を含む。

移動物体の高度が認識済みである場合、計算は、Ｚ軸に沿った位置を認識済みの高度値に固定することにより、実行され得、それにより計算スピードが向上する。

ＰＯＳ＿ｉｎｉｔとＰＯＳ＿ｃａｌｃとの間の変動が十分に低いとみなされる場合、アルゴリズムは終了し、ＰＯＳ＿ｃａｌｃ値が環境の基準系における移動物体の相対位置とみなされる。係る変動が十分に低くない場合、アルゴリズムは再スタートされ、ＰＯＳ＿ｃａｌｃ値は次の反復に対するＰＯＳ＿ｉｎｉｔ値として使用される。

方法が加速されることを可能にするために、各ｘ、ｙ、およびｚ成分に対して推定位置と修正位置との間の修正方向を評価するためにベクトル誤差を考慮に入れることが可能である。

これを実行するために、各成分の最小値および最大値は、次のように、全部のベクトル誤差から判定される（例えばＸ成分に対するＸ_ｍｉｎおよびＸ_ｍａｘ）。
− これらの最小値および最大値が異なる符号を有する場合、方向の推定は不可能である。
− これら２つの値が正である場合、修正は正の方向においてなされ、修正は少なくともＸ_ｍｉｎに等しい。したがってＸ_ｍｉｎのこの修正は、より高速に修正位置に到達するために、直接的に作られ得る。
− これら２つの値が負である場合、修正は負の方向においてなされ、修正は少なくともＸ_ｍａｎに等しい。したがってＸ_ｍａｎのこの修正は、より高速に修正位置に到達するために、直接的に作られ得る。
同じプロセスがｙ成分およびｚ成分に対して実行される。

この方法は、障害物を表現する体積を形成するために分離点が一緒にグループ化される障害物を検出するステップと呼ばれる、分離点を処理するステップ２２も含む。

例えば、環境は隣接するボクセル型の立方体に分割され、この方法は、１組のボクセルをスキャンして、そのボクセルが少なくとも１つの分離点を含むかどうかを判定する。ボクセルが少なくとも１つの分離点を含む場合、当該のボクセルは障害物ボクセルと考えられる。近接する障害物ボクセルは、例えば数学的な形態演算（例えば浸食ステップに先行する２つの膨張ステップなど）により、一緒にグループ化され、体積が形成される。これらの体積に含まれるボクセルは、添え字が添付され、障害物として記録される。かくして検出された障害物の位置が画定される。記録された障害物の位置に関するこの情報は、ナビゲーションを支援（障害物を回避）し、長時間にわたり障害物を追跡することを可能にする。

この方法は、所望の制約（例えば５０Ｈｚ）により移動物体の位置における変化を追跡するために、所定の周波数において実装される。

位置が判定され、潜在的な障害物が判定されると、移動物体はこの情報を使用して、前述の移動物体が動くにあたり必要なナビゲーションおよび案内のための方法２４を実装することができる。ナビゲーション方法２４は、少なくとも１つの移動ステップを含み、係る移動ステップの間、移動物体の位置および任意の潜在的な障害物が、特に移動の行路にわたり定期的な様式で、判定される。

この方法の異なるステップは、異なるモジュール（例えばプロセッサ、マイクロコントローラ、計算器、など）により実装される。１つのステップが単一の専用モジュールにより実装されてもよく、または複数のステップが同一のモジュールにより実装されてもよい。

本発明は説明された実施形態に制限されない。特に、本発明は、車輪付きの移動物体にも適用され得る。その場合、計算は、地面上での２Ｄ運動に対応するｘ座標およびｙ座標に計算を限定することにより、簡略化され得る。

Claims (14)

1. 環境における基準系に基づく３Ｄモデルを形成する１組の幾何学的表面によりモデル化された前記環境に対して移動物体の相対位置を判定するための方法であって、
   − 前記移動物体の基準系における１組の画定された点を取得するために、少なくとも１つの方向における前記移動物体から前記環境までの複数の距離を測定するステップ（１０）と、
   − 前記１組の点と前記環境の前記３Ｄモデルとの間の差異を評価するステップと、
   − 前記差異から、前記環境の前記基準系における前記移動物体の前記相対位置を判定するステップ（２０）と
   を含む、方法。
2. 前記３Ｄモデルは３Ｄ多角形メッシュであること、および、前記幾何学的表面は多角形であること、を特徴とする、請求項１に記載の判定方法。
3. 複数の距離を判定する前記ステップ（１０）は少なくとも１つのレーザスキャナにより実行されることを特徴とする、請求項１または請求項２に記載の判定方法。
4. 複数の距離を測定する前記ステップ（１０）は、交差する平面においてスキャンするよう構成された少なくとも２つのレーザスキャナにより実行されることを特徴とする、請求項３に記載の判定方法。
5. 前記１組の点と前記環境の前記３Ｄモデルとの間の差異を評価する前記ステップは、
   − 前記位置の推定から、および前記移動物体の姿勢から、前記移動物体の前記基準系における前記１組の点を前記環境の前記基準系における点群に変換するステップ（１６）と、
   − 前記点群における各点に対して、前記点と前記環境の前記３Ｄモデルの表面との間のノルムを計算するステップ（１８）と
   を含むことを特徴とする、請求項１〜請求項４のうちのいずれか１項に記載の判定方法。
6. 前記点群における各点に対して前記点と前記環境の前記３Ｄモデルの表面との間のノルムを計算する前記ステップ（１８）の前に、
   − 前記環境における各点が複数の立方体に含まれるよう、前記環境を重畳立方体に分解するステップと、
   − 各立方体に対して、前記立方体に対して非ゼロの交点を有する前記環境の前記３Ｄモデルの表面を判定するステップと
   を含むことと、
   前記点群における各点に対して前記点と前記環境の前記３Ｄモデルの表面との間のノルムを計算する前記ステップ（１８）は、
   − 前記立方体のうちの中心立方体とよばれる１つの立方体であって、前記点群における前記点がその中に配置され、かつ、前記点群における前記点がその表面の各表面から最も遠位にある、立方体を選択するステップと、
   − 前記中心立方体に対して交点を有する、近接表面と呼ばれる前記表面のリストを取得するステップと、
   − 前記点と各近接表面との間のノルムを計算するステップと、
   − 前記近接表面から前記点に対して最も近位にある表面を判定するステップであって、前記最も近位にある表面は、前記表面と前記点との間のノルムが最低である表面であり、前記ノルムは、前記点と前記環境の前記３Ｄモデルとの間のノルムと考えられる、ステップと
   を含むことと
   を特徴とする、請求項５に記載の判定方法。
7. 点のノルムが所定閾値よりも大きい、分離点と呼ばれる点は前記点群から抽出され、前記方法は、前記近接した分離点が一緒にグループ化されて障害物を表現する体積が形成される、障害物を検出するステップ（２２）を含むことを特徴とする、請求項６に記載の判定方法。
8. 前記環境の地面または天井に対応する点を前記点群から除去するステップ（１２）を含むことを特徴とする、請求項５〜請求項７のうちのいずれか１項に記載の判定方法。
9. 不明瞭または冗長な点を前記点群から除去するステップ（１４）を含むことを特徴とする、請求項５〜請求項８のうちのいずれか１項に記載の判定方法。
10. 移動物体を移動させる少なくとも１つのステップを含む、環境において前記移動物体をナビゲーションするための方法であって、前記移動物体の位置は、前記移動ステップの間、請求項１〜請求項９のうちのいずれか１項に記載の判定方法により判定されることを特徴とする、方法。
11. 移動物体を移動させる少なくとも１つのステップを含む、環境において前記移動物体をナビゲーションするための方法であって、前記移動物体の位置および障害物に対応する体積の位置が請求項７に記載の判定方法により判定されること、および、前記方法は障害物に対応する前記体積を回避する少なくとも１つのステップを含むことを特徴とする、方法。
12. 環境における基準系に基づく３Ｄモデルによりモデル化された前記環境に対して移動物体の相対位置を判定するための、前記移動物体上に搭載されたシステムであって、
    − 前記移動物体の基準系における１組の画定された点を取得するために、少なくとも１つの方向において前記移動物体から前記環境までの複数の距離を測定するための手段と、
    − 前記１組の点と前記環境の前記３Ｄモデルとの間の差異を評価するためのモジュールと、
    − 前記差異から、前記環境の前記基準系における前記移動物体の相対位置を判定するためのモジュールと
    を含むことを特徴とする、システム。
13. 通信ネットワークからダウンロード可能であり、および／または、コンピュータ可読および／またはプロセッサ実行可能な媒体上に格納可能である、コンピュータ・プログラム製品であって、請求項１〜請求項９のうちのいずれか１項に記載の判定方法を実装するためのプログラム・コード命令を含むことを特徴とする、コンピュータ・プログラム製品。
14. 完全または部分的に取り外し可能であり、請求項１〜請求項９のうちのいずれか１項に記載の判定方法を実装するための１組のコンピュータ実行可能命令を含むコンピュータ・プログラムを格納する、コンピュータ可読格納手段。

Images