JP2015206733A - 位置検出装置及び位置検出方法 - Google Patents

Abstract

【課題】円筒状物標の両径方向端までの方向が取得されているか不明であっても円筒状物標の中心位置を測定できる位置検出装置及び位置検出方法を提供する。【解決手段】位置検出装置３０は、周囲の物体までの距離及び方向を取得することにより周囲の物体の複数の位置点を測定し、位置が測定された複数の位置点の中から円筒状物標の外周の候補点の集合を抽出する。位置検出装置３０は、円周及び中心位置が設定された円形と候補点の集合により形成される形状との間の合致度を算出し、合致度に応じて円形の中心位置を円筒状物標の中心位置として特定するか否かを判断する。【選択図】図３

Description

本発明は、位置検出装置及び位置検出方法に関する。

円筒状物標の中心座標を測定する技術として、例えば特許文献１に記載の計測方法がある。この計測方法では、円筒状物標の両径方向端までの各々の方向を測定し、これら方向の中央線上で円筒状物標までの距離を測定して円筒状物標の直径を算出する。中央線と円筒状物標の外周との交点から中央線に沿って半径分進んだ位置を中心位置として求める。

特開２０１３−２１７８０７号公報

しかしながら、特許文献１に記載の測定方法では、円筒状物標の両径方向端までの各々の方向が取得されない場合には円筒状物標の中心位置を求めることができない。
本発明の課題は、円筒状物標の両径方向端までの各々の方向が取得されているか否かが不明な場合であっても円筒状物標の中心位置を測定できる位置検出装置及び位置検出方法を提供することである。

本発明の一態様に係る位置検出装置は、周囲の物体までの距離及び方向を取得することにより周囲の物体の複数の位置点を測定し、位置が測定された複数の位置点の中から円筒状物標の外周の候補点の集合を抽出する。位置検出装置は、円周及び中心位置が設定された円形と候補点の集合により形成される形状との間の合致度を算出し、合致度に応じて円形の中心位置を円筒状物標の中心位置として特定するか否かを判断する。

本発明によれば、円筒状物標の両径方向端までの各々の方向が取得されているか否かが不明な場合であっても円筒状物標の中心位置を測定することができる。

本発明に係る位置検出装置を備える車両の構成例を示す図である。 位置測定部による走査点の位置の測定の説明図である。 本発明の第１実施形態に係る位置検出装置の機能構成を示す図である。 候補点の抽出領域の第１例の説明図である。 測定位置情報の修正処理の一例の説明図である。 位置測定部による走査間隔の説明図である。 近距離領域のセグメントと設定された円形との間の合致度の決定方法の一例の説明図である。 遠距離領域のセグメントと設定された円形との間の合致度の決定方法の一例の説明図である。 車両の絶対位置の算出処理の一例の説明図である。 位置検出装置の動作の一例の説明図である。 本発明の第２実施形態に係る位置検出装置の機能構成を示す図である。 候補点の抽出領域の第２例の説明図である。

以下、本発明の実施形態について図面を参照しながら詳細に説明する。
（第１実施形態）
（構成）
図１は、本発明に係る位置検出装置を備える車両の構成例を示す図である。本発明に係る位置検出装置は、例えば車両１に搭載され、車両１と円筒状物標との相対位置を検出するために使用される。位置検出装置は、検出した相対位置を用いて車両１の現在位置を検出する。車両検出装置により検出された車両１の位置は、例えば、車両１に搭載されたナビゲーション装置において車両１の現在位置を取得するために使用したり、又は車両１の自動運転に使用することができる。また、車両検出装置により検出された車両１の位置は、例えば車両１が走行車線の中央付近を継続して走行するよう操作力を制御する運転支援に使用することができる。なお、本発明に係る位置検出装置の使用用途はこれに限定されない。本発明に係る位置検出装置は、円筒状物標までの相対位置の検出に広く使用することができる。また、位置検出装置は、車両以外の移動体に搭載されてもよい。

車両１は、車輪２と、車速センサ３と、ステアリングホイール４と、操舵角センサ５と、位置測定部６と、ヨーレートセンサ７と、コントローラ１０と、地図情報記憶部２０を備える。

車速センサ３は、車輪２の車輪速を検出し、車輪速に基づいて車両１の自車速度を算出する。車速センサ３は、算出した車速情報をコントローラ１０へ出力する。車速センサ３は、例えば車輪速パルスを計測するロータリエンコーダなどのパルス発生器を備えていてよい。

操舵角センサ５は、ステアリングホイール４の操舵角を検出し、検出した操舵角情報をコントローラ１０へ出力する。操舵角センサ５は、ステアリング軸などに設けられる。操舵角センサ５は、操向輪の転舵角を操舵角情報として検出してもよい。

位置測定部６は、車両１周囲の物体の表面を走査して車両１から物体表面上の走査点までの距離を測定し且つ走査点までの方向を取得することにより、車両１に対する走査点の相対位置を測定する。位置測定部６は、例えば、車両１に搭載されたレーザレンジファインダを備えてよい。

図２は、位置測定部６による走査点の位置の測定の説明図である。図２において、ｙ軸方向は車両１の横方向でありｘ軸方向は車両１の前後方向である。図４及び図５においても同様である。位置検出装置３０が車両１に搭載される場合、例えば位置測定部６は、物体５０の表面上の走査点６０の相対位置を、車両１の位置を原点とする相対座標系における走査点６０までの距離ｒ１と方向θ１によって定まる位置として測定してよい。

位置測定部６は、方向θ１として車両１の前後方向に対する走査点６０の水平角を取得してよい。例えば位置測定部６は、方向θ１として車両１を原点とし前後方向を基準とする走査点６０の方位を取得してよい。円筒状物標が地上に立設された円筒状の物体であり円筒状物標の軸方向が鉛直方向である場合、方向θ１は、軸方向と垂直な面内において車両１と走査点６０を結ぶ直線と車両１の前後方向とがなす角度として取得される。以下の説明において位置測定部６により測定された相対位置を「測定位置情報」と表記することがある。また、以下の説明において車両１に対する相対位置を単に「相対位置」と表記することがある。位置測定部６は、測定位置情報をコントローラ１０へ出力する。

