(57)【要約】【課題】１つのセンサで移動体の方位角を算出し、進行方向の変化量を得ることが可能な測定装置を提供する。【解決手段】測定装置は、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する。そして、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する。【選択図】図１０
2017177962.tif 000002
【特許請求の範囲】
【請求項１】
移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得部と、
前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出部と、
を備えることを特徴とする測定装置。
【請求項２】
前記取得部は、前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得し、
前記算出部は、前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度及び少なくとも前記移動体と前記２つの地物までのそれぞれの距離に基づき、前記変化量を算出することを特徴とする請求項１に記載の測定装置。
【請求項３】
前記変化は第１時刻から第２時刻における時間的変化であり、前記変化量は単位時間あたりの前記移動体の前記進行方向の変化量であることを特徴とする請求項１又は２に記載の測定装置。
【請求項４】
前記変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第１情報を補正する補正部を更に備えることを特徴とする請求項１乃至３いずれか一項に記載の測定装置。
【請求項５】
測定装置により実行される測定方法であって、
移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得工程と、
前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出工程と、
を備えることを特徴とする測定方法。
【請求項６】
コンピュータを備える測定装置によって実行されるプログラムであって、
移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得部、
前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出部、
として前記コンピュータを機能させることを特徴とするプログラム。
【請求項７】
請求項６に記載のプログラムを記憶した記憶媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の進行方向の変化を検出する技術に関する。
【背景技術】
【０００２】
移動体の前方に存在する物体を認識して、移動体のヨーレートを補正する手法が知られている。特許文献１は、移動体の前方に存在する固定物を認識し、その固定物が自車両に対して相対的に移動する移動軌跡に基づいて、ヨーレートセンサのずれ量を補正する手法を記載している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−６６７７７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載の手法では、上記の移動軌跡の他に、車速、旋回半径からのずれ量、固定物が横方向に相対的に移動する横移動量など、推定ヨーレートの算出に用いるパラメータが多く、これらのパラメータを取得するセンサのずれ量も考慮しなくてはならない。
【０００５】
本発明が解決しようとする課題としては、上記のものが例として挙げられる。本発明は、１つのセンサで移動体の方位角を算出し、進行方向の変化量を得ることが可能な測定装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
請求項１に記載の発明は、測定装置であって、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得部と、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出部と、を備えることを特徴とする。
【０００７】
請求項５に記載の発明は、測定装置により実行される測定方法であって、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得工程と、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出工程と、を備えることを特徴とする。
【０００８】
請求項６に記載の発明は、コンピュータを備える測定装置によって実行されるプログラムであって、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得部、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出部、として前記コンピュータを機能させることを特徴とする。
【図面の簡単な説明】
【０００９】
【図１】第１実施例の第１例による方位角変化量の算出方法を示す。
【図２】第１実施例の第２例による方位角変化量の算出方法を示す。
【図３】第１実施例による係数更新装置の構成を示すブロック図である。
【図４】第１実施例による係数更新処理のフローチャートである。
【図５】第２実施例による方位角変化量の算出方法を示す。
【図６】地図座標系及び車両座標系を説明する図である。
【図７】第２実施例による車両の方位角の算出方法を示す。
【図８】第２実施例による係数更新装置の構成を示すブロック図である。
【図９】第２実施例による係数更新処理のフローチャートである。
【図１０】３つの地物と移動車両との位置関係の例を示す。
【図１１】３つの地物と移動車両との位置関係の他の例を示す。
【発明を実施するための形態】
【００１０】
本発明の好適な実施形態では、測定装置は、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得部と、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出部と、を備える。
【００１１】
上記の測定装置は、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する。そして、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する。これにより、移動体から計測できる任意の地物を利用して、移動体の進行方向の変化量を算出することができる。
【００１２】
上記の測定装置の一態様では、前記取得部は、前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得し、前記算出部は、前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度及び少なくとも前記移動体と前記２つの地物までのそれぞれの距離に基づき、前記変化量を算出する。この態様では、センサなどを用いた地物の計測により、移動体の進行方向の変化量を算出することができる。
【００１３】
上記の測定装置の他の一態様では、前記変化は第１時刻から第２時刻における時間的変化であり、前記変化量は単位時間あたりの前記移動体の前記進行方向の変化量である。
【００１４】
上記の測定装置の他の一態様は、前記変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第１情報を補正する補正部を更に備える。この態様では、単位時間あたりの前記進行方向の変化量に基づいて、移動体のヨーレートを算出するための第１情報を補正する。一例では、第１情報は、角速度センサの感度及びオフセットを含む。
【００１５】
本発明の他の好適な実施形態では、測定装置により実行される測定方法は、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得工程と、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出工程と、を備える。これにより、移動体から計測できる任意の地物を利用して、移動体の進行方向の変化量を算出することができる。
【００１６】
本発明の他の好適な実施形態では、コンピュータを備える測定装置によって実行されるプログラムは、移動体から少なくとも３つの地物までの距離を含む第１距離群及び前記少なくとも３つの地物についての地物間距離を含む第２距離群を取得する取得部、前記第１距離群及び前記第２距離群のうち少なくとも一方に基づき特定される前記少なくとも３つの地物のうちの２つの地物と、前記移動体との位置関係の変化に基づき、前記移動体の進行方向の変化量を算出する算出部、として前記コンピュータを機能させる。これにより、移動体から計測できる任意の地物を利用して、移動体の進行方向の変化量を算出することができる。このプログラムは、記憶媒体に記憶して利用することができる。
【実施例】
【００１７】
以下、図面を参照して本発明の好適な実施例について説明する。
【００１８】
［１］背景
従来のカーナビゲーション装置に搭載されている自車位置推定システムでは、車速パルスから算出される速度とジャイロセンサの出力から算出される方位角を用いて車両の移動状態を測定し、ＧＰＳ情報と統合することにより現在位置を推定する複合航法システムが用いられている。複合航法システムによる自車位置推定精度向上のためには、車体速度と方位角（ヨー角）を正確に把握する必要がある。
【００１９】
ジャイロセンサの感度とオフセットは温度や振動などによって変化する特性がある。そのため、それらの誤差を含んだ状態で角速度を積分すると、誤差が蓄積し、計算される方位角は実際の値から大きくずれてしまう。従って、方位角を高精度に推定するためには、ジャイロセンサの感度とオフセットを継続的に補正することが必要である。
【００２０】
従来のカーナビゲーション装置では、ＧＰＳ情報に基づく方位変化量を用いて感度とオフセットの補正を行っている。そのため、都市部のようなＧＰＳの受信状態の悪い環境では補正の精度が劣化する。また、トンネルや地下駐車場のようなＧＰＳ測位ができない環境では補正ができない。よって、ＧＰＳの受信状態に関わらず、継続的に感度とオフセットの補正を行うことが要求される。
【００２１】
本実施例による係数更新装置は、ＧＰＳ情報を用いずに、外界センサによる地物の計測結果に基づいて車両の方位変化量を算出し、ジャイロセンサの感度とオフセットを補正する。外界センサとしては、カメラやＬｉＤＡＲ（Ｌｉｇｈｔ  Ｄｅｔｅｃｔｉｏｎ  Ａｎｄ  Ｒａｎｇｉｎｇ）、ミリ波レーダーなどが挙げられる。
【００２２】
具体的には、係数更新装置は、以下の工程によりジャイロセンサの感度及びオフセットを補正する。
（１）時刻ｔ−１及び時刻ｔにおいて、２つの地物を検出する。
（２）各時刻の地物計測結果に基づいて、時刻ｔ−１から時刻ｔの間の車両の方位角の変化量を算出する。
（３）逐次最小二乗法により、感度係数とオフセット係数を推定する。
【００２３】
［２］ジャイロセンサの感度とオフセットの推定方法
真の角速度Ψ＿ｄｏｔ（ｔ）と、ジャイロセンサで検出する角速度ωｔとの関係は、感度係数αとオフセット係数βを用いて次のように表すことができる。
【００２４】
【数１】
2017177962.tif 000003
外界センサによる地物計測値から、車両の方位角の変化量（以下、「方位角変化量」と呼ぶ。）ΔΨＬを取得できた場合、これをその時間間隔Δｔで除することにより、次のように角速度を算出する。
【００２５】
【数２】
2017177962.tif 000004
外界センサによる地物計測値から求めた角速度（「計測ヨーレート」とも呼ぶ。）を真の角速度とみなすと、次のように表すことができる。
【００２６】
【数３】
2017177962.tif 000005
よって、次のように置くと、
【００２７】
【数４】
2017177962.tif 000006
以下の１次式で表すことができる。
【００２８】
【数５】
2017177962.tif 000007
従って、ｘｔとｙｔの複数のデータがあれば、逐次最小二乗法等により、αとβを求めることができる。即ち、外界センサを用いて車両の方位角変化量ΔΨを取得できれば、ジャイロセンサの感度とオフセットを推定することができる。
【００２９】
［３］方位角変化量の算出方法
以下、外界センサを用いて車両の方位角変化量ΔΨを求める方法を説明する。
【００３０】
［３．１］第１実施例
第１実施例は、外界センサによる地物の測定値を用いて方位角変化量ΔΨを求めるものである。
【００３１】
［３．１．１］方位角変化量の算出方法（第１例）
時刻ｔ−１及び時刻ｔにおいて、同一の地物を２つ以上計測できたとき、その計測結果を用いて車両の方位角変化量を算出することができる。ここで、外界センサからは、自車両に対する地物の相対距離と相対角度（ｒ、φ）が取得できるものとする。図１は、第１実施例の第１例により車両の方位角変化量を算出する方法を説明する図である。図１の例では、時刻ｔ−１と時刻ｔにおいて、外界センサにより地物Ａと地物Ｂが計測できたものとする。なお、第１例では、各時刻において、外界センサにより同じ２つの地物を計測できることが必要である。
【００３２】
車両の方位角変化量ΔΨは、時刻ｔ−１及び時刻ｔにおける地物Ａの方位角φＡ，ｔ−１、φＡ，ｔと、図１における角度Δθ、γを用いて、以下のように算出することができる。
【００３３】
まず、三角形の内角と外角の関係から、
【００３４】
【数６】
2017177962.tif 000008
が得られる。従って、方位角変化量ΔΨは、
【００３５】
【数７】
2017177962.tif 000009
で得られる。
【００３６】
ここで、角度θｔ−１は、余弦定理より、
【００３７】
【数８】
2017177962.tif 000010
となる。同様に、角度θｔは、
【００３８】
【数９】
2017177962.tif 000011
となる。
【００３９】
よって、外界センサにより得られた方位角φＡ，ｔ−１、φＡ，ｔと、上記の式（８）、（９）により得られた角度θｔ−１、θｔを式（７）に代入することにより、方位角変化量ΔΨを算出することができる。
【００４０】
［３．１．２］方位角変化量の算出方法（第２例）
上記の第１例は地物Ａ側の角度を用いたが、地物Ｂ側の角度を用いても同様に方位角変化量ΔΨを算出することができる。図２は、第１実施例の第２例により車両の方位角変化量を算出する方法を説明する図である。図２の例でも、時刻ｔ−１と時刻ｔにおいて、外界センサにより地物Ａと地物Ｂが計測できたものとする。なお、第２例でも、各時刻において、外界センサにより同じ２つの地物を計測できることが必要である。
【００４１】
車両の方位角変化量ΔΨは、時刻ｔ−１及び時刻ｔにおける地物Ａの方位角φＢ，ｔ−１、φＢ，ｔと、図２における角度Δθ’、γ’を用いて、以下のように算出することができる。
【００４２】
まず、三角形の内角と外角の関係から、
【００４３】
【数１０】
2017177962.tif 000012
が得られる。
【００４４】
従って、方位角変化量ΔΨは、
【００４５】
【数１１】
2017177962.tif 000013
で得られる。
【００４６】
ここで、角度θ’ｔ−１は、余弦定理より、
【００４７】
【数１２】
2017177962.tif 000014
となる。同様に、角度θ’ｔは、
【００４８】
【数１３】
2017177962.tif 000015
となる。
【００４９】
よって、外界センサにより得られた方位角φＢ，ｔ−１、φＢ，ｔと、上記の式（１２）、（１３）により得られた角度θ’ｔ−１、θ’ｔを式（１１）に代入することにより、方位角変化量ΔΨを算出することができる。
【００５０】
［３．１．３］地物計測値
上記の第１例及び第２例では、外界センサから地物の相対距離ｒと地物の相対角度φが取得できるものとしているが、地物の相対距離ｒと相対角度φではなく、地物の相対位置（ｘ，ｙ）が取得できる場合には、以下の式によって相対位置（ｘ，ｙ）から相対距離ｒ及び相対角度φを求めて使用すればよい。
【００５１】
【数１４】
2017177962.tif 000016
［３．１．４］地物間の距離
上記の第１例及び第２例では、式（８）、（９）、（１２）、（１３）において２つの地物間の距離Ｒを使用している。地物間の距離Ｒは以下の方法により取得することができる。
【００５２】
まず、地物間の距離Ｒは、外界センサによる地物計測値を用いて算出することができる。具体的に、外界センサから自車両に対する地物の相対距離ｒと相対角度φが取得できる場合、余弦定理により地物間の距離Ｒは以下のように算出できる。
【００５３】
【数１５】
2017177962.tif 000017
一方、外界センサから自車両に対する地物の相対位置（ｘ，ｙ）が取得できる場合、地物間の距離Ｒは以下のように算出できる。
【００５４】
【数１６】
2017177962.tif 000018
また、地物の位置情報を有する高精度地図を利用できる場合には、高精度地図から地物間の距離Ｒを取得しても良い。外界センサによる地物計測値から地物間距離を算出する場合は、計測精度によって地物間距離の精度も変わってしまう。つまり、計測精度が悪いと、算出される地物間距離の精度も悪くなり、後で計算する車両の方位角変化量の精度も悪くなる。高精度地図を用いると、この問題を回避することができ、得られる車両の方位角変化量の精度も向上させることができる。
【００５５】
［３．１．５］係数更新装置
次に、ジャイロセンサの感度係数α及びオフセット係数βを更新する係数更新装置について説明する。図３は、第１実施例による係数更新装置１０の構成を示すブロック図である。図示のように、係数更新装置１０は、ジャイロセンサ１１と、外界センサ１２と、進行方向取得部１３と、地物計測部１４と、地物間距離計算部１５と、係数計算部１６と、方位角変化量計算部１７とを備える。なお、進行方向取得部１３、地物計測部１４、地物間距離計算部１５、係数計算部１６、及び、方位角変化量計算部１７は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。
【００５６】
ジャイロセンサ１１は、検出した角速度ωｔを進行方向取得部１３及び係数計算部１６へ供給する。進行方向取得部１３は、ジャイロセンサ１１から供給された角速度ωｔに基づいて車両の進行方向Ｈｄを取得し、地物計測部１４に供給する。
【００５７】
外界センサ１２は、例えばカメラ、ＬｉＤＡＲ、ミリ波レーダーなどである。地物計測部１４は、外界センサ１２の出力に基づいて地物までの距離及び地物との角度を計測する。具体的には、地物計測部１４は、時刻ｔ−１において、車両から２つの地物Ａ、Ｂまでの距離（相対距離）ｒＡ,ｔ−１、ｒＢ，ｔ−１を計測するとともに、進行方向取得部１３から供給された車両の進行方向Ｈｄｔ−１に対する２つの地物Ａ、Ｂの方位角（相対角度）φＡ，ｔ−１、φＢ，ｔ−１を算出し、地物間距離計算部１５及び方位角変化量計算部１７に供給する。また、地物計測部１４は、時刻ｔにおいて、車両から２つの地物Ａ、Ｂまでの距離ｒＡ,ｔ、ｒＢ，ｔを計測するとともに、進行方向取得部１３から供給された車両の進行方向Ｈｄｔに対する２つの地物Ａ、Ｂの方位角φＡ，ｔ、φＢ，ｔを算出し、地物間距離計算部１５及び方位角変化量計算部１７に供給する。
【００５８】
地物間距離計算部１５は、地物計測部１４が計測した２つの地物Ａ、Ｂの距離ｒＡ、ｒＢ及び方位角φＡ、φＢに基づいて、前述の式（１５）により地物Ａ、Ｂ間の地物間距離Ｒを算出して方位角変化量計算部１７へ供給する。なお、高精度地図を利用できる場合は、地物間距離計算部１５は、高精度地図から地物間距離Ｒを取得しても良い。
【００５９】
方位角変化量計算部１７は、地物計測部１４から供給された方位角φＡ，ｔ−１、φＢ，ｔ−１、φＡ，ｔ、φＢ，ｔ、及び、地物間距離計算部１５が算出した地物間距離Ｒに基づいて、前述の式（７）〜（９）又は式（１１）〜（１３）により、車両の方位角変化量ΔΨを算出して係数計算部１６に供給する。
【００６０】
係数計算部１６は、ジャイロセンサ１１から供給された角速度ωｔと、方位角変化量計算部１７から供給された方位角変化量ΔΨに基づいて、前述の式（２）〜（３）により、ジャイロセンサの感度係数α及びオフセット係数βを算出し、更新する。
【００６１】
上記の構成において、地物計測部１４は本発明の第１取得部の一例であり、方位角変化量計算部１７は本発明の算出部の一例であり、地物間距離計算部１５は本発明の第２取得部の一例であり、係数計算部は本発明の補正部の一例である。また、方位角変化量は本発明における進行方向の変化量に相当する。
【００６２】
［３．１．６］係数更新処理
次に、上記の係数更新装置１０による係数更新処理について説明する。図４は、係数更新処理のフローチャートである。なお、以下の説明では、前述した方位角変化量の算出方法の第１例を使用するものとする。
【００６３】
まず、係数更新装置１０は、外界センサ１２により地物Ａ、Ｂを検出し（ステップＳ１０）、前述の式（９）により角度θｔを算出する（ステップＳ１１）。次に、係数更新装置１０は、ｆｌａｇ＝１であるか否かを判定する（ステップＳ１２）。なお、「ｆｌａｇ」は処理の開始時に「０」にリセットされている。ｆｌａｇ＝１でない場合（ステップＳ１２：ＮＯ）、係数更新装置１０はｆｌａｇに「１」をセットし（ステップＳ１３）、ステップＳ１０へ戻る。
【００６４】
一方、ｆｌａｇ＝１である場合（ステップＳ１２：ＹＥＳ）、２つの時刻において地物Ａ、Ｂが検出され、角度θｔ−１、θｔが得られているので、係数更新装置１０は、１時刻前の地物Ａの方位角φＡ，ｔ−１及び角度θｔ−１と、現在時刻の地物Ａの方位角φＡ，ｔ及び角度θｔから、前述の式（７）を用いて方位角変化量ΔΨを算出する（ステップＳ１４）。
【００６５】
次に、係数更新装置１０は、方位角変化量ΔΨと時間間隔Δｔから、式（２）を用いて真の角速度（計測ヨーレート）Ψ＿ｄｏｔ（ｔ）を算出し（ステップＳ１５）、得られた計測ヨーレートΨ＿ｄｏｔ（ｔ）とジャイロセンサの出力ωｔから、前述の式（３）により感度係数αとオフセット係数βを算出して更新する（ステップＳ１６）。
【００６６】
［３．２］第２実施例
［３．２．１］方位角変化量の算出方法
第２実施例では、高精度地図と外界センサによる地物計測結果とを用いて車両の方位角変化量を算出する。図５は、第２実施例による方位角変化量の算出方法を説明する図である。図５では、地図座標系（Ｘｍ，Ｙｍ）上に車両の位置を示している。ここで、地図座標系と車両座標系との関係を図６に示す。図６（Ａ）に示すように、地図座標系はＸｍ軸とＹｍ軸により規定される。一方、図６（Ｂ）に示すように、車両座標系は、車両の前後方向を示すＸｖ軸と、車両の左右方向を示すＹｖ軸と、車両の上下方向を示すＺｖ軸により規定される。
【００６７】
図５に戻り、第２実施例では、まず、時刻ｔ−１で、外界センサを用いて高精度地図に登録されている地物を少なくとも２つ計測する。図５の例では、地物Ａと地物Ｂが計測される。そして、高精度地図から得られる地物Ａ、Ｂの位置情報と、外界センサによる計測結果から車両の方位角Ψｔ−１を算出する。
【００６８】
次に、時刻ｔにおいても同様に、外界センサを用いて高精度地図に登録されている地物を少なくとも２つ計測する。図５の例では、地物Ｃと地物Ｄが計測される。そして、高精度地図から得られる地物Ｃ、Ｄの位置情報と、外界センサによる計測結果から車両の方位角Ψｔを算出する。
【００６９】
次に、各時刻における車両の方位角Ψｔ−１、Ψｔの差分から、下記の式により、時刻ｔ−１から時刻ｔの間の車両の方位角の変化量ΔΨを求める。
【００７０】
【数１７】
2017177962.tif 000019
なお、第２実施例においては、車両の方位角は各時刻で独立に計算できるので、時刻ｔ−１と時刻ｔで外界センサにより同じ２つの地物を検出できる必要はない。図５の例では、外界センサは、時刻ｔ−１では地物Ａ、Ｂを検出しており、時刻ｔでは地物Ａ、Ｂとは異なる地物Ｃ、Ｄを検出している。
【００７１】
次に、各時刻における車両の方位角の算出方法について説明する。図７は、第２実施例による車両の方位角の計算方法を示す。いま、高精度地図から地物Ａの位置（ｘｍＡ，ｙｍＡ）と地物Ｂの位置（ｘｍＢ，ｙｍＢ）を取得でき、外界センサを用いて自車両に対する地物Ａの相対位置（ｘｖＡ，ｙｖＡ）と地物Ｂの相対位置（ｘｖＢ，ｙｖＢ）を取得できるものとする。地物Ｂから地物Ａへのベクトルｐは、地図座標系においては、
［ｘｍＡ−ｘｍＢ  ｙｍＡ−ｙｍＢ］Ｔ
と表され、車両座標系においては、
［ｘｖＡ−ｘｖＢ  ｙｖＡ−ｙｖＢ］Ｔ
と表される。ここで、次のようにおく。
【００７２】
【数１８】
2017177962.tif 000020
地図座標系から車両座標系への方向余弦行列Ｃは、次のようになる。
【００７３】
【数１９】
2017177962.tif 000021
従って、
【００７４】
【数２０】
2017177962.tif 000022
となる。
【００７５】
式（２０）×ｄｘｍ＋式（２１）×ｄｙｍより、
【００７６】
【数２１】
2017177962.tif 000023
が得られる。
【００７７】
また、式（２０）×ｄｙｍ−式（２１）×ｄｘｍより、
【００７８】
【数２２】
2017177962.tif 000024
が得られる。よって、式（２２）〜（２４）を用いて、車両の方位角Ψを算出することができる。
【００７９】
こうして、時刻ｔ−１及び時刻ｔにおいてそれぞれ車両の方位角Ψｔ−１、Ψｔを算出し、式（１７）に代入することにより、車両の方位角変化量ΔΨを算出することができる。
【００８０】
［３．２．２］係数更新装置
次に、第２実施例による係数更新装置について説明する。図８は、第２実施例による係数更新装置２０の構成を示すブロック図である。図示のように、係数更新装置２０は、ジャイロセンサ２１と、外界センサ２２と、地物計測部２３と、係数計算部２４と、方位角変化量計算部２５と、地図データベース（ＤＢ）２６とを備える。なお、地物計測部２３、係数計算部２４、及び、方位角変化量計算部２５は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。
【００８１】
ジャイロセンサ２１は、検出した角速度ωｔを係数計算部２４へ供給する。外界センサ２２は、例えばカメラ、ＬｉＤＡＲ、ミリ波レーダーなどである。地物計測部２３は、外界センサ２２の出力に基づいて、２つの地物の相対位置（ｘｖ，ｙｖ）を計測し、方位角変化量計算部２５へ供給する。
【００８２】
方位角変化量計算部２５は、高精度地図を記憶した地図ＤＢ２６から、２つの地物の相対位置（ｘｍ，ｙｍ）を取得する。そして、方位角変化量計算部２５は、２つの地物の相対位置（ｘｖ，ｙｖ）、（ｘｍ，ｙｍ）を用いて、式（１８）〜（２４）により方位角Ψｔを算出する。この処理を異なる２つの時刻で行うことにより、方位角変化量計算部２５は、方位角Ψｔ−１とΨｔを算出し、式（１７）により方位角変化量ΔΨを算出して係数計算部２４へ供給する。
【００８３】
係数計算部２４は、ジャイロセンサ２１から供給された角速度ωｔと、方位角変化量計算部２５から供給された方位角変化量ΔΨに基づいて、前述の式（２）〜（３）により、ジャイロセンサの感度係数α及びオフセット係数βを算出し、更新する。
【００８４】
［３．２．３］係数更新処理
次に、第２実施例による係数更新処理について説明する。図９は、第２実施例による係数更新処理のフローチャートである。
【００８５】
まず、係数更新装置２０は、外界センサ１２により２つの地物の位置を取得する（ステップＳ２０）。次に、係数更新装置２０は、地図ＤＢ２６に記憶されている高精度地図から、それら２つの地物の位置を取得する（ステップＳ２１）。次に、係数更新装置２０は、外界センサより取得した２つの地物の位置と、高精度地図より取得した２つの地物の位置に基づいて、式（１８）〜（２４）により、方位角Ψを算出する（ステップＳ２２）。
【００８６】
次に、係数更新装置２０は、ｆｌａｇ＝１であるか否かを判定する（ステップＳ２３）。なお、「ｆｌａｇ」は処理の開始時に「０」にリセットされている。ｆｌａｇ＝１でない場合（ステップＳ２３：ＮＯ）、係数更新装置２０はｆｌａｇに「１」をセットし（ステップＳ２４）、ステップＳ２０へ戻る。
【００８７】
一方、ｆｌａｇ＝１である場合（ステップＳ２３：ＹＥＳ）、２つの時刻においてそれぞれ２つの地物が検出され、方位角Ψｔ−１とΨｔが得られているので、係数更新装置２０は、式（１７）により方位角変化量ΔΨを算出する（ステップＳ２５）。
【００８８】
次に、係数更新装置２０は、方位角変化量ΔΨと時間間隔Δｔから、式（２）を用いて真の角速度（計測ヨーレート）Ψ＿ｄｏｔ（ｔ）を算出する（ステップＳ２６）。そして、係数更新装置２０は、計測ヨーレートΨ＿ｄｏｔ（ｔ）とジャイロセンサの出力ωｔから、前述の式（３）により感度係数αとオフセット係数βを算出し、更新する（ステップＳ２７）。
【００８９】
［４］３つ以上の地物を計測できた場合の処理
上記の係数更新処理では２つの地物を計測しているが、同時に３つ以上の地物を計測できた場合には、以下の方法により方位角変化量ΔΨを算出することができる。
【００９０】
（Ａ）平均値を使用する方法
同時に３つ以上の地物を測定できた場合、方位角変化量ΔΨを複数通り計算し、それらを平均した値を係数の更新に使用することができる。
【００９１】
例えば、地物を３個計測できた場合、図１０に示すように、地物１と地物２、地物２と地物３、地物３と地物１の組み合わせを選ぶことができる。それぞれの組み合わせにおいて、前述の係数更新処理により時刻ｔ−１から時刻ｔの間の方位角変化量ΔΨを計算する。地物１と地物２の組み合わせにより得られた方位角変化量をΔΨ１２とし、地物２と地物３の組み合わせにより得られた方位角変化量をΔΨ２３とし、地物３と地物１の組み合わせにより得られた方位角変化量をΔΨ３１とすると、以下のようにこれらを平均した値を方位角変化量ΔΨとして使用することができる。
【００９２】
【数２３】
2017177962.tif 000025
これにより、方位角変化量ΔΨの精度を統計的に向上させることができる。
【００９３】
（Ｂ）地物までの距離を考慮して２つの地物を選択する方法
同時に３つ以上の地物を計測できた場合、そのうちの信頼度の高い２つの地物の組み合わせに基づいて方位角変化量を求めることにより、高精度で方位角変化量を得ることができる。一般的に、外界センサによる計測は、距離が遠いほど精度が下がる。従って、同時に３つ以上の地物を計測できた場合、車両からの距離が近い方から２つの地物を選択し、それらに基づいて第１実施例又は第２実施例の方法により方位角変化量ΔΨを求める。
【００９４】
例えば、図１０の例のように３つの地物１〜３が計測できた場合、車両からの３つの地物までの距離は、Ｌ１＜Ｌ３＜Ｌ２である。よって、係数更新装置は、車両からの距離が近い方の２つの地物、即ち、地物１及び地物３を用いて方位角変化量ΔΨを求めればよい。
【００９５】
（Ｃ）地物間の距離を考慮して２つの地物を選択する方法
通常、地物同士の位置が近すぎると計算精度が低下する。そこで、同時に３つ以上の地物を計測できた場合、予め決定した所定の閾値Ｌｔｈよりも近い２つの地物の組み合わせを排除する。
【００９６】
例えば、図１１の例のように３つの地物１〜３が計測できた場合、各地物間の距離を閾値Ｌｔｈと比較する。図１１の例では、Ｌ１２＜Ｌｔｈ、Ｌ２３＞Ｌｔｈ、Ｌ３１＞Ｌｔｈであるとする。この場合、地物間の距離が閾値Ｌｔｈよりも短い組み合わせ、即ち、地物１と地物２の組み合わせを除外し、地物２と地物３の組み合わせ、及び、地物１と地物３の組み合わせを使用して方位角変化量ΔΨを求めればよい。具体的には、上記の（Ｂ）の方法により車両から地物までの距離を考慮すると、地物２より地物１の方が車両から近い（Ｌ１＜Ｌ２）ので、係数更新装置は、地物１と地物３の組み合わせに基づいて方位角変化量ΔΨを求めればよい。
【００９７】
その代わりに、上記の（Ａ）の方法を使用し、地物１と地物２の組み合わせにより得られる方位角変化量ΔΨ１２と地物１と地物３の組み合わせにより得られる方位角変化量ΔΨ３１との平均値を方位角変化量ΔΨとしても良い。なお、上記の方法では、予め所定距離を閾値Ｌｔｈとして定めているが、その代わりに、計測された３つ以上の地物間距離の平均値を閾値Ｌｔｈとして使用しても良い。
【００９８】
［５］変形例
上記の実施例では、２つの地物の計測結果から地物間距離Ｒを算出方法と、高精度地図の地図データを用いて地物間距離Ｒを取得する方法のいずれかを用いているが、両者を組み合わせて使用してもよい。例えば、高精度地図データが存在するエリアにおいては高精度地図データを使用して地物間距離Ｒを求め、高精度地図データが存在しないエリアでは地物の計測結果から地物間距離Ｒを求めてもよい。また、車両の状況に応じて、いずれか精度の高い方で得られた地物間距離Ｒを用いることとしてもよい。
【符号の説明】
【００９９】
１１、２１  ジャイロセンサ
１２、２２  外界センサ
１３  進行方向取得部
１４、２３  地物計測部
１５  地物間距離計算部
１６、２４  係数計算部
１７、２５  方位角変化量計算部
【図１】
2017177962.tif 000026
【図２】
2017177962.tif 000027
【図３】
2017177962.tif 000028
【図４】
2017177962.tif 000029
【図５】
2017177962.tif 000030
【図６】
2017177962.tif 000031
【図７】
2017177962.tif 000032
【図８】
2017177962.tif 000033
【図９】
2017177962.tif 000034
【図１０】
2017177962.tif 000035
【図１１】
2017177962.tif 000036