特許 7606681 融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体

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

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

(11)【特許番号】

(24)【登録日】2024-12-18

(45)【発行日】2024-12-26

(54)【発明の名称】融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体

(51)【国際特許分類】

   G05D 1/43 20240101AFI20241219BHJP

【ＦＩ】

G05D1/43

【請求項の数】 10

(21)【出願番号】P 2023521741

(86)(22)【出願日】2021-12-02

(65)【公表番号】

(43)【公表日】2023-10-27

(86)【国際出願番号】 CN2021135015

(87)【国際公開番号】W WO2022142992

(87)【国際公開日】2022-07-07

【審査請求日】2023-04-10

(31)【優先権主張番号】202011586133.3

(32)【優先日】2020-12-29

(33)【優先権主張国・地域又は機関】CN

(73)【特許権者】

【識別番号】522070592

【氏名又は名称】深▲せん▼市普渡科技有限公司

【氏名又は名称原語表記】Ｓｈｅｎｚｈｅｎ Ｐｕｄｕ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．， Ｌｔｄ．

【住所又は居所原語表記】Ｒｏｏｍ ５０１， Ｂｕｉｌｄｉｎｇ Ａ， Ｂｌｏｃｋ １， Ｐｈａｓｅ １， Ｓｈｅｎｚｈｅｎ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｉｎｎｏ Ｖａｌｌｅｙ， Ｄａｓｈｉ １ｓｔ Ｒｏａｄ， Ｘｉｌｉ Ｃｏｍｍｕｎｉｔｙ， Ｘｉｌｉ Ｓｔｒｅｅｔ， Ｎａｎｓｈａｎ Ｄｉｓｔｒｉｃｔ， Ｓｈｅｎｚｈｅｎ， Ｃｈｉｎａ

(74)【代理人】

【識別番号】110002871

【氏名又は名称】弁理士法人坂本国際特許商標事務所

(72)【発明者】

【氏名】周陽

(72)【発明者】

【氏名】張濤

(72)【発明者】

【氏名】陳美文

(72)【発明者】

【氏名】劉運航

(72)【発明者】

【氏名】何科君

【審査官】神山 貴行

(56)【参考文献】

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

【文献】中国特許出願公開第１０６９０８７７５（ＣＮ，Ａ）

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

Ｇ０５Ｄ １／００－ １／８７

Ｇ０１Ｓ ７／４８－ ７／５１

Ｇ０１Ｓ １７／００－１７／９５

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

【請求項1】

融合測位方法であって、
ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得することと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得することと、
前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得することと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、前記ロボットの位置姿勢を反復最適化して、前記道路標識Ｌ_ｉと前記道路標識Ｌ_jの前記道路標識地図上の位置差値である誤差Ｅ_１ が最小となるように前記ロボットの位置を推定し、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が最小となるように前記ロボットの姿勢を推定することと、を含む
ことを特徴とする前記融合測位方法。

【請求項2】

前記ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のレーダ点群関連データを取得することは、
前記２次元レーザレーダにより前記環境物をサンプリングし、前記環境物の原初レーダ点群データを得ることと、
前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることと、
前記環境物のうちの道路標識Ｌ_ｉの歪み除去点群データをクラスタリングして、前記道路標識Ｌ_ｉの点群クラスタを得ることと、
前記点群クラスタのロボット本体座標系での幾何学的中心座標を求め、前記点群クラスタの前記ロボット本体座標系での幾何学的中心座標を前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉとすることと、を含む
ことを特徴とする請求項１に記載の融合測位方法。

【請求項3】

前記原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることは、
１走査周期後の、前記２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定することと、
相対的位置姿勢△Ｔと、前記原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを前記原初レーダ点群データのうちの各点の座標とすることであって、前記相対的位置姿勢△Ｔは、前記原初レーダ点群データのうちの各点の、前記新たな原点に対して得られたロボット位置姿勢データであることと、を含む
ことを特徴とする請求項２に記載の融合測位方法。

【請求項4】

前記環境物の原初レーダ点群データに対して歪み除去を行って、前記環境物の歪み除去点群データを得ることの後に、
前記融合測位方法は、
前記環境物の歪み除去点群データをのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較することと、
前記環境物の歪み除去点群データのうちのレーザ光反射強度が前記予め設定された光強度閾値以上である点群データを、前記環境物のうちの道路標識Ｌ_ｉの歪み除去点群データとして保持することと、をさらに含む
ことを特徴とする請求項２に記載の融合測位方法。

【請求項5】

前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得することは、
前記ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定することと、
前記複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、前記環境物の歪み除去点群データを前記２次元グリッド地図に投影し、前記各々の位置姿勢候補の前記２次元グリッド地図上のマッチングスコアを算出することと、
前記複数の位置姿勢候補のうち、前記２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢として確定することと、を含む
ことを特徴とする請求項１に記載の融合測位方法。

【請求項6】