図１を参照する。ヨーレートセンサ７は、車両１のヨーレート（車体が旋回する方向への回転角の変化速度）を検出し、検出したヨーレート情報をコントローラ１０へ出力する。地図情報記憶部２０には、円筒状物標の中心位置の位置情報が記憶されている。円筒状物標は、例えば道路脇に設置される電柱、信号柱、標識柱、照明柱などであってよい。

地図情報記憶部２０に記憶された位置情報は、地上の固定点を原点とする絶対座標系上の絶対位置によって、円筒状物標の中心位置を表す。以下の説明において、地図情報記憶部２０に記憶された絶対座標系上の絶対位置を単に「絶対位置」と表記することがある。地図情報記憶部２０は、例えば、ナビゲーション装置内の地図情報データ記憶装置であってよい。

コントローラ１０は、ＣＰＵ（Central Processing Unit）と、ＲＯＭ（Read Only Memory）およびＲＡＭ（Random Access Memory）等のＣＰＵ周辺部品とを含む電子制御ユニットである。コントローラ１０は、位置測定部６から出力された測定位置情報の中から円柱状物標の候補点を抽出する。コントローラ１０は、抽出された候補点に基づいて円柱状物標の中心位置の相対位置を特定する。コントローラ１０は、地図情報記憶部２０から円柱状物標の中心位置の絶対位置を読み出す。コントローラ１０は、特定された円柱状物標の中心位置の相対位置と読み出された円柱状物標の中心位置の絶対位置とに基づいて、車両１の現在の絶対位置を算出する。

以下、コントローラ１０による処理を詳細に説明する。図３は、本発明の第１実施形態に係る位置検出装置の機能構成を示す図である。位置検出装置３０は、前述の位置測定部６及び地図情報記憶部２０と、候補点抽出部３１と、記憶部３２と、移動量算出部３３と、測定位置情報修正部３４と、物標位置特定部３５と、位置算出部３６を備える。候補点抽出部３１、移動量算出部３３、測定位置情報修正部３４、物標位置特定部３５、及び位置算出部３６による後述の処理は、コントローラ１０により実行される。

位置測定部６は、測定位置情報を候補点抽出部３１へ出力する。候補点抽出部３１は、位置測定部６から受信した測定位置情報の中から、円筒状物標の外周の候補点を抽出する。このため、候補点抽出部３１は候補点を抽出する抽出領域を設定する。

図４は、候補点の抽出領域の第１例の説明図である。ハッチングされた領域は抽出領域を示す。候補点抽出部３１は、横方向位置が所定範囲内である領域を抽出領域として設定する。すなわち、候補点抽出部３１は車両１との横方向位置の差が所定範囲外の測定位置情報を候補点として抽出しない。

例えば、車両１の走行車線の幅をＷｖとし走行車線の脇の歩道の幅をＷｐとする。例えば、車線の幅Ｗｖは３ｍであってよく、歩道の幅Ｗｐは１．５ｍであってよい。車両１の左側の抽出領域５１は、横方向位置ｙがＷｖ／２＜ｙ＜Ｗｖ／２＋Ｗｐを満足する領域に設定される。候補点抽出部３１は、抽出領域５１に存在する円筒状物標５２の測定位置情報を候補点として抽出し、抽出領域５１の外に存在する物体５３の測定位置情報を抽出しない。

車両１の右側の抽出領域は、横方向位置ｙが−（３Ｗｖ／２＋Ｗｐ）＜ｙ＜−３Ｗｖ／２を満足する領域に設定される。候補点抽出部３１は、抽出領域５４に存在する円筒状物標５５の測定位置情報を候補点として抽出し、抽出領域５４の外に存在する物体５６の測定位置情報を抽出しない。

候補点抽出部３１は、設定された抽出領域内の測定位置情報にセグメンテーション処理を施す。候補点抽出部３１は、閾値Ｓ以下の大きさのセグメントに属する候補点のみを円筒状物標の候補点として抽出する。閾値Ｓは例えば１ｍであってよい。候補点抽出部３１は、抽出された候補点を記憶部３２に記憶する。

図３を参照する。車速センサ３は、車両１の車速情報を移動量算出部３３へ出力する。ヨーレートセンサ７は、車両１のヨーレート情報を移動量算出部３３へ出力する。移動量算出部３３は、受信した車速情報及びヨーレート情報に基づいて、時刻ｔ−１から時刻ｔまでの間の車両１の移動量（ｄｘ，ｄｙ，ｄφ）を算出する。

時刻ｔは、直近の位置測定部６の測定時刻であり、時刻ｔ−１は、時刻ｔより前の位置測定部６の測定時刻である。例えば、位置測定部６の測定間隔がＴｍであり、時刻ｔは時刻ｔ−１より時間Ｔｍだけ遅れた時刻であってよい。ｄｘ及びｄｙは、それぞれ車両１の前後方向及び横方向における並進移動量を示す。ｄφは、車体が旋回する方向への回転量を示す。移動量算出部３３は、算出した移動量を測定位置情報修正部３４と位置算出部３６へ出力する。

測定位置情報修正部３４は、上述の候補点の抽出処理で時刻ｔに測定された測定位置情報から候補点が抽出される前に、記憶部３２に記憶された時刻ｔ−１における候補点の位置情報を時刻ｔにおける位置情報に修正する。以下の説明において測定位置情報修正部３４により修正された位置情報を「修正位置情報」と表記することがある。

時刻ｔ−１における候補点の位置情報には、時刻ｔ−１に測定された測定位置情報だけでなく、時刻ｔ−１より前に測定され測定位置情報修正部３４によって時刻ｔ−１における位置情報に修正された修正位置情報が含まれてもよい。例えば時刻ｔ−１における候補点の位置情報は、時刻ｔ−１より前の時刻ｔ−２に測定され、時刻ｔ−１に測定された測定位置情報から候補点が抽出される前に測定位置情報修正部３４によって時刻ｔ−１における位置情報に修正された修正位置情報を含んでもよい。

