JP5355443B2 - 位置補正システム - Google Patents

Description

この発明は、例えばＧＰＳによって検出された車両の位置を補正する位置補正システムに関する。

現在、屋外測位に関しては、全地球測位システム（ＧＰＳ：Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）が主流となっている。ＧＰＳは、ＧＰＳ衛星からのＧＰＳ信号を受信し、受信したＧＰＳ信号に基づいて地球上における移動体（例えば、車両）の位置を検出するものである。

しかしながら、高層建造物の多い都市部等では、例えばビルの谷間のように、測定場所からＧＰＳ衛星を見ることができない場合があり、測定環境が悪化することがある。測定環境が悪化すると、車両の位置を全く検出できない場合や、測定誤差が大きくなる場合がある。

そこで、ＧＰＳ信号の受信状況が悪い環境においては、他の方法によって車両の位置を検出する必要がある。一般的には、車両の角度（走行方向）または角速度を検出するジャイロスコープおよび車両の加速度を検出する加速度センサ等のデバイスを車両に別途搭載し、ＧＰＳで車両の位置を検出できない場合に、ジャイロスコープおよび加速度センサからの出力に基づいて、車両の位置を算出している。

また、このとき、車両に搭載されたカメラおよびレーザからの出力に基づいて、車両と周辺地物との相対位置関係を算出し、算出された相対位置関係を用いて、ジャイロスコープおよび加速度センサの出力に基づいて算出された車両の位置を補正する現在地検出装置が提案されている（例えば、特許文献１参照）。

特開２００７−２３２６９０号公報

しかしながら、従来技術には、以下のような課題がある。
従来の現在地検出装置では、車両の位置を補正することはできるものの、車両の角度（走行方向）または角速度を補正することはできない。これは、車両の角度（または角速度）を検出する手段を、ジャイロスコープのみに依存しているからである。

すなわち、従来の現在地検出装置では、「補正の瞬間」における車両の位置を補正することはできるが、ジャイロスコープで検出された車両の角度に誤差が含まれている場合には、車両位置の補正後、すぐに車両の位置に誤差が生じるという問題がある。なお、この誤差は、走行距離が長くなるにつれて大きくなる。

さらに、ジャイロスコープ等のデバイスには、使用している間に誤差が増加する誤差蓄積の問題がある。そのため、何らかの形でジャイロスコープの誤差を解消しなければ、測定誤差が大きくなるという問題もある。

この発明は、上記のような課題を解決するためになされたものであり、移動体の位置を補正するとともに、角度検出手段で検出された移動体の角度を補正して、移動体の位置誤差の拡大を防止することができる位置補正システムを得ることを目的とする。

この発明に係る位置補正システムは、移動体の現在位置を検出する位置検出手段と、移動体の所定の基準に対する角度を検出する角度検出手段と、移動体の移動範囲における地物の位置があらかじめ記録された地物位置記録手段と、移動体の周囲を撮影する撮影手段と、移動体の周囲にレーザを送出して、移動体と周辺環境との距離を測定する距離測定手段と、撮影手段で撮影された画像内における、地物位置記録手段に記録された特定の地物に対する距離測定手段による測定結果から移動体と特定の地物との相対関係を求め、相対関係に基づいて位置検出手段で検出された移動体の現在位置を補正するとともに、複数回の相対関係に基づいて角度検出手段で検出された移動体の角度を補正する位置補正手段とを備えたものである。

この発明に係る位置補正システムによれば、位置補正手段は、撮影手段で撮影された画像内における、地物位置記録手段に記録された特定の地物に対する距離測定手段による測定結果から移動体と特定の地物との相対関係を求め、複数回の相対関係に基づいて角度検出手段で検出された移動体の角度を補正する。
そのため、移動体の位置を補正するとともに、角度検出手段で検出された移動体の角度を補正して、移動体の位置誤差の拡大を防止することができる位置補正システムを得ることができる。

