JP6601613B2 - 位置推定方法、位置推定装置及び位置推定プログラム - Google Patents

Description

本発明は、位置推定方法、位置推定装置及び位置推定プログラムに関する。

位置推定装置として、レーザセンサ又は撮像装置等のセンサを用いて周囲の地物の位置を測定し、測定された周囲の地物の位置に基づいて自己位置を推定するものが知られている。このような位置推定装置では、測定中における各センサの位置及び姿勢が推定され、推定された位置及び姿勢と周囲の地物の位置とに基づいて自己位置が推定されるため、低精度の測定結果の除去が必要となる。

例えば、非特許文献１には、移動ロボットに搭載されたカメラによって撮影された複数の画像に含まれる複数の特徴点座標に基づいてカメラの位置姿勢が推定され、推定された位置姿勢から移動ロボットの自己位置が推定される位置推定方法が記載されている。この位置推定方法では、撮影された最初の画像から特徴点が抽出され、特徴点追跡手法によって移動後の画像に含まれる特徴点が追跡され、各画像における特徴点に基づいて並進ベクトル及び回転行列が算出されることによってカメラの位置姿勢が推定される。

川西亮輔, 外２名, "全方位カメラを用いた周囲環境測定のための特徴点フローモデルによる位置姿勢推定", 映像情報メディア学会技術報告, ２００９年２月, Vol.33, No.11, pp.65-68

従来の位置推定方法においては、レーザセンサ又は撮像装置等によって測定されて得られた複数の情報から抽出される特徴点として、その追跡に誤対応が生じないように、コーナー点、エッジ交点等が抽出されていた。しかしながら、このような位置推定方法によって屋外における自己位置推定が実施される場合、植栽や樹木の揺れ並びに移動する車両及び人等に対応する特徴点が抽出されることがあった。この場合、取得された複数の情報毎に、特徴点の現実空間における位置が異なることがあったため、特徴点の抽出又は追跡の精度が低下するという問題が生じていた。

本発明は、このような課題を解決すべくなされたものであり、周囲の地物の位置を測定するセンサによって取得された情報に基づく自己位置推定の精度を向上させることを可能とする位置推定方法、位置推定装置及び位置推定プログラムを提供することを目的とする。

本発明に係る位置推定方法は、複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定するコンピュータによる位置推定方法であって、センサから出力された点群をフレーム毎に取得し、取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、平面点群により形成される平面の位置をフレーム毎に特定し、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトル及び回転行列を算出し、フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、移動体の軌跡を推定することを含む。

また、本発明に係る位置推定方法において、センサは、パルス状に発光するレーザを照射して地物の位置を測定するレーザ照射部を有し、出力された点群に含まれる各点は、センサが搭載された位置を中心にレーザ照射部を回転させることにより順次測定された地物のセンサ座標系の位置を示し、検出することは、フレーム毎に、点群に含まれる各点のうちの所定の点と次の点との距離を算出し、算出した各距離に基づいて点群をセグメントに分類し、セグメントに分類された点群に含まれる点の数が所定数以上であり且つセグメントに分類された点群によって形成される形状が直線状であるセグメントを特定し、複数の特定されたセグメントの中から、各特定されたセグメントの法線ベクトル及び各特定されたセグメントの位置に応じて平面を形成するセグメントを検出し、検出されたセグメントに分類された点群を平面点群として抽出することを含むことが好ましい。

また、本発明に係る位置推定方法において、算出することは、所定のフレームにおける特定された平面の位置と、所定のフレームにおける特定された平面の位置に最も近い次のフレームにおける特定された平面の位置とによって、移動体の並進ベクトル及び回転行列を算出することを含むことが好ましい。

また、本発明に係る位置推定方法において、算出することは、所定のフレームにおける特定された平面の法線方向と次のフレームにおける特定された平面の法線方向とが所定の角度以内である場合、所定のフレームにおける特定された平面の位置と次のフレームにおける特定された平面の位置とによって、移動体の移動体の並進ベクトル及び回転行列を算出することを含むことが好ましい。

また、本発明に係る位置推定方法において、算出することは、所定のフレームにおける特定された平面の法線ベクトルと次のフレームにおける特定された平面の法線ベクトルとによって、センサの並進ベクトル及び回転行列を算出することを含むことが好ましい。

また、本発明に係る位置推定方法において、算出することは、算出された並進ベクトル及び回転行列を、所定のフレームにおける特定された平面と次のフレームにおける特定された平面とによって最適化し、最適化された並進ベクトル及び回転行列を、所定のフレームと次のフレームにおけるフレーム間の移動体の並進ベクトル及び回転行列とすることを含むことが好ましい。

また、本発明に係る位置推定方法において、最適化することは、所定のフレームにおける平面点群を、算出された並進ベクトル及び回転行列によって移動させ、移動した所定のフレームにおける平面点群と次のフレームにおける平面点群とに対する、移動した平面点群により形成される平面の法線方向の分散が最小となるように並進ベクトル及び回転行列を最適化することを含むことが好ましい。

本発明に係る位置推定装置は、複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサが搭載された移動体の軌跡を推定する位置推定装置であって、センサから出力された点群をフレーム毎に取得する取得部と、取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出する抽出部と、平面点群により形成される平面の位置をフレーム毎に特定する特定部と、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトル及び回転行列を算出する算出部と、フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、移動体の軌跡を推定する推定部とを備える。

本発明に係る位置推定プログラムは、複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサが搭載された移動体の軌跡を推定する位置推定プログラムであって、センサから出力された点群をフレーム毎に取得し、取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、平面点群により形成される平面の位置をフレーム毎に特定し、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトル及び回転行列を算出し、フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、移動体の軌跡を推定することをコンピュータに実行させる。