前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、前記ロボットの位置姿勢を反復最適化して、誤差Ｅ_１ が最小となるように前記ロボットの位置を推定し、誤差Ｅ_２が最小となるように前記ロボットの姿勢を推定することは、
前記ロボットの位置姿勢を状態として非線形最小二乘法問題を構築することと、
前記誤差Ｅ_１を前記非線形最小二乘法問題の誤差制約条件として、前記ロボットの位置姿勢を反復最適化して、前記誤差Ｅ_１ が最小となるように前記ロボットの位置を推定し、前記誤差Ｅ_２が最小となるように前記ロボットの姿勢を推定することと、を含む
ことを特徴とする請求項１に記載の融合測位方法。

【請求項7】

前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、前記ロボットの位置姿勢を反復最適化して、誤差Ｅ_１ が最小となるように前記ロボットの位置を推定し、誤差Ｅ_２が最小となるように前記ロボットの姿勢を推定することは、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築することと、
前記粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、前記ロボットの推定位置姿勢を補正し続けて、誤差Ｅ_１ が最小となるように前記ロボットの位置を補正し、誤差Ｅ_２が最小となるように前記ロボットの姿勢を補正することと、を含む
ことを特徴とする請求項１に記載の融合測位方法。

【請求項8】

融合測位装置であって、
ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得するための第１の取得モジュールと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得するための第２の取得モジュールと、
前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得するための第３の取得モジュールと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、前記ロボットの位置姿勢を反復最適化して、前記道路標識Ｌ_ｉと前記道路標識Ｌ_jの前記道路標識地図上の位置差値である誤差Ｅ_１ が最小となるように前記ロボットの位置を推定し、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が最小となるように前記ロボットの姿勢を推定するための反復最適化モジュールと、を備える
ことを特徴とする前記融合測位装置。

【請求項9】

デバイスであって、
メモリと、プロセッサと、前記メモリに記憶され、前記プロセッサ上で実行可能なコンピュータープログラムと、を備え、
前記プロセッサが前記コンピュータープログラムを実行することにより、請求項１に記載の方法のステップが実現される
ことを特徴とする前記デバイス。

【請求項10】

コンピュータープログラムが記憶されるコンピューター可読記憶媒体であって、
前記コンピュータープログラムがプロセッサによって実行されることにより、請求項１に記載の方法のステップが実現される
ことを特徴とするコンピューター可読記憶媒体。

【発明の詳細な説明】

【技術分野】

【0001】

［関連出願の参照］
本出願は、２０２０年１２月２９日に中国特許庁に提出された、出願番号が２０２０１１５８６１３３３であり、出願名称が「融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体」である中国特許出願の優先権を主張し、その全内容は参照により本出願に組み込まれる。
本発明は、ロボット分野に関し、特に、融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体に関する。

【背景技術】

【0002】

移動ロボットは、様々な場面に応用されており、正確な測位はロボットがスムーズに任務を完了するための鍵である。一般に、ロボットの測位は、３次元または２次元のレーザレーダにより実現され、そのうち、２次元レーザレーダの測位方法では、主に、レーザレーダの点群データと予め構築された２次元グリッド地図とをマッチングさせてロボットの位置姿勢の推定を実現する。しかしながら、このような測位方法は、実際の応用場面において、長い廊下や、広大な場所など、ある自由度が２次元レーザレーダの観測範囲を超えてしまう場合があるため、いくつかの問題があった。このような場合に、２次元レーザレーダでは、ロボットの２次元空間における位置姿勢の３つの自由度を完全に拘束することができず、ロボットの位置姿勢推定に誤りが生じる。

【0003】

ロボットが長い廊下や広大な環境にあってロボットを測位する際に遭遇する上記の問題に対して、１つの方策として、高光反射率の道路標識（Ｌａｎｄｍａｒｋ）を人手で作成して補助的な測位を行い、たとえば、光反射板または光反射柱を用いて補助的な測位を行う。しかしながら、光反射柱または光反射板などの高光反射率の道路標識を用いて補助的な測位を行うだけでは、環境改修に対する要求の高い環境に適合することはできない。

【発明の概要】

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

【0004】

本発明の様々な実施例は、融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体を提供する。

【0005】

融合測位方法であって、
ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得することと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得することと、
前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得することと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、前記道路標識Ｌ_ｉと前記道路標識Ｌ_jの前記道路標識地図上の位置差値である誤差Ｅ_１、および、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が、最小となるまで、前記ロボットの位置姿勢を反復最適化することと、を含む。

【0006】

