JP7610414B2 - 自己位置推定装置 - Google Patents

Description

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

自動運転・運転支援システムの適用範囲を拡大するためには、自己位置推定に基づく地図からの情報取得が重要である。しかしながら、例えば高速道路においては、運転支援システム用の地図が整備されているが、一般道や自宅周辺などの住宅街における地図整備の目途はたっていない。したがって、地図を自己生成し、自己位置推定できる技術が求められる。

これに対して、特許文献１では、「所定領域内の固定物の配置位置を含む地図情報を記憶し、地図再作成の際、所定領域内をロボット１０を移動させつつ（１）乃至（５）を繰返し実行する。（１）エンコーダ２１を積算してロボットの自己位置を算出する（２）ロボットの所定視野角範囲内にある固定物及び移動可能物までの距離を測定する（３）測定した距離と、地図情報から得られる固定物までの距離とから補正自己位置を求める（４）測定値の内、移動可能物までの距離を表す測定値を抽出する（５）抽出した測定値と補正自己位置とから移動可能物の配置位置を算出する。そして、算出した移動可能物の配置位置を集約処理して新たな配置位置を求め、求めた新たな配置位置を外部に出力する。」と記載されている。

特開２０１１－２１０１２１号公報

特許文献１に記載されている従来技術では、自動で自己位置を補正しつつ移動し、さらに自動で地図を再作成することで、環境地図（点群地図）の再作成に要するコストを低減し、かつ環境地図をタイムリーに再作成することができる。しかしながら、ランドマークの観測誤差情報が考慮されていないため、ランドマークの観測誤差が大きい場合に自己位置推定の精度が不十分となる問題がある。地図を自己生成する際、地図に登録したランドマークを用いて自己位置推定を実施するため、自己位置推定に用いるランドマークを自動運転・運転支援システムに求められる高い精度で、地図に登録する必要がある。

本発明は、上記事情に鑑みてなされたもので、観測点となるランドマークの誤差量を見積もり、自己生成点群地図の質を改善することで、センサによる観測誤差が大きい場合でも、高精度な自己位置推定を実現することができる自己位置推定装置を提供することを目的とする。

本発明の代表的な自己位置推定装置の一つを示せば、自車両に搭載された車載センサにより得られた外界情報の点群データから点群地図を生成し、前記点群地図上の前記自車両の位置である自己位置を推定する自己位置推定装置であって、前記点群地図からランドマークを検出するランドマーク検出部と、検出した前記ランドマークの観測位置情報を蓄積するランドマーク情報蓄積部と、前記ランドマークの観測位置情報を頻度分布化したときに検出される誤差分布から抽出される点群座標に基づいて前記ランドマークの位置を補正し、前記誤差分布の広がりの大きさに基づいて前記ランドマークの重要度を計算するランドマーク位置補正部と、前記ランドマーク位置補正部から得られた前記ランドマークの位置を補正した補正地図を自己生成地図として保存する自己地図生成部と、保存された前記自己生成地図と前記自車両の現在走行中に新たに前記ランドマーク位置補正部から得られた前記補正地図とを前記重要度で重み付けして照合することで前記自車両の現在走行中の自己位置推定を行う自己位置推定部と、を備えることを特徴とする。

本発明によれば、観測点となるランドマークの誤差量を見積もり、自己生成点群地図の質を改善することで、センサによる観測誤差が大きい場合でも、高精度な自己位置推定が可能となる。

上記した以外の課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

本発明の第１の実施の形態にかかる自己地図生成・自己位置推定装置１００の構成を示すブロック図。 本発明の第１の実施の形態にかかる自己地図生成・自己位置推定装置１００の動作を表す機能ブロック図。 ピーク位置の検出、観測誤差の推定方法の概略を示す図であり、現在観測している地点を表す図。 ピーク位置の検出、観測誤差の推定方法の概略を示す図であり、ランドマーク蓄積情報１３１の様態を表す図。 ランドマーク情報蓄積部２０６の動作を示すフローチャート。 ランドマーク位置補正部２０７の動作を示すフローチャート。 自己地図生成部２０８の動作を示すフローチャート。 自己位置推定部２０９の動作を示すフローチャート。 本発明の第２の実施の形態にかかる、複数の外界センサ搭載時の自己地図生成・自己位置推定装置１００の構成を示すブロック図。 本発明の第２の実施の形態にかかる、複数の外界センサ搭載時の自己地図生成・自己位置推定装置１００の動作を表す機能ブロック図。 センサ協調誤差推定部２２０の動作を示すフローチャート。 本発明の第３の実施の形態にかかる自己地図生成・自己位置推定装置１００を備える自動運転装置１０２の構成を示すブロック図。

以下、図１～図１１を参照して、本発明にかかる自己地図生成装置、および自己位置推定装置の実施の形態を説明する。なお、本発明の実施の形態を説明するための全図において、同一の機能を有する部分には同一の符号を付し、その繰り返し説明は省略する場合がある。

［第１の実施の形態］
図１は、本発明の第１の実施の形態にかかる自己地図生成・自己位置推定装置１００の構成を示すブロック図である。自己地図生成・自己位置推定装置１００は、車両に搭載され、車載センサにより得られた外界情報の点群データから点群地図（自己生成点群地図）を生成し、点群地図上の自己位置（自車位置）を推定する装置である。自己地図生成・自己位置推定装置１００は、センサ群（１０５、１１０、１１１）と、車載処理装置１０１とから構成される。車載センサであるセンサ群は、車載処理装置１０１と信号線で接続され、車載処理装置１０１と各種データを授受する。

車載処理装置１０１は、演算部１２０と、ＲＡＭ１３０と、記憶部１４０と、インターフェース１５０と、ＲＯＭ１６０を備える。演算部１２０はＣＰＵである。ＦＰＧＡなど他の演算処理装置で全部、または一部の演算処理を実行するように構成しても良い。ＲＡＭ１３０は、読み書き可能な記憶領域であり、車載処理装置１０１の主記憶装置として動作する。ＲＡＭ１３０には、後述するランドマーク蓄積情報１３１、ランドマーク補正情報１３２、自己生成地図１３３が格納される。ＲＯＭ１６０は、読み取り専用の記憶領域であり、後述するプログラムが格納される。このプログラムはＲＡＭ１３０で展開されて、演算部１２０により実行される。演算部１２０は、プログラムを読み込んで実行することにより、オドメトリ計算部１２１、ランドマーク検出部１２２、ランドマーク情報蓄積部１２３、ランドマーク位置補正部１２４、自己地図生成部１２５、自己位置推定部１２６として動作する。記憶部１４０は、不揮発性の記憶装置であり、車載処理装置１０１の補助記憶装置として動作する。記憶部１４０には自己生成地図１４１が格納される。後述するが、記憶部１４０に記憶される自己生成地図１４１は、ＲＡＭ１３０に格納される自己生成地図１３３とは別のものを指す場合がある。

センサ群は、車両の周囲（外界情報）を観測する外界センサ１０５と、車速センサ１１０と、舵角センサ１１１とを含む。