図５は、測定位置情報の修正処理の一例の説明図である。参照符号１ａは、時刻ｔ−１における車両１の位置を示す。参照符号１ｂは、時刻ｔにおける車両１の位置を示す。図５に示すとおり時刻ｔにおける車両１の位置は、時刻ｔ−１における車両１の位置よりｘ軸方向及びｙ軸方向にそれぞれｄｘ及びｄｙだけ移動する。車両１のヨー角はｄφだけ増加する。
時刻ｔ−１における候補点６１の測定位置の距離及び角度がそれぞれｒ１及びθ１であり、ｘ軸方向位置ｘ１がｘ１＝ｒ１×ｃｏｓθ１であり、ｙ軸方向位置ｙ１がｙ１＝ｒ１×ｓｉｎθ１であると想定する。測定位置情報修正部３４は、次式（１）〜（３）に従って、記憶部３２に記憶された時刻ｔ−１における候補点６１の位置情報の値を、時刻ｔにおける推定位置（ｒ２，θ２）に修正する。

図３を参照する。候補点抽出部３１は、上述の候補点の抽出処理において時刻ｔに測定された測定位置情報から候補点を抽出する際に、時刻ｔ−１における位置情報から時刻ｔにおける位置情報に修正された修正位置情報を記憶部３２から読み込む。候補点抽出部３１は、時刻ｔに測定した測定位置情報と修正位置情報とを混合する。そして、候補点抽出部３１は、混合して得られた位置情報の集合から上述の候補点の抽出処理により円筒状物標の候補点を抽出する。すなわち、候補点抽出部３１は、混合して得られた位置情報の集合のうち、候補点の抽出領域内にあり且つ閾値Ｓ以下の大きさのセグメントに属する候補点のみを円筒状物標の候補点として抽出する。候補点抽出部３１は、抽出されたセグメントを物標位置特定部３５に出力する。

物標位置特定部３５は、候補点抽出部３１から出力されたセグメントを受信する。物標位置特定部３５はこれらセグメントの中に、車両１との離間距離が所定距離Ｌ以下のセグメントがあるか否かを判断する。所定距離Ｌは、例えば１０ｍであってよい。

図６は、位置測定部６による走査間隔の説明図である。図６の破線は、位置測定部６が一定のステップ角度で車両１の右前方領域を走査した場合における各々の測定方向を示す。図６に示すように、車両１に比較的近い円筒状物標６２の外周の走査点の間隔は比較的小さいのに対し、車両１から比較的遠い円筒状物標６３の外周の走査点の間隔は比較的大きくなる。以下の説明において車両１からの離間距離が閾値距離Ｌ以下の領域を「近距離領域」と表記することがある。車両１からの離間距離が閾値距離Ｌより大きい領域を「遠距離領域」と表記することがある

近距離領域の円筒状物標の外周は比較的密に走査される。このため、物標位置特定部３５は、近距離領域のセグメントに基づいて円筒状物標の半径及び中心位置を算出する。例えば物標位置特定部３５は、セグメントに含まれる候補点の位置のうち最も離れた２点間の距離の半分を円筒状物標の半径Ｒとして算出する。例えば物標位置特定部３５は、最も離れた２点間の中点を円筒状物標の中心位置として算出する。

そして、物標位置特定部３５は、半径及び中心位置が算出された近距離領域のセグメントが、円筒状物標の測定結果であるか否かを判断する。物標位置特定部３５は、算出した半径Ｒ及び中心位置の円形を設定する。すなわち、算出した半径Ｒの円周及び中心位置を持つ円形を設定する。物標位置特定部３５は、セグメントにより形成される形状と設定された円形と間の合致度に応じて、セグメントが円筒状物標の測定結果であるか否かを判断する。

例えば物標位置特定部３５は、合致度が閾値以上である場合にセグメントが円筒状物標の測定結果であると判断し、合致度が閾値未満の場合にセグメントが円筒状物標の測定結果でないと判断してよい。図７は、近距離領域のセグメントと設定された円形との間の合致度の決定方法の一例の説明図である。円形７０は、設定された円形でありその半径はＲである。円形７１は、円形７０と同じ中心点と半径（Ｒ−Δ）を有する。円形７２は、円形７０と同じ中心点と半径（Ｒ＋Δ）を有する。

例えば物標位置特定部３５は、セグメントに属する候補点７３が円形７１と円形７２との間の範囲にあるか否かを判断する。例えば物標位置特定部３５は、セグメントに属する候補点７３の総数に対する、円形７１と円形７２との間の範囲にある候補点７３の数の割合に応じて合致度を算出する。すなわち、物標位置特定部３５は、セグメントに属する候補点７３の総数に対する、円形７０の円周線から所定長Δ未満離れた候補点７３の数の割合に応じて、合致度を算出する。閾値は例えば８０％であってよい。

セグメントが円筒状物標の測定結果であると判断した場合、物標位置特定部３５は、設定した円形７０の中心位置を近距離領域の円筒形物標の中心位置の相対位置として位置算出部３６へ出力する。セグメントが円筒状物標の測定結果であると判断した場合、物標位置特定部３５は、設定した円形７０の円の大きさとして半径Ｒを記憶部３２に記憶する。物標位置特定部３５は、円形７０の半径Ｒの代わりに直径２Ｒを記憶部３２に記憶してもよい。

次に、物標位置特定部３５は、遠距離領域の円筒状物標の中心位置を特定する。物標位置特定部３５は、遠距離領域の円筒状物標の仮想半径Ｒｄを設定する。物標位置特定部３５により近距離領域の円筒形物標について算出された半径Ｒが記憶部３２に記憶されている場合には、物標位置特定部３５は、算出された半径Ｒを仮想半径Ｒｄとして設定する。近距離領域の円筒形物標について算出された複数個の半径が記憶部３２に記憶されている場合には、物標位置特定部３５は、これら半径のいずれか、平均値又は中間値を仮想半径Ｒｄとして設定してよい。これら複数個の半径は、例えば、複数個の異なる円筒形物標について算出した半径であってよく、又は異なる複数時刻に同じ円筒形物標について算出した半径であってもよい。

