JP6843712B2 - 車載処理装置 - Google Patents

Description

本発明は、車載処理装置に関する。

車両の挙動を制御して所定の駐車位置まで自動走行する自動駐車装置や駐車支援装置が提案されている。

特許文献１には、過去に運転者が前記車両を前記駐車位置へと駐車させた際に前記車両に搭載された撮像手段により撮像された車両周囲の画像内の代表ポイントの軌跡を、そのときの運転者による運転操作に関する情報と対応付けて記憶する記憶手段と、前記駐車位置への誘導を行う際に、前記撮像手段により撮像される車両周囲の画像内の代表ポイントを検出し、検出した現在の代表ポイントが、前記記憶手段に記憶されている過去の代表ポイントの軌跡上に沿うように、前記車両の挙動を制御する制御手段を備えることを特徴とする駐車支援装置が開示されている。

特開2007-055378号公報

特許文献１においては、「記憶モード」と「駐車モード」と「通常モード」の３つの
動作モードから、自車乗員の操作入力によってモード選択するが、モード選択時に車両を一旦停車する必要があり、操作がわずらわしい。仮に走行中に実施する場合には、乗員１名のとき運転車が走行中に操作することになり危険が生じる。

本発明は、上記の点に鑑みてなされたものであり、その目的とするところは、モードの選択を自動で行う車載処理装置を提供することである。

上記課題を解決する本発明の車載処理装置は、
物体の一部を表す点の第１座標系における座標が複数含まれる点群データが経度緯度の測位情報と対応付けて格納される記憶部と、
車両の周囲の情報を取得するセンサの出力を取得するセンサ入力部と、
前記車両の移動に関する情報を取得する移動情報取得部と、
前記センサ入力部、および前記移動情報取得部が取得する情報に基づき、第２座標系における前記車両の位置および物体の一部を表す点の前記第２座標系における座標が複数含まれる局所周辺情報を生成する局所周辺情報作成部と、
前記点群データと前記局所周辺情報とに基づき前記第１座標系と前記第２座標系の関係を推定し、前記第１座標系における前記車両の位置を推定する位置推定部と、
予め設定された登録地点に前記車両が接近しかつ前記車両の周囲の点群データが前記記憶部に格納されていない場合に前記点群データを取得して前記記憶部に記憶する地図記憶モードと、前記登録地点に前記車両が接近しかつ前記車両の周囲の点群データが前記記憶部に格納されている場合に前記位置推定部により前記車両の位置を推定する位置推定モードとを、前記車両の緯度経度の測位情報に基づいて遷移させるモード切り替え部を備えることを特徴とする。

本発明によれば、自車乗員の操作入力なしで、走行中に自動的に適切なモードに切り替えることが可能となり、地図記憶や駐車前に自車両を停車して操作入力するわずらわしさから解放される。したがって、ドライバーによる手動運転から自動運転や自動駐車への切り替えが非常にスムーズになる。

本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。また、上記した以外の、課題、構成及び効果は、以下の実施形態の説明により明らかにされる。

本発明にかかる車載処理装置を含む自動駐車システム１００の構成図。 点群地図１２４Ａの一例を示す図。 車載処理装置１２０の動作モードとその遷移条件を示す図。 車載処理装置１２０の地図記憶モードの動作を表すフローチャート。 車載処理装置１２０の位置推定モードの動作を表すフローチャート。 自動駐車フェーズの自己位置推定処理を表すフローチャート。 自動駐車フェーズの局所周辺情報選定処理を表すフローチャート。 自動駐車フェーズのマッチング処理を表すフローチャート。 自動駐車フェーズの自動駐車処理を表すフローチャート。 図９（ａ）は駐車場９０１の一例を示す平面図、図９（ｂ）はＲＡＭ１２２に保存されるランドマークの点群を可視化した図。 図１０（ａ）は点群地図１２４Ａの点群データを可視化した一例を示す図、図１０（ｂ）は新たに検出した点群データを可視化した一例を示す図。 駐車場９０１における車両１の現在位置を示す図。 車両１が図１１に示した位置においてセンサ群によって抽出された点群を駐車場座標に変換したデータを示す図。 車両１の駐車場座標系における位置の推定が誤差を含んでいた場合の、点群地図１２４Ａと図１１に示した局所周辺情報１２２Ｂとの対比を示す図。 図１４（ａ）〜（ｃ）は、図１３に示した局所周辺情報１２２Ｂを駐車枠の幅の整数倍移動させた場合の点群地図１２４Ａとの関係を示す図。 図１５（ａ）〜（ｃ）は、旋回時誤差が生じた場合のマッチング方法を説明する図。

以下、図１〜図１５を参照して、本発明にかかる車載処理装置の一実施形態を説明する。

図１は、本発明にかかる車載処理装置を含む自動駐車システム１００の構成図である。自動駐車システム１００は、車両１に搭載される。自動駐車システム１００は、センサ群１０２〜１０４、１０７〜１０９と、入出力装置群１１０、１１１、１１４と、車両１を制御する制御装置群１３０〜１３３と、車載処理装置１２０とから構成される。センサ群、入出力装置群、および制御装置群は車載処理装置１２０と信号線で接続され、車載処理装置１２０と各種データを授受する。

車載処理装置１２０は、演算部１２１と、ＲＡＭ１２２と、ＲＯＭ１２３と、記憶部１２４と、インタフェース１２５とを備える。演算部１２１はＣＰＵである。演算部１２１は、ＦＰＧＡなど他の演算処理装置で全部または一部の演算処理を実行するように構成してもよい。ＲＡＭ１２２は読み書き込み可能な記憶領域であり、車載処理装置１２０の主記憶装置として動作する。ＲＡＭ１２２には、後述するアウトライアリスト１２２Ａ、および後述する局所周辺情報１２２Ｂが格納される。ＲＯＭ１２３は読み取り専用の記憶領域であり、後述するプログラムが格納される。このプログラムはＲＡＭ１２２で展開されて演算部１２１により実行される。演算部１２１は、プログラムを読み込んで実行することにより、点群取得部１２１Ａ、地図生成部１２１Ｂ、位置推定部１２１Ｃおよび、モード切り替え部１２１Ｄとして動作する。

記憶部１２４は不揮発性の記憶装置であり、車載処理装置１２０の補助記憶装置として動作する。記憶部１２４には、点群地図１２４Ａが格納される。点群地図１２４Ａとは、例えば１または複数の駐車場データである。駐車場データとは、ある駐車場の位置情報、すなわち緯度・経度、駐車領域を示す座標、およびその駐車場に存在するランドマークを構成する点の座標の集合である。ランドマークについては後述する。インタフェース１２５は、車載処理装置１２０と自動駐車システム１００を構成する他の機器との情報の授受を行う。

センサ群は、車両１の周囲を撮影するカメラ１０２と、車両１の周囲の物体有無を観測するソナー１０３やレーダ１０４と、カーナビゲーションシステム１０７と、車速センサ１０８と、舵角センサ１０９とを含む。

カメラ１０２は、撮影して得られた画像（以下、撮影画像）を車載処理装置１２０に出力する。車載処理装置１２０は、カメラ１０２の撮影画像を用いて後述するランドマークの測位を行う。カメラ１０２の焦点距離や撮像素子サイズなどの内部パラメータ、および車両１へのカメラ１０２の取り付け位置や取り付け姿勢である外部パラメータは既知であり、ＲＯＭ１２３にあらかじめ保存されている。車載処理装置１２０は、ＲＯＭ１２３に格納された内部パラメータおよび外部パラメータを用いて、被写体とカメラ１０２との位置関係を算出することができる。

ソナー１０３、レーダ１０４はそれぞれ特定波長の音波や電波を放射し、物体に反射して戻ってくる反射波を受波するまでの時間を計測することで、反射物位置を観測する。ソナー１０３、レーダ１０４は、反射物位置を点群として観測し、車載処理装置１２０に出力する。

カーナビゲーションシステム１０７は、ＧＰＳ受信機１０７Ａと記憶部１０７Ｂとを含む。記憶部１０７Ｂは、不揮発性の記憶装置であり、道路地図１０７Ｃを記憶する。道路地図１０７Ｃは、道路接続構造に関する情報と、道路位置に対応する緯度経度を含む。カーナビゲーションシステム１０７では、ＧＰＳ受信機１０７Ａによる位置情報と、対応する道路地図１０７Ｃを照合し、自車が走行している道路位置（ナビ自己位置）の緯度、経度を算出する。なおカーナビゲーションシステム１０７により算出される緯度および経度の精度は高精度でなくてもよく、たとえば数ｍ〜１０ｍ程度の誤差が含まれてもよい。カーナビゲーションシステム１０７は、算出した緯度、経度、および道路識別番号を車載処理装置１２０に出力する。

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

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

入力装置１１０には、車載処理装置１２０へのユーザによる動作指令が入力される。入力装置１１０は、応答ボタン１１０Ａと、自動駐車ボタン１１０Ｂとを含む。表示装置１１１はたとえば液晶ディスプレイであり、車載処理装置１２０から出力される情報が表示される。なお、入力装置１１０と表示装置１１１とが一体として、たとえばタッチ操作に対応する液晶ディスプレイとして構成されてもよい。この場合は、液晶ディスプレイの所定の領域がタッチされることにより、応答ボタン１１０Ａ、あるいは自動駐車ボタン１１０Ｂが押し操作されたと判断してもよい。

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