本発明に係る位置推定方法、位置推定装置及び位置推定プログラムでは、周囲の地物の位置を測定するセンサによって取得された情報に基づく自己位置推定の精度を向上させることが可能となる。

位置推定システムの概要を説明するための模式図である。 位置推定システム１の概略構成の一例を示す図である。 位置推定装置２の概略構成の一例を示す図である。 点群テーブルのデータ構造の一例を示す図である。 位置推定処理の動作フローの一例を示す図である。 １フレームの点群をマッピングした３次元空間の一例を示す模式図である。 平面抽出処理の動作フローの一例を示す図である。 特定の１つのレイヤーにおける各セグメントをマッピングした２次元空間の一例を示す模式図である。 特定の１つのレイヤーにおける各直線状のセグメントをマッピングした２次元空間の一例を示す模式図である。 特定の１つのフレームにおける各直線状のセグメントをマッピングした３次元空間の一例を示す模式図である。 直線状のセグメントにおける法線ベクトルの一例を説明するための模式図である。 特定の１つのフレームにおける各直線状のセグメントの法線ベクトルをマッピングした３次元空間の一例を示す模式図である。 特定の１つのフレームにおける平面点群をマッピングした３次元空間の一例を示す模式図である。 並進ベクトル・回転行列算出処理の動作フローの一例を示す図である。 隣り合う２つのフレームの平面点群のマッチングを説明するための模式図である。 （ａ）平面点群の平面への射影を説明するための模式図である。（ｂ）マッチング候補の平面点群の平面への射影を説明するための模式図である。 Ｃｏｎｖｅｘ ｈｕｌｌを説明するための模式図である。 最適化処理の動作フローの一例を示す図である。 （ａ）平面点群の実際の平面及びマッチング平面を説明するための模式図である。（ｂ）平面点群の最適化を説明するための模式図である。 移動体の軌跡を説明するための模式図である。

以下、図面を参照しつつ、本発明の様々な実施形態について説明する。ただし、本発明の技術的範囲はそれらの実施形態に限定されず、特許請求の範囲に記載された発明とその均等物に及ぶ点に留意されたい。

１．本実施形態の概略
図１は、本実施形態の位置推定システムの概要を説明するための模式図である。本実施形態の位置推定システムでは、複数の地物の位置を所定の測定時間毎に測定し、測定した地物の複数のセンサ座標系の位置を示す点群ＰＴを出力するセンサを搭載した移動体の軌跡が推定される。位置推定装置は、コンピュータであるが、本発明が適用可能であればどのような形態でもよい。例えば、位置推定装置は、パーソナルコンピュータ、サーバ、ノートＰＣ、多機能携帯電話機（所謂「スマートフォン」）、携帯情報端末（Personal Digital Assistant, PDA）、タブレット端末、タブレットＰＣ、携帯型ゲーム機等でもよい。移動体は、自動車、原動機付自転車、又は軽車両（自転車等）等である。

センサは、周囲にある地物等の物体までの距離及び角度を連続して測定し、測定された距離及び角度に基づいてセンサ座標系の３次元位置を出力する。センサは、ＬｉＤＡＲ（Light Detection and Ranging）又はレーザレンジファインダ等のレーザ測距装置である。レーザ測距装置のレーザ照射部分は、パルス状に発光するレーザを照射して散乱光を測定し、照射から検出までの時間に基づいて反射した位置を測定する。レーザ照射部分は、垂直視野角約４０度の範囲を測定できるように３２個のレーザ送受信センサを備え、接地面と平行面上に３６０度回転する。このレーザ測距装置では、垂直視野角約４０度の範囲内において３２本のレーザが同時に照射され、レーザ照射部分の回転により、周囲３６０度の地物の位置が測定される。なお、センサとして、レーダ測距装置、撮像装置等が用いられてもよい。センサとして撮像装置が用いられる場合、例えば、２地点において、撮像装置がセンサの周囲にある同一物体を撮像することにより、三角測量によって物体の位置が測定される。

位置推定装置は、以下に説明する、取得工程、抽出工程、平面特定工程、算出工程、及び推定工程を実行する。図１（１）に示すとおり、位置推定装置が取得工程を実行する前に、センサによる測定が行われる。まず、センサが移動体に搭載され、移動体が移動を開始する。そして、移動体が移動を続けながら、センサは、所定の測定時間毎（例えば、０．１秒毎）にレーザ照射部分を１回転させながら、移動体周囲の地物までの距離及び角度を測定し、地物のセンサ座標系の３次元位置を複数出力する。以上により、センサによる測定が終了する。

取得工程として、位置推定装置は、センサから出力された点群をフレーム毎に取得する。すなわち、位置推定装置は、出力された複数の地物の３次元位置を示す点群ＰＴを、レーザ照射部分が１回転するたびに１フレームずつ順次取得する。以下、センサのレーザ照射部分が１回転されることによって取得された点群を１フレームの点群と称する。なお、点群ＰＴに含まれる各点は、各フレームにおいてセンサによって測定された測距値及び角度に基づく複数のセンサ座標系の３次元位置（ｘ、ｙ、ｚ）で表される。図１（２）に示すとおり、特定のフレームにおいて点群ＰＴによって示される位置は、センサ周囲の地物の表面上の位置である。