外界センサ１０５は、周囲の対象物までの距離が観測できるもの（換言すれば、距離センサとして機能し得るもの）、例えばステレオカメラを示す。ステレオカメラは、２つのカメラが横並びで一定基線長離れて１ユニット化されているものでも良いし、２つのカメラがラフに取り付けられた状態で、キャリブレーションにより、ステレオカメラとして活用できるようにしたものでも良いし、２つのカメラが縦並びで構成されたものでもよいし、３つ以上のカメラを活用したものでも良い。また、外界センサ１０５は、単眼カメラでモーションステレオ法を活用しても良いし、LiDAR、ミリ波レーダー、ソナー等のセンサでもよい。本実施の形態では、２つのカメラが横並びで一定基線長離れて１ユニット化されているステレオカメラを想定して説明する。外界センサ１０５は、撮影して得られた画像、および距離算出のために必要な視差を、車載処理装置１０１に出力する。ここで、外界センサ１０５は撮影画像のみを車載処理装置１０１に出力して、演算部１２０で視差を算出しても良い。

車載処理装置１０１は、外界センサ１０５から得られた撮影画像および、視差を用いて、後述するランドマークの測位を行う。外界センサ１０５の取り付け位置や取り付け姿勢である外部パラメータは既知であり、ＲＯＭ１６０にあらかじめ保存されている。あるいは、車載処理装置１０１において、撮影画像、視差、車速センサ１１０、舵角センサ１１１を用いて、演算部１２０で推定しても良い。また、ステレオカメラの２つのカメラ同士の相対関係を示す外部パラメータ、およびカメラの焦点距離や撮像素子サイズなどの内部パラメータは既知であり、ＲＯＭ１６０にあらかじめ保存されている。車載処理装置１０１は、ＲＯＭ１６０に格納された内部パラメータおよび外部パラメータおよび視差を用いて、外界センサ１０５と被写体の位置関係を算出することができる。

車速センサ１１０、および舵角センサ１１１はそれぞれ、車載処理装置１０１が搭載された車両の車速と舵角を測定し、車載処理装置１０１に出力する。車載処理装置１０１は、車速センサ１１０および舵角センサ１１１の出力を用いて公知のデッドレコニング技術によって、車載処理装置１０１が搭載された車両の移動量および移動方向を算出する。

（機能ブロック構成）
図２は、演算部１２０が実行するオドメトリ計算部１２１、ランドマーク検出部１２２、ランドマーク情報蓄積部１２３、ランドマーク位置補正部１２４、自己地図生成部１２５、自己位置推定部１２６が有する機能を機能ブロックとして表し、機能ブロックの処理順、また、機能ブロック同士、および機能ブロックとＲＡＭ１３０、ＲＯＭ１６０、記憶部１４０とのデータの流れを示す図である。車載処理装置１０１の自己地図生成・自己位置推定を担う機能ブロックとしての自己地図生成・自己位置推定部２５０は、センサ値取得部２０１、オドメトリ推定部２０２、ランドマーク検出部２０３、距離算出部２０４、世界座標計算部２０５、ランドマーク情報蓄積部２０６、ランドマーク位置補正部２０７、自己地図生成部２０８、自己位置推定部２０９、記憶部１４０を備える。

演算部１２０におけるオドメトリ計算部１２１に対応する機能ブロックがオドメトリ推定部２０２、演算部１２０におけるランドマーク検出部１２２に対応する機能ブロックがランドマーク検出部２０３、演算部１２０におけるランドマーク情報蓄積部１２３に対応する機能ブロックがランドマーク情報蓄積部２０６、演算部１２０におけるランドマーク位置補正部１２４に対応する機能ブロックがランドマーク位置補正部２０７、演算部１２０における自己地図生成部１２５に対応する機能ブロックが自己地図生成部２０８、演算部１２０における自己位置推定部１２６に対応する機能ブロックが自己位置推定部２０９である。

（センサ値取得部２０１）
センサ値取得部２０１は、外界センサ１０５から出力される信号を取得する。ステレオカメラの場合、画像と視差である。ステレオカメラは、連続して高い頻度、たとえば毎秒３０回撮影する。ステレオカメラが撮影して得られた画像および視差は、撮影するたびに車載処理装置１０１に送信される。センサ値取得部２０１は、これを一定頻度で受信し、受信した画像および視差をランドマーク検出部２０３に出力する。以降の処理部２０３～２０９は、受信するたびに動作する。

（オドメトリ推定部２０２）
オドメトリ推定部２０２は、車速センサ１１０および舵角センサ１１１から送信される車両の速度および舵角を用いて、車両運動を推定する。例えば、公知のデッドレコニング技術を用いても良いし、カメラを用いた公知のビジュアルオドメトリ技術を用いて推定しても良いし、公知のカルマンフィルタ等を併用しても良い。後述の図１１に示す自動運転システムのように、ＧＰＳを備える場合は、その情報も活用して良い。

（ランドマーク検出部２０３）
ランドマーク検出部２０３は、センサ値取得部２０１から得られた画像および視差を用いて、ランドマークを検出する。ランドマークとはセンサにより識別可能な特徴を有する物体であり、たとえば路面ペイントの１種であるレーンマークや、停止線、横断歩道や、そのほか規制表示、車両の走行の妨げとなる障害物である建物の壁などである。本実施の形態では、移動体である車両や人間はランドマークに含めない。センサ値取得部２０１を介して外界センサ１０５から入力される情報（詳しくは、外界センサ１０５により得られた外界情報の点群データから生成した点群地図）に基づき、車両の周辺に存在するランドマーク、すなわちセンサにより識別可能な特徴を有する物体を検出する。ランドマーク情報は、画素単位で得られていても、物体としてグルーピングされて得られていても良い。ランドマークが画像認識により、例えばレーンマークであるとか、横断歩道であると、識別されていても良いし、識別されていなくても良い。ここで得られたランドマーク情報は、距離算出部２０４に出力される。

（距離算出部２０４）
距離算出部２０４は、ランドマーク検出部２０３で得られたランドマーク情報に基づき、ランドマークまでの距離を算出する。ランドマークが存在する画素ごとに、あるいはグルーピングされた物体について、距離を検出する。ステレオカメラの場合、視差値とＲＯＭ１６０に格納された内部パラメータ、カメラ間の外部パラメータから、三角測量の原理によって距離を測定できる。さらに、その距離情報から、対応する画素に映る物体の三次元座標を測定できる。これを用いて、ランドマークが存在する画素、あるいは、グルーピングされた物体について、対応する三次元座標（観測位置情報）を測定する。ただし、視差はブロックマッチング等の処理によって算出するものであり、必ず誤差が含まれる。この誤差によって、距離に誤差が発生し、物体の三次元座標には観測誤差が含まれる。この三次元座標の観測誤差は、ステレオカメラを含むカメラ全般において、原点と物体を結ぶ直線方向に発生する性質がある。他の外界センサ（距離センサ）を用いた場合、同様の誤差が発生する場合がある。誤差が大きい状態で、後述する自己地図生成部２０８によって自己地図（自己生成地図）を生成し、後述する自己位置推定部２０９で自己位置推定する場合、誤差の影響を受けて、自己位置推定の精度が低下する。したがって、この誤差を何らかの方法で、低減することが望ましい。また、この三次元座標は、センサ座標系における観測値であり、センサからの相対座標値である。ここで得られた相対座標値は、世界座標計算部２０５に出力される。