車両制御装置１３０は、車載処理装置１２０の動作指令に基づき、操舵装置１３１、駆動装置１３２、および制動装置１３３を制御する。操舵装置１３１は、車両１のステアリングを操作する。駆動装置１３２は、車両１に駆動力を与える。駆動装置１３２はたとえば、車両１が備えるエンジンの目標回転数を増加させることにより車両１の駆動力を増加させる。制動装置１３３は、車両１に制動力を与える。

（ランドマーク測位）
ランドマークとはセンサにより識別可能な特徴を有する物体であり、たとえば路面ペイントの１種である駐車枠線や、車両の走行の妨げとなる障害物である建物の壁などである。本実施の形態では、移動体である車両や人間はランドマークに含めない。車載処理装置１２０は、カメラ１０２と、ソナー１０３と、レーダ１０４の少なくとも一つから入力される情報に基づき、車両１の周辺に存在するランドマーク、すなわちセンサにより識別可能な特徴を有する点を検出する。以下では、外界センサから入力される情報、すなわちカメラ１０２と、ソナー１０３と、レーダ１０４の少なくとも一つから入力される情報に基づくランドマークの検出を「ランドマーク測位」と呼ぶ。

車載処理装置１２０は、カメラ１０２の撮影画像を対象に、以下のように画像認識プログラムを動作させることで駐車枠などの路面ペイントなどを検出する。駐車枠の検出は、まず入力画像からソーベルフィルタなどでエッジを抽出する。次に、たとえば白から黒への変化であるエッジの立ち上がりと、黒から白への変化であるエッジの立ち下がりのペアを抽出する。そしてこのペアの間隔が、あらかじめ定めた第１の所定の距離、すなわち駐車枠を構成する白線の太さと略一致すると、このペアを駐車枠の候補とする。同様の処理により駐車枠の候補を複数検出し、駐車枠の候補同士の間隔があらかじめ定めた第２の所定の距離、すなわち駐車枠の白線の間隔と略一致すると、それらを駐車枠として検出する。駐車枠以外の路面ペイントは以下の処理を実行する画像認識プログラムで検出される。まず入力画像からソーベルフィルタなどでエッジを抽出する。エッジ強度があらかじめ定めた一定の値よりも大きく、エッジ同士の間隔が白線の幅に相当するあらかじめ定めた距離である画素を探索することにより検出できる。

車載処理装置１２０はたとえば既知のテンプレートマッチングにより車両や人間を検出し、測定結果から除外する。また、以下のようにして検出した移動体を測定結果から除外してもよい。すなわち車載処理装置１２０は、内部パラメータおよび外部パラメータを用いて撮影画像における被写体とカメラ１０２との位置関係を算出する。次に車載処理装置１２０は、カメラ１０２が連続して取得した撮影画像において被写体を追跡することで車両１と被写体の相対的な速度を算出する。最後に車載処理装置１２０は、車速センサ１０８および舵角センサ１０９の出力を用いて車両１の速度を算出し、被写体との相対的な速度と一致しなければ被写体が移動体であると判断し、この移動体に関する情報を測定結果から除外する。

（点群地図１２４Ａ）
図２は、記憶部１２４に格納される点群地図１２４Ａの一例を示す図である。図２では、点群地図１２４Ａとして２つの駐車場データが格納されている例を示している。各駐車場データは、駐車領域の座標と、ランドマークを構成する点の二次元平面上の座標と、記憶時に走行した経路座標、さらに各座標と対応する緯度経度から構成される。駐車領域の座標、およびランドマークを構成する点の座標は、その駐車場データに固有の座標系（第１座標系）における座標である。以下では駐車場データにおける座標系を「駐車場座標系」と呼ぶ。駐車場座標系は、たとえば記録開始時の車両１の座標が原点とされ、記録開始時の車両１の進行方向がＹ軸、記録開始時の車両１の右方向がＸ軸とされる。駐車領域の座標は、たとえば駐車領域を矩形とした場合にその矩形領域の４つの頂点の座標として記録される。ただし、駐車領域は矩形に限定されず矩形以外の多角形や楕円形状でもよい。

（アウトライアリスト１２２Ａ）
アウトライアリスト１２２Ａには、車載処理装置１２０が処理対象外とする局所周辺情報１２２Ｂの点の情報が格納される。アウトライアリスト１２２Ａは、後述するように車載処理装置１２０により適宜更新される。

（局所周辺情報１２２Ｂ）
局所周辺情報１２２Ｂには、車載処理装置１２０が後述する位置推定モード２０３において検出したランドマークを構成する点の座標が格納される。この座標は局所周辺情報１２２Ｂの記録を開始した際の車両１の位置および姿勢を基準に、たとえばその位置を原点とし、車両１の進行方向をＹ軸とし、進行方向右をＸ軸とする座標系（第２座標系）における座標である。この座標系を以下では「局所座標系」と呼ぶ。

（車載処理装置１２０の動作概要）
車載処理装置１２０の動作概要を説明する。図３は車載処理装置１２０がもつ動作モードとその遷移条件を説明する図である。この図が説明する動作は、車載処理装置１２０におけるモード切り替え部１２１Ｄの動作と等しい。車載処理装置１２０は、主に５つの動作モード、すなわち通常走行モード２０１と、地図記憶モード２０２と、位置推定モード２０３と、自動駐車モード２０４と、ギブアップモード２０５とを有する。車載処理装置１２０は、通常走行モード２０１からモード切り替えの制御を開始する。

通常走行モード２０１ではユーザにより車両１が運転される。この際、点群地図の生成や点群地図上での自己位置推定は実施せず、モードの遷移条件の監視のみ行う。モードの遷移は、あらかじめ登録した駐車したい地点付近（登録地点）の緯度経度と、カーナビゲーションシステム１０７から取得した自己位置の緯度経度情報（ナビ自己位置）を比較し、登録地点を中心とする一定範囲以内に自車が進入した場合、例えば、ナビ自己位置が登録地点の１００ｍ範囲以内に進入した場合に、モード遷移する。

現在走行中の地点付近の点群地図が登録されていない場合は、地図記憶モード２０２に遷移し、現在走行中の地点付近の点群地図が登録されている場合は位置推定モード２０３に遷移する。付近の点群地図が登録されているか否かは、ランドマークに対応する緯度・経度情報から判定する。自己位置の緯度経度情報と、ランドマークの緯度経度情報を比較し、あらかじめ定められた距離以内のランドマークがある場合は地図登録済み、あらかじめ定められた距離以内のランドマークがない場合は地図未登録と判断する。

地図記憶モード２０２ではユーザにより車両１が運転され、車載処理装置１２０は車両１が備えるセンサからの情報に基づき駐車場データ、すなわち駐車場に存在する白線や障害物の情報と、駐車位置の情報と、走行経路の情報を収集する。車載処理装置１２０は、収集した情報を点群地図１２４Ａとして記憶部１２４に格納する。

モードの遷移については、あらかじめ登録した駐車したい地点付近（登録地点）の緯度経度と、カーナビゲーションシステム１０７から取得した自己位置の緯度経度情報を比較し、登録地点を中心とする一定範囲以内から外に出た場合、例えばナビ自己位置が登録地点から離れてその距離が１００ｍ以上となった場合には、地図記憶モード２０２から通常走行モード２０１に遷移する。また、途中から点群地図登録済みの区間に進入した場合には、地図記憶モード２０２から位置推定モード２０３に遷移する。

自車が地図登録済みの区間に進入したか否かについては、自己位置（ナビ自己位置）の緯度経度情報と、ランドマークの緯度経度情報を比較し、ナビ自己位置を中心としてあらかじめ定められた距離以内にランドマーク（対応点群）があるか否かによって判断される。そして、ランドマークがある場合は自車が地図登録済みの区間に進入したと判断する。

位置推定モード２０３ではユーザにより車両１が運転され、車載処理装置１２０は車両１が備えるセンサからの情報と、点群地図１２４Ａの情報を照合し、点群地図中の自車の位置を推定する。車載処理装置１２０は、センサからの情報に基づき車両１の周囲に存在する白線や障害物を検出し、点群地図１２４Ａと照合することにより現在位置を推定する。

モードの遷移については、登録地点の緯度経度と、カーナビゲーションシステム１０７から取得した自己位置の緯度経度情報を比較し、登録地点を中心とする一定範囲以内から外に自車が出た場合、例えばナビ自己位置が登録地点から離れてその距離が１００ｍ以上となった場合には、位置推定モード２０３から通常走行モード２０１に遷移する。また、途中から点群地図の未登録区間に進入した場合には、対応点群なしと判断され、位置推定モード２０３から地図記憶モード２０２に遷移する。

自車が点群地図の未登録区間に進入したか否かについては、自己位置の緯度経度情報と、ランドマークの緯度経度情報を比較し、自己位置を中心としてあらかじめ定められた距離以内にランドマーク（対応点群）があるか否かによって判断され、ランドマークがない場合は自車が点群地図の未登録区間に進入したと判断する。