抽出工程として、位置推定装置は、取得した点群ＰＴから、平面を形成する点群を平面点群としてフレーム毎に抽出する。図１（３）に示すとおり、平面を形成する点群ＰＴとは、一辺ａが他の二辺ｂ，ｃよりも極めて短い直方体の内部に分布される点群ＰＴである。一辺ａが他の二辺ｂ，ｃよりも極めて短い場合とは、例えば、一辺ａが他の二辺ｂ，ｃの１／４００の長さよりも短い場合などである。位置推定装置は、点群に対して後述する主成分分析を行い、辺ａ，ｂ，ｃの軸を求めて平面点群を抽出する。なお、平面点群によって示される位置は、例えば、建築物等の地物の同一壁面上の位置を示していると推定される。

平面特定工程として、位置推定装置は、平面点群により形成される平面の位置をフレーム毎に特定する。図１（４）に示すとおり、位置推定装置は、各フレームにおいて、平面点群に対して後述する主成分分析を行い、２次元平面ＰＬを特定する。

算出工程として、位置推定装置は、所定のフレームにおいて特定された平面の位置と、次のフレームにおいて特定された平面の位置との関係からフレーム間の移動体の並進ベクトルＴ及び回転行列Ｒを算出する。まず、位置推定装置は、複数のフレーム毎に、各フレームにおける平面と次のフレームにおける平面とのマッチングを行う。次に、図１（５）に示すとおり、位置推定装置は、マッチングが成立したフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する。

推定工程として、位置推定装置は、フレーム間の移動体の並進ベクトルＴ及び回転行列Ｒをフレーム毎に順次算出して、移動体の軌跡を推定する。すなわち、位置推定装置は、各フレームにおいて算出された移動体の並進ベクトルＴ及び回転行列Ｒを、フレームの順番に従って、累積加算していき移動体の移動軌跡を推定する。例えば図１（６）に示すとおり、位置推定装置は、測定開始位置の３次元位置（Ｘ，Ｙ，Ｚ）を（０，０，０）として、フレーム間の並進ベクトルＴ及び回転行列Ｒに基づいて移動体の軌跡を推定する。

これにより、本実施形態の位置推定装置は、移動や揺れのない建築物等の地物の壁面に位置する平面点群を、位置推定の対象として抽出することができるため、自己位置推定の精度を向上させることが可能となる。

２．位置推定システム１の構成
図２は、位置推定システム１の概略構成の一例を示す図である。

位置推定システム１は、位置推定装置２、センサ３、センサ３が搭載された移動体４を有する。センサ３は、測定された３次元位置の情報を出力するための出力インターフェース（図示せず）を備える。位置推定装置２は、センサ３と有線又は無線接続され、センサ３の出力インターフェース経由でセンサ３から出力された点群の情報を取得する。

なお、センサ３は、出力インターフェースを介して、ＣＤ−ＲＯＭ（compact disk read only memory）、ＤＶＤ−ＲＯＭ（digital versatile disk read only memory）等のコンピュータ読み取り可能な可搬型記録媒体の記録装置と接続されてもよい。この場合、可搬型記録媒体の記録装置は、センサ３の出力インターフェースを介して入力された点群の置の情報を、可搬型記録媒体に記録する。そして、位置推定装置２は、点群の情報が記録された可搬型記録媒体を読み込むことにより、当該点群の情報を取得する。

２．１．位置推定装置２の構成
図３は、位置推定装置２の概略構成の一例を示す図である。また、図４は、記憶部２１が記憶する点群テーブルのデータ構造の一例を示す図である。

図３に示すように、位置推定装置２は、記憶部２１と、入力部２２と、出力部２３と、処理部２４とを備える。位置推定装置２は、センサから出力された点群を取得し、取得された点群に基づいてセンサを搭載した移動体の軌跡を推定する。

記憶部２１は、例えば、磁気テープ装置、磁気ディスク装置、又は光ディスク装置のうちの少なくとも一つを備える。記憶部２１は、処理部２４での処理に用いられるオペレーティングシステムプログラム、ドライバプログラム、アプリケーションプログラム、データ等を記憶する。例えば、記憶部２１は、アプリケーションプログラムとして、点群を取得する処理、取得された点群に基づく移動体の位置推定処理、及び位置推定処理の結果に係る表示データを作成する処理等を、処理部２４に実行させるための位置推定プログラム等を記憶する。位置推定プログラムは、例えばＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ等のコンピュータ読み取り可能な可搬型記録媒体から、公知のセットアッププログラム等を用いて記憶部２１にインストールされてもよい。

また、記憶部２１は、データとして、取得した点群を管理するための点群テーブル等を記憶する。さらに、記憶部２１は、所定の処理に係る一時的なデータを一時的に記憶してもよい。

図４は、点群テーブルのデータ構造の一例を示す図である。

点群テーブルには、垂直視野角約４０度の範囲内において３２本のレーザを同時に照射し、レーザ照射部分の回転により周囲３６０度の地物の位置を測定するセンサによる測定結果のデータが格納される。図４に示すように、点群テーブルには、レーザＩＤ毎に、フレーム番号と、レーザの照射によって測定された測距値及び各レーザに固有の垂直視野角に基づくセンサ座標系の３次元位置（ｘ、ｙ、ｚ）とが、測定時間に従って順次関連付けられて記憶される。なお、１回のレーザの照射によって測定された測距値及び垂直視野角に基づくセンサ座標系の３次元位置（ｘ、ｙ、ｚ）が、１レコードとして記憶され、各レコードは、測定された順番に、レコード１、レコード２、レコード３、・・・として記憶される。レーザＩＤは、３２本のレーザをそれぞれ識別するための識別情報である。フレーム番号は、フレームを識別するための番号であり、レーザ照射部分が１回転するたびに時系列順に付与される。