この発明の実施の形態１に係る位置補正システムを搭載した車両を示す構成図である。 この発明の実施の形態１に係る位置補正システムを含むシステム全体を示すブロック構成図である。 この発明の実施の形態１に係る位置補正システムを搭載した車両において、２回の車両補正を行った場合の位置関係を示す説明図である。 この発明の実施の形態１に係る位置補正システムの位置補正部の動作を示すフローチャートである。 この発明の実施の形態１に係る位置補正システムにおいて、カメラの光軸中心を原点とした座標系で表された点群が、カメラ画像内のどの座標に映るかを表す計算式を示す説明図である。 この発明の実施の形態１に係る位置補正システムにおける車両座標系から平面直角座標系への変換、およびその際のジャイロスコープの角度誤差をεとしたときの誤差値を算出する計算式を示す説明図である。 この発明の実施の形態１に係る位置補正システムの効果を説明するための説明図である。 この発明の実施の形態１に係る位置補正システムの効果を説明するための説明図である。 この発明の実施の形態１に係る位置補正システムの効果を、従来技術と比較して示す説明図である。

以下、この発明の位置補正システムの好適な実施の形態につき図面を用いて説明するが、各図において同一、または相当する部分については、同一符号を付して説明する。
なお、以下の実施の形態では、この位置補正システムが測定対象となる車両に搭載されている場合について説明する。

実施の形態１．
図１は、この発明の実施の形態１に係る位置補正システムを搭載した車両１（移動体）を示す構成図である。また、図２は、この発明の実施の形態１に係る位置補正システムを含むシステム全体を示すブロック構成図である。

図１、２において、このシステムは、制御装置１０、カメラ２１（撮影手段）、レーザレンジファインダ２２（距離測定手段）、ＧＰＳ受信機２３（位置検出手段）、ジャイロスコープ２４（角度検出手段）、カメラ撮影データ３１、カメラ撮影時刻データ３２、距離測定データ３３、距離測定時刻データ３４および地物位置データ３５（地物位置記録手段）を備えている。

カメラ２１は、車両１の前方を定期的に撮影し、撮影した画像を制御装置１０に出力してカメラ撮影データ３１に記録するとともに、撮影時刻をカメラ撮影時刻データ３２に記録する。レーザレンジファインダ２２は、車両１の前方にレーザを送出して、車両１と周辺環境との距離を測定し、測定した距離を制御装置１０に出力して距離測定データ３３に記録するとともに、測定時刻を距離測定時刻データ３４に記録する。

このとき、レーザレンジファインダ２２は、レーザを送出する角度を定期的に変化させて、車両１の前方全体をカバーする。
なお、レーザレンジファインダ２２による周辺環境との距離測定の頻度は、カメラ２１による車両１の前方撮影の頻度よりも高いものとする。一般的には、レーザレンジファインダ２２は、車両１の周囲を１秒間に５０回以上走査し、カメラ２１は、車両１の前方を１秒間に５回程度撮影する。

ＧＰＳ受信機２３は、ＧＰＳ衛星（図示せず）からのＧＰＳ信号を受信し、受信したＧＰＳ信号を制御装置１０に出力する。ジャイロスコープ２４は、車両１の所定の基準に対する角度（走行方向）または角速度を検出し、検出結果を制御装置１０に出力する。ここでは、ジャイロスコープ２４は、車両１の角度（走行方向）を検出しているものとする。

制御装置１０は、位置検出部１１（位置検出手段）と、位置補正部１２（位置補正手段）とを有している。また、地物位置データ３５には、車両１が走行する環境下における地物２の位置があらかじめ測定され、測定された位置情報が記録されている。ただし、全ての地物の位置を測定することは困難なので、一部の地物についてのみ位置が測定される。

位置検出部１１は、ＧＰＳ受信機２３からのＧＰＳ信号に基づいて、地球上における車両１の位置を検出する。
位置補正部１２は、ＧＰＳ衛星からのＧＰＳ信号を受信できない場合に、カメラ２１による撮影結果、レーザレンジファインダ２２による測定結果および地物位置データ３５に記録された位置情報に基づいて位置検出部１１で検出された車両１の位置を補正するとともに、ジャイロスコープ２４で検出された車両１の角度を補正する。

なお、位置検出部１１による車両１の位置検出に関しては、他のセンサ等から得られた情報、例えば車両速度、車輪の回転数、ハンドル角度といった車両情報を併せて利用してもよい。また、車両１の角度検出に関しては、ジャイロスコープ２４に限定されるものではなく、例えばＧＰＳ受信機２３を複数搭載して角度を検出してもよい。

ここで、車両１の位置検出おいて、真値として使える値に注目すると、正しい値として得られるものは、地物位置データ３５に記録された地物２の位置のみである。そのため、この地物２の位置から車両１の角度を求めることができれば、ジャイロスコープ２４で検出された車両１の角度を、地物２の位置から求めた角度によって補正することが可能となる。このとき、真値である地物２の位置から求めた角度と、誤差が蓄積するジャイロスコープ２４で検出された角度とでは、どちらが正確かは明らかである。

