特許 7092741 自己位置推定方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2022-06-20

(45)【発行日】2022-06-28

(54)【発明の名称】自己位置推定方法

(51)【国際特許分類】

   G05D 1/02 20200101AFI20220621BHJP

【ＦＩ】

G05D1/02 H

【請求項の数】 5

(21)【出願番号】P 2019235104

(22)【出願日】2019-12-25

(65)【公開番号】P2021103482

(43)【公開日】2021-07-15

【審査請求日】2019-12-25

(73)【特許権者】

【識別番号】592246761

【氏名又は名称】財団法人車輌研究測試中心

(74)【代理人】

【識別番号】100108453

【弁理士】

【氏名又は名称】村山 靖彦

(74)【代理人】

【識別番号】100110364

【弁理士】

【氏名又は名称】実広 信哉

(74)【代理人】

【識別番号】100133400

【弁理士】

【氏名又は名称】阿部 達彦

(72)【発明者】

【氏名】王 ▲兪▼芳

(72)【発明者】

【氏名】蔡 宜學

【審査官】大古 健一

(56)【参考文献】

【文献】特開２０１３－２００６０４（ＪＰ，Ａ）

【文献】国際公開第２０１９／０５４２０９（ＷＯ，Ａ１）

【文献】特開２０１８－４３４３（ＪＰ，Ａ）

【文献】特開２０１４－１７４６８３（ＪＰ，Ａ）

【文献】特開２０１６－１５７１９７（ＪＰ，Ａ）

【文献】特開２００９－２７１５１３（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００ － １／１２

(57)【特許請求の範囲】

【請求項1】

目標物体に配置された定位システムによって実行される自己位置推定方法において、
前記目標物体の周囲環境を連続的に検出して、最新の検出時点に関する前記周囲環境の情報及び過去の検出時点に関する前記周囲環境の情報を含む第１の検出情報と、前記周囲環境の情報に関する第２の検出情報と、を得、前記過去の検出時点と前記最新の検出時点との間には、所定の検出期間があるステップ（Ａ）と、
前記第１の検出情報に基づいて、前記周囲環境にある１つ以上の周囲オブジェクトを定義するステップ（Ｂ）と、
前記１つ以上の周囲オブジェクトから１つ以上の追跡オブジェクトを選択するステップ（Ｃ）と、
各前記追跡オブジェクトに対して、該追跡オブジェクトの前記最新の検出時点における情報及び該追跡オブジェクトの前記過去の検出時点における情報を示す前記第１の検出情報に基づいて、該追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類するステップ（Ｄ）と、
前記第２の検出情報から移動オブジェクトに分類された前記追跡オブジェクトに該当する情報が除去された更新後第２の検出情報を得るステップ（Ｅ）と、
前記更新後第２の検出情報に基づいて、自己位置推定及び環境地図作成演算法により前記目標物体の位置を推定するステップ（Ｆ）と、が実行され、
前記定位システムは、前記目標物体の前記周囲環境を連続的に検出して、前記第１の検出情報を得るライダーモジュールを有し、
前記第１の検出情報は、前記最新の検出時点に関する前記周囲環境の最新の点群データと、前記過去の検出時点に関する前記周囲環境の過去の点群データと、を含み、
前記ステップ（Ｂ）において、前記過去の点群データをクラスタリングして、少なくとも１つの第１の点群クラスターが判断されると、該少なくとも１つの第１の点群クラスターを前記周囲オブジェクトとして定義すると共に、前記最新の点群データをクラスタリングして、少なくとも１つの第２の点群クラスターが判断されると、該少なくとも１つの第２の点群クラスターを前記周囲オブジェクトとして定義し、
前記最新の点群データと前記過去の点群データは、それぞれ違う時点で得られた複数の点群データ部分を含み、
前記ステップ（Ａ）と前記ステップ（Ｂ）との間には、
前記最新の点群データ及び前記過去の点群データに対して、各時点における前記目標物体の位置及び角度に基づいて、前記最新の点群データ及び前記過去の点群データにおける最初の時点で得られた最初の点群データ部分を除いた各前記点群データ部分を校正して、過去の校正済み点群データ及び最新の校正済み点群データを得るステップ（Ｇ）が実行され、
前記過去の校正済み点群データは、前記過去の点群データにおける前記最初の点群データ部分と、前記ステップ（Ｇ）に校正された前記過去の点群データにおける複数の前記点群データ部分と、を含み、
前記最新の校正済み点群データは、前記最新の点群データにおける前記最初の点群データ部分と、前記ステップ（Ｇ）に校正された前記最新の点群データにおける複数の前記点群データ部分と、を含み、
前記ステップ（Ｂ）において、前記過去の校正済み点群データで前記過去の点群データを書き換え、且つ、前記最新の校正済み点群データで前記最新の点群データを書き換えてから、前記点群データをクラスタリングする
ことを特徴とする自己位置推定方法。