物標位置特定部３５により近距離領域の円筒形物標について算出された半径Ｒが記憶部３２に記憶されていない場合には、物標位置特定部３５は、所定値Ｒ０を記憶部３２から読み出す。物標位置特定部３５は、記憶部３２から読み出された所定値Ｒ０を仮想半径Ｒｄとして設定する。所定値Ｒ０は例えば０．３ｍであってよい。

物標位置特定部３５は、設定された仮想半径Ｒｄを半径として有する円形を設定する。すなわち、仮想半径Ｒｄを持つ円周を有する円形を設定する。物標位置特定部３５は、遠距離領域のセグメントにより形成される形状と設定された円形との間の合致度を算出する。

図８は、遠距離領域のセグメントと設定された円形との間の合致度の決定方法の一例の説明図である。円形７５は、設定された円形でありその半径はＲｄである。円形７６は、円形７５と同じ中心点と半径（Ｒｄ−Δ）を有する。円形７７は、円形７５と同じ中心点と半径（Ｒｄ＋Δ）を有する。

例えば物標位置特定部３５は、近距離領域のセグメントと円形との合致度の場合と同様に、遠距離領域のセグメントと設定された円形との間の合致度を算出してよい。例えば物標位置特定部３５は、セグメントに属する候補点７８が円形７６と円形７７との間の範囲にあるか否かを判断してよい。例えば物標位置特定部３５は、セグメントに属する候補点７８の総数に対する、円形７６と円形７７との間の範囲にある候補点７８の数の割合に応じて合致度を算出する。

物標位置特定部３５は、合致度が最も大きい円形を算出する。例えば円形物標位置特定部３５は、中心位置を変えながら半径Ｒｄの円形を複数回繰り返し設定し、これら設定された円形の中から合致度が最も大きい円形を選択してもよい。物標位置特定部３５は、所定の上限回数だけ繰り返し円形を設定しても合致度が所定の下限閾値を超えない場合、セグメントが円筒状物標でないと判断してよい。物標位置特定部３５は、合致度が最も大きい円形の中心位置を、遠距離領域の円筒形物標の中心位置の相対位置として位置算出部３６へ出力する。すなわち、物標位置特定部３５は、セグメントと設定された円形との間の合致度に応じて、設定された円形の中心位置を円筒状物標の中心位置として特定するか否かを判断する。なお、物標位置特定部３５は、合致度が最も大きい円形に代えて、合致度が所定値より大きい円形又は合致度が最も大きく且つ合致度が所定値より大きい円形を算出し、その中心位置を円筒形物標の中心位置の相対位置として位置算出部３６へ出力してもよい。また、物標位置特定部３５は、中心位置が異なる複数の円形について合致度を順次算出し、最初に合致度が閾値以上になる円形を見つけた時に、この円形の中心位置を円筒状物標の中心位置の相対位置としてもよい。

図３を参照する。位置算出部３６は、地図情報記憶部２０から円筒状物標の中心位置の絶対位置を読み出す。位置算出部３６は、物標位置特定部３５から円筒状物標の中心位置の相対位置を受信する。位置算出部３６は、地図情報記憶部２０から読み出した円筒状物標の中心位置の絶対位置と、物標位置特定部３５から受信した円筒状物標の中心位置の相対位置に基づいて、時刻ｔにおける車両１の絶対位置及び車両１の前後方向の向きを算出する。以下の説明において、車両１の前後方向の向きを単に「車両１の向き」と表記することがある。

図９は、車両１の絶対位置の算出処理の一例の説明図である。参照符号１ａは、時刻ｔ−１における車両１の位置を示す。参照符号１ｂは、時刻ｔにおける車両１の位置を示す。時刻ｔ−１における車両１の絶対位置（Ｘｔ−１，Ｙｔ−１）及び向き（Φｔ−１）は算出済みであると想定する。初回算出時に（Ｘｔ−１，Ｙｔ−１，Φｔ−１）が算出されていない場合には、他の測位手段による測定値を使用してよい。他の測位手段は、例えばＧＰＳ（Global Positioning System）などであってよい。

位置算出部３６は、時刻ｔにおける車両１の仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）を決定する。例えば位置算出部３６は、時刻ｔ−１における絶対位置（Ｘｔ−１，Ｙｔ−１）及び向き（Φｔ−１）を移動量（ｄｘ，ｄｙ，ｄφ）により修正し、時刻ｔにおける仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）の初期値を決定する。

位置算出部３６は、地図情報記憶部２０に記憶されている円筒状物標の中から車両１の絶対位置の算出に使用する円筒状物標を選択する。例えば位置算出部３６は、仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）に応じて、車両１の前方領域付近に存在する円筒状物標を車両１の絶対位置の算出に使用する円筒状物標として選択する。選択された円筒状物標の絶対位置をＰｍ１、Ｐｍ２及びＰｍ３とする。また、Ｐｍ１の座標を（Ｘｍ１，Ｙｍ１）とし、Ｐｍ２の座標を（Ｘｍ２，Ｙｍ２）としＰｍ３の座標を（Ｘｍ３，Ｙｍ３）とする。

位置算出部３６は、車両１の絶対位置及び向きが仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）であると仮定して、物標位置特定部３５から受信した円筒状物標の中心位置の相対位置を絶対位置へ変換する。そして、位置算出部３６は、物標位置特定部３５から受信した円筒状物標の中から、車両１の絶対位置の算出のために選択された円筒状物標に最も近い円筒状物標を選択する。物標位置特定部３５から受信した円筒状物標のうち、絶対位置Ｐｍ１、Ｐｍ２及びＰｍ３の円筒状物標に最も近い円筒状物標の絶対位置をそれぞれＰｄ１、Ｐｄ２、Ｐｄ３とする。Ｐｄ１の座標を（Ｘｄ１，Ｙｄ１）とし、Ｐｄ２の座標を（Ｘｄ２，Ｙｄ２）とし、Ｐｄ３の座標を（Ｘｄ３，Ｙｄ３）とする。また、絶対位置Ｐｍ１とＰｄ１との間の誤差をＤ１とし、絶対位置Ｐｍ２とＰｄ２との間の誤差をＤ２とし、絶対位置Ｐｍ３とＰｄ３との間の誤差をＤ３とする。