融合測位装置であって、
ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、前記環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと前記環境物の歪み除去点群データとを含む、前記環境物のレーダ点群関連データを、取得するための第１の取得モジュールと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、前記道路標識地図上の前記位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得するための第２の取得モジュールと、
前記環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、前記ロボットの前記２次元グリッド地図でのグローバル位置姿勢を、前記ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得するための第３の取得モジュールと、
前記道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、前記道路標識Ｌ_jの前記道路標識地図上の位置Ｐ_j、前記ロボットの推定位置姿勢、および前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、前記道路標識Ｌ_ｉと前記道路標識Ｌ_jの前記道路標識地図上の位置差値である誤差Ｅ_１、および、前記ロボットの推定位置姿勢と前記グローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が、最小となるまで、前記ロボットの位置姿勢を反復最適化するための反復最適化モジュールと、を備える。

【0007】

デバイスであって、メモリと、プロセッサと、前記メモリに記憶され、前記プロセッサ上で実行可能なコンピュータープログラムと、を備え、前記プロセッサが前記コンピュータープログラムを実行することにより、上記の融合測位方法の技術的解決手段のステップが実現される。

【0008】

コンピュータープログラムが記憶されるコンピューター可読記憶媒体であって、前記コンピュータープログラムがプロセッサによって実行されることにより、上記の融合測位方法の技術的解決手段のステップが実現される。

【0009】

本発明の１つまたは複数の実施例の詳細は、以下の図面および説明に記載されている。本発明のその他の特徴および利点は、明細書、図面、および特許請求の範囲から明らかになる。

【0010】

以下、本発明の実施例または従来技術の技術的解決手段をより明確に説明するために、実施例または従来技術の説明に必要な図面を簡単に説明するが、明らかに、以下の説明での図面は、本発明のいくつかの実施例に過ぎず、当業者であれば、発明的な努力を費やすことなく、これらの図面から他の実施例の図面を得ることができる。

【図面の簡単な説明】

【0011】

【図1】本発明の実施例によって提供される融合測位方法のフローチャートである。

【図2】本発明の実施例によって提供される融合測位装置の構成模式図である。

【図3】本発明の実施例によって提供されるデバイスの構成模式図である。

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

【0012】

以下、本発明の理解を容易にするために、関連図面を参照して本発明をより完全に説明する。図面は、本発明の好ましい実施例を示す。しかしながら、本発明は、多くの異なる形態で実現することができ、本明細書に記載の実施例に限定されない。逆に、これら実施例は、本発明の開示内容の理解をより徹底的かつ完全にすることを目的として提供される。

【0013】

別途定義されない限り、本明細書で使用されるすべての技術用語および科学用語は、発明の技術分野に属する当業者によって一般的に理解されるものと同じ意味を有する。本発明の明細書で使用される用語は、具体的な実施例を説明することのみを目的とし、本発明を限定することを意図しない。本明細書で使用される「および／または」という用語は、１つまたは複数の関連する列挙された項目の任意のおよびすべての組み合わせを含む。

【0014】

本発明は、ロボットに応用することができる融合測位方法を提案し、当該ロボットは、伝菜ロボットなどの、レストランで作業するロボットであってもよいし、病院などの医療現場で作業する薬送りロボットであってもよいし、倉庫などの現場で作業する搬送ロボットであってもよい。図１に示すように、融合測位方法は、主に、ステップＳ１０１からＳ１０４を含み、詳しくは以下のとおりである。

【0015】

ステップＳ１０１において、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを取得する。

【0016】

本発明の実施例において、環境とは、ロボットが作業する環境を意味し、環境物とは、ロボットが作業する環境中のあらゆる物体を意味する。環境物は、環境中に設置された道路標識、および、ある荷物、木、壁、または机などのその他の物体を含む。道路標識は、ロボットが作業する環境において、２次元レーザレーダの可視範囲内に設置された光反射柱、光反射バーなどの棒状物であってもよく、ここで、光反射柱の直径が５ｃｍ程度であってもよく、光反射バーの幅も５ｃｍ程度であってもよい。より正確な補助的な測位を行うためには、これらの光反射柱、光反射バーなどの道路標識を少なくとも３つ以上設置する必要があり、また、できる限り同じ直線上に設置しなく、間隔もできる限り１米より大きいように保持する必要がある。

【0017】

本発明の一実施例として、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のレーダ点群関連データを取得することは、ステップＳ１０１１からステップＳ１０１４により実現でき、以下説明する。

【0018】

ステップＳ１０１１において、２次元レーザレーダにより環境物をサンプリングして、環境物の原初レーダ点群データを得る。

【0019】

本発明の実施例において、ロボットに搭載された２次元レーザレーダのサンプリング過程では、従来の技術と同様に、周囲の環境にレーザビームを発射して、リアルタイムで現在環境を走査し、飛行時間測距法を用いてロボットと環境中の道路標識との間の距離を算出し、レーザビームが環境物に衝突した度に、当該レーザビームのロボットに対する角度、レーザ光源と衝突した環境物との間の距離などの情報を取得し、複数のレーザビームが環境物に衝突したときの上記の情報によって環境物の原初レーダ点群データが構成される。２次元レーザレーダがサンプリングした道路標識Ｌ_ｉは、環境中のいずれかの道路標識である。