また、自車両の位置推定に成功しており、かつ、自己位置が駐車位置から所定の範囲内にあること、かつユーザが自動駐車ボタン１１０Ｂを押下している間のみ、位置推定モード２０３から自動駐車モード２０４に遷移する。すなわち、ユーザの承認のもとに自動制御を実施する。また、点群地図中の自己位置が見つけられない場合は、ギブアップモード２０５に遷移する。

自動駐車モード２０４では車載処理装置１２０により車両１が制御され、位置推定モード２０３で得られた点群地図中の自己位置、および自動駐車モード２０４においても継続的に行う地図中の自己位置推定結果に基づき、点群地図に記憶された駐車位置に車両１が駐車される。車載処理装置１２０は、自動駐車モード２０４においても、位置推定モード２０３に引き続き、センサからの情報に基づき車両１の周囲に存在する白線や障害物を検出し、点群地図１２４Ａと照合することにより現在位置を推定する。モードの遷移についてはユーザが自動駐車ボタン１１０Ｂを離した場合は、自動駐車モード２０４から位置推定モード２０３に遷移する。すなわち、自動駐車制御が停止する。また、ユーザが自動駐車ボタン１１０Ｂを押下している状態で、自己位置をロストした場合には、ギブアップモード２０５に遷移する。

ギブアップモード２０５では、ユーザに表示装置１１１を介して自動駐車できない旨を提示し、本モード中は地図記憶と、位置推定と、自動駐車のいずれも実施しない。モードの遷移については、ユーザが駐車完了するか、登録地点からあらかじめ定めた所定距離離れた場合に、通常走行モード２０１に遷移する。

以下では、地図記憶モード２０２と、位置推定モード２０３と、自動駐車モード２０４について詳しく説明する。

（地図記憶モード２０２）
登録地点を中心とする所定範囲に入り、かつ点群地図が記録されていない（対応点群なし）場合に、車載処理装置１２０は通常走行モード２０１から地図記憶モード２０２に遷移する。例えば、ナビ自己位置が登録地点を中心とする１００ｍ内の範囲に入り、かつ対応点群がない場合に、地図記憶モード２０２で動作する。地図記憶モード２０２で動作している間、表示装置１１１の画面に地図記憶モード２０２で動作している旨が表示される。そして、ユーザが駐車位置まで移動させた後、停車し、パーキングブレーキを作動させると、そのパーキングブレーキの作動をトリガーとして、表示装置１１１に「今回の走行経路周辺を地図として記憶させますか？」という文言が表示される。この段階で初めてユーザの判断および操作が必要となり、その前の点群抽出までの動作は、ユーザが判断操作することなく全て自動で行われる。そして、ユーザが応答ボタン１１０Ａを押下すれば地図を記憶し、車載処理装置１２０は地図記憶モード２０２を終了する。ユーザが応答ボタン１１０Ａを押下しない場合は、今回の走行経路周辺を地図として記憶せず、地図記憶モード２０２を終了する。車載処理装置１２０による地図記憶モード２０２の動作は、ランドマークを構成する点群の抽出と、抽出した点群の記録の２つの処理に分けられる。

車載処理装置１２０による点群の抽出処理を説明する。
車載処理装置１２０は、地図記憶モード２０２に遷移すると、ＲＡＭ１２２に一時的な記録領域を確保する。そして車載処理装置１２０は、地図記憶モード２０２が終了するか、他のモードに遷移するまで以下の処理を繰り返す。すなわち車載処理装置１２０は、カメラ１０２の撮影画像と、ソナー１０３とレーダ１０４による観測情報の少なくとも一つに基づきランドマークを構成する点群を抽出する。また車載処理装置１２０は、車速センサ１０８および舵角センサ１０９の出力に基づき、カメラ１０２が前回撮影してから今回撮影するまでに車両１が移動した移動量および移動方向を算出する。そして車載処理装置１２０は、車両１との位置関係、および車両１の移動量や移動方向に基づき抽出した点群をＲＡＭ１２２に記録する。さらに車載処理装置１２０は、カーナビゲーションシステム１０７が出力する緯度経度を点群とあわせて記録する。車載処理装置１２０はこの処理を繰り返す。

車両１の位置、および点群の座標は、記録座標系の座標値として記録される。「記録座標系」は、たとえば記録を開始した際の車両１の位置を原点（０、０）とし、記録開始時の車両１の進行方向（姿勢）をＹ軸、記録開始時の車両１の右方向をＸ軸とした座標系の座標値として扱われる。そのため同一の駐車場において点群を記録しても、記録を開始した際の車両１の位置や姿勢により設定される記録座標系が異なるので、異なる座標にランドマークを構成する点群が記録される。

ユーザは車両を目的の駐車位置に駐車させてパーキングブレーキを引く。パーキングブレーキを作動させたことをトリガーとして、表示装置１１１に「今回の走行経路周辺を地図として記憶させますか？」という文言が表示される。ユーザが応答ボタン１１０Ａを押下すれば地図として記憶する。車載処理装置１２０は、地図として記憶することが承認されると、現在位置を駐車位置としてＲＡＭ１２２に記録する。駐車位置はたとえば車両１を矩形に近似した際の四隅の座標として記録される。

次に車載処理装置１２０は以下のように点群の記録処理を行う。
車載処理装置１２０は、記録した駐車場の座標、すなわち緯度経度が、すでに点群地図１２４Ａに記録されているいずれかの駐車場の座標と略一致するか否かを判断する。両者が略一致しない場合は、車載処理装置１２０はＲＡＭ１２２に保存した点群の情報を、新たな駐車場データとして点群地図１２４Ａに記録する。両者が略一致する場合は、車載処理装置１２０は駐車場の座標が略一致する点群の情報を１つの駐車場の点群としてマージ（統合）するか否かを判断する。この判断のために車載処理装置１２０はまず、駐車場データに含まれる駐車位置と、ＲＡＭ１２２に記録した駐車位置が一致するように座標変換を行い、次に点群地図１２４Ａの点群とＲＡＭ１２２に保存した点群との一致度である点群一致度を算出する。そして車載処理装置１２０は、算出された点群一致度が閾値よりも大きければ両者をマージすると判断し、閾値以下であればマージしないと判断する。点群一致度の算出については後述する。

車載処理装置１２０は、マージしないと判断した場合はＲＡＭ１２２に保存した点群を新たな駐車場データとして点群地図１２４Ａに記録する。車載処理装置１２０は、マージすると判断した場合は、点群地図１２４Ａの既存の駐車場データにＲＡＭ１２２に保存した点群を追加する。

（記憶モードのフローチャート）
図４は、車載処理装置１２０の地図記憶モード２０２の動作を表すフローチャートである。以下に説明する各ステップの実行主体は車載処理装置１２０の演算部１２１である。演算部１２１は、図４に示す処理を行う場合に地図生成部１２１Ｂとして機能する。

ステップＳ５０１では、地図記憶モード２０２に入ったか否かが判断される。地図記憶モード２０２に入ったと判断される場合はＳ５０１Ａに進み、地図記憶モード２０２に入っていない場合はステップＳ５０１に留まる。ステップＳ５０１Ａでは、ＲＡＭ１２２に新たな記録領域を確保する。この記憶領域には、抽出した点群と車両１の現在位置が前述の記録座標系の座標で記録される。

ステップＳ５０２では、センサ群から情報を取得して前述のランドマーク測位、すなわちカメラ１０２の撮影画像、ソナー１０３、レーダ１０４を用いたランドマークを構成する点群の抽出とカーナビゲーションシステム１０７から出力された緯度経度観測値との紐付けを行う。続くステップＳ５０３では、カメラ１０２、ソナー１０３、レーダ１０４がセンシングしてから最新のセンシングを行うまでの時間における車両１の移動量を推定し、ＲＡＭ１２２に記録した記録座標系における車両１の現在位置を更新する。車両１の移動量は複数の手段により推定可能であり、たとえば前述のようにカメラ１０２の撮影画像における路面に存在する被写体の位置の変化から車両１の移動量を推定できる。ソナー１０３やレーダ１０４のセンシング結果から推定しても良い。また、カーナビゲーションシステム１０７として誤差が小さい高精度なＧＰＳ受信機が搭載されている場合には、その出力を利用してもよい。車速センサ１０８と舵角センサ１０９から車両１の移動量を推定しても良い。次にステップＳ５０４に進む。

ステップＳ５０４では、ステップＳ５０２において抽出した点群を、ステップＳ５０３において更新した現在位置に基づき、記録座標系の座標としてＲＡＭ１２２に保存する。続くステップＳ５０５では、応答ボタン１１０Ａが押される記憶承認操作がされたか否かを判断し、記憶承認操作がされたと判断する場合はステップＳ５０５Ａに進み、記憶承認操作がされていないと判断する場合はステップＳ５０２に戻る。ステップＳ５０５Ａでは、カーナビゲーションシステム１０７から車両１の現在の緯度経度を取得するとともに、駐車位置、すなわち車両１の現在位置であって車両１の四隅の記録座標系における座標を記録する。次にステップＳ５０６に進む。