（世界座標計算部２０５）
世界座標計算部２０５は、距離算出部２０４で得られたセンサからの相対座標値となっているランドマークの距離情報（観測位置情報）を、オドメトリ推定部２０２から得られた自車の運動情報および、ＲＯＭ１６０に格納されている、カメラの取り付け位置や取り付け姿勢を示す外部パラメータを用いて、世界座標値に変換できる。この世界座標値は、ＧＰＳを備えず、デッドレコニングやビジュアルオドメトリで推定する場合は、ある座標、ある軸を基準とした世界座標値となる。例えば、車載処理装置１０１が起動した位置を原点、直後に進んだ方向をＸ軸、それに直交する軸をＹ軸、Ｚ軸といった形でとればよい。後述の図１１に示す自動運転システムのように、ＧＰＳを備える場合は、緯度、経度のような絶対世界座標値を得ることができる。ここで得られた各ランドマークの世界座標値は、ランドマーク情報蓄積部２０６に出力される。

（ランドマーク情報蓄積部２０６）
ランドマーク情報蓄積部２０６では、世界座標計算部２０５で得られた各ランドマークの測定（観測）位置情報を時系列的に蓄積する。この情報を、後述するランドマーク位置補正部２０７で活用することで、各ランドマークの位置測定誤差を低減する。ランドマーク情報蓄積部２０６では、例えば、グリッドマップと呼ばれる空間をボクセル状に区切ったマップ上に、世界座標計算部２０５で得られた各ランドマークの測定位置情報（世界座標値）を時系列的に投票していく。ここで言う投票とは、対応するボクセルに含まれる観測値が１つ得られたとき、ボクセルの値を＋１する処理である。これをセンサ値取得部２０１でセンサ値（信号）を受信するたびに繰り返していくと（すなわち、頻度分布化すると）、観測回数が多い場所と、観測回数が少ない場所で（頻度分布の）差が出てくる。そして、観測値には、ステレオカメラの場合、視差誤差に由来する測定距離誤差があるため、グリッドマップの投票値に反映され、観測物体ごとの誤差分布（すなわち、測定位置情報を頻度分布化したときに検出される誤差分布）がグリッドマップ上にあらわれる。これについては、図３Ａ、図３Ｂを用いて後に説明する。本処理ではこのように、グリッドマップへの投票のような形で、ランドマーク情報を時系列的に蓄積する。グリッドマップは２次元でも３次元でも良い。時系列的に蓄積するために、ある時刻にセンサ値取得部２０１で得られたランドマーク全てについての世界座標値の投票が完了した時点で、ＲＡＭ１３０に蓄積された情報を、ランドマーク蓄積情報１３１（図１）として格納する。次時刻において、センサ値取得部２０１でセンサ値（信号）を受信した際に、ＲＡＭ１３０から、蓄積されたランドマーク情報であるランドマーク蓄積情報１３１を読み出し、読み出した情報に、現時刻の観測値（世界座標値）を蓄積する。ランドマーク情報蓄積部２０６の処理フローについては、図４を用いて後ほど説明する。続いて、ランドマーク位置補正部２０７に進む。

（ランドマーク位置補正部２０７）
ランドマーク位置補正部２０７では、ＲＡＭ１３０に格納されたランドマーク蓄積情報１３１（図１）を用いて、ランドマークの検出の位置誤差を補正する。前述のように、ランドマーク情報蓄積部２０６でランドマーク情報を蓄積すると、観測物体ごとの誤差分布（すなわち、測定位置情報を頻度分布化したときに検出される誤差分布）が可視化できる。また、この誤差分布は、センサの特性から（距離センサの視差誤差に起因して）、センサと観測物体を結んだ直線方向（視線方向ないし光線方向）に誤差が広がる傾向を持つ。そこで、ランドマーク蓄積情報１３１（図１）に関して、センサと観測物体を結んだ直線方向（視線方向ないし光線方向）に走査し、誤差分布のピーク位置をランドマーク点群座標として検出することで、物体の真の観測位置を推定する。これについては、図３Ａ、図３Ｂを用いて後に補足説明する。推定した観測位置（＝ピーク位置）をランドマーク補正位置とする。同時に、誤差分布の大きさが大きいほど、その観測物体に関しては、観測信頼度（重要度とも称する）が低い。そこで、ランドマーク位置補正部２０７では、誤差分布の広がり（推定誤差量に対応）を取得し、対象観測物体の信頼度（重要度）として、後段の処理で活用する。ランドマーク補正位置、および、対応するランドマークの誤差分布の広がりの大きさ（推定誤差量）を、ランドマーク補正情報１３２（図１）として、ＲＡＭ１３０に格納する。ランドマーク位置補正部２０７の処理フローについては、図５を用いて後ほど説明する。次に自己地図生成部２０８に進む。

（自己地図生成部２０８）
自己地図生成部２０８では、ランドマーク蓄積情報１３１、およびランドマーク位置補正部２０７で得られたランドマーク補正情報１３２を用いて、地図（詳しくは、ランドマークの位置を補正した補正地図）を生成する。地図は、ランドマーク点に対応する点群としての表現でも良いし、認識した物体とその座標という形の表現でも良い。点群として出力する場合、自己地図は、ランドマーク補正情報１３２で得られたピーク位置情報に基づき、ランドマーク蓄積情報１３１において、ピーク位置を中心とする一定範囲のランドマークについて、ランドマーク点群として自己生成地図に出力する。

自己地図生成部２０８は、自己地図生成・自己位置推定部２５０のモードによって動作が変わる。自己地図生成モードと、自己位置推定モードの２つのモードがある。２つのモードは、自己地図生成・自己位置推定部２５０を動作させる際に何らかの形で決められているものとする。例えばインターフェース１５０（図１）から、何らかの手段でモード情報を入力するなどしても良い。

（自己地図生成モード）
自己地図生成モードは、自己地図生成を目的とするものである。自己地図生成モードでは、後段の自己位置推定部２０９は実施されない。自己地図生成部２０８は、地図保存命令を受け取ったら、ランドマーク蓄積情報１３１で得られている全範囲を、自己生成地図１４１（図１）として記憶部１４０に格納（保存）する。格納後は、後段の自己位置推定部２０９には移らず、処理終了となる。