【請求項2】

前記第２の検出情報は、前記最新の点群データと同じものであり、
前記ステップ（Ｅ）において、前記第２の検出情報から、移動オブジェクトに分類された前記追跡オブジェクトに対応する前記第２の点群クラスターを除去する
ことを特徴とする請求項１に記載の自己位置推定方法。

【請求項3】

前記ステップ（Ｄ）は、
各前記追跡オブジェクトに対して、該追跡オブジェクトに係る前記第１の点群クラスターに基づいて、該追跡オブジェクトの前記過去の検出時点に関する過去位置を得るサブステップ（Ｄ１）と、
各前記追跡オブジェクトに対して、該追跡オブジェクトに係る前記第２の点群クラスターに基づいて、該追跡オブジェクトの前記最新の検出時点に関する最新位置を得るサブステップ（Ｄ２）と、
各前記追跡オブジェクトに対して、前記サブステップ（Ｄ２）で得られた該追跡オブジェクトの前記最新位置と、前記目標物体の前記最新の検出時点における位置及び角度と、前記目標物体の前記過去の検出時点における位置及び角度と、に基づいて、該追跡オブジェクトの前記過去の検出時点の仮定過去位置を推定するサブステップ（Ｄ３）と、
各前記追跡オブジェクトに対して、該追跡オブジェクトの前記過去位置と前記仮定過去位置との距離が所定の距離閾値より大きい場合には、該追跡オブジェクトを移動オブジェクトに分類し、該追跡オブジェクトの前記過去位置と前記仮定過去位置との距離が前記距離閾値以下の場合には、該追跡オブジェクトを静止オブジェクトに分類するサブステップ（Ｄ４）と、を含む
ことを特徴とする請求項１又は請求項２に記載の自己位置推定方法。

【請求項4】

前記定位システムは、前記目標物体の前記周囲環境を撮影して、前記第２の検出情報を得る画像撮影モジュールを有し、
前記ステップ（Ｄ）と前記ステップ（Ｅ）との間には、
移動オブジェクトに分類された各前記追跡オブジェクトに対して、該追跡オブジェクトが対応する前記第２の点群クラスターに基づいて、該追跡オブジェクトの第１の代表座標を得るステップ（Ｈ）と、
移動オブジェクトに分類された各前記追跡オブジェクトに対して、前記第１の検出情報に係る点群座標系から前記第２の検出情報に係るピクセル座標系に変換するための座標変換パラメーターセットに基づいて、該追跡オブジェクトの前記第１の代表座標を前記第２の検出情報に係る前記ピクセル座標系に対応する第２の代表座標に変換するステップ（Ｉ）と、が実行され、
前記ステップ（Ｅ）において、移動オブジェクトに分類された各前記追跡オブジェクトの前記第２の代表座標に基づいて、前記第２の検出情報から、移動オブジェクトに分類された各前記追跡オブジェクトの情報を除去する
ことを特徴とする請求項１に記載の自己位置推定方法。

【請求項5】

前記ステップ（Ｃ）において、前記追跡オブジェクトは、サイズが所定のサイズ範囲内にある前記周囲オブジェクトから選択される
ことを特徴とする請求項１に記載の自己位置推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定及び環境地図作成の技術に関し、具体的には、物体追跡に基づく自己位置推定及び環境地図作成の技術に関する。

【背景技術】

【0002】

従来の定位技術には、ＧＰＳ（汎地球測位システムＧｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）がよく使用されている。しかし、障害物が多い又は天気が悪い場合には、ＧＰＳの定位精度が落ちる。そこで、ＧＰＳの代わりに、周囲の環境から自己位置を推定するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術が、例えばロボット掃除機や自動運転車に利用されている。