【0020】

ステップＳ１０１２において、原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得る。

【0021】

２次元レーザレーダが環境をサンプリングする際にロボットが移動しているため、サンプリングされる点毎にレーザレーダ座標系における座標の原点は異なる。たとえば、２次元レーザレーダが２番目の点をサンプリングする際に、ロボットに変位が発生したので、２次元レーザレーダが位置するレーザレーダ座標系の原点にも変位が発生した。同様に、２番目の点から１走査周期の終了時の最後の点まで、サンプリングされる点毎に、レーザレーダ座標系の原点がいずれも異なるため、レーダ点群データに歪みが発生し、このような歪みを除去する必要がある。具体的には、原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得ることは、１走査周期後の、２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定し、相対的位置姿勢△Ｔと、原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを原初レーダ点群データのうちの各点の座標とすることであってもよい。そのうち、相対的位置姿勢△Ｔとは、原初レーダ点群データのうちの各点の、新たな原点に対して得られたロボット位置姿勢データである。また、上記の、１走査周期後の、２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定することとは、２次元レーザレーダの１走査周期が終了し、２次元レーザレーダが１フレームのレーザレーダ点群データの最後の点をサンプリングした際に、このときの２次元レーザレーダの中心点を２次元レーザレーダが位置するレーザレーダ座標系の新たな原点として確定することである。

【0022】

ステップＳ１０１３において、環境物のうちの道路標識Ｌ_ｉの歪み除去点群データをクラスタリングして、道路標識Ｌ_ｉの点群クラスタを得る。

【0023】

前述したように、環境物には、環境中に設置されている光反射柱、光反射バーなどの道路標識が含まれるため、ステップＳ１０１２で得られた環境物の歪み除去点群データにも、環境物のうちの道路標識Ｌ_ｉの歪み除去点群データが含まれる。環境物のうちの道路標識Ｌ_ｉの歪み除去点群データを抽出する方法として、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得た後、環境物の歪み除去点群データのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較することにより、レーザ光反射強度が予め設定された光強度閾値よりも小さい点を除去し、環境物の歪み除去点群データのうちのレーザ光反射強度が予め設定された光強度閾値以上である（すなわち、予め設定された光強度閾値よりも大きいまたはそれと等しい）点群データを保持し、これらのレーザ光反射強度が予め設定された光強度閾値以上である点群データを、環境物のうちの道路標識Ｌ_ｉの歪み除去点群データとする。たとえば、２次元レーザレーダで検出可能なレーザ光反射強度を０～１と仮定すると、光反射柱、光反射バーなどの道路標識のレーザ光反射強度が通常０.８５以上であるため、予め設定された光強度閾値を０.８５に設定し、そして保持環境物の歪み除去点群データのうちの、レーザ光反射強度が０.８５以上である点群データを保持し、これらのレーザ光反射強度が０.８５以上である点群データは、環境物のうちの道路標識Ｌ_ｉの歪み除去点群データとされる。

【0024】

ノイズのある密度ベースのクラスタリング（Ｄｅｎｓｉｔｙ-Ｂａｓｅｄ Ｓｐａｔｉａｌ Ｃｌｕｓｔｅｒｉｎｇ ｏｆ Ａｐｐｌｉｃａｔｉｏｎｓ wｉｔｈ Ｎｏｉｓｅ，ＤＢＳＣＡＮ）アルゴリズムには、任意の形状の稠密データセットをクラスタリングでき、クラスタリングと同時に異常点を発見でき、クラスタリングの結果には偏りが生じることが少ないなどの利点があることを考慮して、本発明の実施例において、ＤＢＳＣＡＮアルゴリズムを用いて環境物のうちの道路標識Ｌ_ｉの歪み除去点群データをクラスタリングして、道路標識Ｌ_ｉの点群クラスタを得ることができる。

【0025】

ステップＳ１０１４において、道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標を求め、道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標を道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉとする。

【0026】

クラスタリングにより道路標識Ｌ_ｉの点群クラスタを得た後に、道路標識Ｌ_ｉの点群クラスタを１つの質点とみなすことができ、幾何学的または物理的の方法により道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標を求めて、道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標を道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉとすることができる。本発明の実施例において、光反射柱の直径と光反射バーの幅が小さいため、道路標識Ｌ_ｉの点群クラスタの幾何学的中心を簡単に求めることで、光反射柱、光反射バーなどの道路標識の幾何学的中心をほぼ近似的に表すことができる。

【0027】

ステップＳ１０２において、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得する。

【0028】

本発明の実施例において、道路標識地図とは、予め作製された、道路標識（Ｌａｎｄｍａｒｋ）の世界座標系での位置を反映した地図であり、道路標識地図での点は１つの道路標識とみなされる。前述したように、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉは即ち、道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標である。具体的には、ｋＮＮ（ｋ-Ｎｅａｒｅｓｔ Ｎｅｉｇｈｂｏｒ，ｋ-最近傍）アルゴリズムなどの最近傍アルゴリズムを用いて、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得することができる。

