特許 7468392 自己位置推定装置

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

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

(11)【特許番号】

(24)【登録日】2024-04-08

(45)【発行日】2024-04-16

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

(51)【国際特許分類】

   G05D 1/242 20240101AFI20240409BHJP

【ＦＩ】

G05D1/242

【請求項の数】 7

(21)【出願番号】P 2021020954

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

(65)【公開番号】P2022123568

(43)【公開日】2022-08-24

【審査請求日】2023-05-16

(73)【特許権者】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100124062

【弁理士】

【氏名又は名称】三上 敬史

(74)【代理人】

【識別番号】100148013

【弁理士】

【氏名又は名称】中山 浩光

(72)【発明者】

【氏名】上田 祐規

(72)【発明者】

【氏名】古室 達也

【審査官】岩▲崎▼ 優

(56)【参考文献】

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

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

【文献】米国特許出願公開第２０２０／００１１６７５（ＵＳ，Ａ１）

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

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

Ｇ０１Ｃ ２１／００ー２１／３６

Ｇ０１Ｃ ２３／００ー２５／００

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

【請求項1】

移動体の周囲にレーザを照射し、レーザの反射光を受光することで、前記移動体の周囲に存在する物体を検出して点群データを取得するレーザ検出部と、
前記移動体が走行するエリアの点群地図を記憶する記憶部と、
前記レーザ検出部により取得された点群データと前記記憶部に記憶された前記点群地図の参照点群とに基づいて、前記移動体の自己位置を推定する自己位置推定部と、
前記自己位置推定部により得られた前記移動体の自己位置推定値に対応する中心点群の周囲に複数の仮想点群を設定する仮想点群設定部と、
前記中心点群と最も近い前記参照点群との距離を算出して、前記中心点群と前記参照点群との一致度を表す第１スコアを算出すると共に、前記仮想点群設定部により設定された前記仮想点群と最も近い前記参照点群との距離を算出して、前記仮想点群と前記参照点群との一致度を表す第２スコアを算出するスコア算出部と、
前記スコア算出部により算出された前記第１スコア及び前記第２スコアを用いて、前記自己位置推定部による前記移動体の自己位置推定の信頼度を算出する信頼度算出部とを備える自己位置推定装置。

【請求項2】

前記第１スコアを基準としたスコア比率を算出するスコア比率算出部と、
前記スコア比率算出部により算出されたスコア比率に基づいて、前記自己位置推定の信頼度を算出するための関数を導出する関数導出部とを更に備え、
前記スコア算出部は、前記自己位置推定値を中心とした仮想円上の複数の地点に対応する各仮想点群と前記最も近い参照点群との距離を算出し、
前記関数導出部は、前記スコア比率と前記仮想円の半径との関係を表す関数を導出し、
前記信頼度算出部は、前記関数導出部により導出された関数に基づいて、前記自己位置推定の信頼度を算出する請求項１記載の自己位置推定装置。

【請求項3】

前記スコア比率算出部は、前記第１スコアに対する前記第２スコアの比率を前記スコア比率として算出し、
前記関数導出部は、前記仮想円上の前記複数の地点に対応する前記各仮想点群において最大となるスコア比率と前記仮想円の半径との関係を表す１次関数を導出し、
前記信頼度算出部は、前記関数導出部により導出された１次関数と予め決められた判定閾値との交点となる前記仮想円の半径を前記自己位置推定の信頼度とする請求項２記載の自己位置推定装置。

【請求項4】

前記仮想円は、前記移動体の自己位置推定値を中心として同心円状に複数層有している請求項２または３記載の自己位置推定装置。

【請求項5】

前記第２スコアが前記第１スコアよりも高いときに、当該第２スコアが得られる前記仮想点群に対応する地点の位置を新たな前記自己位置推定値として補正する自己位置補正部を更に備え、
前記仮想点群設定部は、前記自己位置補正部により補正された自己位置推定値に対応する中心点群の周囲に前記複数の仮想点群を設定する請求項１～４の何れか一項記載の自己位置推定装置。