位置算出部３６は、地図情報記憶部２０から読み込んだ絶対位置Ｐｍ１、Ｐｍ２及びＰｍ３と、物標位置特定部３５から受信した相対位置から変換して得た絶対位置Ｐｄ１、Ｐｄ２、Ｐｄ３との間の誤差Ｅを次式（４）に基づいて算出する。

次式（４）において、Ｎは、車両１の絶対位置の算出のために選択される円筒状物標の総数を示す。図９の例ではＮ＝３である。しかしながら、本発明はこれに限定されるものではない。円筒状物標の中心位置の相対位置の総数は２個でもよく、３個より多くてもよい。

位置算出部３６は、誤差Ｅが最小となるような車両１の仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）を、時刻ｔにおける車両１の絶対位置及び向きとして算出する。例えば位置算出部３６は、誤差Ｅを最小にする最適化計算によって時刻ｔにおける車両１の絶対位置及び向きを算出してよい。位置算出部３６は、誤差Ｅを最小にするのに代えて、誤差Ｅが所定値未満となるような車両１の仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）を算出してもよい。

（動作）
次に、本発明の第１実施形態に係る位置検出装置の動作について説明する。図１０は、位置検出装置３０の動作の一例の説明図である。図１０に記載されたステップＳ１０〜Ｓ２２は、車両１のイグニッションスイッチ（図示せず）がオフに切り替わるまで反復実行される。ステップＳ１０において位置測定部６は、車両１周囲の物体の表面を走査して車両１から物体表面上の走査点の測定位置情報を取得する。位置測定部６は、測定位置情報を候補点抽出部３１へ出力する。

ステップＳ１１において移動量算出部３３は、時刻ｔ−１から時刻ｔまでの間の車両１の移動量（ｄｘ，ｄｙ，ｄφ）を算出する。移動量算出部３３は、算出した移動量を測定位置情報修正部３４と位置算出部３６へ出力する。ステップＳ１２において測定位置情報修正部３４は、移動量算出部３３から出力された移動量に基づいて、記憶部３２に記憶された時刻ｔ−１における円筒状物標の外周の候補点の位置情報を、時刻ｔの位置情報に修正する。

ステップＳ１３において候補点抽出部３１は、候補点を抽出する抽出領域を設定する。ステップＳ１４において候補点抽出部３１は、時刻ｔ−１における位置情報から時刻ｔにおける位置情報に修正された候補点の修正位置情報を記憶部３２から読み込む。候補点抽出部３１は、位置測定部６が時刻ｔに測定した測定位置情報に修正位置情報を混合する。候補点抽出部３１は、混合して得られた位置情報の集合のうち、ステップＳ１３で設定した抽出領域内にあり且つ閾値Ｓ以下の大きさのセグメントに属する候補点のみを円筒状物標の候補点として抽出する。候補点抽出部３１は、抽出されたセグメントを物標位置特定部３５に出力する。候補点抽出部３１は、抽出されたセグメントを記憶部３２に記憶する。

ステップＳ１５において物標位置特定部３５は、近距離領域にセグメントがあるか否かを判断する。近距離領域にセグメントがある場合（ステップＳ１５：Ｙ）に動作はステップＳ１６へ進む。近距離領域にセグメントがない場合（ステップＳ１５：Ｎ）に動作はステップＳ１９へ進む。ステップＳ１６において物標位置特定部３５は、近距離領域の円筒状物標の半径及び中心位置を算出する。ステップＳ１７において物標位置特定部３５は、半径及び中心位置が算出された近距離領域のセグメントが、円筒状物標の測定結果であるか否かを判断する。近距離領域のセグメントが円筒状物標の測定結果である場合（ステップＳ１７：Ｙ）に動作はステップＳ１８へ進む。近距離領域のセグメントが円筒状物標の測定結果でない場合（ステップＳ１７：Ｎ）に動作はステップＳ１９へ進む。

ステップＳ１８において物標位置特定部３５は、ステップＳ１６で算出された半径情報を記憶部３２に記憶する。物標位置特定部３５は、ステップＳ１６で算出された半径情報を遠距離領域の円筒状物標の仮想半径Ｒｄとして設定する。また、物標位置特定部３５は、ステップＳ１６で算出された中心位置を円筒形物標の中心位置の相対位置として位置算出部３６へ出力する。その後、動作はステップＳ２０へ進む。

ステップＳ１９において物標位置特定部３５は、記憶部３２から半径情報を読み出す。物標位置特定部３５は、読み出された半径情報を円筒状物標の仮想半径Ｒｄとして設定する。物標位置特定部３５は、記憶部３２に記憶された所定値Ｒ０を仮想半径Ｒｄとして設定する。過去に実行されたループ反復において物標位置特定部３５が算出した半径情報が記憶部３２に記憶されている場合には、物標位置特定部３５は、過去に実行されたループ反復において算出された半径情報を仮想半径Ｒｄとして設定してよい。過去に実行されたループ反復において物標位置特定部３５が算出した半径情報が記憶部３２に記憶されていない場合に、物標位置特定部３５は、記憶部３２に記憶された所定値Ｒ０を仮想半径Ｒｄとして設定してもよい。

ステップＳ２０において物標位置特定部３５は、仮想半径Ｒｄを半径として有する円形を設定する。物標位置特定部３５は、遠距離領域のセグメントと設定された円形との間の合致度を算出する。物標位置特定部３５は、合致度が最も大きな円形の中心位置を、遠距離領域の円筒状物標の中心位置として算出する。物標位置特定部３５は、算出された中心位置を円筒形物標の中心位置の相対位置として位置算出部３６へ出力する。