ステップＳ５０６では、ステップＳ５０５Ａにおいて取得した車両１の現在の緯度経度と略一致する緯度経度を有する駐車場データが、点群地図１２４Ａに記録されているか否かを判断する。車両１の現在の緯度経度が点群地図１２４Ａに記録されているいずれかの駐車場データの緯度経度と略一致すると判断する場合はステップＳ５０７に進み、それ以外の場合はステップＳ５１０に進む。以下では、車両１の現在の緯度経度と緯度経度が略一致すると判断された点群地図１２４Ａの駐車場データをターゲット駐車場データと呼ぶ。

ステップＳ５０７では、駐車位置を基準として、ＲＡＭ１２２に保存した点群データの座標系である記録座標系をターゲット駐車場データの点群データの座標系に変換する。すなわち、ターゲット駐車場データに含まれる駐車位置と、ステップＳ５０５Ａにおいて記録した駐車位置が一致するように記録座標系と駐車場座標系の座標変換式を導出する。そしてこの座標変換式を用いて、ＲＡＭ１２２に記録座標系で保存されたランドマークを構成する点の座標をターゲット駐車場データの駐車場座標系に変換する。

続くステップＳ５０７Ａでは、ＲＡＭ１２２に保存された点群データとターゲット駐車場データの点群一致率ＩＢを算出する。点群一致率ＩＢは以下の式１により算出される。
ＩＢ＝ ２＊Ｄｉｎ／（Ｄ１＋Ｄ２） ・・・式１

ただし式１において「Ｄｉｎ」は、ステップＳ５０７において座標変換した点群データの各点と、ターゲット駐車場データの点群データの各点の距離が所定の距離以内の点の数である。また式１において「Ｄ１」はＲＡＭ１２２に保存された点群データにおける点の数、「Ｄ２」はターゲット駐車場データの点群データにおける点の数である。次にステップＳ５０８に進む。

ステップＳ５０８では、ステップＳ５０７Ａにおいて算出した点群一致率ＩＢが所定の閾値よりも大きいか否かを判断する。点群一致率ＩＢが閾値よりも大きいと判断する場合はステップＳ５０９に進み、点群一致率ＩＢが閾値以下であると判断する場合はステップＳ５１０に進む。

ステップＳ５０９では、マージ処理、すなわち記憶部１２４に格納された点群地図１２４Ａのターゲット駐車場データに、ステップＳ５０７において座標変換した点群データを追加する。ステップＳ５０６またはステップＳ５０８において否定判断された場合に実行されるステップＳ５１０では、ＲＡＭ１２２に保存した点群データ、およびステップＳ５０５Ａにおいて記録した車両１の緯度経度、および駐車位置を新たな駐車場データとして点群地図１２４Ａに記録する。以上で図４のフローチャートを終了する。

（位置推定モード２０３）
ユーザが車両１を運転して登録地点付近まで移動させ、かつ周辺の点群地図が記憶済みであると判定されると、通常走行モード２０１から位置推定モード２０３に移行する。例えば、ナビ自己位置が登録地点を中心とする１００ｍ内の範囲に入り、かつ対応点群がある場合には、位置推定モード２０３を動作させる。この際の車載処理装置１２０の動作をフローチャートで以下説明する。

（位置推定モード２０３のフローチャート）
図５は、車載処理装置１２０の位置推定モードの動作を表すフローチャートである。以下に説明する各ステップの実行主体は車載処理装置１２０の演算部１２１である。

車載処理装置１２０は、点群地図１２４Ａに含まれる複数の駐車場データのうち、車両１の現在位置と略一致する緯度経度を有する点群（駐車場点群）を特定する（ステップＳ６０１）。次に車載処理装置１２０は、初期化処理としてＲＡＭ１２２に格納される局所周辺情報１２２Ｂの初期化、およびＲＡＭ１２２に保存される車両１の現在位置の初期化を行う（ステップＳ６０１Ａ）。具体的には従前の情報が記録されていたら消去し、新たな座標系を設定する。本実施の形態では、この座標系を局所座標系と呼ぶ。この局所座標系はステップＳ６０１Ａが実行された際の車両１の位置および姿勢に基づき設定される。たとえばステップＳ６０１Ａが実行された際の車両１の位置が局所座標系の原点に設定され、ステップＳ６０１Ａが実行された際の向きによりＸ軸およびＹ軸が設定される。また車両１の現在位置の初期化は、車両１の現在位置が原点（０、０）に設定される。

次に、図６Ａに示す手順により自己位置推定、すなわち車両１の駐車場座標系における位置を推定し（ステップＳ６０２）、ステップＳ６０３では自己位置が推定できたか否かを判断する。推定できたと判断する場合はステップＳ６０４に進み、推定できないと判断する場合はステップＳ６０２に戻る。

ステップＳ６０４では、車載処理装置１２０は表示装置１１１に地図上の自己位置推定結果を表示し、続くステップＳ６０５では、自己位置が記憶駐車位置からの所定の範囲内に入っているか否かを判断する。例えば、車両１が記憶駐車位置まで距離Ｎｍまで近づいた場合に所定の範囲内に入っていると判断される。所定の範囲内に入っていると判断された場合は、ステップＳ６０６に進み、所定の範囲内に入っていない、すなわち範囲外と判断された場合はステップ６０４に戻る。

ステップＳ６０６では、車載処理装置１２０は表示装置１１１に自動駐車が可能な旨を表示し、続くステップＳ６０７ではユーザにより自動駐車ボタン１１０Ｂが押されたか否かを判断する。自動駐車ボタン１１０Ｂが押されたと判断する場合はステップＳ６０８に進んで自動駐車モードに遷移し、図８に示す手順により自動駐車処理を実行し、自動駐車ボタン１１０Ｂが押されていないと判断する場合はステップＳ６０６に戻る。

図６Ａを参照して図５のステップＳ６０２において実行される自己位置推定処理の詳細を説明する。演算部１２１は、図６ＡのステップＳ６２１〜Ｓ６２３に示す処理を行う場合に局所周辺情報作成部として機能する。

ステップＳ６２１のランドマーク測位、ステップＳ６２２の自車移動量推定、およびステップＳ６２３の局所周辺情報１２２Ｂの更新はそれぞれ、図４のステップＳ５０２〜Ｓ５０４の処理と概ね同じである。相違点はＲＡＭ１２２に記憶されるデータが局所周辺情報１２２Ｂとして記録される点である。

ステップＳ６２３の実行が完了すると、車載処理装置１２０は図６Ｂに詳細を示す局所周辺情報の選定を行う（ステップＳ６２３Ａ）。この局所周辺情報の選定処理では、局所周辺情報として得られた点群の中から、後述のステップＳ６２４のマッチング処理で利用する点を選定する処理である。局所周辺情報で得られた点群は自車移動量推定の累積誤差により、点群地図と全体形状が異なりマッチングできない場合がある。この局所周辺情報の選定処理により、形状誤差が小さくマッチング可能な範囲の点群を適応的に選定する。

ステップＳ６２４Ａの処理が完了すると、車載処理装置１２０は図７に詳細を示すマッチング処理を行う（ステップＳ６２４）。このマッチング処理では、駐車場座標系と局所座標系との対応関係、すなわち駐車場座標系と局所座標系との座標変換式が得られる。続くステップＳ６２５では車載処理装置１２０は、ステップＳ６２２において更新した局所座標系における車両１の座標と、ステップＳ６２５において得られた座標変換式を用いて、駐車場座標系における車両１の座標、すなわち自己位置を算出する。次にステップＳ６２６に進む。

ステップＳ６２６において車載処理装置１２０は、ステップＳ６２５において算出した位置の信頼性を判断する自己診断を実行する。自己診断は、例えば以下の３つの指標を用いて判断される。

第１の指標では、車速センサ１０８および舵角センサ１０９の出力を用いて公知のデッドレコニング技術によって推定した車両１の移動量と、自己位置推定によって推定された所定期間における移動量を比較し、あらかじめ定めた閾値よりも差が大きい場合は信頼度が低いと判断する。

第２の指標では、マッチング時に計算される対応点の誤差量で判断する。誤差量があらかじめ定めた閾値よりも大きい場合は信頼度が低いと判断する。

第３の指標では、類似解があるか否かの判定を実施する。得られた解から駐車枠の幅分並進するなど、類似解を探索した場合に、対応点誤差が一定以内の点が同じくらいの数あった場合は、信頼度が低いと判断する。これら３つの指標のすべてで信頼度が低いと判断されなかった場合に自己位置が推定できたと判断する。

図６Ｂを参照して図６ＡのステップＳ６２３Ａにおいて実行される局所周辺情報選定処理の詳細を説明する。演算部１２１は、図６Ｂに示す処理を行う場合に局所周辺情報選定部として機能する。

ステップＳ６８０では、ステップＳ６２２の内部で実施している自車移動量計算を利用して、自車の現在位置までの軌跡を計算する。自車移動量から算出した自車両位置の座標点を補間して生成される軌跡が自車軌跡となる。