【0003】

従来のＳＬＡＭは、センサー（例えば、ライダー（LiDAR）、レーダー、カメラ、深度カメラなど）が搭載された定位物体が移動すると共に、その移動時間において区切られた複数の所定のユニット時間ごとに、該センサーによって周囲の環境を数回検出することにより、各ユニット時間ごとに得られた検出結果である、周囲の特徴（例えば、周囲環境にある物体）が記録された複数の環境データを得る上、これを用いて環境地図を作成すると共に、該環境データにおける周囲の特徴を元に自己位置を推定する技術である。特許文献１には、ＳＬＡＭを使用して動作範囲を決定する動作範囲決定装置が提案されている。

【先行技術文献】

【特許文献】

【0004】

【文献】中国特許出願公開第１０８６８８６６０号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかしながら、従来のＳＬＡＭを用いて、環境地図を作成するときには、検出した環境データに移動している移動物体のデータが含まれている場合、移動物体の不確定性により環境地図を正常に作成できず、定位物体の自己位置を正確に推定することが困難になる。

【0006】

したがって、本発明は、従来より、定位精度が高い自己位置推定方法を提供することを目的とする。

【課題を解決するための手段】

【0007】

前記目的を達成するための手段として、本発明は、以下の自己位置推定方法を提供する。

【0008】

本発明の自己位置推定方法は、
目標物体に配置された定位システムによって実行される自己位置推定方法において、前記目標物体の周囲環境を連続的に検出して、最新の検出時点に関する前記周囲環境の情報及び過去の検出時点に関する前記周囲環境の情報を含む第１の検出情報と、前記周囲環境の情報に関する第２の検出情報と、を得、前記過去の検出時点と前記最新の検出時点との間には、所定の検出期間があるステップ（Ａ）と、前記第１の検出情報に基づいて、前記周囲環境にある１つ以上の周囲オブジェクトを定義するステップ（Ｂ）と、前記１つ以上の周囲オブジェクトから、移動オブジェクトである可能性のある１つ以上の追跡オブジェクトを選択するステップ（Ｃ）と、各前記追跡オブジェクトに対して、該追跡オブジェクトの前記最新の検出時点における情報及び該追跡オブジェクトの前記過去の検出時点における情報を示す前記第１の検出情報に基づいて、該追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類するステップ（Ｄ）と、前記第２の検出情報から移動オブジェクトに分類された前記追跡オブジェクトに該当する情報が除去された更新後第２の検出情報を得るステップ（Ｅ）と、前記更新後第２の検出情報に基づいて、自己位置推定及び環境地図作成演算法により前記目標物体の位置を推定するステップ（Ｆ）と、が実行される。

【発明の効果】

【0009】

上記構成によれば、本発明は、各追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類して、第２の検出情報を、第２の検出情報から移動オブジェクトに分類された追跡オブジェクトの情報を除外するよう更新する。そして、更新後第２の検出情報に基づいて自己位置推定及び環境地図作成の演算法により目標物体の位置を推定する。このように、移動オブジェクトに分類された追跡オブジェクトの情報が除外されるので、環境地図を作成し、自己位置を推定するときには、移動オブジェクトの不確定性に影響されない。従って、従来より高い定位精度を得ることができる。

【図面の簡単な説明】

【0010】

【図1】本発明の自己位置推定方法に係る第１の実施形態を実施するための定位システムを示すブロック図である。

【図2】本発明の自己位置推定方法に係る第１の実施形態を示すフローチャートである。

【図3】物体分類ユニットが違う時点で得られた複数の点群データ部分を校正することを示すフローチャートである。

【図4】物体分類ユニットが追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類することを示すフローチャートである。

【図5】本発明の自己位置推定方法に係る第２の実施形態を実施するための定位システムを示すブロック図である。

【図6】本発明の自己位置推定方法に係る第２の実施形態を示すフローチャートである。

【発明を実施するための形態】

【0011】

以下、本発明に係る自己位置推定方法の第１の実施形態について、図面を参照して説明する。

【0012】