（自己位置推定モード）
自己位置推定モードは、自己地図生成モードによって過去に記憶部１４０に格納された自己生成地図１４１（図１）を読み出して、自己生成地図１４１上での自己位置を推定することを目的とする。後段の自己位置推定部２０９において、過去に記憶部１４０に保存された自己生成地図と、現在観測値から自己位置推定モードで（自己地図生成部２０８から）出力される自己生成地図（＝補正地図）を照合する。自己位置推定モードでは、（自己地図生成部２０８から）出力される自己生成地図（＝補正地図）は、現在の自車位置から所定の距離遡った範囲の部分地図となる。部分地図とすることで、照合ミスの防止、照合時間の短縮、オドメトリ誤差の影響を減少する効果がある。部分地図を自己生成地図１３３（図１）として、ＲＡＭ１３０に格納（保存）するとともに、自己位置推定部２０９に出力する。次に自己位置推定部２０９に進む。自己地図生成部２０８の処理フローは、図６を用いて後ほど説明する。

（自己位置推定部２０９）
自己位置推定部２０９は、自己位置推定モードの場合のみ動作する。自己位置推定部２０９は、自己地図生成部２０８から得られた自己生成地図（部分地図、補正地図）１３３と、記憶部１４０に格納された、過去に生成した自己生成地図１４１を照合することで、自己生成地図１４１上の自己位置を推定する。照合は、点群の場合は、例えば、既知の点群マッチング技術であるＩＣＰ(Iterative Closest Point)アルゴリズムを利用することができる。これにより、自己地図生成部２０８から得られた自己生成地図（部分地図、補正地図）１３３を自己生成地図１４１上に変換する座標変換量を求めることができ、その座標変換量を基に座標変換された現在車両の位置から、自己生成地図１４１上の自己位置を得ることができる。推定した自己位置を出力して、自己地図生成・自己位置推定部２５０の処理は終了となる。自己位置推定部２０９の処理フローについては、図７を用いて後ほど説明する。

図３Ａは、現在観測している地点を表す図である。外界センサ１０５がカメラの場合はカメラで撮影された画像もこの図と同様のものとなる。この地点には、電柱３１０、３１１、３１２が存在する。図３Ｂは、この地点において、ランドマーク情報蓄積部２０６によって生成されたランドマーク蓄積情報１３１の様態を表す図である。この例では、理解を簡単にするために、グリッドマップは高さ方向を圧縮した２次元の場合を示している。したがって、俯瞰視点から見下ろした図となっている。図３Ｂ中の３２０、３２１、３２２は、ランドマーク蓄積情報１３１の投票値の等高線を表し、電柱３１０、３１１、３１２のそれぞれの観測結果に対応する。図３Ｂ中の３３０はセンサ位置を表す。

電柱３１０、３１１、３１２は、観測誤差が０であれば、本来は２次元グリッドマップ上であれば、電柱の径に相当する範囲にしか観測結果があらわれない。しかし、観測誤差がある場合は、ステレオカメラなどのセンサの誤差特性から（距離センサの視差誤差に起因して）、センサ位置３３０と電柱３１０、３１１、３１２を結ぶ直線方向（視線方向ないし光線方向）に分布する特性を持ち、ランドマーク蓄積情報１３１の投票値の等高線は３２０、３２１、３２２のような誤差分布を示す。センサによる誤差は、基本的には０を最頻値としてセンサ、対象物、天候、日照条件に応じて分布の幅や形が変化し、その誤差特性が図３Ｂのように、各分布のピーク位置と形に反映される。補助線３４０、３４１、３４２は、センサ位置３３０と電柱３１０に相当する観測結果を結ぶ直線、補助線３４３、３４４、３４５は、センサ位置３３０と電柱３１１に相当する観測結果を結ぶ直線、補助線３４６、３４７、３４８は、センサ位置３３０と電柱３１２に相当する観測結果を結ぶ直線で、この各直線上を走査し、ランドマーク蓄積情報１３１の投票値のピーク位置を検出することで、ランドマークの真の位置が、センサ、対象物、天候、日照条件が変わる中でも推定することができる。この各補助線は、あくまで説明のための代表線であり、もっと細かい分解能で走査、検出しても良い。

また、分布が大きいほど、誤差が大きいことを意味し、すなわち、観測値が信用できない（観測信頼度が低い）ことを意味する。したがって、この分布の大きさを各直線上で求めることで、各ランドマークの信頼度（重要度）を算出することができる。この例では、電柱３１１に対する分布３２１の誤差（推定誤差量）が最も大きいため信用できず（観測信頼度が低く）、電柱３１０に対する分布３１１の誤差（推定誤差量）が最も小さいため信用できる（観測信頼度が高い）ことになる。

ピーク検出は、走査線上のランドマーク蓄積情報１３１の投票値を平滑化したうえで公知のゼロクロス法などを使うことで実現できる。分布の大きさは、ランドマーク蓄積情報１３１の投票値が所定の値以上になるピーク位置からの幅を求めることで得られる。本例では、グリッドマップが２次元の場合を示したが、３次元の場合でも同様である。

（ランドマーク情報蓄積部２０６の動作）
図４にランドマーク情報蓄積部２０６の動作を表すフローチャートを示す。ランドマーク情報蓄積部２０６は、世界座標計算部２０５から実行指令を受けるたびに、以下の動作を実行する。以下に説明する各ステップの実行主体は演算部１２０である。

ステップＳ４０１のランドマーク蓄積情報取得部では、ＲＡＭ１３０に格納された前時刻までで得られたランドマーク蓄積情報１３１を取得する。次にステップＳ４０２に進む。

ステップＳ４０２の世界座標取得部では、世界座標計算部２０５で計算された、当該時刻のランドマークの世界座標値を取得する。次にステップＳ４０３に進む。

ステップＳ４０３のランドマーク情報追加部では、ステップＳ４０１で得られたランドマーク蓄積情報１３１と、ステップＳ４０２で得られた世界座標計算部２０５で計算された当該時刻のランドマークの世界座標値を用いて、ランドマーク蓄積情報１３１を更新する。ランドマーク蓄積情報１３１は、図３Ａ、図３Ｂの説明でも述べた通り、例えばグリッドマップ等に分割した空間に、ランドマーク点が投票された空間となっている。この空間に、世界座標計算部２０５で計算された当該時刻のランドマークの世界座標値を追加投票し、更新する。これによって、当該時刻に観測されたランドマーク情報をランドマーク蓄積情報１３１に反映することができる。更新したランドマーク蓄積情報１３１は、次時刻以降にランドマーク情報蓄積部２０６、ランドマーク位置補正部２０７で活用する目的でＲＡＭ１３０に格納し、古いものと置き換えて、図４のフローチャートを終了する。

（ランドマーク位置補正部２０７の動作）
図５にランドマーク位置補正部２０７の動作を表すフローチャートを示す。ランドマーク位置補正部２０７は、ランドマーク情報蓄積部２０６から実行指令を受けるたびに、以下の動作を実行する。以下に説明する各ステップの実行主体は演算部１２０である。

ステップＳ５０１のランドマーク蓄積情報取得部では、ランドマーク情報蓄積部２０６で計算され、ＲＡＭ１３０に格納されたランドマーク蓄積情報１３１を取得する。次にステップＳ５０２に進む。