図３に戻り、入力部２２は、データの入力が可能であればどのようなデバイスでもよく、例えば、タッチパネル、キーボタン等である。操作者は、入力部２２を用いて、文字、数字、記号等を入力することができる。入力部２２は、操作者により操作されると、その操作に対応する信号を生成する。そして、生成された信号は、操作者の指示として、処理部２４に供給される。

出力部２３も、映像や画像等の表示が可能であればどのようなデバイスでもよく、例えば、液晶ディスプレイや有機ＥＬ（Electro−Luminescence）ディスプレイ等である。出力部２３は、処理部２４から供給された映像データに応じた映像や、画像データに応じた画像等を表示する。また、出力部２３は、紙などの表示媒体に、映像、画像又は文字等を印刷する出力装置であってもよい。

処理部２４は、一又は複数個のプロセッサ及びその周辺回路を有する。処理部２４は、位置推定装置２の全体的な動作を統括的に制御するものであり、例えば、ＣＰＵである。処理部２４は、記憶部２１に記憶されているプログラム（ドライバプログラム、オペレーティングシステムプログラム、アプリケーションプログラム等）に基づいて処理を実行する。また、処理部２４は、複数のプログラム（アプリケーションプログラム等）を並列に実行できる。

２．２．処理部２４の構成
処理部２４は、取得部２４１、抽出部２４２、平面特定部２４３、算出部２４４、最適化部２４５、及び推定部２４６を備える。これらの各部は、処理部２４が備えるプロセッサで実行されるプログラムにより実現される機能モジュールである。あるいは、これらの各部は、ファームウェアとして位置推定装置２に実装されてもよい。

図５は、位置推定処理の動作フローの一例を示す図である。この動作フローは、予め記憶部２１に記憶されているプログラムに基づいて、主に処理部２４により、位置推定装置２の各要素と協働して実行される。

まず、センサが、移動体が移動している間に回転されるレーザ照射部分によって測定された地物の位置を示す点群ＰＴを出力し、位置推定装置２の取得部２４１は、出力された点群ＰＴを１回転毎に１フレームずつ順次取得する（ステップＳ１０１）。取得部２４１は、取得した点群ＰＴを記憶部２１の点群テーブルに記憶する。

図６は、１フレームの点群をマッピングした３次元空間の一例を示す模式図である。図６に示すように、１フレームの点群は、３２本のレーザを同時に照射するセンサのレーザ照射部分の回転によって測定された、周囲３６０度の複数の地物のセンサ座標系の３次元位置である。以下、１フレームにおけるセンサ１本分の点群を１レイヤーの点群と称する。

図５に戻り、次に、位置推定装置２の抽出部２４２及び平面特定部２４３は、取得部２４１によって点群ＰＴが取得されると、平面を抽出するための平面抽出処理を実行する（ステップＳ１０２）。平面抽出処理の詳細については後述する。

次に、位置推定装置２の算出部２４４は、抽出部２４２及び平面特定部２４３によって平面抽出処理が行われると、並進ベクトル・回転行列算出処理を実行する（ステップＳ１０３）。並進ベクトル・回転行列算出処理の詳細については後述する。

次に、位置推定装置２の最適化部２４５は、算出部２４４によって並進ベクトル・回転行列算出処理が実行されると、最適化処理を実行する（ステップＳ１０４）。最適化処理の詳細については後述する。

そして、位置推定装置２の推定部２４６は、最適化部２４５によって最適化処理が実行されると、推定処理を実行し（ステップＳ１０５）、一連のステップを終了する。なお、位置推定処理の一連のステップが終了すると、操作者の指示に応じて、位置推定装置２の処理部２４は、出力部２３に、位置推定処理の結果（移動体の移動軌跡等）を表示する。

図７は、位置推定装置２の抽出部２４２及び平面特定部２４３による平面抽出処理の動作フローの一例を示す図である。図７に示す平面抽出処理は、図５のステップＳ１０２において実行される。

まず、抽出部２４２は、点群テーブルに記憶された複数のフレームの点群のうち、１つのフレームの点群を取得する（ステップＳ２０１）。以下のステップＳ２０２〜Ｓ２０７は、取得された１つのフレームの点群に対して実行される。

次に、抽出部２４２は、レイヤー毎に、取得した点群のうちの所定の点と次の点との間の距離を算出する（ステップＳ２０２）。すなわち、抽出部２４２は、レーザＩＤ毎に、所定のレコードの３次元位置（ｘ、ｙ、ｚ）と、当該レコードの次のレコードの３次元位置（ｘ、ｙ、ｚ）とを特定し、特定した両位置を端点とする線分の距離を算出する。以下、算出された距離を点間距離と称する。

次に、抽出部２４２は、各レイヤーの点群を点間距離に基づいてセグメントに分類する（ステップＳ２０３）。すなわち、抽出部２４２は、レーザＩＤ毎に、レコードの順番に従って各点間距離を累積加算した値が閾値を超えた場合、当該閾値を超える前までに累積加算した全ての点間距離における端点を示す各点を１つのセグメントとする。なお、以下、セグメントに含まれる全ての点を、セグメントに含まれる点群と称する場合がある。図８は、特定の１つのレイヤーにおける全てのセグメントをマッピングした２次元空間の一例を示す模式図である。