ステップＳ６８１では、局所周辺情報の点群に関して、形状誤差が小さくマッチング可能な有効範囲を算出する。この有効範囲は、ステップＳ６８０で算出した自車軌跡の長さや形状から判定する。局所周辺情報として得られる点群は、距離が長くなるほど、また車両の旋回量が大きくなるほど移動量の推定誤差が発生しやすい。逆に点群が少なすぎると、マッチングが困難になる。そこで、現在位置からあらかじめ定めた最低距離Ｄ[m]分を軌跡に沿って遡った範囲の点群を取得する。それ以降、軌跡の接線の角度の変化量を累積し、あらかじめ定めた角度しきい値θ[deg]以上に変化するところまでの軌跡周辺の点群を取得する。軌跡に対して、軌跡を中心としてあらかじめ定めた範囲X[m]×Y[m]の範囲の点群を局所周辺情報の点群の有効範囲とする。すなわち、有効範囲は得られた軌跡に沿った形状となる。

ステップＳ６８２ではステップＳ６８１で得られた有効範囲内の点を、局所周辺情報の点群として取得する。

図７を参照して図６ＡのステップＳ６２４において実行されるマッチング処理の詳細を説明する。演算部１２１は、図６Ａに示す処理を行う場合に位置推定部１２１Ｃとして機能する。

ステップＳ６４１ではＲＡＭ１２２に格納されたアウトライアリスト１２２Ａを局所周辺情報１２２Ｂに対して適用し、局所周辺情報１２２Ｂに含まれる点群のうちアウトライアリスト１２２Ａに記載された点を一時的に処理対象外とする。この適用範囲はステップＳ６４２〜Ｓ６５３であり、ステップＳ６５４では従前にアウトライアリスト１２２Ａに含まれていた点も対象となる。ただし図７に示すフローチャートの初回実行時にはステップＳ６４１〜Ｓ６４３が実行できないため、ステップＳ６５０から実行を開始する。次にステップＳ６４１Ａに進む。

ステップＳ６４１Ａでは、最新の撮影画像から検出した点群、すなわち図６ＡのステップＳ６２１において検出したランドマークを構成する点群の座標を、駐車場座標系の座標に変換する。この変換は、ステップＳ６２２において更新された車両１の局所座標系における位置、および前回算出した局所座標系から駐車場座標系への座標変換式を用いることにより実現される。

続くステップＳ６４２では瞬間一致度ＩＣが算出される。瞬間一致度ＩＣは、以下の式２により算出される。
ＩＣ＝ ＤＩｉｎ／ＤＩａｌｌ ・・・式２

ただし式２において「ＤＩｉｎ」は、ステップＳ６４１Ａにおいて駐車場座標系に変換された最新の撮影画像から検出された点群のうち、最も近い点群地図１２４Ａを構成する点までの距離があらかじめ定めた閾値以下の点の数である。また式２において「ＤＩａｌｌ」は、ステップＳ６２１において検出された点群の数である。次にステップＳ６４３に進む。

ステップＳ６４３では、ステップＳ６４２において算出した瞬間一致度ＩＣが閾値よりも大きいか否かを判断する。瞬間一致度ＩＣが閾値よりも大きいと判断する場合はステップＳ６５０に進み、瞬間一致度ＩＣが閾値以下であると判断する場合はステップＳ６４４に進む。

ステップＳ６４４では、点群地図１２４Ａの対象となる駐車場データ、すなわち点群データから周期的な特徴、たとえば複数並んだ駐車枠を検出する。前述のように、点群地図に含まれる点群は画像のエッジなどを抽出して得られるので、白線の太さに相当する間隔で並ぶ点から駐車枠線を検出できる。続くステップＳ６４５では、ステップＳ６４４において周期的特徴を検出したか否かを判断し、検出したと判断する場合はステップＳ６４６に進み、検出できなかったと判断する場合はステップＳ６５０に進む。ステップＳ６４６では周期的特徴の周期、たとえば駐車枠の幅を算出する。ここでいう駐車枠の幅とは、駐車枠を構成する白線同士の間隔である。次にステップＳ６４７に進む。

ステップＳ６４７では、前回のステップＳ６５３において算出された座標変換式を基準として、この座標変換式を複数とおりに変化させて全体一致度ＩＷをそれぞれ算出する。座標変換式は、点群地図が検出した周期的特徴の整数倍ずれるように複数通りに変化させる。全体一致度ＩＷは、以下の式３により算出される。
ＩＷ＝ ＤＷｉｎ／ＤＷａｌｌ ・・・式３

ただし式３において「ＤＷｉｎ」は、前述の座標変換式を用いて局所周辺情報１２２Ｂを構成する点を駐車場座標系に変換した点のうち、最も近い点群地図１２４Ａを構成する点までの距離があらかじめ定めた閾値以下の点の数である。また式３において「ＤＷａｌｌ」は、ステップＳ６２１において検出された点の数である。次にステップＳ６４８に進む。

ステップＳ６４８では、ステップＳ６４７において算出した複数の全体一致度ＩＷのうち、最大の全体一致度ＩＷを与える座標変換式をＲＡＭ１２２に記憶してステップＳ６５０に進む。

ステップＳ６５０における対応付け処理、ステップＳ６５１における誤差最小化処理、およびステップＳ６２５における収束判定処理は既知の点群マッチング技術であるＩＣＰ(Iterative Closest Point)アルゴリズムを利用することができる。ただしステップＳ６５０における初期値の設定は本実施の形態に特有なので詳しく説明し、他は概略のみ説明する。

ステップＳ６４３において肯定判定された場合、ステップＳ６４５において否定判定された場合、ステップＳ６４８の実行が終わった場合、またはステップＳ６５２において否定判定された場合に実行されるステップＳ６５０では、点群地図１２４Ａの駐車場データに含まれる点群と局所周辺情報１２２Ｂに含まれる点群との対応付けが算出される。ステップＳ６４３またはステップＳ６４８の次に実行される場合は、局所周辺情報１２２Ｂの点群データはＲＡＭ１２２に記録された座標変換式を用いて座標変換された値を用いる。すなわち、ステップＳ６４３において肯定判定されてステップＳ６５０が実行される場合には、前回実行されたステップＳ６５３において算出された座標変換式が使用される。一方、ステップＳ６４８の次にステップＳ６５０が実行される場合は、ステップＳ６４８において記憶された座標変換式が使用される。次にステップＳ６５１に進む。

ステップＳ６５１では、対応点の誤差が最小となるように座標変換式が変更される。たとえば、ステップＳ６５０において対応付けられた点同士の距離の指標の和が最小となるように座標変換式が変更される。対応付けられた点同士の距離の指標の和として距離の絶対値の和を採用することができる。続くステップＳ６５２では、誤差が収束したか否かを判断し、収束したと判断する場合はステップＳ６５３に進み、収束していないと判断する場合はステップＳ６５０に戻る。続くステップＳ６５３では、最後にステップＳ６５１において変更された座標変換式をＲＡＭ１２２に保存してステップＳ６５４に進む。

ステップＳ６５４では、次のようにアウトライアリスト１２２Ａを更新する。まずＲＡＭ１２２に格納されている既存のアウトライアリスト１２２Ａをクリアする。次に局所周辺情報１２２Ｂの点群をステップ６５３において記録した座標変換式を用いて駐車場座標系に変換し、局所周辺情報１２２Ｂを構成するそれぞれの点とその点が対応する点群地図１２４Ａを構成する点までの距離、すなわちユークリッド距離を算出する。そして、算出された距離があらかじめ定められた距離よりも長い場合は、その局所周辺情報１２２Ｂの点をアウトライアリスト１２２Ａに加える。ただしこのときに、空間的に端部に位置することをアウトライアリスト１２２Ａに加えるさらなる条件としてもよい。空間的な端部とは、記録を開始した際に取得された点など、他の点までの距離が遠い点である。以上の処理によりアウトライアリスト１２２Ａが更新される。以上で図７のフローチャートを終了する。

（自動駐車モード２０４のフローチャート）
図８を参照して図５のステップＳ６０８において実行される自動駐車モード２０４での処理の詳細を説明する。以下に説明する各ステップの実行主体は車載処理装置１２０である。

自車両の位置推定に成功しており、かつ、自己位置が駐車位置から所定の範囲内にあること、かつユーザが自動駐車ボタン１１０Ｂを押下している間のみ、自己位置推定モード２０３から自動駐車モード２０４に遷移する。すなわち、ユーザの承認のもとに自動制御を実施する。

ステップＳ６６１では、駐車場座標系における車両１の位置を推定する。本ステップにおける処理は図５のステップＳ６０４と同様なので説明を省略する。続くステップＳ６６２では、ステップＳ６６１において推定した位置から、点群地図１２４Ａに格納されている駐車位置までの走行経路を既知の経路生成手法により生成する。次にステップＳ６６３に進む。

ステップＳ６６３では、車両制御装置１３０を介して操舵装置１３１、駆動装置１３２、および制動装置１３３を制御し、ステップＳ６６２において生成した経路に沿って車両１を駐車位置まで移動させる。自動駐車ボタン１１０Ｂがユーザにより押され続けている場合だけ駆動装置１３２に動作指令を出力する。また、カメラ１０２の撮影画像や、ソナー１０３およびレーダ１０４から人物や移動車両などが抽出されたら制動装置１３３を動作させて車両１を停止させる。続くステップＳ６６４ではステップＳ６６１と同様に車両１の位置を推定する。続くステップＳ６６５では駐車が完了したか否か、すなわち車両１が駐車位置に到達したか否かを判断し、駐車が完了していないと判断する場合はステップＳ６６３に戻り、駐車が完了したと判断する場合は図８のフローチャートを終了する。