【請求項6】

前記仮想点群設定部は、前記仮想点群の姿勢が前記中心点群の姿勢と異なるように、前記複数の仮想点群を前記中心点群の周囲に設定する請求項１～５の何れか一項記載の自己位置推定装置。

【請求項7】

前記中心点群及び前記仮想点群のフィルタリングを行うフィルタ部を更に備え、
前記スコア算出部は、前記フィルタ部によりフィルタリングが行われた前記中心点群及び前記仮想点群と前記最も近い参照点群との距離を算出する請求項１～６の何れか一項記載の自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自己位置推定装置に関する。

【背景技術】

【0002】

従来の自己位置推定装置としては、例えば特許文献１に記載されているような技術が知られている。特許文献１に記載の自己位置推定装置は、車両の周囲の障害物の有無及び障害物までの距離を計測するＬＲＦ（レーザレンジファインダ）と、車両が自律走行を行う際に必要となる制御パラメータ及び地図等の情報を記憶する記憶部と、車両を設定された走行経路に追従して走行するように制御する車両制御部と、記憶部に記憶された地図とＬＲＦにより計測された計測データとを照合して、車両の現在位置及び姿勢を推定する位置姿勢推定部と、位置姿勢推定部による推定結果の信頼性を評価する信頼性評価部と、信頼性評価部による評価の結果として信頼性が低いことを示す場合に、車両制御部において用いられる制御パラメータ及び制御入力を変更する制御変更部とを備えている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１５－３６８４０号公報

【発明の概要】

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

【0004】

しかしながら、上記従来技術においては、車両（移動体）が走行するエリアの環境が変わると、移動体の自己位置の推定精度と自己位置推定の信頼度との相関特性が大きく変化することがある。

【0005】

本発明の目的は、移動体が走行するエリアの環境が変わっても、移動体の自己位置の推定精度との相関特性の変化が少ない自己位置推定の信頼度を得ることができる自己位置推定装置を提供することである。

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

【0006】

本発明の一態様に係る自己位置推定装置は、移動体の周囲にレーザを照射し、レーザの反射光を受光することで、移動体の周囲に存在する物体を検出して点群データを取得するレーザ検出部と、移動体が走行するエリアの点群地図を記憶する記憶部と、レーザ検出部により取得された点群データと記憶部に記憶された点群地図の参照点群とに基づいて、移動体の自己位置を推定する自己位置推定部と、自己位置推定部により得られた移動体の自己位置推定値に対応する中心点群の周囲に複数の仮想点群を設定する仮想点群設定部と、中心点群と最も近い参照点群との距離を算出して、中心点群と参照点群との一致度を表す第１スコアを算出すると共に、仮想点群設定部により設定された仮想点群と最も近い参照点群との距離を算出して、仮想点群と参照点群との一致度を表す第２スコアを算出するスコア算出部と、スコア算出部により算出された第１スコア及び第２スコアを用いて、自己位置推定部による移動体の自己位置推定の信頼度を算出する信頼度算出部とを備える。

【0007】

このような自己位置推定装置においては、移動体の周囲にレーザが照射されることで、移動体の周囲に存在する物体が検出され、物体の点群データと点群地図の参照点群とに基づいて、移動体の自己位置が推定される。そして、移動体の自己位置推定値に対応する中心点群の周囲に複数の仮想点群が設定される。そして、中心点群と最も近い参照点群との距離が算出されて、中心点群と参照点群との一致度を表す第１スコアが算出されると共に、仮想点群と最も近い参照点群との距離が算出されて、仮想点群と参照点群との一致度を表す第２スコアが算出される。そして、第１スコア及び第２スコアを用いて、移動体の自己位置推定の信頼度が算出される。このように自己位置推定の信頼度の算出において第１スコア及び第２スコアを用いることにより、移動体の自己位置の推定精度と自己位置推定の信頼度との相関が高くなる。これにより、移動体が走行するエリアの環境が変わっても、移動体の自己位置の推定精度との相関特性の変化が少ない自己位置推定の信頼度が得られる。