次に、抽出部２４２は、Ｓ２０３において分割したセグメントのうち、セグメントに含まれる点群の数が所定数以上であり且つセグメントに含まれる点群の形状が直線状であるセグメントを検出する（ステップＳ２０４）。ステップＳ２０４では、まず、抽出部２４２は、セグメントに含まれる点群の数が所定数以上であるか否か判断する。次に、抽出部２４２は、点群の数が所定数以上であるセグメントに含まれる全て点群に対して、主成分分析を行う。次に、抽出部２４２は、第１主成分の分散が第２主成分の分散及び第３主成分の分散よりも所定比率以上大きい場合，主成分分析を行った点群を含むセグメントを直線状のセグメントとして検出する。これにより、セグメントに含まれる点群によって形成される形状が、第１主成分を軸とする直線状であると判定される。

図９は、特定の１つのレイヤーにおける各直線状のセグメントをマッピングした２次元空間の一例を示す模式図であり、図１０は、特定の１つのフレームにおける各直線状のセグメントをマッピングした３次元空間の一例を示す模式図である。

次に、抽出部２４２は、Ｓ２０４において特定された直線状のセグメントの法線ベクトルを算出する（ステップＳ２０５）。ステップＳ２０５では、まず、抽出部２４２は、Ｓ２０４において特定された全ての直線状のセグメントを取得する。次に、抽出部２４２は、取得した全ての直線状のセグメントのそれぞれに対して、各直線状のセグメントを中心とした所定範囲内の他の直線状のセグメントを検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された他の直線状のセグメントとの内積のなす角が所定閾値以下の他の直線状のセグメントを近傍セグメントとして検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された近傍セグメントとに含まれる点群に対して主成分分析を行う。そして、抽出部２４２は、第１主成分の分散及び第２主成分の分散が第３主成分の分散よりも一定比率以上（例えば、４００倍以上）大きい場合、第１主成分及び第２主成分を軸とした平面に対する法線ベクトルを、各直線状のセグメントの法線ベクトルとして算出する。以下、図１１（ａ）及び（ｂ）を参照して、ステップＳ２０５の処理について具体的に説明する。

図１（ａ）は、直線状のセグメントにおける法線ベクトルの一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥ毎に、ステップＳ２０４において検出された直線状のセグメントが特定されている。例えば、セグメントＳ_１５は、レイヤーＡにおける直線状のセグメントである。また、セグメントＳ_１１、Ｓ_１２、Ｓ_１６は、レイヤーＢにおける直線状のセグメントである。また、セグメントＳ_１３、Ｓ_１４は、レイヤーＣにおける直線状のセグメントである。なお、図１１（ａ）及び（ｂ）において示される全てのセグメントは、直線状のセグメントである。図１１（ａ）に示すように、抽出部２４２は、セグメントＳ_１１を中心とした所定範囲内にある他のセグメントＳ_１２〜Ｓ_１６を検出する。次に、抽出部２４２は、セグメントＳ_１１とセグメントＳ_１２〜Ｓ_１６のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ａ）では、セグメントＳ_１２〜Ｓ_１６の全てにおいて、セグメントＳ_１１との内積のなす角度が所定閾値以下であるので、抽出部２４２は、セグメントＳ_１１〜Ｓ_１６に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ１１の法線ベクトルとして特定する。

図１１（ｂ）は、直線状のセグメントにおける法線ベクトルの他の一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥは、図１１（ａ）と同様である。セグメントＳ_２４、Ｓ_２５は、レイヤーＣにおける直線状のセグメントである。また、セグメントＳ_２１、Ｓ_２３、Ｓ_２６は、レイヤーＤにおける直線状のセグメントである。また、セグメントＳ_２２は、レイヤーＥにおける直線状のセグメントである。図１１（ｂ）に示すように、抽出部２４２は、セグメントＳ_２１を中心とした所定範囲内にある他のセグメントＳ_２２〜Ｓ_２６を検出する。次に、抽出部２４２は、セグメントＳ_２１とセグメントＳ_２２〜Ｓ_２６のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ｂ）では、セグメントＳ_２２〜Ｓ_２５のそれぞれとセグメントＳ_２１との内積のなす角度は、それぞれ所定閾値以下であり、セグメントＳ_２６とセグメントＳ_２１との内積のなす角度は所定閾値よりも大きい。したがって、抽出部２４２は、セグメントＳ_２１〜Ｓ_２５に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ_２１の法線ベクトルとして特定する。

図１２は、特定の１つのフレームにおける各直線状のセグメントの法線ベクトルをマッピングした３次元空間の一例を示す模式図である。

図７に戻り、次に、抽出部２４２及び平面特定部２４３は、平面を形成する点群を平面点群として抽出する（ステップＳ２０６）。ステップＳ２０６では、まず、抽出部２４２は、各直線状のセグメントを無向グラフの節点ｖ_kとして扱い、法線ベクトル間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖ_i、ｖ_jに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、２つの直線状のセグメントの法線ベクトルの内積が所定閾値（例えば、１０度）以内である場合、法線ベクトル間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ａ_ij＝１とし、無向辺が存在しない場合、ａ_ij＝０とする隣接行列Ａ＝［ａ_ij］を作成する。なお、各直線状のセグメントの数がｎである場合、ｉ＝１，２，３，・・・，ｎであり、ｊ＝１，２，３，・・・，ｎであり、隣接行列Ａ＝［ａ_ij］は、ｎ×ｎの正方行列である。このようにして、各直線状のセグメントの法線ベクトル間の類似度を示す隣接行列Ａ＝［ａ_ij］が作成される。