（動作例）
図９〜図１４を参照して、地図記憶モード２０２、位置推定モード２０３および自動駐車モード２０４の具体的な動作を説明する。

図９（ａ）は、駐車場９０１の一例を示す平面図である。駐車場９０１は建物９０２の周囲に設けられる。駐車場９０１の出入り口は図示左下に１か所だけある。図９（ａ）に示す四角は路面ペイントである駐車枠を表しており、ハッチングで示す駐車枠９０３が車両１の駐車領域（駐車が完了した際に駐車位置となる領域）である。本動作例では、ランドマークは駐車枠線のみとして説明する。本動作例では図９（ａ）に示すように車両１を三角形で示し、三角形の鋭角が車両１の進行方向を示す。

（動作例｜地図記憶モード２０２ その１）
駐車場９０１の入り口付近で通常走行モード２０１から地図記憶モード２０２に遷移すると、車載処理装置１２０はランドマーク測位を開始し、駐車枠線を構成する点の座標を記録する（図４のステップＳ５０１：ＹＥＳ、Ｓ５０２〜Ｓ５０４）。そして車両１は応答ボタン１１０Ａが押されて記憶処理が承認されるまで、図４のステップＳ５０２〜Ｓ５０４の処理を繰り返す。

図９（ｂ）は、ＲＡＭ１２２に保存されるランドマークの点群を可視化した図である。図９（ｂ）において、実線はＲＡＭ１２２に保存されたランドマークの点群を表し、破線はＲＡＭ１２２に保存されていないランドマークを示す。車両１のカメラ１０２、ソナー１０３、レーダ１０４はセンシング可能な範囲に限界があるため、図９（ｂ）に示すように車両１が駐車場９０１の入り口付近にいる際は駐車場９０１の入り口付近の駐車枠線だけが記録される。ユーザが車両１を駐車場９０１の奥まで移動させると、車載処理装置１２０は駐車場９０１の全体のランドマークの点群を記録することができる。

ユーザが車両１を駐車枠９０３の中に停車させて記録完了ボタンとなる応答ボタン１１０Ａを押すと、車載処理装置１２０はカーナビゲーションシステム１０７から車両１の緯度経度を取得するとともに、車両１の四隅の座標を記録する（ステップＳ５０５：ＹＥＳ、Ｓ５０５Ａ）。点群地図１２４Ａに車両１の現在の緯度経度と略一致する緯度経度が記録されていない場合は（ステップＳ５０６：ＮＯ）、ＲＡＭ１２２に保存した点群を点群地図１２４Ａを構成する新たなデータ、すなわち新たな駐車場データとして記録する（ステップＳ５１０）。

（動作例｜地図記憶モード２０２ その２）
別な例として、図１０（ａ）に示す点群データが点群地図１２４Ａの駐車場データとして記録されており、新たに図１０（ｂ）に示す点群データが得られた場合を説明する。図１０（ａ）に示す点群データは、たとえば図９（ａ）に示した駐車場９０１の入り口から右寄りを走行して駐車位置まで到達した場合に得られた点群データである。車両１が図９（ａ）に比べて右寄りを走行したので、図１０（ａ）に破線で示した駐車枠の点群データが取得されていない。

図１０（ｂ）に示す点群データは、たとえば駐車場９０１の入り口から左寄りを走行して駐車位置まで到達した場合に得られた点群データである。図９（ａ）に比べて左寄りを走行したので、図１０（ｂ）に破線で示した駐車枠の点群データが取得されていない。また図１０（ｂ）に示す点群データは、記憶モードに遷移した際に車両１が駐車場９０１と正対していなかったので、図１０（ａ）に比べて駐車場９０１が傾いているように記録されている。

このような状態においてユーザが応答ボタン１１０Ａで記憶を承認したとき、点群地図１２４Ａに車両１の現在の緯度経度と略一致する緯度経度が記録されていると判断されると（Ｓ５０６：ＹＥＳ）、図１０（ａ）と図１０（ｂ）の駐車位置、すなわち駐車枠９０３を基準に座標変換が行われる（ステップＳ５０７）。そして点群一致率ＩＢを算出し（ステップＳ５０７Ａ）、その点群一致率ＩＢが所定の閾値よりも大きいと判断されると（ステップＳ５０８：ＹＥＳ）、マージ処理により、図１０（ａ）に示す点群データに図１０（ｂ）に示す点群データが統合される（ステップＳ５０９）。この統合により、図１０（ａ）では記録されていなかった図示左側の駐車枠線の点群が新たに記録されるとともに、すでに記録されていた図示右側や図示上部の駐車枠線を構成する点群の密度が濃くなる。

（動作例｜実行フェーズ その１＝自己位置推定モード２０３＋自動駐車モード２０４）
実行フェーズにおけるマッチング処理の動作例を説明する。この動作例では、あらかじめ点群地図１２４Ａには図９（ａ）に示した駐車場９０１の全体に相当する点群データが格納されている。

図１１は、図９（ａ）に示す駐車場９０１における車両１の現在位置を示す図である。車両１は図示上方を向いている。図１２〜図１３では、車両１の前方領域である図１０において破線の丸で囲む部分の駐車枠線を示す。

図１２は、車両１が図１１に示した位置においてセンサ群でセンシングされて抽出された点群を、駐車場座標に変換したデータを示す図である。すなわち、図１２に示す点群は局所周辺情報１２２Ｂのうち、最新の撮影画像から検出された点群であり図７のステップＳ６４１Ａにおいて処理されたデータである。ただし図１２では点ではなく破線として表している。また図１２では図１１との対比のために車両１もあわせて表示している。図１２に示すように、車両１の左側には駐車枠線の点群データが切れ目なく存在しており、車両１の右側では駐車枠線の点群データが手前側にのみ存在する。

図１３は、車両１の駐車場座標系における位置の推定が誤差を含んでいた場合における、点群地図１２４Ａと図１１に示した局所周辺情報１２２Ｂとの対比を示す図である。図１３では、従前の位置推定がおおよそ駐車枠の幅の１つ分ずれていたため、車両１の右側に存在する局所周辺情報１２２Ｂは、点群地図１２４Ａとずれが生じている。このような状態で瞬間一致度ＩＣが算出されると（図７のステップＳ６４２）、車両１の右側の前述のずれにより瞬間一致度ＩＣは低い値となる。この値が閾値よりも低いと判断されると（ステップＳ６４３：ＮＯ）、駐車枠を周期的な特徴として検出し（ステップＳ６４４、Ｓ６４５：ＹＥＳ）、点群地図１２４Ａから駐車枠の幅（周期）が算出され（ステップＳ６４６）、駐車枠の幅の整数倍移動させて全体一致度ＩＷが算出される（ステップＳ６４７）。

図１４（ａ）〜（ｃ）は、図１３に示した局所周辺情報１２２Ｂを駐車枠の幅の整数倍移動させた場合の点群地図１２４Ａとの関係を示す図である。図１４（ａ）〜（ｃ）ではそれぞれ、図１４に示した局所周辺情報１２２Ｂは駐車枠の幅の、＋１倍、０倍、―１倍だけ図示上方に移動している。図１４（ａ）では局所周辺情報１２２Ｂが図示上側に駐車枠の幅の１つ分移動しており、局所周辺情報１２２Ｂと点群地図１２４Ａとのずれが拡大している。そのため図１４（ａ）における全体一致度ＩＷは、移動させない場合よりも小さくなる。図１４（ｂ）では局所周辺情報１２２Ｂは移動しておらず、図１３でみたように局所周辺情報１２２Ｂと点群地図１２４Ａは駐車枠の幅の１つ分ずれている。図１４（ｃ）では局所周辺情報１２２Ｂが図示下側に駐車枠の幅の１つ分移動しており、局所周辺情報１２２Ｂは点群地図１２４Ａと略一致している。そのため図１４（ｃ）における全体一致度ＩＷは、移動させない場合よりも大きくなる。

局所周辺情報１２２Ｂの移動量と全体一致度ＩＷの増減は上述する関係にあるので、図１４に示す例では図１４（ｃ）に対応する全体一致度ＩＷが最大と判断され、この移動に対応する座標変換式がＲＡＭ１２２に記憶される（ステップＳ６４８）。このようにして車載処理装置１２０は推定する位置精度を向上させる。

（動作例｜実行フェーズ その１＝自己位置推定モード２０３＋自動駐車モード２０４）

図１５は、車両１の駐車場座標系における位置の推定が誤差を含んでいた場合における、点群地図１２４Ａと図１１に示した局所周辺情報１２２Ｂとの対比を示す図である。

図１５（ａ）は、図９（ａ）に示す駐車場９０１における車両１の現在位置を示す図である。車両１は図示右方を向いている。図１５（ｂ）は、車両１が図１５（ａ）に示した位置に到達するまでに撮影した画像から抽出された点群を、駐車場座標に変換したデータを示す図である。走行軌跡は、有効部分１５１と無効部分１５２を有しており、局所周辺情報の点群は、有効範囲１５３と無効範囲１５４に分かれている。図１５（ｃ）は有効範囲１５３内の点群を用いてマッチングした結果を示している。