【0029】

ステップＳ１０３において、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得する。

【0030】

本発明の実施例において、２次元グリッド地図は、２次元グリッド確率地図または２次元占有グリッド地図（Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐ）とも呼ばれ、このような地図は、平面を個々のグリッドに分割し、各グリッドに１つの占有率（Ｏｃｃｕｐａｎｃｙ）を与えたものである。占有率とは、２次元占有グリッド地図上で１つのグリッドが障碍物に占有されている状態（ｏｃｃｕｐｉｅｄ状態）、障碍物に占有されていない状態（Ｆｒｅｅ状態）、またはこの２つの状態の間に介在する確率である。そのうち、障碍物に占有されている状態を１、障碍物に占有されていない状態を０、２つの状態の間に介在する状態を０～１の間の値で表す。明らかに、占有率は、あるグリッドが障碍物に占有されている可能性の大きさを示しており、１つのグリッドの占有率が大きいほど当該グリッドが障碍物に占有されている可能性が高いことを示しており、そうでない場合は逆である。

【0031】

本発明の一実施例として、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得することは、ステップＳ１０３１からステップＳ１０３３により実現でき、以下説明する。

【0032】

ステップＳ１０３１において、ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定する。

【0033】

ロボットの位置姿勢とは、ロボットの世界座標系での状態を意味し、当該状態を位置と姿勢で表し、具体的には、ロボットの世界座標系での横座標ｘ、縦座標ｙ、および横軸に対する角度θ、すなわち（ｘ，ｙ，θ）で、ロボットのある時刻における位置姿勢を表している。推定により得られたロボットの現在時刻の前の時刻の位置姿勢を（ｘ，ｙ，θ）とすると、当該位置姿勢において各分量に増分△ｘ、増分△ｙ、および増分△θを与えると、位置姿勢検索空間（△ｘ，△ｙ，△θ）には複数の位置姿勢候補（ｘ_１，ｙ_１，θ_１）、（ｘ_２，ｙ_２，θ_２）、…、（ｘ_ｉ，ｙ_ｉ，θ_ｉ）、…、（ｘ_ｎ，ｙ_ｎ，θ_ｎ）が存在することになる。

【0034】

ステップＳ１０３２において、複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、環境物の歪み除去点群データを２次元グリッド地図に投影し、各々の位置姿勢候補の２次元グリッド地図上のマッチングスコアを算出する。

【0035】

ここで、環境物の歪み除去点群データを２次元グリッド地図に投影するとは、ある変換関係に従って、環境物の歪み除去点群データを２次元グリッド地図上のある占有位置またはグリッドに変換することであり、位置姿勢候補が異なると、環境物の歪み除去点群データが２次元グリッド地図上の異なるグリッドに変換されることになる。ある位置姿勢候補（ｘ_ｉ，ｙ_ｉ，θ_ｉ）を想定し、環境物の歪み除去点群データを（ｘ_j，ｙ_j，θ_j）とし、位置姿勢候補（ｘ_ｉ，ｙ_ｉ，θ_ｉ）に対応して当該（ｘ_j，ｙ_j，θ_j）を２次元グリッド地図上に投影したグリッド位置を

で表すと、

となる。
そのうち、zｋは、２次元グリッド地図上で位置が

であるグリッドの番号である。

【0036】

前述したように、異なるグリッドには異なる占有率が与えられている。ある位置姿勢候補（ｘ_ｉ，ｙ_ｉ，θ_ｉ）に対応する２次元グリッド地図上のマッチングスコアは、環境物の歪み除去点群データのうちの各歪み除去点群データを２次元グリッド地図上の異なるグリッドに投影した場合の、これら異なるグリッドの占有率の和であり、すなわち、環境物の歪み除去点群データのうちのいずれかの歪み除去点群データ（ｘ_j，ｙ_j，θ_j）を２次元グリッド地図上の位置が

であるグリッドに投影し、対応する占有率をＯ_jとすると、位置姿勢候補（ｘ_ｉ，ｙ_ｉ，θ_ｉ）に対応して、当該環境物の歪み除去点群データを２次元グリッド地図に投影した場合の、２次元グリッド地図上のマッチングスコアは

であり、そのうち、ｍは、当該環境物の歪み除去点群データの歪み除去点群データの数である。

【0037】

ステップＳ１０３３において、複数の位置姿勢候補のうち、２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を、ロボットの２次元グリッド地図でのグローバル位置姿勢として確定する。

【0038】

ステップＳ１０３２の分析から分かるように、上記のｎ個の位置姿勢候補について、ｎ個の異なるマッチングスコア

が得られ、ｎ個の位置姿勢候補のうち２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を、ロボットの２次元グリッド地図でのグローバル位置姿勢として確定し、すなわち、

を選択し、