次に、抽出部２４２は、隣接行列Ａの作成処理と同様に、各直線状のセグメントを無向グラフの節点ｖ_kとして扱い、各直線状のセグメントの位置間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖ_i、ｖ_jに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、特定の直線状のセグメントと他の直線状のセグメントとの間の距離を算出し、特定の直線状のセグメントとの距離が短い所定数（例えば、５つ）の他の直線状のセグメントを特定する。そして、特定の直線状のセグメントと、特定の直線状のセグメントとの距離が短い所定数の他の直線状のセグメントとの間において、位置間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ｂ_ij＝１とし、無向辺が存在しない場合、ｂ_ij＝０とする隣接行列Ｂ＝［ｂ_ij］を作成する。このようにして、各直線状のセグメントの位置間の類似度を示す隣接行列Ｂ＝［ｂ_ij］が作成される。

次に、抽出部２４２は、次の式（１）によって、隣接行列Ａ及び隣接行列Ｂの要素積Ｃを算出する。

次に、抽出部２４２は、次の式（２）によって、ベキ乗行列のブール和を算出する。

次に、抽出部２４２は、次の式（３）に示すとおり、ベキ乗行列のブール和Ｃ_kとＣ_k+1とが変わらなくなる場合におけるＣ_k（又はＣ_k+1）を可到達行列Ｄとして算出する。

次に、抽出部２４２は、可到達行列Ｄに対して、ガウスの消去法の前進消去を行うことにより重複する行を削除し、Ｄの上三角行列を算出する。次に、抽出部２４２は、Ｄの上三角行列において、要素が０でない行を１つの平面点群であると判定し、列に各平面点群に属する直線状のセグメントを抽出する。なお、行列のランクは、当該フレームにおける平面点群の総数である。

そして、平面特定部２４３は、抽出された各平面点群に属する直線状のセグメントの点群に対し主成分分析を行う。平面特定部２４３は、主成分分析によって算出された第１主成分及び第２主成分に基づき各平面点群の平面の式を算出することにより、平面の位置を特定する。以上により、ステップＳ２０６の処理が終了する。図１３は、特定の１つのフレームにおいて、同一の平面を形成する平面点群をマッピングした３次元空間の一例を示す模式図である。

図７に戻り、次に、平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームであるか否かを判定する（ステップＳ２０７）。平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームでない場合（ステップＳ２０７−Ｎｏ）、ステップＳ２０１に処理を戻す。

平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームである場合（ステップＳ２０７−Ｙｅｓ）、一連のステップを終了する。

図１４は、位置推定装置２の算出部２４４による並進ベクトル・回転行列算出処理の動作フローの一例を示す図である。図１４に示す並進ベクトル・回転行列算出処理は、図５のステップＳ１０３において実行される。

まず、算出部２４４は、抽出部２４２がステップＳ２０６において抽出した複数のフレームの平面点群のうち、特定のフレームの平面点群及び特定のフレームの次のフレームの平面点群を取得する（ステップＳ３０１）。以下、特定のフレームの次のフレームを、単に次のフレームと称する場合がある。

まず、算出部２４４は、ステップＳ３０１で取得した特定のフレームの平面点群と次のフレームの平面点群とのマッチング候補を検出する（ステップＳ３０２）。ステップＳ３０２では、まず、算出部２４４は、特定のフレームの各平面点群の重心位置及び次のフレームの各平面点群の重心位置を算出し、特定のフレームの各平面点群の重心位置と次のフレームの各平面点群の重心位置との重心間距離を算出する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトル及び次のフレームの各平面点群の法線ベクトルを算出する。なお、算出部２４４が実行する法線ベクトルの算出処理は、抽出部２４２がステップＳ２０５において実行する法線ベクトルの算出処理と同様の処理である。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、特定のフレームの各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であると判定された場合、当該特定のフレームの平面点群と当該次のフレームの平面点群とを互いにマッチング候補として関連付ける。以上により、ステップＳ３０２の処理が終了する。

算出部２４４が実行するステップＳ３０２の処理により、特定のフレームの平面点群と次のフレームの平面点群との誤マッチングを防止することができる。以下、図１５を参照して、隣り合う２つのフレームの平面点群のマッチングについて具体的に説明する。

図１５は、隣り合う２つのフレームの平面点群のマッチングを説明するための模式図である。図１５では、平面点群Ａ１及びＡ２は、特定のフレームの平面点群であり、平面点群Ｂ１及びＢ２は、次のフレームの平面点群である。センサ３を搭載した移動体４の移動方向は、図１５に示す矢印のとおりであり、特定のフレームの平面点群Ａ１が位置する実際の平面は、次のフレームの平面点群Ｂ１が位置する平面であり。特定のフレームの平面点群Ａ２が位置する実際の平面は、次のフレームの平面点群Ｂ２が位置する平面である。

特定のフレームの各平面点群に対して、各平面点群の法線ベクトルとの重心間距離が一番短い次のフレームの平面点群をマッチング候補とした場合、図１５に示す模式図では、平面点群Ｂ２が平面点群Ａ１のマッチング候補となる。このような誤マッチングを防止するため、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。これにより、平面点群Ａ１の法線ベクトルと平面点群Ｂ２の法線ベクトルとの誤マッチングを防止することができる。

図１４に戻り、次に、算出部２４４は、特定のフレームのマッチング候補の平面点群と次のフレームのマッチング候補の平面点群とのマッチングを判定する（ステップＳ３０３）。