図１５（ｂ）に示す例では、車両１の右旋回時に誤差が生じており、全体としての点群形状が変化している。したがって、図１５（ａ）に示す局所周辺情報１２２Ｂと駐車場点群とをそのまま全体でマッチングしても一致しないことが推察できる。

本実施形態では、図６ＢのステップＳ６８１の処理により、自車軌跡の有効部分１５１を中心として所定範囲の点群を局所周辺情報の点群の有効範囲１５３としている。走行軌跡の有効部分１５１は、自己位置から走行軌跡を逆方向に遡って、大きく旋回している部分で終了している。走行軌跡の有効部分１５１を中心として、所定の範囲内に入る点（有効範囲内の点）を局所周辺情報の点群として取得し、マッチングに利用する。走行軌跡の無効部分１５２を中心として所定の範囲内に入る点（無効範囲１５４内の点）は、マッチングに利用しない。したがって、正しいマッチングを実施できる。

上述した第１の実施の形態によれば、次の作用効果が得られる。
（１）車載処理装置１２０は、物体の一部を表す点の第１座標系（駐車場座標系）における座標が複数含まれる点群データ（点群地図１２４Ａ）が格納される記憶部１２４と、車両１の周囲の情報を取得するカメラ１０２の出力を取得するセンサ入力部（インタフェース１２５）と、車両１の移動に関する情報を取得する移動情報取得部（インタフェース１２５）と、センサ入力部、および移動情報取得部が取得する情報に基づき、第２座標系（局所座標系）における車両の位置および物体の一部を表す点の第２座標系（局所座標系）における座標が複数含まれる局所周辺情報１２２Ｂを生成する局所周辺情報作成部と、点群地図１２４Ａおよび局所周辺情報１２２Ｂに基づき第１座標系と第２座標系の関係を推定し、第１座標系における車両１の位置を推定する位置推定部１２１Ｃとを備える。

車載処理装置１２０は、点群地図１２４Ａと局所周辺情報１２２Ｂとに基づき駐車場座標系と局所座標系の座標変換式を推定し、駐車場座標系における車両１の位置を推定する。点群地図１２４Ａはあらかじめ記憶部１２４に記憶された情報であり、局所周辺情報１２２Ｂはカメラ１０２、車速センサ１０８、および舵角センサ１０９の出力から生成される。すなわち車載処理装置１２０は、記録された点群の座標系とは異なる座標系の点群の情報を取得し、異なる座標系間の対応関係に基づいて、記録された座標系における車両１の位置を推定することができる。また車載処理装置１２０は、点群地図１２４Ａと局所周辺情報１２２Ｂとに基づき駐車場座標系と局所座標系の座標変換式を推定するので、局所周辺情報１２２Ｂの点群データの一部にノイズが含まれていても影響を受けにくい。すなわち、車載処理装置１２０による車両１の位置推定は外乱に強い。

（２）位置推定部１２１Ｃは、局所周辺情報１２２Ｂを構成するそれぞれの点に対応する点を点群データの中から探索し（図７のステップＳ６５０）、対応する点同士の距離が最小になるように第１座標系と第２座標系との座標変換式を推定する（図７のステップＳ６５１）。

（３）位置推定部１２１Ｃは、点群データまたは局所周辺情報１２２Ｂを構成する点と、この点と対応する点までの距離があらかじめ定められた閾値よりも遠い局所周辺情報の点データを除外して、すなわちアウトライアリスト１２２Ａを適用して（図７のステップＳ６４１、Ｓ６５３）探索および推定を行う。そのため、ノイズ成分とみなすことのできる距離が離れた点群データを計算対象から除外するので、座標変換式の精度が向上することができる。

（４）点群データ、および局所周辺情報に含まれる点は二次元空間上の座標として表される。位置推定部１２１Ｃは、対応する点同士の距離があらかじめ定められた閾値よりも遠く、かつ局所周辺情報１２２Ｂにおける空間的に端部に位置する点を除外して探索および推定を行う。

点群地図１２４Ａの駐車場データに格納される点群は、ユーザが記録開始ボタン１１０Ａを押した地点よりも駐車位置の近くのランドマークに関するものである。その地点よりも駐車領域から遠い地点で図５に示すフローチャートの動作が開始されると、駐車場データに格納されるいずれの点とも対応しない点が局所周辺情報１２２Ｂに含まれる。そのような点を含めてＩＣＰ、すなわち図７のステップＳ６５０〜Ｓ６５２の処理を行うと適切な解が得られない。そこでこれらを排除することで適切な解が得られるようにする。

（５）点群データには周期的な特徴が含まれる。位置推定部１２１Ｃは、第１座標系と第２座標系との座標変換式を推定した後に、対応する点同士の距離が短くなるように、周期的な特徴の１周期分の距離に基づき第１座標系と第２座標系との座標変換式を補正する（図７のステップＳ６４６〜Ｓ６４８）。

一般に、点群データに周期的な特徴が含まれると、その周期に相当する距離の整数倍ずれてマッチングしやすい傾向にある。一度そのようにずれてマッチングしてしまうと、繰り返し処理の性質から正しい位置へマッチングさせることが困難である。そこで繰り返し処理の解が収束した後にその周期の整数倍ずらすことでこの問題を解決する。換言すると、繰り返し計算により大域解から周期的な特徴の数周期分ずれた局所解に陥った可能性を考慮し、前述の周期の数周期分ずらすことにより大域解、または大域解により近い局所解を得ることができる。

（６）従前に推定した第１座標系における車両の位置、センサ入力部が取得する最新の情報、および移動情報取得部が取得する最新の情報に基づき作成される第１座標系における局所周辺情報と、第１座標系における点群データとの一致度を示す指標である瞬間一致度ＩＣが、あらかじめ定められた閾値よりも小さい場合に、位置推定部は補正を行う（図７のステップＳ６４１Ａ〜ステップＳ６４３）。そのため、図７のステップＳ６４４〜Ｓ６４８の処理を常時実行するのではなく、その処理の必要性を検討して必要と判断される場合のみ実行する。

（７）車載処理装置１２０は、センサ入力部、および移動情報取得部が取得する情報に基づき、移動体を除いた物体の一部を表す点の第３座標系（記録座標系）における座標が複数含まれる第２点群データを作成して、点群地図１２４Ａとして記憶部１２４に格納する点群取得部１２１Ａを備える。そのため車載処理装置１２０は、車載処理装置１２０を搭載する車両１が走行する際に点群地図１２４Ａを作成することができる。点群地図１２４Ａの作成処理と車両１の位置推定処理とはランドマーク測位の点で共通しており、プログラムモジュールを共通して使用することができる。

（８）第３座標系（記録座標系）は第２点群データの作成を開始した際の車両の位置および姿勢に基づいて設定される。点群取得部１２１Ａは、第２点群データの作成を開始した際の車両の位置または姿勢が異なることにより座標系が異なる複数の第２点群データが得られると、車両１の駐車位置を基準として異なる座標系の関係を推定して複数の第２点群データを統合する（図４のステップＳ５０７、Ｓ５０９）。そのため、点群データを取得するたびに異なる記録座標系が設定されても、複数の点群データを統合することができる。これは点群データの取得を開始する位置やその際の車両１の姿勢は様々であるが、同じ駐車位置に車両１が駐車される点に注目している。

（９）車載処理装置１２０は、位置推定部１２１Ｃの出力に基づき車両を駆動し、第１座標系におけるあらかじめ指定された駐車位置に車両を移動させる車両制御装置１３０と、車両１の位置に関する情報（緯度経度）を受信するカーナビゲーションシステム１０７から車両１の緯度経度を取得する位置情報取得部（インタフェース１２５）とを備える。点群地図１２４Ａを構成する点は駐車場の構成物の一部を表す点である。記憶部１２４には点群データの静止物からなる駐車場の緯度経度もあわせて格納される。カーナビゲーションシステム１０７が測定する位置と駐車場の位置との距離の差があらかじめ定めた距離よりも短い場合に、車両制御装置１３０を用いて車両１を駐車位置に移動させる制御部、すなわち演算部１２１を備える。そのため車載処理装置１２０は、車両１に搭載されるいずれのセンサも駐車位置を直接観測できない遠方をスタート地点として、点群地図１２４Ａに含まれる駐車位置へ車両１を自動駐車することができる。

（１０）車載処理装置１２０は、カーナビゲーションシステム１０７と、カメラ１０２、ソナー１０３およびレーダ１０４の周囲の点群をセンシングするセンサ群とを備え、地図生成部１２１Ｂにおいて、図２に示すような点群と緯度経度情報を紐付けしたデータフォーマットの点群地図１２４Ａを生成する。緯度経度情報を参照することで、周囲の点群の有無や道路地図との関係を判断でき、記憶モードや位置推定モードに自動で遷移することができる。そのためユーザがモード遷移のための操作をする必要がなくなり、記憶や自動駐車に走行しながらスムーズに移行することができる。

上述した第１の実施の形態は、以下のように変形してもよい。
（１）車載処理装置１２０は、複数のカメラと接続されてもよい。車載処理装置１２０は、複数のカメラの撮影画像を用いることで、車両１の周辺に存在する広範囲のランドマークから点群を抽出することができる。