一方、レーザレンジファインダ２２によるレーザ測定の現状を鑑みるに、測定の際に、地物２が１回だけ認識される状態というのは稀である。このことは、レーザ測定の際の車両速度が比較的低速（約３０ｋｍ／ｈ）であり、また、カメラ２１による撮影頻度も少なくない（１秒間に５回程度）ためである。つまり、１つの地物２に対して、複数回の撮影が可能であるということである。

さらに、上述したように、レーザレンジファインダ２２による周辺環境との距離測定の頻度は、カメラ２１による車両１の前方撮影の頻度よりも高いことから、複数のカメラ画像のそれぞれに対応したレーザ測定結果が得られる可能性が高い。
すなわち、１つの地物２に対して、車両１と地物２との相対関係が、複数回得られる可能性が高いと考えられる。

そこで、この実施の形態１では、ジャイロスコープ２４で検出された車両１の角度を補正するために、上記複数回の車両１と地物２との相対関係に基づいて、車両１の角度を算出する方法を示す。以下、車両１と地物２との相対関係を求めることを、「車両補正」と称する。

なお、同一の地物２について、車両補正を複数回行う際、その補正の時間間隔（使用したレーザ測定結果を得た時刻の間隔）は、高々数百ミリ秒程度である。このとき、車両１が交差点等で方向を変えようとしていない限り、急激に車両１の走行方向が変わるとは考えにくい。すなわち、車両補正の間の車両１は、直進している（走行方向が変化しない）と近似することができる。

図３は、この発明の実施の形態１に係る位置補正システムを搭載した車両１において、２回の車両補正を行った場合の位置関係を示す説明図である。
図３において、１回目の車両補正による車両１の位置から、２回目の車両補正による車両１の位置を直線で結んだ方向が、車両１の角度（走行方向）となる。また、角度の誤差は、２回目の車両補正による距離に基づいて算出することができる。

なお、実際には、３次元空間上では、角度は３つ必要なので、図３だけでは算出することができないが、同処理を３式立てる（地物２を４回認識する）ことによって算出することができる。

次に、図４に示したフローチャートを参照しながら、位置補正部１２の具体的な動作について説明する。
図４は、カメラ撮影データ３１に記録されたカメラ２１による撮影結果と、距離測定データ３３に記録されたレーザレンジファインダ２２による測定結果に基づいて、車両１の位置および角度を補正するフローを示したものである。ここでは、ＧＰＳ受信機２３の受信感度が悪く、車両１の位置精度がメートルオーダでずれている可能性があるものとする。また、ジャイロスコープ２４にも誤差（１度未満〜数度程度）がのっているとする。

まず、位置補正部１２は、カメラ撮影データ３１に記録されたカメラ画像を時系列にチェックし、画像処理によって地物２の検知を行う（ステップＳ１）。
続いて、位置補正部１２は、カメラ画像内（車両１の前方）に地物２があるか否かを判定する（ステップＳ２）。

ステップＳ２において、カメラ画像内に地物２がある（すなわち、Ｙｅｓ）と判定された場合には、位置補正部１２は、その地物２の位置真値が地物位置データ３５に記録されているか否かを判定する（ステップＳ３）。
これは、カメラ２１による撮影時の位置として、位置検出部１１で暫定的に検出された車両１の位置（メートルオーダの誤差を含む）を利用して範囲を絞り込むことによって行われる。具体的には、メートルオーダの誤差があり得るので、カメラ２１による撮影時の位置から１０メートル四方程度の狭い範囲を対象として、地物位置データ３５に該当すると考えられるデータがあるか否かを判定する。

ステップＳ３において、その地物２の位置真値が地物位置データ３５に記録されている（すなわち、Ｙｅｓ）と判定された場合には、位置補正部１２は、レーザレンジファインダ２２による測定結果（レーザによる測定結果を３次元座標に直したもの。以下、「点群」と称する）で地物２に当っているものがあるか否かを判定する（ステップＳ４）。
具体的には、点群の位置をカメラ画像上にマッピングして画像内位置を算出し、地物２の範囲内に収まる１つを選択する。