に対応する位置姿勢候補（ｘ_ｋ，ｙ_ｋ，θ_ｋ）をロボットの２次元グリッド地図でのグローバル位置姿勢として確定し、当該グローバル位置姿勢をロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとする。

【0039】

ステップＳ１０４において、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値である誤差Ｅ_１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が、最小となるまで、ロボットの位置姿勢を反復最適化する。

【0040】

本発明の一実施例として、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの位置姿勢を反復最適化することは、ロボットの位置姿勢を状態として非線形最小二乘法問題を構築し、誤差Ｅ_１を非線形最小二乘法問題の誤差制約条件として、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの位置姿勢を反復最適化することであってもよく、ここで、誤差Ｅ_１は、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値であり、誤差Ｅ_２は、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である。

【0041】

本発明の別の一実施例として、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの位置姿勢を反復最適化することは、さらに、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築し、粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの推定位置姿勢を補正し続けることであってもよく、ここでの誤差Ｅ_１と誤差Ｅ_２の意味は、前述した実施例で言及された誤差Ｅ_１と誤差Ｅ_２の意味と同じである。本発明の実施例において、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＫＦ）は、非線形の場合の標準のカルマンフィルタの拡張形態であり、効率の高い再帰型フィルタすなわち自己回帰フィルタであり、その基本的な考え方は、テイラー級数展開を用いて非線形システムを線形化し、次にカルマンフィルタフレームワークを用いてデータをフィルタリングすることである。粒子フィルタは、状態空間で伝搬する１セットのランダムサンプルを探すことにより確率密度関数を近似的に表し、積分運算の代わりにサンプル平均値を用いて、システム状態の最小分散推定を得る過程であり、これらのサンプルは、「粒子」と具象的に呼ばれる。

【0042】

上記の道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築し、粒子フィルタモデルを用いて、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの推定位置姿勢を補正し続けることは、ロボットの推定位置姿勢と予め設定された時間帯における位置姿勢変化量に基づいて、現在粒子群を取得することであって、粒子群のうちの各々の粒子はロボットの１つの位置姿勢を表し、粒子群はロボットの位置姿勢の確率分布を表すものであることと、現在粒子群のうちの各々の粒子に重みを割り当て、割り当てられた重みに基づいてロボットの第１の推定位置姿勢を得ることであって、重みは、各々の粒子が現在実際の位置姿勢である確率を表すことと、第１の推定位置姿勢、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、予め設定された走査マッチングモデルを用いて走査マッチングを行って、第２の推定位置姿勢を得ることと、誤差Ｅ_１と誤差Ｅ_２が最小となるまで、第２の推定位置姿勢を補正し続け、この時に得られた推定位置姿勢をロボットの最終位置姿勢とすることと、を含んでもよい。

【0043】

上記の図１に例示した融合測位方法から分かるように、本発明の技術的解決手段において、一方では、環境物をサンプリングし、環境物のうちの道路標識Ｌ_ｉの位置Ｐ_ｉに最も近い道路標識Ｌ_jをマッチングし、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値Ｅ_１を取得し、もう一方では、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの推定位置姿勢とロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値Ｅ_２を取得し、最後に、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの位置姿勢を反復最適化する。道路標識のみで測位を補助する従来技術と比較すると、本発明の技術的解決手段は、道路標識測位と２次元グリッド地図測位を融合することで、ロボットの測位をより正確に行うことができる。

【0044】

図２は、本発明の実施例によって提供される融合測位装置であり、当該融合測位装置は、第１の取得モジュール２０１と、第２の取得モジュール２０２と、第３の取得モジュール２０３と、反復最適化モジュール２０４と、を備えてもよく、詳しくは以下のとおりである。

【0045】

第１の取得モジュール２０１は、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを、取得する。
第２の取得モジュール２０２は、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得する。
第３の取得モジュール２０３は、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得する。
反復最適化モジュール２０４は、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値である誤差Ｅ_１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が、最小となるまで、ロボットの位置姿勢を反復最適化する。

【0046】

代替的には、図２に例示された第１の取得モジュール２０１は、サンプリングユニットと、補正ユニットと、クラスタリングユニットと、中心求めユニットと、を備えてもよい。
サンプリングユニットは、２次元レーザレーダにより環境物をサンプリングして、環境物の原初レーダ点群データを得る。
補正ユニットは、原初レーダ点群データのうちの各点の相対的位置姿勢に基づいて、環境物の原初レーダ点群データに対して歪み除去を行って、環境物の歪み除去点群データを得る。
クラスタリングユニットは、環境物のうちの道路標識Ｌ_ｉの歪み除去点群データをクラスタリングして、道路標識Ｌ_ｉの点群クラスタを得る。
中心求めユニットは、道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標を求め、道路標識Ｌ_ｉの点群クラスタのロボット本体座標系での幾何学的中心座標を道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉとする。

【0047】