（第１の実施形態）
図１は本発明の自己位置推定方法に係る第１の実施形態を実施するための定位システムを示すブロック図である。本発明の自己位置推定方法に係る第１の実施形態は、目標物体の位置を推定することに適用し、目標物体に配置された定位システム１によって実行される。図１に示されているように、該定位システム１は、第１のセンサーユニット１１と、自己位置推定及び環境地図作成ユニット１２と、第１のセンサーユニット１１と自己位置推定及び環境地図作成ユニット１２とに電気的接続されている物体分類ユニット１３と、を具える。また、本実施形態において、目標物体は、自動運転車であるが、本発明としては、これに限定されない。

【0013】

第１のセンサーユニット１１は、目標物体の周囲環境を連続的に検出して、最新の検出時点に関する周囲環境の情報及び過去の検出時点に関する周囲環境の情報を含む第１の検出情報と、周囲環境の情報に関する第２の検出情報と、を得るように構成されている。過去の検出時点と最新の検出時点との間には、所定の検出期間がある。

【0014】

また、第１のセンサーユニット１１としては、例えば、ライダー、レーダー、カメラなどを適用することができる。本実施形態において、第１のセンサーユニット１１は、ライダーモジュールを有し、従って、本実施形態における第１の検出情報は、最新の検出時点に関する周囲環境の最新の点群データと、過去の検出時点に関する周囲環境の過去の点群データと、を含む。最新の点群データと過去の点群データは、それぞれ違う時点で得られた複数の点群データ部分を含む。また、各時点において、任意の隣接する２つの時点の間には、所定の期間がある。

【0015】

本実施形態において、第２の検出情報は、最新の点群データと同じものであり、第１の検出情報は、物体分類ユニット１３に提供され、第２の検出情報は、自己位置推定及び環境地図作成ユニット１２に提供される。

【0016】

自己位置推定及び環境地図作成ユニット１２は、第２の検出情報に基づいて、従来の自己位置推定及び環境地図作成演算法により、目標物体の周囲環境に関する地図を作成して、目標物体の位置を推定するように構成されている。

【0017】

物体分類ユニット１３は、第１の検出情報に基づいて、周囲環境にある１つ以上の周囲オブジェクトを定義して、１つ以上の周囲オブジェクトから、１つ以上の追跡オブジェクトを選択して、該追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類するように構成されている。

【0018】

図２は本発明の自己位置推定方法に係る第１の実施形態を示すフローチャートである。図１と図２に示されているように、本発明の自己位置推定方法に係る第１の実施形態は、ステップ２１～ステップ２８が実行される。

【0019】

ステップ２１において、第１のセンサーユニット１１は、目標物体の周囲環境を連続的に検出して、最新の検出時点に関する周囲環境の情報及び過去の検出時点に関する周囲環境の情報を含む第１の検出情報と、周囲環境の情報に関する第２の検出情報と、を得て、第１の検出情報を物体分類ユニット１３に提供し、第２の検出情報を自己位置推定及び環境地図作成ユニット１２に提供する。本実施形態において、第１のセンサーユニット１１は、ライダーモジュールを有し、第１の検出情報は、最新の検出時点に関する周囲環境の最新の点群データと、過去の検出時点に関する周囲環境の過去の点群データと、を含み、第２の検出情報は、最新の点群データと同じものであり、最新の点群データと過去の点群データは、それぞれ違う時点で得られた複数の点群データ部分を含む。

【0020】

ステップ２２において、物体分類ユニット１３は、最新の点群データ及び過去の点群データに対して、各時点における目標物体の位置及び角度（つまり、目標物体が向かっている方向）に基づいて、最新の点群データ及び過去の点群データにおける最初の時点で得られた最初の点群データ部分を除いた各点群データ部分を校正して、過去の校正済み点群データ及び最新の校正済み点群データを得る。

【0021】

過去の校正済み点群データは、過去の点群データにおける最初の点群データ部分と、校正された過去の点群データにおける複数の点群データ部分と、を含み、最新の校正済み点群データは、最新の点群データにおける最初の点群データ部分と、校正された最新の点群データにおける複数の点群データ部分と、を含む。

【0022】