ステップＳ２１において位置算出部３６は、地図情報記憶部２０から円筒状物標の中心位置の絶対位置を読み出す。位置算出部３６は、地図情報記憶部２０から読み出した円筒状物標の中心位置の絶対位置と、物標位置特定部３５から受信した円筒状物標の中心位置の相対位置に基づいて、時刻ｔにおける車両１の絶対位置及び車両１の前後方向の向きを算出する。

ステップＳ２２において、位置検出装置３０は車両１のイグニッションスイッチ（ＩＧＮ）がオフであるであるか否かを判断する。イグニッションスイッチ（ＩＧＮ）がオフでない場合（ステップＳ２２：Ｎ）に動作はステップＳ１０に戻る。イグニッションスイッチがオフである場合（ステップＳ２２：Ｙ）に位置検出装置３０の動作が終了する。

地図情報記憶部２０は、位置記憶部に対応する。記憶部３２は、物標情報記憶部に対応する。車速センサ３、ヨーレートセンサ７及び移動量算出部３３は、位置算出部に対応する。測定位置情報修正部３４は、修正部に対応する。セグメントは、候補点の集合に対応する。近距離領域の円筒形物標は第１円筒形物標に対応する。遠距離領域の円筒状物標は第２円筒状物標に対応する。時刻ｔ−１は第１時点に対応する。時刻ｔは第２時点に対応する。

（本実施形態の効果）
（１）第１実施形態に係る位置検出装置３０は、周囲の物体までの距離及び方向を取得することにより周囲の物体表面の複数の位置点を測定し、位置が測定された複数の位置点の中から円筒状物標の外周の候補点の集合を抽出する。そして、半径及び中心位置が設定された円形と候補点の集合により形成される形状との間の合致度を算出し、合致度に応じて円形の中心位置を円筒状物標の中心位置として特定するか否かを判断する。このように、半径及び中心位置が設定された円形との合致度に応じてこの円形の中心位置を円筒状物標の中心位置として特定することで、円筒状物標上の走査点が少ない場合においても精度良く中心位置を算出することができる。この結果、円筒状物標の両径方向端までの各々の方向が取得されていなくても精度良く中心位置を算出することができる。

（２）第１実施形態に係る位置検出装置３０は、円形との合致度が閾値以上であるか否かに応じて円形の中心位置を円筒状物標の中心位置として特定するか否かを判断する。このように、合致度が閾値以上の場合に円形の中心位置を円筒状物標の中心位置として採用することで、円筒状物標の中心位置の算出精度の低下を防止することができる。

（３）第１実施形態に係る位置検出装置３０は、中心位置が異なる複数の円形について合致度をそれぞれ算出し、これら複数の円形のうち合致度が最も大きな円形又は合致度が閾値以上且つ最も大きな円形の中心位置を円筒状物標の中心位置として特定する。このため、複数の円形との合致度が閾値以上である場合に、最も精度が高いと考えられる中心位置を選択することができる。または、本実施形態に係る位置検出装置３０は、中心位置が異なる複数の円形について合致度を順次算出する際に最初に合致度が閾値以上になる円形の中心位置を円筒状物標の中心位置として特定する。このため、合致度が閾値以上になった段階で計算を終了することができるため、処理が早くなり計算量が低減される。

（４）第１実施形態に係る位置検出装置３０は、円形の円周線からの距離が閾値長さ未満である候補点が候補点の集合に占める割合に応じて合致度を算出する。このように、候補点と円形の円周線との乖離程度によって合致度を算出するので合致度を正確に算出することができる。

（５）第１実施形態に係る位置検出装置３０は、近距離領域の第１円筒状物標の外周の候補点を含むセグメントに基づいて第１円筒状物標の半径を算出する。そして、遠距離領域の第２円筒状物標の外周の候補点を含むセグメントと第１円筒状物標の半径を有する円形との合致度に応じてこの円形の中心位置を第２円筒状物標の中心位置として特定する。このように、第２円筒状物標の中心位置の決定に使用する円形の半径を、第１円筒状物標の測定結果に従って設定するので、例えば固定的な設定値を半径に設定する場合に比べて中心位置の算出精度が向上する。

（６）第１実施形態に係る位置検出装置３０は、遠距離領域の第２円筒状物標の中心位置の決定に使用する円形の半径を、走査点数が比較的多い近距離領域の第１円筒状物標の測定結果に従って設定する。このため半径をより正確に設定することができるため、走査点数が比較的少ない遠距離領域の円筒状物標の中心位置の算出精度が向上する。

（７）第１実施形態に係る位置検出装置３０において、位置測定部６は、車両１に搭載され、周囲の物体までの距離及び水平角を取得することにより周囲の物体表面の複数の位置点を測定する。このため、地上に立設された円筒状物標の位置を車両１から測定することができる。また、位置検出装置３０は、車両１との横方向位置の差が閾値範囲外の点を候補点として抽出しない。このような範囲で候補点を検出することで、例えば電柱などの道路脇に存在する円筒状物標を精度良く検出できるとともに、抽出領域を限定することで位置検出装置３０の計算量を低減することができる。

（８）第１実施形態に係る位置検出装置３０において、位置測定部６は、車両１に搭載され、周囲の物体までの距離及び水平角を取得することにより周囲の物体表面の複数の位置点の相対位置を測定する。このため、地上に立設された円筒状物標との相対位置を車両１から測定することができる。また、位置検出装置３０は、円筒状物標の中心の相対位置と地図情報記憶部２０に予め記憶された円筒状物標の中心の絶対位置に基づいて車両１の絶対位置を算出する。このように、本実施形態に係る位置検出装置３０により測定された円筒状物標の相対位置に基づいて車両１の絶対位置を算出することで、車両１の位置を精度良く算出することができる。