代替的には、上記の補正ユニットは、新たな原点確定ユニットと、乗算ユニットと、を備えてもよい。
新たな原点確定ユニットは、１走査周期後の、２次元レーザレーダが位置するレーザレーダ座標系の新たな原点を確定する。
乗算ユニットは、相対的位置姿勢△Ｔと、原初レーダ点群データのうちの各点の旧原点のレーザレーダ座標系での座標Ｐとを乗算し、乗算後の結果△Ｔ*Ｐを原初レーダ点群データのうちの各点の座標とする。そのうち、相対的位置姿勢△Ｔは、原初レーダ点群データのうちの各点の、新たな原点に対して得られたロボット位置姿勢データである。

【0048】

代替的には、図２に例示された装置は、比較モジュールと、選別モジュールと、をさらに備えてもよい。
比較モジュールは、環境物の歪み除去点群データのうちの各点に対応するレーザ光反射強度と予め設定された光強度閾値とを比較する。
選別モジュールは、環境物の歪み除去点群データのうちの、レーザ光反射強度が予め設定された光強度閾値以上である点群データを、環境物のうちの道路標識Ｌ_ｉの歪み除去点群データとして保持する。

【0049】

代替的には、図２に例示された第３の取得モジュール２０３は、検索ユニットと、マッチングユニットと、グローバル位置姿勢確定ユニット、を備えてもよい。
検索ユニットは、ロボットの現在時刻の前の時刻の位置姿勢から、位置姿勢検索空間における複数の位置姿勢候補を確定する。
マッチングユニットは、複数の位置姿勢候補のうちの各々の位置姿勢候補を利用して、環境物の歪み除去点群データを２次元グリッド地図に投影し、各々の位置姿勢候補の２次元グリッド地図上のマッチングスコアを算出する。
グローバル位置姿勢確定ユニットは、複数の位置姿勢候補のうち、２次元グリッド地図上のマッチングスコアが最も高い位置姿勢候補を、ロボットの２次元グリッド地図でのグローバル位置姿勢として確定する。

【0050】

代替的には、図２に例示された反復最適化モジュール２０４は、問題構築ユニットと、第１の補正ユニットと、を備えてもよい。
構築ユニットは、ロボットの位置姿勢を状態として非線形最小二乘法問題を構築する。
第１の補正ユニットは、誤差Ｅ_１を非線形最小二乘法問題の誤差制約条件として、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの位置姿勢を反復最適化する。

【0051】

代替的には、図２に例示された反復最適化モジュール２０４は、モデル構築ユニットと、第２の補正ユニットと、を備えてもよい。
モデル構築ユニットは、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、粒子フィルタモデルまたは拡張カルマンフィルタモデルを構築する。
第２の補正ユニットは、粒子フィルタモデルまたは拡張カルマンフィルタモデルを用いて、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの推定位置姿勢を補正し続ける。

【0052】

以上の技術的解決手段の説明から分かるように、本発明の技術的解決手段において、一方では、環境物をサンプリングし、環境物のうちの道路標識Ｌ_ｉの位置Ｐ_ｉに最も近い道路標識Ｌ_jをマッチングし、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値Ｅ_１を取得し、もう一方では、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの推定位置姿勢とロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値Ｅ_２を取得し、最後に、誤差Ｅ_１および誤差Ｅ_２が最小となるまで、ロボットの位置姿勢を反復最適化する。道路標識のみで測位を補助する従来技術と比較すると、本発明の技術的解決手段は、道路標識測位と２次元グリッド地図測位を融合することで、ロボットの測位をより正確に行うことができる。

【0053】

図３は、本発明の一実施例によって提供されるデバイスの構成模式図である。図３に示すように、当該実施例のデバイス３は、主に、プロセッサ３０と、メモリ３１と、メモリ３１に記憶され、プロセッサ３０上で実行可能の、融合測位方法のプログラムなどのコンピュータープログラム３２と、を備える。プロセッサ３０がコンピュータープログラム３２を実行する際に、上記の融合測位方法の実施例のステップが実現され、たとえば、図１に示すステップＳ１０１からＳ１０４が実現される。または、プロセッサ３０がコンピュータープログラム３２を実行する際に、上記の各装置の実施例の各モジュール/ユニットの機能が実現され、たとえば、図２に示す第１の取得モジュール２０１、第２の取得モジュール２０２、第３の取得モジュール２０３、および反復最適化モジュール２０４の機能が実現される。

【0054】