具体的には、例えば、最新の点群データと過去の点群データは、それぞれ最初の時点から第Ｎの時点で得られた最初の点群データ部分から第Ｎの点群データ部分を含む。ステップ２２において、物体分類ユニット１３は、第２の時点で得られた第２の点群データ部分から第Ｎの時点で得られた第Ｎの点群データ部分を校正する。過去の校正済み点群データは、過去の点群データにおける最初の点群データ部分と、校正された過去の点群データにおける第２の点群データ部分から第Ｎの点群データ部分と、を含む。最新の校正済み点群データは、最新の点群データにおける最初の点群データ部分と、校正された最新の点群データにおける第２の点群データ部分から第Ｎの点群データ部分と、を含む。

【0023】

本実施形態において、最新の点群データにおける最初の時点とは、最新の点群データに対応する検出時点であり、また、過去の点群データにおける最初の時点とは、過去の点群データに対応する検出時点であるため、それぞれの最初の時点で得られた最初の点群データ部分を校正する必要はない。

【0024】

また、目標物体が自動運転車であれば、各時点における該目標物体の位置及び角度は、自動運転車から得られた速度信号及び回転角度（舵角）信号に基づいて、従来のバイシクルモデルにより得ることができる。

【0025】

図３は物体分類ユニットが違う時点で得られた複数の点群データ部分を校正することを示すフローチャートである。図１と図３に示されているように、ステップ２２は、サブステップ２２１とサブステップ２２２とが実行される。

【0026】

サブステップ２２１において、物体分類ユニット１３は、最新の点群データ及び過去の点群データに対して、最初の時点から第Ｎの時点における目標物体の位置及び角度に基づいて、第２の時点から第Ｎの時点をそれぞれ校正するための（Ｎ－１）個の第１の平行移動校正行列と（Ｎ－１）個の第１の回転校正行列とを得る。

【0027】

詳しくは、本実施形態において、物体分類ユニット１３は、第ｒの時点における目標物体の位置及び第（ｒ－１）の時点における目標物体の位置に基づいて、第（ｒ－１）の時点から第ｒの時点までの目標物体の変位（即ち、目標物体に配置されている第１のセンサーユニット１１の変位）を計算して、第ｒの時点に対応する第１の平行移動校正行列を得る。例えば、第３の時点に対応する第１の平行移動校正行列は、第３の時点における目標物体の位置及び第２の時点における目標物体の位置に基づいて、第２の時点から第３の時点までの目標物体の変位を計算して得るものである。

【0028】

更に、物体分類ユニット１３は、第ｒの時点における目標物体の角度及び第（ｒ－１）の時点における目標物体の角度に基づいて、第（ｒ－１）の時点から第ｒの時点までの目標物体の回転角度（即ち、目標物体に配置されている第１のセンサーユニット１１の回転角度）を計算して、第ｒの時点に対応する第１の回転校正行列を得る。例えば、第３の時点に対応する第１の回転校正行列は、第３の時点における目標物体の角度及び第２の時点における目標物体の角度に基づいて、第２の時点から第３の時点までの目標物体の回転角度を計算して得るものである。ここで、ｒは２以上Ｎ以下の自然数である。

【0029】

これにより、物体分類ユニット１３は、第２の時点から第Ｎの時点にそれぞれ対応する（Ｎ－１）個の第１の平行移動校正行列と（Ｎ－１）個の第１の回転校正行列とを得る。

【0030】

サブステップ２２２において、物体分類ユニット１３は、最新の点群データ及び過去の点群データに対して、サブステップ２２１で得られた、第２の時点から第Ｎの時点にそれぞれ対応する（Ｎ－１）個の第１の平行移動校正行列及び（Ｎ－１）個の第１の回転校正行列に基づいて、第２の点群データ部分から第Ｎの点群データ部分を校正して、過去の校正済み点群データ及び最新の校正済み点群データを得る。

【0031】

詳しくは、本実施形態において、物体分類ユニット１３は、第２の時点から第ｒの時点に対応する（ｒ－１）個の第１の平行移動校正行列及び（ｒ－１）個の第１の回転校正行列により、第ｒの時点で得られた第ｒの点群データ部分を校正する。例えば、第３の点群データ部分を校正するために、第２の時点に対応する第１の平行移動校正行列及び第１の回転校正行列と、第３の時点に対応する第１の平行移動校正行列及び第１の回転校正行列と、が用いられる。これで、第２の点群データ部分から第Ｎの点群データ部分を校正する。校正された第２の点群データ部分から第Ｎの点群データ部分は、最初の点群データ部分と共に、校正済み点群データを構成する。