（２）車載処理装置１２０は、車速センサ１０８および舵角センサ１０９からセンシング結果を受信しなくてもよい。この場合、車載処理装置１２０は、カメラ１０２の撮影画像を用いて車両１の移動を推定する。車載処理装置１２０は、ＲＯＭ１２３に格納された内部パラメータおよび外部パラメータを用いて、被写体とカメラ１０２との位置関係を算出する。そして複数の撮影画像においてその被写体を追跡することにより、車両１の移動量および移動方向を推定する。

（３）点群地図１２４Ａや局所周辺情報１２２Ｂなどの点群情報は３次元情報として格納されてもよい。３次元の点群情報は、２次元平面上に投影することにより第１の実施の形態と同様に２次元で他の点群と比較してもよいし、３次元同士で比較してもよい。この場合、車載処理装置１２０は、以下のようにランドマークの三次元点群を得ることができる。すなわち、車速センサ１０８および舵角センサ１０９の出力に基づき算出した車両１の移動量と、カメラ１０２が出力する複数の撮影画像とを用いて、公知のモーションステレオ技術や、そのモーション推定部分を内界センサや測位センサで補正した情報を用いることで、静止立体物の３次元点群を得ることができる。

（４）車載処理装置１２０は、図７のステップＳ６４３において、１回だけの否定判定によりステップＳ６４４に進むのではなく、数回連続で否定判定された場合にステップＳ６４４に進んでもよい。

（５）車載処理装置１２０は、図７のステップＳ６４３の判断に代えて、局所周辺情報１２２Ｂにおけるアウトライアと判断されている点の割合があらかじめ定めた閾値よりも大きいか否かを判断してもよい。その割合が閾値よりも大きい場合はステップＳ６４４に進み、その割合が閾値以下の場合はステップＳ６５０に進む。さらに車載処理装置１２０は、図７のステップＳ６４３の判断に加えて前述の割合が大きい場合のみステップＳ６４４に進んでもよい。

（６）車載処理装置１２０は、図７のステップＳ６４４、Ｓ６４６の処理をあらかじめ行ってもよい。さらにその処理結果を記憶部１２４に記録してもよい。

（７）車載処理装置１２０は、ユーザからの動作指令を車両１の内部に設けられた入力装置１１０からだけでなく、通信装置１１４から受信してもよい。たとえばユーザの所持する携帯端末と通信装置１１４が通信を行い、ユーザが携帯端末を操作することにより、車載処理装置１２０は自動駐車ボタン１１０Ｂが押された場合と同様の動作を行ってもよい。この場合は、車載処理装置１２０はユーザが車両１の内部に居る場合だけでなく、ユーザが降車した後にも自動駐車を行うことができる。

（８）車載処理装置１２０は、点群地図１２４Ａに記録された駐車位置だけでなく、ユーザにより指定された位置に駐車を行ってもよい。ユーザによる駐車位置の指定は、たとえば車載処理装置１２０が表示装置１１１に駐車位置の候補を表示し、ユーザが入力装置１１０によりそのいずれかを選択することにより行われる。

（９）車載処理装置１２０は、通信装置１１４を経由して外部から点群地図１２４Ａを受信してもよいし、作成した点群地図１２４Ａを通信装置１１４を経由して外部に送信してもよい。また、車載処理装置１２０が点群地図１２４Ａを送受信する相手は他の車両に搭載された別の車載処理装置１２０でもよいし、駐車場を管理する組織が管理する装置でもよい。

（１０）自動駐車システム１００は、カーナビゲーションシステム１０７に代えて携帯端末を備え、携帯端末が通信を行う基地局の識別情報を緯度経度の代わりに記録してもよい。基地局の通信範囲は数百ｍ程度に限られるので、通信を行う基地局が同一であれば同一の駐車場の可能性が高いからである。

（１１）駐車場データに含まれる周期的な特徴は駐車枠に限定されない。たとえば路面ペイントの１つである横断歩道を構成する複数の直線なども周期的な特徴である。また、駐車場データがレーザレーダなどで取得した、壁などの障害物の情報から構成される場合は、規則的に並んだ柱も周期的な特徴である。

（１２）上述した実施の形態では移動体である車両や人間はランドマークに含めなかったが、移動体をランドマークに含めてもよい。その場合は移動体であるランドマークと移動体以外のランドマークを識別可能に記憶してもよい。

（１３）車載処理装置１２０は、記録モードにおいて、検出したランドマークを識別し、それぞれのランドマークの識別結果を点群地図１２４Ａに併せて記録してもよい。ランドマークの識別には、撮影画像から得られるランドマークの形状情報や色情報、さらに公知のモーションステレオ技術によるランドマークの立体形状情報が用いられる。ランドマークはたとえば、駐車枠、駐車枠以外の路面ペイント、縁石、ガードレール、壁、などのように識別される。さらに車載処理装置１２０は、移動体である車両や人間をランドマークに含め、他のランドマークと同様に識別結果を点群地図１２４Ａに併せて記録してもよい。この場合は車両と人間をあわせて「移動体」として識別および記録してもよいし、車両と人間を個別に識別および記録してもよい。

以上、本発明の実施形態について詳述したが、本発明は、前記の実施形態に限定されるものではなく、特許請求の範囲に記載された本発明の精神を逸脱しない範囲で、種々の設計変更を行うことができるものである。例えば、前記した実施の形態は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施形態の構成の一部を他の実施形態の構成に置き換えることが可能であり、また、ある実施形態の構成に他の実施形態の構成を加えることも可能である。さらに、各実施形態の構成の一部について、他の構成の追加・削除・置換をすることが可能である。

１、１Ａ、１Ｂ … 車両
１００、１００Ａ、１００Ｂ … 自動駐車システム
１０２ … カメラ
１０７ … カーナビゲーションシステム
１０８ … 車速センサ
１０９ … 舵角センサ
１１０Ａ … 応答ボタン
１１０Ｂ … 自動駐車ボタン
１２０ … 車載処理装置
１２１ … 演算部
１２１Ａ … 点群取得部
１２１Ｂ … 地図生成部
１２１Ｃ … 位置推定部
１２１Ｄ … モード切り替え部
１２２Ａ … アウトライアリスト
１２２Ｂ … 局所周辺情報
１２４ … 記憶部
１２４Ａ … 点群地図
１２５ … インタフェース
１３０ … 車両制御装置
ＩＢ … 点群一致率
ＩＣ … 瞬間一致度
ＩＷ … 全体一致度

Claims (6)

1. 物体の一部を表す点の第１座標系における座標が複数含まれる点群データが経度緯度の測位情報と対応付けて格納される記憶部と、
   車両の周囲の情報を取得するセンサの出力を取得するセンサ入力部と、
   前記車両の移動に関する情報を取得する移動情報取得部と、
   前記センサ入力部、および前記移動情報取得部が取得する情報に基づき、第２座標系における前記車両の位置および物体の一部を表す点の前記第２座標系における座標が複数含まれる局所周辺情報を生成する局所周辺情報作成部と、
   前記点群データと前記局所周辺情報とに基づき前記第１座標系と前記第２座標系の関係を推定し、前記第１座標系における前記車両の位置を推定する位置推定部と、
   予め設定された登録地点に前記車両が接近しかつ前記車両の周囲の点群データが前記記憶部に格納されていない場合に前記点群データを取得して前記記憶部に記憶する地図記憶モードと、前記登録地点に前記車両が接近しかつ前記車両の周囲の点群データが前記記憶部に格納されている場合に前記位置推定部により前記車両の位置を推定する位置推定モードとを、前記車両の緯度経度の測位情報に基づいて遷移させるモード切り替え部を備えることを特徴とする車載処理装置。
2. 請求項１に記載の車載処理装置において、
   前記モード切り替え部は、前記位置推定モードで前記位置推定部により前記車両の位置が推定され、かつ、前記登録地点の近傍で前記車両の自動制御ボタンが操作された場合に、前記車両の自動制御を行う自動制御モードに遷移させることを特徴とする車載処理装置。
3. 請求項２に記載の車載処理装置において、
   前記モード切り替え部は、前記位置推定モードもしくは前記自動制御モードにおいて、前記車両の位置を見失った場合に、前記地図記憶モードと前記位置推定モードと前記自動制御モードのいずれも実施しないギブアップモードにモードを切り替えることを特徴とする車載処理装置。
4. 請求項１に記載の車載処理装置において、
   前記車両の走行距離と軌跡形状に基づき前記局所周辺情報の点群から有効点群を選定する選定手段を有し、
   前記位置推定部は、該選定手段により選定された局所周辺情報の有効点群と前記点群データとのマッチングを行い、前記第１座標系における前記車両の位置を算出することを特徴とする車載処理装置。
5. 請求項１に記載の車載処理装置において、
   前記地図記憶モードでは、前記車両のパーキングブレーキが作動され、かつ前記点群データを前記記憶部に記憶するための応答ボタンが押下されることによって、前記登録地点周辺の点群データが前記記憶部に記憶されることを特徴とする車載処理装置。
6. 請求項２に記載の車載処理装置において、
   前記自動制御モードでは、前記位置推定部により前記車両の位置推定に成功しており、かつ、前記車両の位置が前記登録地点から所定の範囲内にあり、かつ、前記車両を自動制御するための前記自動制御ボタンが押下されている間だけ、前記車両の自動制御が行われることを特徴とする車載処理装置。

Images