ステップＳ３０３では、まず、算出部２４４は、マッチング候補として互いに関連付けられた特定のフレームの平面点群及び次のフレームの平面点群を取得し、特定のフレームの平面点群に対して主成分分析を行う。算出部２４４は、図１６（ａ）に示すように、主成分分析によって算出された第１主成分及び第２主成分を軸とした２次元平面を算出し、算出された２次元平面に、特定のフレームのマッチング候補の平面点群を射影することにより次元圧縮を行う。次に、算出部２４４は、図１６（ｂ）に示すように、特定のフレームのマッチング候補の平面点群に対して算出された２次元平面に、次のフレームのマッチング候補の平面点群を射影する。次に、算出部２４４は、図１７に示すように、次元圧縮された特定のフレームのマッチング候補の平面点群に対してドロネー三角形分割を求め、求められたドロネー三角形分割に基づいてＣｏｎｖｅｘ ｈｕｌｌを作成する。次に、算出部２４４は、作成したＣｏｎｖｅｘ ｈｕｌｌの内部に、射影された次のフレームのマッチング候補の平面点群が包含される包含率を算出し、算出された包含率が所定の閾値（例えば、６０％）以上である場合は、マッチングが成立したと判定する。以上により、ステップＳ３０３の処理が終了する。

図１４に戻り、次に、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群に基づいてフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する（ステップＳ３０４）。

ステップＳ３０４では、まず、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群のそれぞれに対する平面の式を算出する。以下、前者を、特定のフレームのマッチング平面と称し、後者を、次のフレームのマッチング平面と称する。なお、算出部２４４が実行する平面の式の算出処理は、平面特定部２４３がステップＳ２０６において実行する平面の式の算出処理と同様である。次に、算出部２４４は、特定のフレームのマッチング平面の法線ベクトルＮ₁と次のフレームのマッチング平面の法線ベクトルＮ₂とにより次の式（４）に示す目的関数を最小二乗法を用いて最小化する行列Ｒ₁を算出する。

次に、算出部２４４は、行列のｐｏｌａｒ ｄｅｃｏｍｐｏｓｉｔｉｏｎを利用して、次の式（５）及び（６）によってフレーム間の回転行列Ｒを算出する。

次に、算出部２４４は、特定のフレームのマッチング平面の垂線の足からなるベクトルｄ₁と次のフレームのマッチング平面の垂線の足からなるベクトルｄ₂とから、次の式（７）によってフレーム間の並進ベクトルＴを算出する。以上により、ステップＳ３０４の処理が終了する。

そして、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームでない場合（ステップＳ３０５−Ｎｏ）、ステップＳ３０１に処理を戻す。また、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームである場合（ステップＳ３０５−Ｙｅｓ）、一連のステップを終了する。

図１８は、位置推定装置２の最適化部２４５による最適化処理の動作フローの一例を示す図である。図１８に示す最適化処理は、図５のステップＳ１０４において実行される。以下、図１９（ａ）及び（ｂ）を参照して、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群について説明する。

図１９（ａ）は、平面点群の真の平面及びマッチング平面を説明するための模式図である。図１９（ａ）に示すように、特定のフレームの平面点群の真の平面（実際の平面）と特定のフレームのマッチング平面との間に誤差が生じている場合がある。同様に、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との間に誤差が生じている場合がある。

図１９（ｂ）は、平面点群の最適化を説明するための模式図である。特定のフレームの平面点群の真の平面と特定のフレームのマッチング平面との間に誤差が生じている場合や、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との誤差が生じている場合、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させると、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じる。

最適化部２４５は、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じることが無いように、フレーム間の並進ベクトルＴ及び回転行列Ｒを最適化する。

まず、最適化部２４５は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群、並びに、特定のフレーム及び次のフレームのフレーム間の並進ベクトルＴ及び回転行列Ｒを取得する（ステップＳ４０１）。

次に、最適化部２４５は、フレーム間の並進ベクトルＴ及び回転行列Ｒの最適化処理を行う（ステップＳ４０２）。ステップ４０２では、最適化部２４５は、まず、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させ、マッチングが成立した特定のフレームの平面点群とマッチングが成立した次のフレームの平面点群とを重ねる。次に、最適化部２４５は、マッチングが成立した全ての特定のフレームの平面点群及び次のフレームの平面点群の法線方向の分散が最小となるような目的関数を設定し、レーベンバーグ・マルカート法によって、並進ベクトルＴ及び回転行列Ｒの最適化を行う。なお、レーベンバーグ・マルカート法による最適化は、並進ベクトルＴ及び回転行列Ｒを交互に最適化し、並進ベクトルＴに対して最適化を行っている場合は、回転行列Ｒを固定し、回転行列Ｒに対して最適化を行っている場合は、並進ベクトルＴを固定する。以上により、ステップＳ４０２の処理が終了する。

そして、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームでない場合（ステップＳ４０３−Ｎｏ）、ステップＳ４０１に処理を戻す。また、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームである場合（ステップＳ４０３−Ｙｅｓ）、一連のステップを終了する。

図２０は、移動体の軌跡を説明するための模式図である。位置推定処理のステップＳ１０５で示したとおり、位置推定装置２の推定部２４６は、最適化部２４５によって最適化された並進ベクトルＴ及び回転行列Ｒに基づいて、移動体の軌跡を算出する。推定部２４６は、最初のフレームを取得した移動体の位置をワールド座標系の座標（Ｘ，Ｙ，Ｚ）＝（０，０，０）とし、フレーム間の並進ベクトルＴ及び回転行列Ｒを、フレームの順番に従って順次累積加算し、各フレームを取得した移動体の位置のワールド座標系の座標を算出する。

以上説明してきたように、位置推定装置２は、移動や揺れのない建築物等の地物の壁面に位置する平面点群を、位置推定の対象として抽出するができるため、自己位置推定の精度を向上させることが可能となる。したがって、位置推定装置２は、周囲の地物の位置を測定するセンサによって取得された情報に基づく自己位置推定の精度を向上させることが可能となる。