【0032】

目標物体が移動すると、検出時点における隣接する２つの時点における目標物体の変位により、該２つの時点で得られた点群データ部分がずれてしまう。そのため、ステップ２２が実行され、上記の点群データ部分のずれを解決する。また、目標物体の速度が所定の速度閾値より小さい場合には、隣接する２つの時点における目標物体の変位が小さくなり、点群データ部分のずれを無視することができる。従って、目標物体の速度が前記速度閾値より小さい場合、ステップ２２を省略してもよい。

【0033】

図２に示されているように、ステップ２３において、物体分類ユニット１３は、第１の検出情報に基づいて、周囲環境にある１つ以上の周囲オブジェクトを定義する。具体的に、物体分類ユニット１３は、過去の点群データをクラスタリングして、少なくとも１つの第１の点群クラスターが判断されると、該少なくとも１つの第１の点群クラスターを周囲オブジェクトとして定義すると共に、最新の点群データをクラスタリングして、少なくとも１つの第２の点群クラスターが判断されると、該少なくとも１つの第２の点群クラスターを周囲オブジェクトとして定義する。更に、ステップ２２が実行された場合、過去の校正済み点群データで過去の点群データを書き換え、且つ、最新の校正済み点群データで最新の点群データを書き換えてから、点群データをクラスタリングする。

【0034】

ステップ２４において、物体分類ユニット１３は、１つ以上の周囲オブジェクトから１つ以上の追跡オブジェクトを選択する。なお、本実施形態において、追跡オブジェクトは、サイズが所定のサイズ範囲にある周囲オブジェクトから選択される。また、ステップ２３において定義された周囲オブジェクトが１つのみである場合、ステップ２４においては当該１つの周囲オブジェクトを追跡オブジェクトとして選択する。

【0035】

ステップ２５において、物体分類ユニット１３は、各追跡オブジェクトに対して、該追跡オブジェクトの最新の検出時点における情報及び該追跡オブジェクトの過去の検出時点における情報を示す第１の検出情報に基づいて、該追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類する。

【0036】

図４は物体分類ユニットが追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類することを示すフローチャートである。図１と図４に示されているように、ステップ２５は、サブステップ２５１～サブステップ２５４が実行される。

【0037】

サブステップ２５１において、物体分類ユニット１３は、各追跡オブジェクトに対して、該追跡オブジェクトに係る第１の点群クラスターに基づいて、該追跡オブジェクトの過去の検出時点に関する過去位置を得る。

【0038】

サブステップ２５２において、物体分類ユニット１３は、各追跡オブジェクトに対して、該追跡オブジェクトに係る第２の点群クラスターに基づいて、該追跡オブジェクトの最新の検出時点に関する最新位置を得る。

【0039】

サブステップ２５３において、物体分類ユニット１３は、各追跡オブジェクトに対して、サブステップ２５２で得られた該追跡オブジェクトの最新位置と、目標物体の最新の検出時点における位置及び角度と、目標物体の過去の検出時点における位置及び角度と、に基づいて、該追跡オブジェクトの過去の検出時点の仮定過去位置を推定する。また、該目標物体の最新の検出時点における位置及び角度と、目標物体の過去の検出時点における位置及び角度は、上述したように、従来のバイシクルモデルにより得ることができる。

【0040】

具体的に、物体分類ユニット１３は、最新の検出時点における目標物体の位置及び過去の検出時点における目標物体の位置に基づいて、過去の検出時点から最新の検出時点までの目標物体の変位を計算して、最新の検出時点に対応する第２の平行移動校正行列を得る。更に、物体分類ユニット１３は、最新の検出時点における目標物体の角度及び過去の検出時点における目標物体の角度に基づいて、過去の検出時点から最新の検出時点までの目標物体の回転角度を計算して、最新の検出時点に対応する第２の回転校正行列を得る。そして、物体分類ユニット１３は、各追跡オブジェクトに対して、該追跡オブジェクトの最新位置と、第２の平行移動校正行列と、第２の回転校正行列とに基づいて、該追跡オブジェクトの過去の検出時点の仮定過去位置を推定する。