（９）第１実施形態に係る位置検出装置３０は車両１に搭載され、第１の複数の位置点の相対位置を第１時点ｔ−１に測定し、及び第２の複数の位置点の相対位置を第２時点ｔに測定する。そして、第１時点ｔ−１及び第２時点ｔとの間の車両１の移動量を検出し、第１の複数の位置点の中から抽出された第１の候補点の集合の相対位置を移動量に基づき修正する。そして、修正された第１の候補点の集合及び第２の複数の位置点の中から第２の候補点の集合を抽出する。このように、異なる時刻において測定された測定結果を混合することで円筒状物標の位置検出に使用する位置情報数を増やすことができる。このため、例えば遠距離領域の円筒状物標のような、１回の測定の走査点が少ない円筒状物標の中心位置の算出精度を向上することができる。

（変形例）
地図情報記憶部２０に、車線数、車線の幅及び歩道の幅の少なくとも一つの情報が記憶されていてもよい。候補点抽出部３１は、車両１の位置に応じて車線数、車線の幅及び歩道の幅の少なくとも一つの情報を読み出して、この情報に応じて円筒状物標の候補点の抽出領域を動的に決定してもよい。このように車両１の位置に応じて動的に抽出領域を変更することでより適切な抽出領域を設定することができる。

移動量算出部３３は、車両１の車速及び操舵角センサ５が検出したステアリングホイール４の操舵角に基づいてヨーレートを算出してもよい。移動量算出部３３は、算出したヨーレートに基づいて車両１の回転量ｄφを算出してもよい。車速及び操舵角に基づいてヨーレートを算出することでヨーレートセンサ７を省略することができる。

物標位置特定部３５は、候補点が形成する形状と設定された円形との合致度を算出する際に、候補点により形成される近似曲線、及びその曲率中心と曲率半径を算出してもよい。物標位置特定部３５は、設定された円形の中心位置と曲率中心との差分、及び／又は設定された円形の半径と曲率半径との差分に基づいて合致度を算出してもよい。このような算出方法でも合致度を算出することができる。

位置算出部３６は、車両１の絶対位置を算出する際に、物標位置特定部３５から受信した円筒状物標の中心位置の相対位置の平均座標を算出してもよい。また、位置算出部３６は、地図情報記憶部２０から読み込んだ絶対位置Ｐｍ１、Ｐｍ２及びＰｍ３の平均座標を算出してもよい。位置算出部３６は、絶対位置Ｐｍ１、Ｐｍ２及びＰｍ３の平均座標のＸ座標値及びＹ座標値から、相対位置の平均座標のＸ座標値及びＹ座標値を差し引いて、車両１の絶対位置を算出してもよい。このような算出方法によればより簡易に車両１の絶対位置を算出できる。

（第２実施形態）
（構成）
図１１は、本発明の第２実施形態に係る位置検出装置３０の機能構成を示す図である。第１実施形態と同様な構成には同一の符号を付する。また、第２実施形態におけるヨーレートセンサ７及び車速センサ３の動作は、第１実施例におけるこれらの動作と同様である。第２実施形態に係る位置検出装置３０の位置測定部６、記憶部３２、移動量算出部３３、測定位置情報修正部３４、及び物標位置特定部３５の動作は、第１実施例におけるこれらの動作と同様である。

地図情報記憶部２０には、円筒状物標の中心の絶対位置が記憶されている。地図情報記憶部２０は、例えば、ナビゲーション装置内の地図情報データ記憶装置であってよい。地図情報記憶部２０に記憶された円筒状物標の中心の絶対位置は、候補点抽出部３１及び位置算出部３６によって読み出される。

位置算出部３６は、上記の第１実施形態における位置算出部３６の動作に加えて、時刻ｔにおける車両１の仮の絶対位置（Ｘｔｔ，Ｙｔｔ）及び仮の向き（Φｔｔ）を候補点抽出部３１へ出力する。候補点抽出部３１は、上記の第１実施形態における候補点抽出部３１の動作に加えて、円筒状物標の中心の絶対位置を地図情報記憶部２０から読み出す。また、時刻ｔにおける車両１の仮の絶対位置及び向きを位置算出部３６から受信する。候補点抽出部３１は、円筒状物標の中心の絶対位置と車両１の仮の絶対位置及び向きに基づいて、候補点を抽出する抽出領域を設定する。

図１２は、候補点の抽出領域の第２例の説明図である。地図情報記憶部２０から読み出した円筒状物標８０の絶対位置Ｐｍ１及び円筒状物標８１の絶対位置Ｐｍ２の座標を、それぞれ（Ｘｍ１，Ｙｍ１）及び（Ｘｍ２，Ｙｍ２）とする。候補点抽出部３１は、車両１の絶対位置及び向きが、仮の絶対位置及び向き（Ｘｔｔ，Ｙｔｔ，Φｔｔ）であると仮定して、円筒状物標の中心の絶対位置Ｐｍ１及びＰｍ２を相対位置ｐｍ１及びｐｍ２に変換する。相対位置ｐｍ１の座標を（ｘｍ１,ｙｍ１）とし相対位置ｐｍ２の座標を（ｘｍ２,ｙｍ２）とする。

候補点抽出部３１は、相対位置ｐｍ１及びｐｍ２に基づいて抽出領域として設定する。例えば候補点抽出部３１は、相対位置ｐｍ１及びｐｍ２を含んだ所定面積の領域を抽出領域として設定する。例えば抽出領域は、相対位置ｐｍ１及びｐｍ２をそれぞれ中心とする所定半径Ｒｒの円内の領域であってもよく、又は、相対位置ｐｍ１及びｐｍ２をそれぞれ重心とする矩形領域であってもよい。

半径Ｒｒは例えば３ｍであってよい。半径Ｒｒは車両１の仮の絶対位置及び向き（Ｘｔｔ，Ｙｔｔ，Φｔｔ）の精度に応じて適宜設定してよい。例えば、車両１の仮の絶対位置及び向きの精度は移動量（ｄｘ，ｄｙ，ｄφ）が大きくなると低下する。このため、候補点抽出部３１は、移動量が増大すると半径Ｒｒが増大するように半径Ｒｒを動的に変更してもよい。候補点抽出部３１は、車速及び／又はヨーレートが増大すると半径Ｒｒが増大するように半径Ｒｒを動的に変更してもよい。