ステップＳ５０２のランドマーク蓄積完了判定部では、蓄積が完了したランドマークグリッドを判定する。世界座標値において、センサの現在位置の測定範囲外に出た場所をランドマークの蓄積が完了したと判定する。ランドマークの蓄積が完了したグリッドが、後段の処理の対象となる。次にステップＳ５０３に進む。

ステップＳ５０３のピーク位置走査部では、図３Ａ、図３Ｂを用いて説明したように、ステップＳ５０１で取得したランドマーク蓄積情報１３１上において、各ランドマークとセンサ位置を結ぶ直線上に走査線を設定する。走査線は、ステップＳ５０２で得られたランドマークの蓄積が完了したと判定される各グリッドに対して設定される。次にステップＳ５０４に進む。

ステップＳ５０４のランドマーク出力位置計算部では、ステップＳ５０１で取得したランドマーク蓄積情報１３１上において、ステップＳ５０３で設定された走査線上を走査し、誤差分布のピーク位置を点群座標として検出する。ピーク検出は、走査線上のランドマーク蓄積情報１３１の投票値を平滑化したうえで公知のゼロクロス法などを使うことで実現できる。ピーク位置を算出したら、ランドマーク補正情報１３２として、ＲＡＭ１３０に格納する。１つの走査線から複数のピークが得られることもある。次にステップＳ５０５に進む。

ここで、本明細書におけるピーク位置とは、走査線上のランドマーク蓄積情報１３１の投票値に平滑化等の何らかの信号処理をした上での純粋な頂点位置でも良いし、走査線上のランドマーク蓄積情報１３１の投票値に平滑化等の何らかの信号処理をした上でしきい値処理し、残った座標全てでも良いし、その残った座標の平均値でも良いし、ピーク検出に関する信号処理方法や出力される点数は、ピーク位置付近の点に絞って残すものであれば良い。

ステップＳ５０５のランドマーク分布計算部では、ステップＳ５０３のピーク位置走査部で設定された走査線上を走査し、ステップＳ５０４のランドマーク出力位置計算部で得られたピーク位置ごとの誤差分布の大きさを計算する。誤差分布の大きさは、ランドマーク蓄積情報１３１の投票値が所定の値以上になるピーク位置からの幅を求めること、などで得られる。得られた各ピーク位置ごとの誤差分布の大きさは、ランドマーク補正情報１３２として、ＲＡＭ１３０に格納し、図５のフローチャートを終了する。

（自己地図生成部２０８の動作）
図６に、自己地図生成部２０８の動作を表すフローチャートを示す。自己地図生成部２０８は、ランドマーク位置補正部２０７から実行指令を受けるたびに、以下の動作を実行する。以下に説明する各ステップの実行主体は演算部１２０である。

ステップＳ６０１のランドマーク情報取得部では、ＲＡＭ１３０に格納されたランドマーク蓄積情報１３１とランドマーク補正情報１３２を取得する。次にステップＳ６０２に進む。

ステップＳ６０２の範囲設定部では、自己地図生成・自己位置推定部２５０のモードによって動作が切り替わる。自己地図生成モードの場合、ランドマーク蓄積情報１３１で生成された地図全体が対象範囲となる。自己位置推定モードの場合、現在の自車位置から所定の距離遡った範囲が対象範囲となる。したがって、ランドマーク蓄積情報１３１で生成された一部分（部分地図）が対象範囲である。これにより、後段の自己位置推定部２０９の照合ミスの防止、照合時間の短縮、オドメトリ誤差の影響を減少する効果がある。対象範囲を決めたところで、次にステップＳ６０３に進む。

ステップＳ６０３の地図出力部では、ステップＳ６０１のランドマーク情報取得部で得られたランドマーク蓄積情報１３１とランドマーク補正情報１３２、およびステップＳ６０２の範囲設定部で得られた対象範囲を用いて、ランドマーク補正情報１３２で得られたピーク位置情報に基づき、ランドマーク蓄積情報１３１において、ピーク位置を中心とする一定範囲のランドマークについて、ランドマーク点群として自己生成地図（１４１、または、１３３）に出力する。例えば、ランドマーク蓄積情報１３１においてピーク値から走査線方向を走査して、ピーク値の８０％の値をもつ範囲をランドマーク点群座標として出力する。この８０％は一例であり、実際には種々の値を設定しても良い。あるいは、範囲を狭くしてピーク値１点のみをランドマーク点群座標として出力しても良い。

自己地図生成モードの場合で、地図記録終了指令を受けた場合、得られたランドマーク点群座標を記憶部１４０に自己生成地図１４１として格納して、図６のフローを終了する。さらに、自己地図生成・自己位置推定部２５０も終了する。地図記録終了指令を受けていない場合は、得られたランドマーク点群座標を自己生成地図１３３としてＲＡＭ１３０に格納して、図６のフローを終了する。次の自己位置推定部２０９には進まず、センサ値取得部２０１に戻る。自己位置推定モードの場合、得られたランドマーク点群座標を自己生成地図１３３としてＲＡＭ１３０に格納して、図６のフローを終了する。

（自己位置推定部２０９の動作）
図７は自己位置推定部２０９の動作を表すフローチャートである。自己位置推定部２０９は、自己地図生成部２０８から実行指令を受けるたびに、以下の動作を実行する。以下に説明する各ステップの実行主体は演算部１２０である。本処理は、自己地図生成・自己位置推定部２５０の自己位置推定モードの場合のみ実行される。

ステップＳ７０１の過去地図情報取得部では、記憶部１４０に格納された自己生成地図１４１を取得する。自己生成地図１４１は、過去の走行により取得された地図である。取得したら次のステップＳ７０２に進む。

ステップＳ７０２の現在地図情報取得部では、自己地図生成部２０８によってＲＡＭ１３０に格納された、現在走行中に観測された自己生成地図（部分地図、補正地図）１３３を取得する。取得したら次のステップＳ７０３に進む。

ステップＳ７０３の荷重計算部では、ＲＡＭ１３０に格納された、ランドマーク補正情報１３２を取得し、ランドマーク補正情報１３２がもつ各ピーク位置ごとの誤差分布の大きさ情報を取得する。この誤差分布の大きさ情報（推定誤差量に対応）に基づき、自己位置推定する際の各ランドマークの重み情報（重要度または信頼度）を計算する。設定方針としては、分布が大きければ大きいほど、ランドマークの重みとしては小さく設定する。分布の大きさに対する重みの計算式は、方針を守りつつ、自由に設定しても良い。次にステップＳ７０４に進む。