【0041】

サブステップ２５４において、物体分類ユニット１３は、各追跡オブジェクトに対して、該追跡オブジェクトの過去位置及び仮定過去位置に基づいて、該追跡オブジェクトを移動オブジェクト又は静止オブジェクトに分類する。具体的に、物体分類ユニット１３は、各追跡オブジェクトに対して、該追跡オブジェクトの過去位置と仮定過去位置との距離が所定の距離閾値より大きい場合には、該追跡オブジェクトを移動オブジェクトに分類し、該追跡オブジェクトの過去位置と仮定過去位置との距離が前記距離閾値以下の場合には、該追跡オブジェクトを静止オブジェクトに分類する。

【0042】

図１と図２に示されているように、ステップ２６において、物体分類ユニット１３は、ステップ２５で得られた分類結果を自己位置推定及び環境地図作成ユニット１２に送信する。該分類結果には、移動オブジェクトに分類された追跡オブジェクトに該当する情報が示されている。また、該分類結果に示されている追跡オブジェクトに該当する情報は、該追跡オブジェクトに対応する第２の点群クラスターであってもよいが、該追跡オブジェクトに対応する第２の点群クラスターに基づいて得られた座標（例えば、対応する第２の点群クラスターの中心座標）であってもよい。

【0043】

ステップ２７において、自己位置推定及び環境地図作成ユニット１２は、第２の検出情報から移動オブジェクトに分類された追跡オブジェクトに該当する情報を除去して、第２の検出情報から移動オブジェクトに分類された追跡オブジェクトに該当する情報が除去された更新後第２の検出情報を得る。

【0044】

ステップ２８において、自己位置推定及び環境地図作成ユニット１２は、更新後第２の検出情報に基づいて、自己位置推定及び環境地図作成演算法により目標物体の位置を推定する。

【0045】

更新後第２の検出情報には、移動オブジェクトに分類された追跡オブジェクトの情報が含まれていないので、自己位置推定及び環境地図作成ユニット１２が環境地図を作成し、自己位置を推定するときには、移動オブジェクトの不確定性による影響がなくなるため、本実施形態は、従来より、定位精度が高い。

【0046】

（第２の実施形態）
図５は本発明の自己位置推定方法に係る第２の実施形態を実施するための定位システムを示すブロック図である。図５に示されているように、本発明の第２の実施形態を実行する定位システム１は、自己位置推定及び環境地図作成ユニット１２に電気的に接続される第２のセンサーユニット１４を更に具える点を除いて、第１の実施形態を実行する定位システム１と同じである。また、第２のセンサーユニット１４は、目標物体の周囲環境を検出して、第１のセンサーユニット１１の代わりに第２の検出情報を得て、該第２の検出情報を自己位置推定及び環境地図作成ユニット１２に提供するように構成されている。

【0047】

本実施形態において、第２のセンサーユニット１４は、深度カメラのような画像撮影モジュールを有する。該画像撮影モジュールは、目標物体の周囲環境を撮影し、撮影された画像を第２の検出情報として提供する。また、第２のセンサーユニット１４は、第１のセンサーユニット１１と同期している。つまり、第１のセンサーユニット１１が周囲環境を検出すると同時に第２のセンサーユニット１４が目標物体の周囲環境を撮影する。

【0048】

図６は本発明の自己位置推定方法に係る第２の実施形態を示すフローチャートである。図５と図６に示されているように、本発明の自己位置推定方法に係る第２の実施形態には、ステップ３０～ステップ３９が実行される。

【0049】

本実施形態におけるステップ３１～ステップ３４は、第１の実施形態におけるステップ２２～ステップ２５と同じであるので、ここでは省略する。第１の実施形態に比して、本実施形態において、第２のセンサーユニット１４により得られた第２の検出情報は、第１のセンサーユニット１１により得られた第１の検出情報と異なるので、ステップ３４で得られた分類結果における移動オブジェクトに分類された追跡オブジェクトの情報を転換する必要がある。したがって、ステップ３５とステップ３６が実行される。

【0050】