【0008】

自己位置推定装置は、第１スコアを基準としたスコア比率を算出するスコア比率算出部と、スコア比率算出部により算出されたスコア比率に基づいて、自己位置推定の信頼度を算出するための関数を導出する関数導出部とを更に備え、スコア算出部は、自己位置推定値を中心とした仮想円上の複数の地点に対応する各仮想点群と最も近い参照点群との距離を算出し、関数導出部は、スコア比率と仮想円の半径との関係を表す関数を導出し、信頼度算出部は、関数導出部により導出された関数に基づいて、自己位置推定の信頼度を算出してもよい。このような構成では、移動体の自己位置推定値に対応する中心点群と参照点群との一致度を表す第１スコアを基準としたスコア比率が高い場合は、自己位置推定値が収束し得る範囲が広くなるため、自己位置推定の信頼度が悪化する。そこで、スコア比率と仮想円の半径との関係を表す関数を用いることにより、自己位置推定の信頼度の算出精度が向上する。

【0009】

スコア比率算出部は、第１スコアに対する第２スコアの比率をスコア比率として算出し、関数導出部は、仮想円上の複数の地点に対応する各仮想点群において最大となるスコア比率と仮想円の半径との関係を表す１次関数を導出し、信頼度算出部は、関数導出部により導出された１次関数と予め決められた判定閾値との交点となる仮想円の半径を自己位置推定の信頼度としてもよい。このような構成では、仮想円上の複数の地点に対応する各仮想点群において最大となるスコア比率と仮想円の半径との関係を表す１次関数と判定閾値とを用いることにより、簡単な計算処理によって自己位置推定の信頼度を算出することができる。

【0010】

仮想円は、移動体の自己位置推定値を中心として同心円状に複数層有していてもよい。このような構成では、自己位置推定の信頼度の計算の分解能が高くなるため、自己位置推定の信頼度の算出精度が向上する。

【0011】

自己位置推定装置は、第２スコアが第１スコアよりも高いときに、当該第２スコアが得られる仮想点群に対応する地点の位置を新たな前記自己位置推定値として補正する自己位置補正部を更に備え、仮想点群設定部は、自己位置補正部により補正された自己位置推定値に対応する中心点群の周囲に前記複数の仮想点群を設定してもよい。このような構成では、第２スコアが第１スコアよりも高いときに、移動体の自己位置推定値を補正することにより、自己位置の推定精度が向上する。

【0012】

仮想点群設定部は、仮想点群の姿勢が中心点群の姿勢と異なるように、複数の仮想点群を中心点群の周囲に設定してもよい。このような構成では、中心点群の姿勢に対して仮想点群の姿勢を変えることにより、自己位置推定の信頼度の計算の分解能が高くなるため、自己位置推定の信頼度の算出精度が向上する。

【0013】

自己位置推定装置は、中心点群及び仮想点群のフィルタリングを行うフィルタ部を更に備え、スコア算出部は、フィルタ部によりフィルタリングが行われた中心点群及び仮想点群と最も近い参照点群との距離を算出してもよい。このような構成では、フィルタ部によって中心点群及び仮想点群の点の数が減るため、中心点群及び仮想点群と最も近い参照点群との距離を算出する際の計算コストが低くなる。

【発明の効果】

【0014】

本発明によれば、移動体が走行するエリアの環境が変わっても、移動体の自己位置の推定精度との相関特性の変化が少ない自己位置推定の信頼度を得ることができる。

【図面の簡単な説明】

【0015】

【図1】本発明の第１実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。

【図2】中心点群及び参照点群と移動体の自己位置推定値との関係を示す概念図である。

【図3】中心点群の周囲に複数の仮想点群が設定される様子を示す概念図である。

【図4】中心点群及び仮想点群と参照点群との距離を示す概念図である。