図５は、この発明の実施の形態１に係る位置補正システムにおいて、カメラ２１の光軸中心を原点とした座標系で表された点群が、カメラ画像内のどの座標（画像内座標）に映るかを表す計算式を示す説明図である。点群の座標計算においては、レーザレンジファインダ２２による測定結果（距離）を、一旦レーザ送出位置を原点とした座標系であるレーザ座標系の座標として算出し、次にレーザレンジファインダ２２の車両座標系における設置位置および設置角度をパラメータとして車両座標系に変換する。この車両座標系に変換した座標値と、カメラ２１の車両座標系における設置位置および設置角度とに基づいて、点群のカメラ座標系における位置を算出することができる。

ここで、車両座標系は、車両１の前方方向をｘ軸、車両１の上方方向をｙ軸、車両１の右方向をｚ軸とし、座標値が距離（単位はメートル）に等しい座標系と定義する。また、カメラ座標系は、ｘ、ｙ、ｚ軸が車両座標系と同じ方向を向き、カメラ２１のレンズ中心を原点とする座標系と定義する。また、レーザ座標系は、ｘ、ｙ、ｚ軸が車両座標系と同じ方向を向き、レーザ送出位置を原点とする座標系と定義する。

ステップＳ４において、地物２に当たっている点群がある（すなわち、Ｙｅｓ）と判定され、地物２の範囲内に収まる１つの点群が選択されて、車両座標系における地物位置座標（すなわち、車両１と地物２との位置関係）が求められた場合には、位置補正部１２は、地物位置座標を絶対距離系（平面直角座標系）に変換する（ステップＳ５）。

一方、ステップＳ２において、カメラ画像内に地物２がない（すなわち、Ｎｏ）と判定された場合、ステップＳ３において、その地物２の位置真値が地物位置データ３５に記録されていない（すなわち、Ｎｏ）と判定された場合およびステップＳ４において、地物２に当たっている点群がない（すなわち、Ｎｏ）と判定された場合には、位置補正部１２は、直ちに補正処理を終了する。

なお、平面直角座標系とは、日本ではある原点（日本国内に１９箇所あり、測定対象の位置により異なる）から緯度方向をｘ軸、経度方向をｙ軸、高さ方向をｚ軸とした直交座標系である。平面直角座標系については、国土地理院、平面直角座標系（平成十四年国土交通省告示第九号）（http://www.gsi.go.jp/LAW/heimencho.html）に示されているので、詳細な説明は省略する。例えば、東京都（島部除く）内では、東経１３９度５０分０秒、北緯３６度０分０秒（千葉県野田市内）を原点とする。

ここで、車両座標系から平面直角座標系に変換する際、車両の平面直角座標系における向きが必要となる。
図６は、この発明の実施の形態１に係る位置補正システムにおける車両座標系から平面直角座標系への変換、およびその際のジャイロスコープ２４の角度誤差をεとしたときの誤差値を算出する計算式を示す説明図である。ただし、車両１が地面に対して垂直に走行している（車両座標系ｙ軸と平面直角座標の高さ方向とが一致する）ものとする。

図６から分かるように、測定誤差Δｘ、Δｙは、車両座標系ｘ軸（車両進行方向）の座標値、ｚ軸（車両進行方向右向き）の座標値および角度誤差に依存する。このとき、ジャイロスコープ２４の角度誤差ε＝２°とすると、ｓｉｎ（ε／２）＝約０．０１なので、ｘ、ｚ軸方向の距離が１０ｍ程度ならば１０〜２０ｃｍ程度の誤差となる。これは、上述した位置検出部１１で暫定的に検出された位置の誤差に比べれば小さい。

続いて、位置補正部１２は、ステップＳ５で求められた平面直角座標における地物２の車両相対位置と、地物位置データ３５に記録された地物２の位置真値とに基づいて、車両１の位置を求め、車両１の位置を補正する（ステップＳ６）。なお、地物位置データ３５が他の座標系でデータを記録している場合には、平面直角座標系に変換する必要がある。

次に、位置補正部１２は、この地物２による位置補正が２回目であるか否かを判定する（ステップＳ７）。
ステップＳ７において、この地物２による位置補正が２回目でない（１回目である）（すなわち、Ｎｏ）と判定された場合には、位置補正部１２は、この補正後の車両１の位置を記録し（ステップＳ８）、図４の処理を終了する。

一方、ステップＳ７において、この地物２による位置補正が２回目である（すなわち、Ｙｅｓ）と判定された場合には、位置補正部１２は、前回のサイクルのステップＳ８で記録された１回目の位置補正時における車両１の位置と、今回のサイクルのステップＳ６で補正された車両１の位置とに基づいて、車両１の角度を算出する（ステップＳ９）。