ステップ３０において、第１のセンサーユニット１１は、目標物体の周囲環境を連続的に検出して、最新の検出時点に関する周囲環境の情報及び過去の検出時点に関する周囲環境の情報を含む第１の検出情報を得て、物体分類ユニット１３に提供する。同時に、第２のセンサーユニット１４は、周囲環境の情報に関する第２の検出情報を得て、自己位置推定及び環境地図作成ユニット１２に提供する。

【0051】

本実施形態において、第１のセンサーユニット１１は、ライダーモジュールを有し、第１の検出情報は、最新の検出時点に関する周囲環境の最新の点群データと、過去の検出時点に関する周囲環境の過去の点群データと、を含み、最新の点群データと過去の点群データは、それぞれ違う時点で得られた複数の点群データ部分を含む。また、第２のセンサーユニットは、画像撮影モジュールを有し、第２の検出情報は、最新の検出時点に関する周囲環境の画像である。

【0052】

ステップ３５において、物体分類ユニット１３は、移動オブジェクトに分類された各追跡オブジェクトに対して、該追跡オブジェクトが対応する第２の点群クラスターに基づいて、該追跡オブジェクトの第１の代表座標（例えば、対応する第２の点群クラスターの中心座標）を得る。

【0053】

ステップ３６において、物体分類ユニット１３は、移動オブジェクトに分類された各追跡オブジェクトに対して、第１の検出情報に係る点群座標系から第２の検出情報に係るピクセル座標系に変換するための座標変換パラメーターセットに基づいて、該追跡オブジェクトの第１の代表座標を第２の検出情報に係るピクセル座標系に対応する第２の代表座標に変換する。

【0054】

また、該座標変換パラメーターセットは、内部パラメーター行列と外部パラメーター行列とを含む。該内部パラメーター行列は、第２のセンサーユニット１４の焦点距離と、第２のセンサーユニット１４によって得られた画像の中心座標と、により得られる。該外部パラメーター行列は、以下の式１により画成される。

【0055】

【数1】

【0056】

上記式１において、

【数2】

は、第２のセンサーユニット１４の位置を示し、

【数3】

は、第１のセンサーユニット１１の位置を示し、

【数4】

は、三次元回転行列を示し、

【数5】

は、平行移動行列を示す。そして、三次元回転行列と平行移動行列は、共に外部パラメーター行列を画成する。

【0057】

ステップ３７において、物体分類ユニット１３は、ステップ３４で得られた分類結果を自己位置推定及び環境地図作成ユニット１２に送信する。また、該分類結果に示されている追跡オブジェクトに該当する情報は、ステップ３６で得られた該追跡オブジェクトに対応する第２の代表座標である。

【0058】

ステップ３８において、自己位置推定及び環境地図作成ユニット１２は、第２の検出情報から第２の代表座標が対応する追跡オブジェクトが除去された更新後第２の検出情報を得る。

【0059】

ステップ３９において、自己位置推定及び環境地図作成ユニット１２は、更新後第２の検出情報に基づいて、自己位置推定及び環境地図作成演算法により目標物体の位置を推定する。

【0060】

上述した２つの実施形態によれば、本発明に係る自己位置推定方法は、更新後第２の検出情報に基づいて、自己位置推定及び環境地図作成演算法により目標物体の位置を推定する。そして、更新後第２の検出情報には、移動オブジェクトに分類された追跡オブジェクトの情報が含まれていないので、自己位置推定及び環境地図作成ユニット１２が環境地図を作成し、自己位置を推定するときには、移動オブジェクトの不確定性による影響がなくなるため、本発明は、従来より、定位精度が高い。

【0061】

以上、本発明の好ましい実施形態及び変化例を説明したが、本発明はこれらに限定されるものではなく、最も広い解釈の精神および範囲内に含まれる様々な構成として、全ての修飾および均等な構成を包含するものとする。

【産業上の利用可能性】

【0062】

本発明に係る自己位置推定方法は、地図を作成するときに移動物体の不確定性による影響を排除できるので、従来のＳＬＡＭより定位精度が高い。そのため、本発明は、ＳＬＡＭ機能を搭載した様々な物体（例えば、自動運転車、ロボット掃除機など）に対する様々な応用が可能である。

【符号の説明】

【0063】

1 定位システム
11 第１のセンサーユニット
12 自己位置推定及び環境地図作成ユニット
13 物体分類ユニット
14 第２のセンサーユニット

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】