ステップＳ７０４の地図照合部では、ステップＳ７０１の過去地図情報取得部で取得した過去の自己生成地図１４１と、ステップＳ７０２の現在地図情報取得部で取得した現在走行中に観測された自己生成地図（部分地図、補正地図）１３３のランドマークを照合することで、過去の自己生成地図１４１上での現在走行中の自己位置を計算する。照合は、点群の場合は、例えば、既知の点群マッチング技術であるＩＣＰ(Iterative Closest Point)アルゴリズムを利用することができる。ＩＣＰアルゴリズムを利用する際、ステップＳ７０３の荷重計算部で得られたランドマークごとの重み（重要度または信頼度）を反映させて自己位置を計算する。ＩＣＰアルゴリズムでは、各点同士の対応関係を求め、その対応関係における距離誤差を最小化する処理を繰り返すことにより、点群同士の照合を行っていくが、この際、最小化の距離誤差関数にこの重みを反映させる。重みが大きい点の誤差は、相対的に大きな値として計算されることになり、それを最小化しようとする力が働くため、大きな重みが付された点は、他の点に比べて誤差を小さくする働きが強く働く。これにより、大きな重みが付された信頼できる点ほど、誤差を小さくするように働く。この地図同士の照合により、自己地図生成部２０８から得られた自己生成地図（部分地図、補正地図）１３３を自己生成地図１４１上に変換する座標変換量を求めることができ、座標変換量を基に座標変換された現在車両の位置から、自己生成地図１４１上の自己位置を得ることができる。自己生成地図１４１上の自己位置を出力して図７のフローを終了する。終了後はまた、センサ値取得部２０１に戻る。

（作用効果）
上述した第１の実施の形態によれば、次の作用効果が得られる。
すなわち、上述した第１の実施の形態の自己地図生成・自己位置推定装置１００は、自車両に搭載された車載センサにより得られた外界情報の点群データから点群地図を生成するもので、前記点群地図からランドマークを検出するランドマーク検出部２０３と、検出した前記ランドマークの観測位置情報を蓄積するランドマーク情報蓄積部２０６と、前記ランドマークの観測位置情報を頻度分布化したときに検出される誤差分布から抽出される点群座標に基づいて（詳しくは、前記ランドマークの観測位置情報を頻度分布化したときに検出される誤差分布のうちピーク位置を点群座標として抽出し、前記ピーク位置に基づいて）前記ランドマークの位置を補正するランドマーク位置補正部２０７と、前記ランドマーク位置補正部２０７から得られた前記ランドマークの位置を補正した補正地図を自己生成地図として（記憶部１４０またはＲＡＭ１３０に）保存する自己地図生成部２０８とを備える。

また、前記車載センサは、複数のカメラから構成される距離センサ（例えばステレオカメラ）であって、前記誤差分布は、前記距離センサの視差誤差に起因して視線方向（光線方向）に発生するものである。そして、前記ランドマーク位置補正部２０７は、前記距離センサの視線方向（光線方向）に走査して、前記誤差分布のピーク位置を点群座標として抽出する。

また、自己地図生成・自己位置推定装置１００は、保存された前記自己生成地図と前記自車両の現在走行中に新たに前記ランドマーク位置補正部２０７から得られた前記補正地図とを照合することで前記自車両の現在走行中の自己位置推定を行う自己位置推定部２０９を備える。

また、前記自己位置推定部２０９は、前記ランドマーク位置補正部２０７から得られた前記補正地図または前記自己生成地図の少なくとも一方から得られた推定誤差量（各ピーク位置ごとの誤差分布の大きさ情報）を用いて、前記ランドマークの重要度（信頼度）を計算し（Ｓ７０３：荷重計算部）、得られた前記重要度（信頼度）を反映して自己位置推定を行う。

そのため、観測点となるランドマークの誤差量を見積もり、自己生成点群地図の質を改善することで、センサによる観測誤差が大きい場合でも、高精度な自己位置推定が可能となる。

［第２の実施の形態］
図８は、本発明の第２の実施の形態にかかる自己地図生成・自己位置推定装置１００の構成を示すブロック図である。自己地図生成・自己位置推定装置１００は、センサ群（１０６、１０７、１１０、１１１）と、車載処理装置１０１とから構成される。車載センサであるセンサ群は、車載処理装置１０１と信号線で接続され、車載処理装置１０１と各種データを授受する。

車載処理装置１０１は、演算部１２０と、ＲＡＭ１３０と、記憶部１４０と、インターフェース１５０と、ＲＯＭ１６０を備える。演算部１２０は、センサ協調誤差推定部１２７を持つ点が、図１の第１の実施の形態と異なる。また、センサ群として、車両の周囲（外界情報）を観測する異なる２種類の外界センサであるステレオカメラ１０６と単眼カメラ（以下、単にカメラと称する場合がある）１０７を同時に持つ点が、図１の第１の実施の形態と異なる。この実施の形態では、ステレオカメラ１０６と単眼カメラ１０７であるが、他の異なる２種類のセンサの組み合わせでも良い。

（機能ブロック構成）
図９は、複数の外界センサ搭載時の自己地図生成・自己位置推定装置１００の動作を表す機能ブロック図である。演算部１２０が実行するオドメトリ計算部１２１、ランドマーク検出部１２２、ランドマーク情報蓄積部１２３、センサ協調誤差推定部１２７、自己地図生成・自己位置推定部２５０が有する機能を機能ブロックとして表し、機能ブロックの処理順、また、機能ブロック同士、および機能ブロックとＲＡＭ１３０、ＲＯＭ１６０、記憶部１４０とのデータの流れを示す。オドメトリ計算部１２１、ランドマーク検出部１２２、ランドマーク情報蓄積部１２３、自己地図生成・自己位置推定部２５０は、図１の第１の実施の形態で示したものと同一のものであるため、説明は省略する。ただし、自己地図生成・自己位置推定部２５０の外界センサはステレオカメラ１０６とする。図９のセンサ値取得部２０１は、カメラ１０７の情報を取得するものとする。

（センサ協調誤差推定部２２０）
センサ協調誤差推定部２２０は、ランドマーク情報蓄積部２０６から得たカメラ１０７によるランドマーク蓄積情報１３１と、自己地図生成・自己位置推定部２５０から得たステレオカメラ１０６によるランドマーク補正情報１３２（ステレオカメラ１０６に基づき作成した補正地図に対応）を用いて、カメラ１０７から得たランドマーク蓄積情報１３１の誤差量（ランドマーク検出誤差量）を推定するものである。例えば単眼カメラ１０７のモーションステレオ法の場合、実際にはランドマーク情報を密に得られず、ランドマーク蓄積情報１３１の投票が集まらず、正確なピーク位置や誤差分布の大きさを得ることが難しい場合がある。一方、ステレオカメラ１０６はランドマーク情報を密に取得することができ、精度も良いので、このように精度の良いセンサであるステレオカメラ１０６から得たランドマーク補正情報１３２を活用して、精度の低いセンサであるカメラ１０７から得たランドマーク蓄積情報１３１の誤差量を見積もるものである。ランドマーク蓄積情報１３１の誤差量を見積もることができれば、ランドマークの重要度（信頼度）を計算することができ、上述したように自己位置推定時のランドマークごとの重み（重要度ないし信頼度）として反映させることができて、自己位置推定精度が高まる。ただし、この処理は、センサ同士で共通物体を観測できていることが前提となる。共通観測物体について、精度の高いほうのセンサで推定したランドマーク補正情報１３２から得られるランドマーク補正位置を真値とみなし、精度の低いほうのセンサで取得した共通観測物体の観測値の観測位置の真値からの距離の偏差を、精度が低いほうのセンサでの誤差量とする。