１つの地物２が複数回（例えば、２回）検知できる場合、それぞれの検知時における車両１の位置は、時間にしてカメラ２１の撮影間隔程度であり、それほど離れていないと考えられる。また、曲がり角を曲がっているのではない限り１秒未満の間隔で突然車両１の角度が変化するとは考えにくいことから、この２つの車両１の位置の間では、車両１が直線移動（すなわち、平面直角座標系での向きが互いに等しい）をしていると仮定できる。

さらに、車両１が直線運動している際に、短時間でジャイロスコープ２４の角度誤差（ジャイロ誤差）が急激に変動することはない。これらのことから、図６に示した角度誤差による測定誤差は、図７に示された計算式（Ｘ_２−Ｘ_１およびＹ_２−Ｙ_１）によって、図８に示されるように軽減されることが分かる。ここで、車両位置にほとんど変化が無い（図８におけるｎ_ｒ２−ｎ_ｒ１およびｅ_ｒ２−ｅ_ｒ１が０に近い）場合、角度誤差による測定誤差は、ほぼ０の近傍の値となる。これにより、図７によって得られる角度値が、正確さを増していることが分かる。

続いて、位置補正部１２は、ステップＳ９で算出された車両１の角度に基づいて、ジャイロスコープ２４で検出された車両１の角度を補正し（ステップＳ１０）、図４の処理を終了する。これにより、ジャイロスコープ２４の角度誤差（図５に示したεに相当する）が算出され、車両１の角度が補正されたことが分かる。

このように、車両１の角度値を、地表位置真値と誤差を含む角度値とを用いて補正することができる。角度の誤差は、図９に示されるように、車両１の位置が進むにつれて位置誤差の増加として現れるので、結果的に車両１の測位精度を向上させることにも繋がる。

以上のように、実施の形態１によれば、位置補正手段は、撮影手段で撮影された画像内における、地物位置記録手段に記録された特定の地物に対する距離測定手段による測定結果から移動体と特定の地物との相対関係を求め、複数回の相対関係に基づいて角度検出手段で検出された移動体の角度を補正する。
そのため、移動体の位置を補正するとともに、角度検出手段で検出された移動体の角度を補正して、移動体の位置誤差の拡大を防止することができる位置補正システムを得ることができる。

なお、上記実施の形態１では、レーザレンジファインダ２２の測定結果とカメラ２１で撮影されたカメラ画像とを利用して車両１と地物２との相対距離を算出すると説明した。しかしながら、これに限定されず、２台のカメラを用いて同時に撮影された２枚の画像を利用し、ステレオ視によって車両１と地物２との位置関係を求めてもよい。
この場合も、上記実施の形態１と同様の効果を得ることができる。

また、上記実施の形態１では、カメラ画像のみによって地物２の認識を行っているが、これに限定されず、認識の段階で点群を利用してもよい。すなわち、点群を利用して形状を算出し、カメラ画像を利用して色を求めることによって、３次元的に判定を行うことができる。このとき、地物２との位置関係は、求めた地物２の形状から求めることができる。
この場合も、上記実施の形態１と同様の効果を得ることができる。

１ 車両、２ 地物、１１ 位置検出部（位置検出手段）、１２ 位置補正部（位置補正手段）、２１ カメラ（撮影手段）、２２ レーザレンジファインダ（距離測定手段）、２３ ＧＰＳ受信機（位置検出手段）、２４ ジャイロスコープ（角度検出手段）、３５ 地物位置データ（地物位置記録手段）。

Claims (1)

1. 移動体の現在位置を検出する位置検出手段と、
   前記移動体の所定の基準に対する角度を検出する角度検出手段と、
   前記移動体の移動範囲における地物の位置があらかじめ記録された地物位置記録手段と、
   前記移動体の周囲を撮影する撮影手段と、
   前記移動体の周囲にレーザを送出して、前記移動体と周辺環境との距離を測定する距離測定手段と、
   前記撮影手段で撮影された画像内における、前記地物位置記録手段に記録された特定の地物に対する前記距離測定手段による測定結果から前記移動体と前記特定の地物との相対関係を求め、前記相対関係に基づいて前記位置検出手段で検出された前記移動体の現在位置を補正するとともに、複数回の前記相対関係に基づいて前記角度検出手段で検出された前記移動体の角度を補正する位置補正手段と、
   を備えたことを特徴とする位置補正システム。

Images