【図5】移動体の自己位置推定値を中心とした仮想円上に存在する複数の地点の一例を示す概念図である。

【図6】関数導出部により導出された１次関数を用いて自己位置推定の信頼度を算出する手法を示すグラフである。

【図7】スコア算出部により算出されるスコアと自己位置推定値の分散との関係を示すグラフである。

【図8】本発明の第２実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。

【図9】本発明の第３実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。

【図10】仮想点群の姿勢が中心点群の姿勢と異なる状態を、移動体の自己位置推定値を中心とした仮想円上に存在する複数の地点と共に示す概念図である。

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

【0016】

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。

【0017】

図１は、本発明の第１実施形態に係る自己位置推定装置を具備した走行制御装置を示す概略構成図である。図１において、走行制御装置１は、フォークリフト等の移動体２（図２参照）に搭載されている。走行制御装置１は、移動体２を自律走行させる装置である。走行制御装置１は、レーザセンサ３と、記憶部４と、コントローラ５と、駆動部６とを備えている。

【0018】

レーザセンサ３は、移動体２の周囲にレーザを照射し、レーザの反射光を受光することで、移動体２の周囲に存在する物体を検出してスキャン点群データを取得するレーザ検出部である。物体は、例えば建屋の壁や柱等である。スキャン点群は、レーザの反射点であるスキャン点の集まりである。レーザセンサ３としては、例えば２Ｄまたは３Ｄのレーザレンジファインダが用いられる。

【0019】

記憶部４は、移動体２が走行するエリアの点群地図を記憶する。点群地図は、物体の壁面に相当する参照点群Ｐｓ（図２参照）を有している。参照点群Ｐｓを含む点群地図は、レーザセンサ３によって事前に取得されている。

【0020】

コントローラ５は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ５は、自己位置推定部１１と、走行制御部１２と、仮想点群設定部１３と、スコア算出部１４と、スコア比率算出部１５と、関数導出部１６と、信頼度算出部１７とを有している。

【0021】

ここで、レーザセンサ３、記憶部４、コントローラ５の自己位置推定部１１、仮想点群設定部１３、スコア算出部１４、スコア比率算出部１５、関数導出部１６及び信頼度算出部１７は、本実施形態の自己位置推定装置１０を構成している。

【0022】

自己位置推定部１１は、レーザセンサ３により取得されたスキャン点群データと記憶部４に記憶された点群地図の参照点群とに基づいて、移動体２の現在の自己位置を推定する。レーザセンサ３により取得されたスキャン点群データは、移動体２の周囲に存在する物体の検出データである。ここでは、レーザセンサ３により取得されたスキャン点群データは、中心点群Ｐ（図２参照）である。移動体２の自己位置は、移動体２の２次元座標位置（ＸＹ座標位置）及び向き（姿勢）で表される。

【0023】

自己位置推定部１１は、例えばレーザＳＬＡＭ（simultaneous localization andmapping）手法を用いて、移動体２の自己位置の推定演算を行う。ＳＬＡＭは、センサデータ及びマップデータを使って自己位置推定を行う自己位置推定技術である。自己位置推定部１１は、図２に示されるように、中心点群Ｐと参照点群Ｐｓとをマッチングさせて、移動体２の自己位置の推定演算を行う。これにより、移動体２の自己位置推定値Ｑ（図２参照）が得られる。

【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】

２…移動体、３…レーザセンサ（レーザ検出部）、４…記憶部、１０…自己位置推定装置、１１…自己位置推定部、１３…仮想点群設定部、１４…スコア算出部、１５…スコア比率算出部、１６…関数導出部、１７…信頼度算出部、１８…自己位置補正部、１９…フィルタ部、Ａ…判定閾値、Ｃ…仮想円、Ｐ…中心点群、Ｐｓ…参照点群、Ｐ＊…仮想点群、Ｑ…自己位置推定値、Ｑ＊…地点、ｒ…半径。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】