（センサ協調誤差推定部２２０の動作）
図１０はセンサ協調誤差推定部２２０の動作を表すフローチャートである。センサ協調誤差推定部２２０は、ランドマーク情報蓄積部２０６から実行指令を受けるたびに、以下の動作を実行する。以下に説明する各ステップの実行主体は演算部１２０である。

ステップＳ４０１のランドマーク蓄積情報取得部は、図４のものと同様で、ＲＡＭ１３０に格納されたカメラ１０７から得たランドマーク蓄積情報１３１を取得する。次にステップＳ１００２に進む。

ステップＳ１００２のランドマーク補正情報取得部では、ＲＡＭ１３０に格納されたステレオカメラ１０６から得られたランドマーク補正情報１３２（補正地図）を取得する。次にステップＳ１００３に進む。

ステップＳ１００３の共通物体判定部では、ステレオカメラ１０６とカメラ１０７で共通に観測された物体を判定する。センサ同士で共通視野を持っていることが前提となる。所定の既知の画像認識手法でもって判定する。観測範囲中の全ての共通物体の判定が完了したら、次にステップＳ１００４に進む。

ステップＳ１００４のランドマーク出力位置協調計算部では、カメラ１０７から得られたランドマーク蓄積情報１３１と、ステレオカメラ１０６から得られたランドマーク補正情報１３２（補正地図）と、ステップＳ１００３の共通物体判定部で得られた共通物体判定結果を用いて、精度が低いほうのセンサであるカメラ１０７のランドマーク位置を補正する。精度が高いほうのセンサであるステレオカメラ１０６で得られたランドマーク補正情報１３２において、共通物体と判定されたランドマークのピーク位置を真の物体検出座標とする。カメラ１０７から得られたランドマーク蓄積情報１３１のピーク位置は無視される。次にステップＳ１００５に進む。

ステップＳ１００５のランドマーク分布協調計算部では、ステップＳ１００４で得られたランドマーク補正情報１３２と、カメラ１０７から得られたランドマーク蓄積情報１３１を用いて、ランドマークの誤差分布を推定する。ランドマーク蓄積情報１３１のうち、各ランドマーク補正位置から所定距離内にある投票値が所定の値以上をもつグリッドについて、ランドマーク補正位置からの誤差を測定し、頻度分布化する。この頻度分布の幅をランドマークの推定誤差分布（ランドマーク検出誤差量に対応）とする。

（作用効果）
この第２の実施の形態によれば、次の作用効果が得られる。
すなわち、上述した第２の実施の形態の自己地図生成・自己位置推定装置１００は、前記車載センサ（例えばステレオカメラ１０６）に基づき作成した前記補正地図を用いて、前記車載センサと異なる他のセンサ（例えば単眼カメラ１０７）により検出した前記ランドマーク（の観測位置情報）の検出誤差量を推定するセンサ協調誤差推定部２２０をさらに備える。

そのため、複数の外界センサがあり、共通の観測範囲をもつ場合に、精度が高いほうのセンサを用いて、精度が低いほうのセンサのランドマーク位置を補正し、ランドマークの誤差分布（検出誤差量）を推定することができるため、自己生成地図の精度を改善し、誤差分布（検出誤差量）を用いることで、自己位置推定の精度を高めることができる。

［第３の実施の形態］
図１１は、本発明の第３の実施の形態にかかる自己地図生成・自己位置推定装置１００を備える自動運転装置１０２の構成を示すブロック図である。自動運転装置１０２は、自己地図生成・自己位置推定装置１００の変形構成と、通信装置１１３と、表示装置１１４と、車両を制御する車両制御装置群（１７０～１７３）とを備える。

自己地図生成・自己位置推定装置１００は、センサ群（１０５、１１０、１１１、１１２）と、車載処理装置１０１とから構成される。車載センサであるセンサ群、車両制御装置群、通信装置１１３、表示装置１１４は、車載処理装置１０１と信号線で接続され、車載処理装置１０１と各種データを授受する。自己地図生成・自己位置推定装置１００は、ＧＰＳ受信機１１２を備える点のみ、図１の第１の実施の形態と異なる。

ＧＰＳ受信機１１２は、衛星航法システムを構成する複数の衛星から信号を受信し、受信した信号に基づく演算によりＧＰＳ受信機１１２の位置、すなわち緯度および経度を算出する。なお、ＧＰＳ受信機１１２により算出される緯度および経度の精度は高精度でなくてもよく、たとえば数ｍ～１０ｍ程度の誤差が含まれてもよい。ＧＰＳ受信機１１２は、算出した緯度および経度を車載処理装置１０１に出力する。

通信装置１１３は、車両の外部の機器と車載処理装置１０１とが無線で情報を授受するために用いられる。例えば、ユーザが車両の外にいるときに、ユーザが身に着けている携帯端末と通信を行い情報を授受する。通信装置１１３が通信を行う対象は、ユーザの携帯端末に限定されない。

表示装置１１４は、たとえば液晶ディスプレイであり、車載処理装置１０１から出力される情報が表示される。

車両制御装置１７０は、車載処理装置１０１から出力される情報、例えば、自己位置推定部１２６から出力される、自己生成地図１４１上の現在（走行中）の自己位置に基づいて、操舵装置１７１、駆動装置１７２、および制動装置１７３を制御する。操舵装置１７１は、車両のステアリングを操作する。駆動装置１７２は、車両に駆動力を与える。駆動装置１７２はたとえば、車両が備えるエンジンの目標回転数を増加させることにより車両の駆動力を増加させる。制動装置１７３は、車両に制動力を与える。

自動運転装置１０２において、記憶部１４０に格納された自己生成地図１４１は、通信装置１１３を介して、サーバ等に格納（保存）し、他の車両が活用可能としてもよい。また、他の車両に搭載された自動運転装置１０２によって通信装置１１３を介してサーバに格納された自己生成地図１４１を、通信装置１１３を介して取得して記憶部１４０に格納し、自車両の自己生成地図として自己位置推定に活用してもよい。

自動運転装置１０２において、ＧＰＳ受信機１１２の各時刻の受信位置（推定絶対座標）を自己生成地図に付しておき、自己位置推定する際には、現在ＧＰＳ値と近い値をもつ地図があるかを探索して、照合する地図を選択する。ＧＰＳが受信できない場合も、オドメトリ計算部１２１内で、ＧＰＳを参照して絶対位置（絶対座標）を推定する形とし、直近で観測されたＧＰＳ値から推定した絶対位置（絶対座標）を持つ形とする。

自動運転装置１０２（の車両制御装置１７０）は、自己地図生成・自己位置推定装置１００によって生成された自己生成地図１４１と、自己生成地図１４１上の自己位置を当該自己地図生成・自己位置推定装置１００が備える自己位置推定部１２６によって推定し、推定された自己位置とに従って、当該車両を自動運転する。