（本実施形態の効果）
第２実施形態に係る位置検出装置３０は、地図情報記憶部２０に記憶された円筒状物標の中心の絶対位置と、位置算出部３６により算出された車両１の絶対位置とに基づいて、候補点の抽出領域を設定する。このように地図情報記憶部２０に記憶された円筒状物標の位置情報に基づいて抽出範囲を限定するので、位置検出装置３０の計算量が低減し、円筒状物標の検出が容易になる。

以上、限られた数の実施形態を参照しながら説明したが、権利範囲はそれらに限定されるものではなく、上記の開示に基づく実施形態の改変は、当業者にとって自明のことである。

３ 車速センサ
６ 位置測定部
７ ヨーレートセンサ
１０ コントローラ
２０ 地図情報記憶部
３０ 位置検出装置
３１ 候補点抽出部
３２ 記憶部
３３ 移動量算出部
３４ 測定位置情報修正部
３５ 物標位置特定部
３６ 位置算出部

Claims (11)

1. 周囲の物体までの距離及び方向を取得することにより前記周囲の物体の複数の位置点を測定する位置測定部と、
   前記位置測定部により測定された前記複数の位置点の中から円筒状物標の外周の候補点の集合を抽出する候補点抽出部と、
   円周及び中心位置が設定された円形と前記候補点の集合により形成される形状との間の合致度を算出し、前記合致度に応じて前記円形の中心位置を前記円筒状物標の中心位置として特定するか否かを判断する物標位置特定部と、
   を備えることを特徴とする位置検出装置。
2. 前記物標位置特定部は、前記合致度が閾値以上であるか否かに応じて前記円形の中心位置を前記円筒状物標の中心位置として特定するか否かを判断することを特徴とする請求項１に記載の位置検出装置。
3. 前記物標位置特定部は、中心位置が異なる複数の前記円形について前記合致度を各々算出し、前記複数の円形のうち前記合致度が最も大きな円形若しくは前記合致度が閾値以上且つ最も大きな円形を前記円筒状物標の中心位置として特定するか、又は中心位置が異なる複数の前記円形について前記合致度を順次算出する際に最初に前記合致度が閾値以上になる円形の中心位置を前記円筒状物標の中心位置として特定することを特徴とする請求項１に記載の位置検出装置。
4. 前記物標位置特定部は、前記円形の円周線からの距離が閾値長さ未満である候補点が前記候補点の集合に占める割合に応じて、前記合致度を算出することを特徴とする請求項１〜３のいずれか一項に記載の位置検出装置。
5. 前記円筒状物標の円の大きさが記憶される物標情報記憶部を更に備え、
   前記物標情報記憶部は、前記物標位置特定部により算出された第１円筒状物標の円の大きさを記憶し、
   前記物標位置特定部は、前記物標情報記憶部に記憶された前記第１円筒状物標の円の大きさを有する前記円形と、第２円筒状物標の外周の候補点の集合により形成される形状との間の合致度を算出し、前記合致度に応じて前記円形の中心位置を前記第２円筒状物標の中心位置として特定するか否かを判断することを特徴とする請求項１〜４のいずれか一項に記載の位置検出装置。
6. 前記第２円筒状物標は前記第１円筒状物標よりも前記位置測定部から遠いことを特徴とする請求項５に記載の位置検出装置。
7. 前記位置測定部は移動体に搭載されて、前記複数の位置点までの距離及び水平角を取得することにより前記複数の位置点の位置を測定し、
   前記候補点抽出部は、前記移動体との横方向位置の差が閾値範囲内の位置点を前記候補点として抽出することを特徴とする請求項１〜６のいずれか一項に記載の位置検出装置。
8. 前記位置測定部は移動体に搭載されて、前記複数の位置点までの距離及び水平角を取得することにより前記移動体に対する前記複数の位置点の相対位置を測定し、
   前記物標位置特定部は、前記円筒状物標の中心位置として、前記移動体に対する前記円筒状物標の中心の相対位置を特定し、
   前記位置検出装置は、
   前記円筒状物標の中心の絶対位置が記憶された位置記憶部と、
   前記物標位置特定部により特定された前記円筒状物標の中心の相対位置と前記位置記憶部に記憶された前記円筒状物標の中心の絶対位置に基づいて、前記車両の絶対位置を算出する位置算出部と、
   を更に備えることを特徴とする請求項１〜７のいずれか一項に記載の位置検出装置。
9. 前記候補点抽出部は、前記位置記憶部に記憶された前記円筒状物標の中心の絶対位置と、前記位置算出部により算出された前記車両の絶対位置とに基づいて、前記候補点の抽出領域を設定することを特徴とする請求項８に記載の位置検出装置。
10. 前記位置測定部は移動体に搭載されて、前記複数の位置点として第１の複数の位置点までの距離及び水平角を取得することにより前記第１の複数の位置点の相対位置を第１時点に測定し、かつ前記複数の位置点として第２の複数の位置点までの距離及び水平角を取得することにより前記第２の複数の位置点の相対位置を前記第１時点より後の第２時点に測定し、
    前記位置検出装置は、
    前記第１時点及び前記第２時点との間の前記移動体の移動量を検出する移動量検出部と、
    前記候補点の集合として前記候補点抽出部が前記第１の複数の位置点の中から抽出した第１の候補点の集合の相対位置を、前記移動量検出部により検出された前記移動量に基づき修正する修正部と、を備え、
    前記候補点抽出部は、前記修正部により修正された前記第１の候補点の集合と前記第２の複数の位置点とを混合して得られる位置点の集合の中から、前記候補点の集合としての第２の候補点の集合を抽出することを特徴とする請求項１〜９のいずれか一項に記載の位置検出装置。
11. 周囲の物体までの距離及び方向を取得することにより前記周囲の物体の複数の位置点を測定し、
    位置が測定された前記複数の位置点の中から円筒状物標の外周の候補点の集合を抽出し、
    円周及び中心位置が設定された円形と前記候補点の集合により形成される形状との間の合致度を算出し、
    前記合致度に応じて前記円形の中心位置を前記円筒状物標の中心位置として特定するか否かを判断することを特徴とする位置検出方法。

Images