例示的に、融合測位方法のコンピュータープログラム３２は、主に、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを、取得することと、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得することと、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得することと、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値である誤差Ｅ_１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が、最小となるまで、ロボットの位置姿勢を反復最適化することと、を含む。コンピュータープログラム３２は、１つまたは複数のモジュール/ユニットに分割されてもよく、１つまたは複数のモジュール/ユニットは、メモリ３１に記憶され、プロセッサ３０により実行されることにより本発明を実施する。１つまたは複数のモジュール/ユニットは、特定機能を実行することができる一連のコンピュータープログラム命令セグメントであってもよく、当該命令セグメントは、コンピュータープログラム３２のデバイス３上の実行過程を記述したものであってもよい。たとえば、コンピュータープログラム３２は、第１の取得モジュール２０１と、第２の取得モジュール２０２と、第３の取得モジュール２０３と、反復最適化モジュール２０４（仮想装置中のモジュール）との機能に分割されてもよく、各モジュールの具体的な機能は以下のようであってもよい。第１の取得モジュール２０１は、ロボットに搭載された２次元レーザレーダにより環境物をサンプリングし、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと環境物の歪み除去点群データとを含む、環境物のレーダ点群関連データを、取得する。第２の取得モジュール２０２は、環境物のうちの道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉと道路標識地図とを近傍マッチングし、道路標識地図上の位置Ｐ_ｉに最も近い道路標識Ｌ_jの位置Ｐ_jを取得する。第３の取得モジュール２０３は、環境物の歪み除去点群データを用いて２次元グリッド地図で走査してマッチングを行い、ロボットの２次元グリッド地図でのグローバル位置姿勢を、ロボットのグローバル位置姿勢観測値Ｇ_ｐｏｓｅとして取得する。反復最適化モジュール２０４は、道路標識Ｌ_ｉの道路標識地図上の位置Ｐ_ｉ、道路標識Ｌ_jの道路標識地図上の位置Ｐ_j、ロボットの推定位置姿勢、およびグローバル位置姿勢観測値Ｇ_ｐｏｓｅに基づいて、道路標識Ｌ_ｉと道路標識Ｌ_jとの間の道路標識地図上の位置差値である誤差Ｅ_１、および、ロボットの推定位置姿勢とグローバル位置姿勢観測値Ｇ_ｐｏｓｅとの間の差値である誤差Ｅ_２が、最小となるまで、ロボットの位置姿勢を反復最適化する。

【0055】

デバイス３は、プロセッサ３０とメモリ３１とを備えるが、これらに限定されない。当業者が理解するように、図３は単にデバイス３の例であり、デバイス３を限定するものではなく、図示されているよりも多いかまたは少ない構成要素、いくつかの構成要素の組み合わせ、または、異なる構成要素を備えてもよく、たとえば、コンピューティングデバイスは、入力／出力デバイス、ネットワークアクセスデバイス、バスなどを備えることができる。

【0056】

プロセッサ３０とは、中央処理ユニット（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ，ＣＰＵ）であってもよく、他の汎用プロセッサ、デジタル信号プロセッサ （Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ，ＤＳＰ）、専用集積回路（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ，ＡＳＩＣ）、フィールドプログラマブルゲートアレイ（Ｆｉｅｌｄ-Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ，ＦＰＧＡ）または他のプログラマブルロジックデバイス、分離ゲートまたはトランジスタロジックデバイス、分離ハードウェアコンポーネントなどであってもよい。汎用プロセッサは、マイクロプロセッサであってもよく、或いは、当該プロセッサは従来のいずれかのプロセッサなどであってもよい。

【0057】

メモリ３１とは、デバイス３のハードディスクまたは内部メモリなどの、デバイス３の内部記憶ユニットであってもよい。メモリ３１は、デバイス３に搭載されたプラグインハードディスク、スマートメモリカード（Ｓｍａｒｔ Ｍｅｄｉａ Ｃａｒｄ，ＳＭＣ）、セキュアデジタル（Ｓｅｃｕｒｅ Ｄｉｇｉｔａｌ，ＳＤ）カード、フラッシュメモリカード（Ｆｌａｓｈ Ｃａｒｄ）などの、デバイス３の外部記憶デバイスであってもよい。さらに、メモリ３１は、デバイス３の内部記憶ユニットと外部記憶デバイスの両方を備えてもよい。メモリ３１は、コンピュータープログラム、および、デバイスに必要な他のプログラムとデータを記憶するためのものである。メモリ３１は、出力されたデータまたは出力しようとするデータを一時的に記憶するためのものであってもよい。

【0058】

以上説明した実施例の各技術特徴は任意に組み合わせることができ、説明を簡潔にするために、上記の実施例中の各技術特徴のすべての可能な組み合わせを記載していないが、これらの技術特徴の組み合わせは矛盾しない限り、いずれも本明細書に記載された範囲としてみなすべきである。

【0059】

以上説明した実施例は、本発明の幾つかの実施形態を示したにすぎず、その記載は具体的かつ詳細なものであるが、特許請求の範囲を限定するものではない。当業者であれば、本願の思想から逸脱することなく、複数の変形や改良を加えることが可能であり、これらはいずれも本発明の保護範囲に属することを留意すべきである。したがって、本出願の特許請求の保護範囲は、添付の特許請求の範囲に準ずるものとする。

【図1】

【図2】

【図3】