（作用効果）
この第３の実施の形態によれば、次の作用効果が得られる。
すなわち、上述した第３の実施の形態の自己地図生成・自己位置推定装置１００は、前記自己地図生成・自己位置推定装置１００によって生成された前記自己生成地図と、前記自己生成地図上の自己位置を前記自己地図生成・自己位置推定装置１００が備える前記自己位置推定部１２６（２０９）によって推定した前記自己位置とに従って、前記自車両を自動運転する車両制御装置１７０（自動運転装置１０２）を備える。

また、前記自己地図生成・自己位置推定装置１００は、前記自己生成地図１４１を他車両が活用可能とするため、前記自己生成地図１４１を通信装置１１３を介して、サーバに保存する、または、他車両が生成した前記自己生成地図１４１を通信装置１１３を介して取得し、自車両の前記自己生成地図１４１として活用可能である。

また、前記自己地図生成・自己位置推定装置１００は、前記自己生成地図に、ＧＰＳとオドメトリから推定した絶対座標を付与する。

この第３の実施の形態によれば、高精度で推定された自己位置を基に、高精度な車両の自動運転が可能となる。また、自己生成地図をサーバを介して他の車両と共有することで、情報（走行経験）が少ない場合でも、高精度な自己位置推定が可能となる。

なお、本発明は上記した実施形態に限定されるものではなく、様々な変形形態が含まれる。例えば、上記した実施形態は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。本発明の技術的思想の範囲内で考えられるその他の態様も本発明の範囲内に含まれる。また、ある実施形態の構成の一部を他の実施形態の構成に置き換えることが可能であり、また、ある実施形態の構成に他の実施形態の構成を加えることも可能である。また、各実施形態の構成の一部について、他の構成の追加・削除・置換をすることが可能である。また、自己地図生成・自己位置推定装置１００が不図示の入出力インターフェースを備え、必要なときに入出力インターフェースと自己地図生成・自己位置推定装置１００が利用可能な媒体を介して、他の装置からプログラムが読み込まれてもよい。ここで媒体とは、たとえば入出力インターフェースに着脱可能な記憶媒体、または通信媒体、すなわち有線、無線、光などのネットワーク、または当該ネットワークを伝搬する搬送波やディジタル信号を指す。また、プログラムにより実現される機能の一部または全部がハードウェア回路やＦＰＧＡにより実現されてもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ(Solid State Drive)等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。

１００…自己地図生成・自己位置推定装置
１０１…車載処理装置
１０２…自動運転装置
１０５…外界センサ
１０６…ステレオカメラ
１０７…カメラ
１１０…車速センサ
１１１…舵角センサ
１１２…ＧＰＳ受信機
１１３…通信装置
１１４…表示装置
１２０…演算部
１２１…オドメトリ計算部
１２２…ランドマーク検出部
１２３…ランドマーク情報蓄積部
１２４…ランドマーク位置補正部
１２５…自己地図生成部
１２６…自己位置推定部
１２７…センサ協調誤差推定部
１３０…ＲＡＭ
１３１…ランドマーク蓄積情報
１３２…ランドマーク補正情報
１３３…自己生成地図
１４０…記憶部
１４１…自己生成地図
１５０…インターフェース
１６０…ＲＯＭ
１７０…車両制御装置
１７１…操舵装置
１７２…駆動装置
１７３…制動装置
２０１…センサ値取得部
２０２…オドメトリ推定部
２０３…ランドマーク検出部
２０４…距離算出部
２０５…世界座標計算部
２０６…ランドマーク情報蓄積部
２０７…ランドマーク位置補正部
２０８…自己地図生成部
２０９…自己位置推定部
２２０…センサ協調誤差推定部
２５０…自己地図生成・自己位置推定部

Claims (8)

1. 自車両に搭載された車載センサにより得られた外界情報の点群データから点群地図を生成し、前記点群地図上の前記自車両の位置である自己位置を推定する自己位置推定装置であって、
   前記点群地図からランドマークを検出するランドマーク検出部と、
   検出した前記ランドマークの観測位置情報を蓄積するランドマーク情報蓄積部と、
   前記ランドマークの観測位置情報を頻度分布化したときに検出される誤差分布から抽出される点群座標に基づいて前記ランドマークの位置を補正し、前記誤差分布の広がりの大きさに基づいて前記ランドマークの重要度を計算するランドマーク位置補正部と、
   前記ランドマーク位置補正部から得られた前記ランドマークの位置を補正した補正地図を自己生成地図として保存する自己地図生成部と、
   保存された前記自己生成地図と前記自車両の現在走行中に新たに前記ランドマーク位置補正部から得られた前記補正地図とを前記重要度で重み付けして照合することで前記自車両の現在走行中の自己位置推定を行う自己位置推定部と、を備えることを特徴とする自己位置推定装置。
2. 請求項１に記載の自己位置推定装置であって、
   前記ランドマーク位置補正部は、前記ランドマークの観測位置情報を頻度分布化したときに検出される誤差分布のうちピーク位置を点群座標として抽出し、前記ピーク位置に基づいて前記ランドマークの位置を補正し、前記ピーク位置ごとの前記誤差分布の広がりの大きさに基づいて前記ランドマークの重要度を計算することを特徴とする自己位置推定装置。
3. 請求項１に記載の自己位置推定装置であって、
   前記車載センサは、複数のカメラから構成される距離センサであって、前記誤差分布は、前記距離センサの視差誤差に起因して視線方向に発生するものであることを特徴とする自己位置推定装置。
4. 請求項３に記載の自己位置推定装置であって、
   前記ランドマーク位置補正部は、前記距離センサの視線方向に走査して、前記誤差分布のピーク位置を点群座標として抽出することを特徴とする自己位置推定装置。
5. 請求項１に記載の自己位置推定装置であって、
   前記車載センサに基づき作成した前記補正地図を用いて、前記車載センサと異なる他のセンサにより検出した前記ランドマークの検出誤差量を推定するセンサ協調誤差推定部をさらに備えることを特徴とする自己位置推定装置。
6. 請求項１に記載の自己位置推定装置であって、
   前記自己位置推定装置は、前記自己生成地図を他車両が活用可能とするため、前記自己生成地図を通信装置を介して、サーバに保存すること、または、他車両が生成した前記自己生成地図を通信装置を介して取得し、自車両の前記自己生成地図として活用可能であることを特徴とする自己位置推定装置。
7. 請求項１に記載の自己位置推定装置であって、
   前記自己位置推定装置は、前記自己生成地図に、ＧＰＳとオドメトリから推定した絶対座標を付与することを特徴とする自己位置推定装置。
8. 請求項１に記載の自己位置推定装置であって、
   前記自己地図生成部によって生成された前記自己生成地図と、前記自己生成地図上の自己位置を前記自己位置推定装置が備える前記自己位置推定部によって推定した前記自己位置とに従って、前記自車両を自動運転する車両制御装置を備えることを特徴とする自己位置推定装置。

Images