当業者は、本発明の精神及び範囲から外れることなく、様々な変更、置換、及び修正をこれに加えることが可能であることを理解されたい。

１ 位置推定システム
２ 位置推定装置
２１ 記憶部
２２ 入力部
２３ 出力部
２４ 処理部
２４１ 取得部
２４２ 抽出部
２４３ 平面特定部
２４４ 算出部
２４５ 最適化部
２４６ 推定部
３ センサ
４ 移動体

Claims (9)

1. 複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定するコンピュータによる位置推定方法であって、
   前記センサから出力された点群をフレーム毎に取得し、
   前記取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
   前記平面点群により形成される平面の位置をフレーム毎に特定し、
   所定のフレームの平面点群と次のフレームの平面点群との間の、重心位置の関係、法線ベクトルの関係、及び包含率に基づいて、前記所定のフレームの平面点群と前記次のフレームの平面点群とを関連付け、
   前記関連付けられた各平面点群によって形成される各平面の位置の関係からフレーム間の前記移動体の並進ベクトル及び回転行列を算出し、
   前記フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、前記移動体の軌跡を推定する
   ことを含むことを特徴とする位置推定方法。
2. 前記センサは、パルス状に発光するレーザを照射して地物の位置を測定するレーザ照射部を有し、
   前記出力された点群に含まれる各点は、前記センサが搭載された位置を中心に前記レーザ照射部を回転させることにより順次測定された地物のセンサ座標系の位置を示し、
   前記抽出することは、フレーム毎に、
   前記点群に含まれる各点のうちの所定の点と次の点との距離を算出し、前記算出した各距離に基づいて前記点群をセグメントに分類し、
   前記セグメントに分類された点群に含まれる点の数が所定数以上であり且つ前記セグメントに分類された点群によって形成される形状が直線状であるセグメントを特定し、
   複数の前記特定されたセグメントの中から、各前記特定されたセグメントの法線ベクトル及び各前記特定されたセグメントの位置に応じて平面を形成するセグメントを検出し、検出されたセグメントに分類された点群を前記平面点群として抽出することを含む、請求項１に記載の位置推定方法。
3. 前記算出することは、
   前記所定のフレームにおける前記特定された平面の位置と、前記所定のフレームにおける前記特定された平面の位置に最も近い前記次のフレームにおける前記特定された平面の位置とによって、前記移動体の並進ベクトル及び回転行列を算出することを含む、請求項１又は２に記載の位置推定方法。
4. 前記算出することは、
   前記所定のフレームにおける前記特定された平面の法線方向と前記次のフレームにおける前記特定された平面の法線方向とが所定の角度以内である場合、前記所定のフレームにおける前記特定された平面の位置と次のフレームにおける前記特定された平面の位置とによって、前記移動体の前記移動体の並進ベクトル及び回転行列を算出することを含む、請求項１〜３のいずれか一項に記載の位置推定方法。
5. 前記算出することは、前記所定のフレームにおける前記特定された平面の法線ベクトルと前記次のフレームにおける前記特定された平面の法線ベクトルとによって、前記センサの並進ベクトル及び回転行列を算出することを含む、請求項１〜４のいずれか一項に記載の位置推定方法。
6. 前記算出することは、
   前記算出された前記並進ベクトル及び前記回転行列を、前記所定のフレームにおける前記特定された平面と前記次のフレームにおける前記特定された平面とによって最適化し、
   前記最適化された並進ベクトル及び回転行列を、前記所定のフレームと次のフレームにおける前記フレーム間の移動体の並進ベクトル及び回転行列とすることを含む、請求項５に記載の位置推定方法。
7. 前記最適化することは、
   前記所定のフレームにおける平面点群を、前記算出された並進ベクトル及び回転行列によって移動させ、
   移動した所定のフレームにおける平面点群と次のフレームにおける平面点群とに対する、移動した平面点群により形成される平面の法線方向の分散が最小となるように並進ベクトル及び回転行列を最適化することを含む、請求項６に記載の位置推定方法。
8. 複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサが搭載された移動体の軌跡を推定する位置推定装置であって、
   前記センサから出力された点群をフレーム毎に取得する取得部と、
   前記取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出する抽出部と、
   前記平面点群により形成される平面の位置をフレーム毎に特定する特定部と、
   所定のフレームの平面点群と次のフレームの平面点群との間の、重心位置の関係、法線ベクトルの関係、及び包含率に基づいて、前記所定のフレームの平面点群と前記次のフレームの平面点群とを関連付け、前記関連付けられた各平面点群によって形成される各平面の位置の関係からフレーム間の前記移動体の並進ベクトル及び回転行列を算出する算出部と、
   前記フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、前記移動体の軌跡を推定する推定部と
   を備えることを特徴とする位置推定装置。
9. 複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサが搭載された移動体の軌跡を推定する位置推定プログラムであって、
   前記センサから出力された点群をフレーム毎に取得し、
   前記取得した点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
   前記平面点群により形成される平面の位置をフレーム毎に特定し、
   所定のフレームの平面点群と次のフレームの平面点群との間の、重心位置の関係、法線ベクトルの関係、及び包含率に基づいて、前記所定のフレームの平面点群と前記次のフレームの平面点群とを関連付け、
   前記関連付けられた各平面点群によって形成される各平面の位置の関係からフレーム間の前記移動体の並進ベクトル及び回転行列を算出し、
   前記フレーム間の移動体の並進ベクトル及び回転行列をフレーム毎に順次算出して、前記移動体の軌跡を推定する
   ことをコンピュータに実行させることを特徴とする位置推定プログラム。