(57)【要約】【課題】慣性航法時の自己位置推定精度を向上させることができるようにする。【解決手段】姿勢角補正量算出部２４０が、当該自車の位置が取得された場合に、姿勢角補正値を用いずに推定された自車の位置である第２位置と、取得された当該自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出し、第１自己位置推定部２５０が、当該自車の位置が取得されなかった場合、姿勢角補正値を用いて、当該自車の位置である第１位置を推定する。【選択図】図１
2020112490.tif 000002
【特許請求の範囲】
【請求項１】
速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部と、
前記取得部が取得した前記自車の速度と、前記自車の角速度とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部と、
前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出する姿勢角補正量算出部と、
前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値とに基づいて、前記自車の位置である第１位置を推定する第１自己位置推定部と、
を含む自己位置推定装置。
【請求項２】
前記姿勢角補正量算出部は、前記取得部によって前記自車の位置が取得された場合、前記取得部によって前回取得された前記自車の位置と前記第２位置との距離、及び前記取得部が取得した前記自車の位置と前記第２位置との横位置誤差を算出し、前記算出された前記距離と前記横位置誤差とに基づいて、前記姿勢角補正値を算出する
請求項１記載の自己位置推定装置。
【請求項３】
速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部と、
前記取得部が取得した前記自車の速度と、前記自車の角速度と、予め求められた車体横滑り角とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部と、
前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差とに基づいて、姿勢角補正値を算出する姿勢角補正量算出部と、
前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値と、前記車体横滑り角とに基づいて前記自車の位置である第１位置を推定する第１自己位置推定部と、
を含む自己位置推定装置。
【請求項４】
自車の操舵角に基づいて前記自車が直進しているか否かを判定する直進判定部と、
前記直進判定部により前記自車が直進していると判定された場合に、前記自車の速度に対して予め求められた車体横滑り角を設定する滑り角設定部と、
を更に含み、
前記取得部は、更に、舵角センサにより検出された前記自車の操舵角を取得する
請求項３記載の自己位置推定装置。
【請求項５】
前記自車の速度と、車体横滑り角が０となるときの予め求められた速度との差の絶対値が所定の閾値以下である場合に、前記車体横滑り角を０に設定する滑り角設定部
を更に含む請求項３記載の自己位置推定装置。
【請求項６】
前記自車の操舵角と前記自車の速度とを入力として、前記車体横滑り角を推定する車両モデルを用いて前記車体横滑り角を推定し、前記車体横滑り角を推定した値に設定する滑り角設定部
を更に含み、
前記取得部は、更に、舵角センサにより検出された前記自車の操舵角を取得する
請求項３記載の自己位置推定装置。
【請求項７】
前記姿勢角補正量算出部は、前記自車の操舵角に基づいて前記自車が直進していると判定された場合に、第１の時点での、前記取得部によって前回取得された前記自車の位置と前記第２位置との距離、及び前記取得部が取得した前記自車の位置と前記第２位置との横位置誤差と、第２の時点での、前記取得部によって前回取得された前記自車の位置と前記第２位置との距離、及び前記取得部が取得した前記自車の位置と前記第２位置との横位置誤差と、を算出し、
前記算出された前記第１の時点での前記距離及び前記横位置誤差と、前記第２の時点での前記距離及び前記横位置誤差とに基づいて、前記姿勢角補正値として、ヨーレートオフセット誤差及びヨー角オフセット誤差を算出し、
前記第１自己位置推定部は、前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記ヨーレートオフセット誤差及びヨー角オフセット誤差と、前記予め求められた車体横滑り角とに基づいて、前記自車の位置である第１位置を推定する
請求項３乃至６の何れか１項記載の自己位置推定装置。
【請求項８】
コンピュータを、
速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置とを取得する取得部、
前記取得部が取得した前記自車の速度と、前記自車の角速度とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部、
前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出する姿勢角補正量算出部、及び
前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置と、前記姿勢角補正量算出部により算出された前記姿勢角補正値とに基づいて、前記自車の位置である第１位置を推定する第１自己位置推定部
として機能させるためのプログラム。
【請求項９】
コンピュータを、
速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置とを取得する取得部、
前記取得部が取得した前記自車の速度と、前記自車の角速度及び姿勢角と、予め求められた車体横滑り角とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部、
前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差とに基づいて、姿勢角補正値を算出する姿勢角補正量算出部、及び
前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値と、前記車体横滑り角とに基づいて前記自車の位置である第１位置を推定する第１自己位置推定部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来から、車両の自己位置が取得できなくなる場合に、慣性航法時による自己位置推定により車両の自己位置を推定する方法が知られている。
【０００３】
慣性航法による自己位置推定では、実際の走行軌跡との誤差を含むため、この誤差の蓄積により推定した自己位置が実際の自己位置と大きく異なる場合が生じ得る。
【０００４】
特許文献１では、（１）車両の自己位置（ｘ０，y０）、姿勢（ヨー角、Φ０）を与えて、慣性航法により自己位置推定を開始し、（２）反射板到達位置Ｐ１時点において、距離計測手段により推定した自己位置と反射板との距離ｄを求め、（３）車線中央と反射板Ｂ１との距離Ｄ１と、推定した自己位置と車線中央との距離ｅ１と、求めた距離ｄとに基づいて推定位置の誤差ｅ１を下記式（１）で求め、（４）反射板間隔Ｌを用いてＰ１時点における姿勢角補正値ΔΦ１を下記式（２）で算出する。
【０００５】
【数１】
2020112490.tif 000003
【０００６】
そして、慣性航法による自己位置推定によるＰ１時点の位置における姿勢Φを姿勢角補正値ΔΦ１だけ補正することにより、慣性航法による自己位置推定の誤差の蓄積を防止している。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００４−２６４１８２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかし、上記特許文献１の手法では反射板が必須であり、反射板のない場所では使用できない、という問題があった。
【０００９】
また、特許文献１では、姿勢角補正値ΔΦ１の算出方法は明記されているが、姿勢角補正値ΔΦ１の自己位置推定への参照方法が明記されておらず、自己位置推定の精度向上の課題が解決できない、という問題があった。
【００１０】
また、車両のような移動体の自己位置はヨー角ψと滑り角βの和からなる姿勢角Φで決まる。特許文献１の姿勢角補正値ΔΦ１は、ヨー角ψの誤差と未知の滑り角βの２つの和を算出している。滑り角βは操舵入力により変化するが、特許文献１の手法では姿勢角補正値ΔΦ１の算出時点とそれ以降の時点とで滑り角βが変化しないという前提が必要となる。
【００１１】
しかし、実際には操舵入力によって滑り角βが変わるため、姿勢角補正値ΔΦ１に誤差を含んでしまう、という問題があった。
【００１２】
本発明は、上記問題点を解決するためになされたもので、慣性航法時の自己位置推定精度を向上させることができる自己位置推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１３】
上記の目的を達成するために、本発明の自己位置推定装置は、速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部と、前記取得部が取得した前記自車の速度と、前記自車の角速度とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部と、前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出する姿勢角補正量算出部と、前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値とに基づいて、前記自車の位置である第１位置を推定する第１自己位置推定部と、を含んで構成されている。
【００１４】
また、本発明のプログラムは、コンピュータを、速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部、前記取得部が取得した前記自車の速度と、前記自車の角速度とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部、前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出する姿勢角補正量算出部、及び前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値とに基づいて、前記自車の位置である第１位置を推定する第１自己位置推定部として機能させるためのプログラムである。
【００１５】
本発明の自己位置推定装置及びプログラムによれば、取得部が、速度センサにより検出された自車の速度と、角速度センサにより検出された当該自車の角速度と、測位装置により検出された当該自車の位置及び姿勢角とを取得し、第２自己位置推定部が、取得部が取得した当該自車の速度と、当該自車の角速度とに基づいて、当該自車の位置である第２位置を推定し、姿勢角補正量算出部が、取得部によって当該自車の位置が取得された場合、第２自己位置推定部により推定された当該第２位置と、取得部が取得した当該自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出する。
【００１６】
そして、第１自己位置推定部が、取得部によって当該自車の位置が取得されなかった場合、取得部が取得した当該自車の速度、当該自車の角速度、及び取得部によって前回取得された当該自車の位置及び姿勢角と、姿勢角補正量算出部により算出された姿勢角補正値とに基づいて、当該自車の位置である第１位置を推定する。
【００１７】
このように、当該自車の位置が取得された場合に、姿勢角補正値を用いずに推定された自車の位置である第２位置と、取得された当該自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出し、当該自車の位置が取得されなかった場合、姿勢角補正値を用いて、当該自車の位置である第１位置を推定することにより、慣性航法時の自己位置推定精度を向上させることができる。
【００１８】
また、本発明の自己位置推定装置において、前記姿勢角補正量算出部は、前記取得部によって前記自車の位置が取得された場合、前記取得部によって前回取得された前記自車の位置と前記第２位置との距離、及び前記取得部が取得した前記自車の位置と前記第２位置との横位置誤差を算出し、前記算出された前記距離と前記横位置誤差とに基づいて、前記姿勢角補正値を算出することができる。
【００１９】
また、本発明の自己位置推定装置は、速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部と、前記取得部が取得した前記自車の速度と、前記自車の角速度と、予め求められた車体横滑り角とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部と、前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差とに基づいて、姿勢角補正値を算出する姿勢角補正量算出部と、前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値と、前記車体横滑り角とに基づいて前記自車の位置である第１位置を推定する第１自己位置推定部と、を含んで構成されている。
【００２０】
また、本発明のプログラムは、コンピュータを、速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部、前記取得部が取得した前記自車の速度と、前記自車の角速度と、予め求められた車体横滑り角とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部、前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差とに基づいて、姿勢角補正値を算出する姿勢角補正量算出部、及び前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値と、前記車体横滑り角とに基づいて前記自車の位置である第１位置を推定する第１自己位置推定部として機能させるプログラムである。
【００２１】
本発明の自己位置推定装置及びプログラムによれば、取得部が、速度センサにより検出された自車の速度と、角速度センサにより検出された当該自車の角速度と、測位装置により検出された当該自車の位置及び姿勢角とを取得し、第２自己位置推定部が、取得部が取得した当該自車の速度と、当該自車の角速度と、予め求められた車体横滑り角とに基づいて、当該自車の位置である第２位置を推定し、姿勢角補正量算出部が、取得部によって当該自車の位置が取得された場合、当該第２自己位置推定部により推定された当該第２位置と、取得部が取得した当該自車の位置との偏差とに基づいて、姿勢角補正値を算出する。
【００２２】
そして、第１自己位置推定部が、取得部によって当該自車の位置が取得されなかった場合、当該取得部が取得した当該自車の速度、当該自車の角速度、及び取得部によって前回取得された当該自車の位置及び姿勢角と、当該姿勢角補正量算出部により算出された当該姿勢角補正値と、当該車体横滑り角とに基づいて当該自車の位置である第１位置を推定する。
【００２３】
このように、姿勢角補正値を用いずに、当該自車の速度と、当該自車の角速度と、予め求められた車体横滑り角とに基づいて、当該自車の位置である第２位置を推定し、当該自車の位置が取得された場合に、推定された当該第２位置と、取得された当該自車の位置との偏差とに基づいて、姿勢角補正値を算出し、当該自車の位置が取得されなかった場合、当該姿勢角補正値を用いて当該自車の位置である第１位置を推定することにより、慣性航法時の自己位置推定精度を向上させることができる。
【００２４】
また、本発明の自己位置推定装置は、自車の操舵角に基づいて前記自車が直進しているか否かを判定する直進判定部と、前記直進判定部により前記自車が直進していると判定された場合に、前記自車の速度に対して予め求められた車体横滑り角を設定する滑り角設定部と、を更に含み、前記取得部は、更に、舵角センサにより検出された前記自車の操舵角を取得することができる。
【００２５】
また、本発明の自己位置推定装置は、前記自車の速度と、車体横滑り角が０となるときの予め求められた速度との差の絶対値が所定の閾値以下である場合に、前記車体横滑り角を０に設定する滑り角設定部を更に含むことができる。
【００２６】
また、本発明の自己位置推定装置は、前記自車の操舵角と前記自車の速度とを入力として、前記車体横滑り角を推定する車両モデルを用いて前記車体横滑り角を推定し、前記車体横滑り角を推定した値に設定する滑り角設定部を更に含み、前記取得部は、更に、舵角センサにより検出された前記自車の操舵角を取得することができる。
【００２７】
また、本発明の自己位置推定装置の前記姿勢角補正量算出部は、前記自車の操舵角に基づいて前記自車が直進していると判定された場合に、第１の時点での、前記取得部によって前回取得された前記自車の位置と前記第２位置との距離、及び前記取得部が取得した前記自車の位置と前記第２位置との横位置誤差と、第２の時点での、前記取得部によって前回取得された前記自車の位置と前記第２位置との距離、及び前記取得部が取得した前記自車の位置と前記第２位置との横位置誤差と、を算出し、前記算出された前記第１の時点での前記距離及び前記横位置誤差と、前記第２の時点での前記距離及び前記横位置誤差とに基づいて、前記姿勢角補正値として、ヨーレートオフセット誤差及びヨー角オフセット誤差を算出し、前記第１自己位置推定部は、前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置と、前記姿勢角補正量算出部により算出された前記ヨーレートオフセット誤差及びヨー角オフセット誤差と、前記予め求められた車体横滑り角とに基づいて、前記自車の位置である第１位置を推定することができる。
【発明の効果】
【００２８】
以上説明したように、本発明の自己位置推定装置及びプログラムによれば、慣性航法時の自己位置推定精度を向上させることができる、という効果が得られる。
【図面の簡単な説明】
【００２９】
【図１】本発明の第１の実施の形態における自己位置推定装置の概略構成を示すブロック図である。
【図２】本発明の実施の形態におけるセンサ構成の一例を示す図である。
【図３】本発明の実施の形態における車両の速度ベクトルと走行軌跡の例を示す図である。
【図４】本発明の実施の形態における姿勢角誤差計算と自己位置推定の概要を示す図である。
【図５】本発明の実施の形態における横位置誤差及び移動距離との関係を示す図である。
【図６】本発明の第１の実施の形態における姿勢角補正の有無による横位置誤差の比較を示す図である。
【図７】本発明の第１の実施の形態における姿勢角補正誤差推定処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第１の実施の形態における自己位置推定処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第２の実施の形態における自己位置推定装置の概略構成を示すブロック図である。
【図１０】本発明の第２の実施の形態における姿勢角補正誤差推定処理ルーチンの内容を示すフローチャートである。
【図１１】本発明の第２の実施の形態における自己位置推定処理ルーチンの内容を示すフローチャートである。
【図１２】本発明の第３の実施の形態における自己位置推定装置の概略構成を示すブロック図である。
【図１３】本発明の第３の実施の形態における姿勢角補正誤差推定処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第４の実施の形態における自己位置推定装置の概略構成を示すブロック図である。
【図１５】本発明の第４の実施の形態における姿勢角補正誤差推定処理ルーチンの内容を示すフローチャートである。
【図１６】本発明の第５の実施の形態における自己位置推定装置の概略構成を示すブロック図である。
【図１７】本発明の第５の実施の形態におけるＤＲ周期中における２点による自己位置推定の一例を示す図である。
【図１８】本発明の第５の実施の形態におけるヨーレートオフセット誤差及びヨー角オフセット誤差の推定結果の精度を示す図である。
【図１９】本発明の第５の実施の形態におけるヨーレートオフセット誤差及びヨー角オフセット誤差の推定結果を示す図である。
【図２０】本発明の第５の実施の形態における自己位置推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００３０】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００３１】
＜本発明の第１の実施の形態の自己位置推定システムの概要＞
従来の問題点は、反射板の設置が必須の点にあり、また、姿勢角補正量に基づく自己位置推定の具体的な方法が不明である点にあった。
【００３２】
以下、姿勢角の記号は
2020112490.tif 000004
を用いる。姿勢角
2020112490.tif 000005
はＸ−Ｙ平面において車両の進行方向を表す角度であり、速度ベクトルの軸となす角とし、下記式（３）で定義する。
【００３３】
【数２】
2020112490.tif 000006
【００３４】
ここで、
2020112490.tif 000007
はヨー角、βは滑り角である。
【００３５】
本実施形態では、ＧＰＳ、ＰＯＳＬＶ、カメラ等の計測値と高精度地図に基づく自己位置計測値とを用いて車両の姿勢角誤差Δαを算出しておき、慣性航法に切替える時点で誤差補正を行って高精度な自己位置推定を行う。なお、以下では、慣性航法の略語としてＤＲ（Ｄｅａｄ  Ｒｅｃｋｏｎｉｎｇ）を併用する。
【００３６】
ヨーレートセンサの検出信号は、センサオフセット（ドリフトを含む）によりオフセット誤差ΔＲを持つ。また、ヨー角
2020112490.tif 000008
はＧＰＳ、ＰＯＳＬＶ、カメラ等のセンサ取り付け角度誤差により検出値にオフセット誤差
2020112490.tif 000009
を持つ。また、操舵により生じる滑り角βも本装置構成では検出できず、β＝０と仮定するため誤差となる。
【００３７】
本実施形態では、これらを短時間の定常的な姿勢角誤差Δαとみなし、慣性航法に切替える前の姿勢角誤差Δαを求めておき、慣性航法に切替えた後はその誤差値を補正して自己位置推定する。
【００３８】
図３に示すＸ−Ｙ平面上を走行する車両位置は、車両の速度ベクトルＶ、上記式（３）の姿勢角
2020112490.tif 000010
を用いて下記式（４）〜（７）で表される。
【００３９】
【数３】
2020112490.tif 000011
【００４０】
ここで、上記式（４）〜（７）において、Ｘ０及びＹ０は積分開始時点における車両のＸ、Ｙ位置、
2020112490.tif 000012
は、積分開始時点における車両のヨー角であり、積分定数である。また、式（４）〜（７）の各状態量は全て真値とする。また、上記式（４）〜（７）は、右手系とする。
【００４１】
上記式（４）〜（６）に対し、ヨーレートを用いた車両の自己位置Ｘ、Ｙ及び姿勢角
2020112490.tif 000013
の推定は、従来から下記式（８）〜（１１）により行われる。
【００４２】
【数４】
2020112490.tif 000014
【００４３】
上記式（８）〜（１１）中、修飾記号“＾”（サーカムフレックス）は推定値、修飾記号“~”（チルダ）は計測値を表す。また、
2020112490.tif 000015
は、慣性航法切替え直前の自己位置及びヨー角の計測値である。
【００４４】
また、上記式（８）〜（１１）中の
2020112490.tif 000016
は車輪速センサ（図２）の検出値で得るものとし、真値Ｖとほぼ等しいものとする（
2020112490.tif 000017
）。
【００４５】
上記式（１０）の姿勢角推定値
2020112490.tif 000018
は、滑り角βが得られないためβ＝０とし、ヨー角推定値
2020112490.tif 000019
に等しいとしている。
【００４６】
一般に、上記式（１１）におけるヨーレート計測値
2020112490.tif 000020
、ヨー角計測値
2020112490.tif 000021
は下記式（１２）〜（１３）で表現できる。
【００４７】
【数５】
2020112490.tif 000022
【００４８】
すなわち、ヨーレート
2020112490.tif 000023
及びヨー角
2020112490.tif 000024
はヨーレートの真値Ｒ、ヨー角の真値
2020112490.tif 000025
に対してセンサオフセット誤差ΔＲ、ヨー角オフセット誤差
2020112490.tif 000026
が存在する。その結果、上記式（１０）の姿勢角推定値
2020112490.tif 000027
は、姿勢角真値
2020112490.tif 000028
に対して下記式（１４）の姿勢角誤差Δαを生じる。
【００４９】
【数６】
2020112490.tif 000029
【００５０】
自己位置推定精度向上のためには姿勢角誤差Δαの推定と、誤差補正に関する方法の構築が課題となる。図４に提案するＤＲアルゴリズムの概要を示す。
【００５１】
本実施形態で提案するＤＲアルゴリズムは、姿勢角誤差Δαを推定し、姿勢角
2020112490.tif 000030
の補正を行う。当該ＤＲアルゴリズムは、（１）自己位置計測が可能な区間、（２）自己位置計測が不可の区間の２つにより構成される。
【００５２】
＜＜（１）自己位置計測が可能な区間＞＞
姿勢角
2020112490.tif 000031
を用いた自己位置推定値
2020112490.tif 000032
と計測値Ｘ，Ｙとの位置誤差より姿勢角誤差Δαを推定する。
【００５３】
ここで、自己位置推定値
2020112490.tif 000033
の’＊’（上付きアスタリスク）は、誤差Δαを持つ姿勢角
2020112490.tif 000034
を用いて推定した自己位置であることを意味する。
【００５４】
＜＜（２）自己位置計測が不可の区間＞＞
自己位置の計測が不可となる直前の姿勢角誤差Δαを用いて、姿勢角
2020112490.tif 000035
を補正する。
【００５５】
本実施形態では、補正後の姿勢角
2020112490.tif 000036
を
2020112490.tif 000037
として、下記式（１５）で表す。
【００５６】
【数７】
2020112490.tif 000038
【００５７】
そして、補正後の姿勢角
2020112490.tif 000039
を用いて、自己位置
2020112490.tif 000040
を推定する。
【００５８】
＜本発明の第１の実施の形態の自己位置推定システムの構成＞
図１に示すように、本発明の実施の形態に係る自己位置推定システム１０は、車輪速センサ１００と、角速度センサ１１０と、自己位置計測装置としてＧＰＳを使用した場合のＧＰＳセンサ１２０と、自己位置推定装置２００とを備えて構成される。
【００５９】
車輪速センサ１００は、自車の４輪の車輪速Ｕを検出し、検出した自車の車輪速Ｕから自車の速度
2020112490.tif 000041
を検出する。そして、車輪速センサ１００は、算出した自車の速度
2020112490.tif 000042
を、自己位置推定装置２００に渡す。
【００６０】
角速度センサ１１０は、自車のヨーレート
2020112490.tif 000043
を検出する。そして、角速度センサ１１０は、検出したヨーレート
2020112490.tif 000044
を、自己位置推定装置２００に渡す。
【００６１】
ＧＰＳセンサ１２０は、各ＧＰＳ衛星から得られる観測値であるＧＰＳデータを受信し、自車の位置
2020112490.tif 000045
を検出すると共に、速度ベクトルを検出し、速度ベクトルから自車の姿勢角
2020112490.tif 000046
を求める。そして、ＧＰＳセンサ１２０は、検出した自車の位置
2020112490.tif 000047
と、自車の姿勢角
2020112490.tif 000048
とを、自己位置推定装置２００に渡す。
【００６２】
自己位置推定装置２００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００６３】
自己位置推定装置２００は、取得部２１０と、第２自己位置推定部２２０と、位置誤差算出部２３０と、姿勢角補正量算出部２４０と、第１自己位置推定部２５０と、出力部２６０とを備えて構成される。
【００６４】
取得部２１０は、速度センサにより検出された自車の速度と、角速度センサにより検出された自車の角速度と、ＧＰＳセンサにより検出された自車の位置及び姿勢角とを取得する。
【００６５】
具体的には、取得部２１０は、車輪速センサ１００により検出された自車の速度
2020112490.tif 000049
と、角速度センサ１１０により検出された自車のヨーレート
2020112490.tif 000050
と、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000051
及び姿勢角
2020112490.tif 000052
とを取得する。
【００６６】
そして、取得部２１０は、取得した自車の速度
2020112490.tif 000053
と、自車のヨーレート
2020112490.tif 000054
と、自車の位置
2020112490.tif 000055
及び姿勢角
2020112490.tif 000056
とを第２自己位置推定部２２０、位置誤差算出部２３０及び第１自己位置推定部２５０に渡す。
【００６７】
第２自己位置推定部２２０は、取得部２１０が取得した自車の速度と、自車の角速度とに基づいて、自車の位置である第２位置を推定する。
【００６８】
具体的には、第２自己位置推定部２２０は、自己位置計測が可能な区間において、自車の速度
2020112490.tif 000057
と、自車のヨーレート
2020112490.tif 000058
と、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000059
及び姿勢角
2020112490.tif 000060
とに基づいて、姿勢角補正値を用いずに誤差を含む自己位置である第２位置
2020112490.tif 000061
を推定する。
【００６９】
図５は、誤差を含む第２位置
2020112490.tif 000062
のイメージ図である。
【００７０】
図５より、誤差を含む第２位置
2020112490.tif 000063
は誤差補正されないため、車両軌跡から大きくずれ、横位置誤差
2020112490.tif 000064
を生じている。
【００７１】
より具体的には、第２自己位置推定部２２０は、第２位置
2020112490.tif 000065
を下記式（１６）、（１７）により求める。
【００７２】
【数８】
2020112490.tif 000066
【００７３】
ここで、添え字ｉは更新刻み、ΔＴは更新周期である。
【００７４】
そして、第２自己位置推定部２２０は、推定した自己位置推定値
2020112490.tif 000067
を位置誤差算出部２３０に渡す。
【００７５】
位置誤差算出部２３０は、取得部２１０によって前回取得された自車の位置
2020112490.tif 000068
と第２位置
2020112490.tif 000069
との距離
2020112490.tif 000070
、及び取得部２１０が取得した自車の位置
2020112490.tif 000071
と第２位置
2020112490.tif 000072
との横位置誤差
2020112490.tif 000073
を算出する。
【００７６】
具体的には、位置誤差算出部２３０は、移動距離の推定値
2020112490.tif 000074
を、自己位置が計測できなくなる直前のＮ個サンプルのデータを用いて、下記式（２３）により計算する。
【００７７】
【数９】
2020112490.tif 000075
【００７８】
ここで、上記式（２３）の
2020112490.tif 000076
及び
2020112490.tif 000077
のＮは、ｉ＝Ｎサンプル目であることを表す。
【００７９】
次に、位置誤差算出部２３０は、Ｎ時点における姿勢角推定値
2020112490.tif 000078
を用いて車両からみた横位置誤差
2020112490.tif 000079
を下記式（２４）により求める。
【００８０】
【数１０】
2020112490.tif 000080
【００８１】
そして、位置誤差算出部２３０は、算出した距離
2020112490.tif 000081
及び横位置誤差
2020112490.tif 000082
を、姿勢角補正量算出部２４０に渡す。
【００８２】
姿勢角補正量算出部２４０は、取得部２１０によって自車の位置が取得された場合、第２自己位置推定部２２０により推定された第２位置
2020112490.tif 000083
と、取得部２１０が取得した自車の位置
2020112490.tif 000084
との偏差に基づいて、車体横滑り角βを含む姿勢角補正値Δαを算出する。
【００８３】
具体的には、姿勢角補正量算出部２４０は、横位置誤差
2020112490.tif 000085
と移動距離
2020112490.tif 000086
を用いて、下記式（２５）により、姿勢角誤差Δαの推定値である姿勢角誤差
2020112490.tif 000087
を算出する。
【００８４】
【数１１】
2020112490.tif 000088
【００８５】
そして、姿勢角補正量算出部２４０は、算出した姿勢角誤差
2020112490.tif 000089
を姿勢角誤差Δαとして第１自己位置推定部２５０に渡す。
【００８６】
第１自己位置推定部２５０は、取得部２１０によって自車の位置が取得されなかった場合、取得部２１０が取得した自車の速度
2020112490.tif 000090
、自車のヨーレートＲ、及び取得部２１０によって前回取得された自車の位置
2020112490.tif 000091
及び姿勢角
2020112490.tif 000092
と、姿勢角補正量算出部２４０により算出された姿勢角誤差Δαとに基づいて、自車の位置である第１位置
2020112490.tif 000093
を推定する。
【００８７】
具体的には、第１自己位置推定部２５０は、図５の鎖線下部に相当する自己位置推定の計算を行う。
【００８８】
第１自己位置推定部２５０は、姿勢角補正量算出部２４０により直前まで計算していた姿勢角誤差Δαを用いて姿勢角
2020112490.tif 000094
を補正した補正後の姿勢角
2020112490.tif 000095
を用いて、推定した自己位置である第１位置
2020112490.tif 000096
を求める。下記の式（２６）〜式（２９）は第１位置
2020112490.tif 000097
の推定式であり、式（３０）〜（３２）は初期値を設定するための式である。
【００８９】
【数１２】
2020112490.tif 000098
【００９０】
そして、第１自己位置推定部２５０は、推定した第１位置
2020112490.tif 000099
を、出力部２６０に渡す。
【００９１】
出力部２６０は、第１位置
2020112490.tif 000100
を自車の位置として出力する。
【００９２】
図６に、一定周期毎に自己位置計測値を得て姿勢角誤差補正を行い、自己位置推定した結果を示す。縦軸は車両座標系における横位置誤差を示している。
【００９３】
図６より、姿勢角誤差補正を行うことにより横位置誤差が大幅に低減していることが分かる。このため、慣性航法時の自己位置推定精度を向上させることができる。
【００９４】
＜本発明の第１の実施の形態に係る自己位置推定装置２００の動作＞
次に、図７を参照して、第１の実施の形態に係る自己位置推定装置２００の姿勢角補正誤差推定処理ルーチンについて説明する。自己位置推定装置２００は、ＧＰＳセンサ１２０により自己位置が取得できる場合に、ＧＰＳセンサ１２０により自己位置が取得できなくなるまで、姿勢角補正誤差推定処理ルーチンを繰り返す。
【００９５】
まず、ステップＳ１００において、自己位置推定装置２００は、更新刻みｉを０に設定する。
【００９６】
次に、ステップＳ１１０において、取得部２１０は、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000101
を取得する。
【００９７】
ステップＳ１２０において、取得部２１０は、ＧＰＳセンサ１２０により検出された速度ベクトルから自車のヨー角
2020112490.tif 000102
を取得する。
【００９８】
ステップＳ１３０において、取得部２１０は、車輪速センサ１００により検出された自車の速度
2020112490.tif 000103
と、角速度センサ１１０により検出された自車のヨーレート
2020112490.tif 000104
とを取得する。
【００９９】
ステップＳ１４０において、自己位置推定装置２００は、ｉ＝ｉＴであるか否かを判定する。ここで、ｉＴは更新周期であり、少なくとも１以上の値が設定される。ｉＴに複数の値が設定されている場合には、いずれかの値と一致する場合に、ｉ＝ｉＴであると判定する。
【０１００】
ｉ＝ｉＴでない場合（上記ステップＳ１４０のＮＯ）、ステップＳ１５０において、第２自己位置推定部２２０は、上記ステップＳ１３０により取得した自車の速度
2020112490.tif 000105
と、自車のヨーレート
2020112490.tif 000106
と、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000107
及び姿勢角
2020112490.tif 000108
とに基づいて、誤差を含む自己位置である第２位置
2020112490.tif 000109
を推定し、ステップＳ２００に進む。
【０１０１】
一方、ｉ＝ｉＴである場合（上記ステップＳ１４０のＹＥＳ）、ステップＳ１６０において、取得部２１０は、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000110
を取得する。
【０１０２】
ステップＳ１７０において、位置誤差算出部２３０は、上記Ｓ１１０により取得した自車の位置
2020112490.tif 000111
と上記ステップＳ１５０において推定した第２位置
2020112490.tif 000112
との距離
2020112490.tif 000113
を上記式（２３）により計算する。
【０１０３】
ステップＳ１８０において、位置誤差算出部２３０は、上記Ｓ１１０により取得した自車の位置
2020112490.tif 000114
と上記ステップＳ１５０において推定した第２位置
2020112490.tif 000115
との横位置誤差
2020112490.tif 000116
を、上記式（２４）を用いて算出する。
【０１０４】
ステップＳ１９０において、姿勢角補正量算出部２４０は、上記ステップＳ１７０により算出した横位置誤差
2020112490.tif 000117
と、上記ステップＳ１８０により算出した移動距離
2020112490.tif 000118
を用いて、上記式（２５）により、姿勢角誤差Δαの推定値である姿勢角誤差
2020112490.tif 000119
を算出する。
【０１０５】
ステップＳ２００において、自己位置推定装置２００は、ｉ≧ｉＤＲであるか否かを判定する。ここで、ｉＤＲは、ＤＲ周期であり、ｉＤＲ＞ｉＴである。
【０１０６】
ｉ≧ｉＤＲでない場合（上記ステップＳ２００のＮＯ）、ステップＳ２１０において、自己位置推定装置２００は、ｉに１を加算し、ステップＳ１３０に戻る。
【０１０７】
一方、ｉ≧ｉＤＲである場合（上記ステップＳ２００のＮＯ）、処理を終了する。
【０１０８】
次に、図８を参照して、本発明の第１の実施の形態に係る自己位置推定装置２００の自己位置推定処理ルーチンについて説明する。自己位置推定装置２００は、ＧＰＳセンサ１２０により自己位置が取得できなくなった場合に、ＧＰＳセンサ１２０により再び自己位置が取得できるようになるまで、自己位置推定処理ルーチンを繰り返す。
【０１０９】
まず、ステップＳ３００において、自己位置推定装置２００は、ｉに１を設定する。
【０１１０】
次に、ステップＳ３１０において、取得部２１０は、ＧＰＳセンサ１２０により検出された速度ベクトルから前回取得された自車のヨー角
2020112490.tif 000120
を取得する。
【０１１１】
ステップＳ３２０において、取得部２１０は、ＧＰＳセンサ１２０により前回取得した自車の位置
2020112490.tif 000121
を取得する。
【０１１２】
ステップＳ３３０において、取得部２１０は、車輪速センサ１００により検出された自車の速度
2020112490.tif 000122
と、角速度センサ１１０により検出された自車のヨーレート
2020112490.tif 000123
とを取得する。
【０１１３】
ステップＳ３４０において、第１自己位置推定部２５０は、姿勢角補正量算出部２４０により直前まで計算していた姿勢角誤差Δαを用いて姿勢角
2020112490.tif 000124
を補正した補正後の姿勢角
2020112490.tif 000125
を算出する（上記式（２８））。
【０１１４】
ステップＳ３５０において、第１自己位置推定部２５０は、上記ステップＳ３４０により算出した補正後の姿勢角
2020112490.tif 000126
を用いて、推定した自己位置である第１位置
2020112490.tif 000127
を求める（上記式（２６）〜式（２９））。
【０１１５】
ステップＳ３６０において、自己位置推定装置２００は、ｉに１を加算し、ステップＳ３３０に戻る。自己位置推定装置２００は、ＧＰＳセンサ１２０により自己位置が取得できるようになるまで、自己位置推定処理ルーチンを繰り返す。
【０１１６】
以上説明したように、本発明の実施の形態に係る自己位置推定装置によれば、当該自車の位置が取得された場合に、姿勢角補正値を用いずに推定された自車の位置である第２位置と、取得された当該自車の位置との偏差に基づいて、車体横滑り角を含む姿勢角補正値を算出し、当該自車の位置が取得されなかった場合、姿勢角補正値を用いて、当該自車の位置である第１位置を推定することにより、慣性航法時の自己位置推定精度を向上させることができる。
【０１１７】
＜本発明の第２〜４の実施の形態に係る自己位置推定システムの概要＞
第１の実施の形態に係る自己位置推定システムでは、滑り角βが未知である場合について、滑り角βを含む姿勢角誤差Δαを算出する場合について説明した。
【０１１８】
本発明の第２〜第４の実施の形態では、滑り角βを既知として、滑り角βと姿勢角誤差Δαとを用いて慣性航法時の自己位置の推定を行う場合を示す。
【０１１９】
具体的には、第２の実施の形態では、滑り角βがほぼ一定（β＝０である場合を含む）となる直進時に滑り角βを設定し、第３の実施の形態では、β＝０となる特定の車速での走行時に滑り角βを設定し、第４の実施の形態では、車両モデルに基づき時間的に変化する滑り角βを推定し、滑り角βを含まない姿勢角誤差Δαを算出する。第４の実施の形態では、滑り角の時間的変化が小さいことを前提とした従来法では適用できなかった走行条件である。
【０１２０】
＜本発明の第２の実施の形態の自己位置推定システムの構成＞
本発明の第２の実施の形態に係る自己位置推定システム２０の構成について説明する。なお、第１の実施の形態に係る自己位置推定システム１０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１２１】
図９に示すように、本発明の実施の形態に係る自己位置推定システム２０は、車輪速センサ１００と、角速度センサ１１０と、自己位置計測装置としてＧＰＳを使用した場合のＧＰＳセンサ１２０と、舵角センサ１３０と、自己位置推定装置３００とを備えて構成される。
【０１２２】
舵角センサ１３０は、自車の操舵角を検出し、検出した自車の操舵角を、自己位置推定装置３００に渡す。
【０１２３】
自己位置推定装置３００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０１２４】
自己位置推定装置３００は、取得部３１０と、直進判定部３１２と、滑り角設定部３１４と、第２自己位置推定部３２０と、位置誤差算出部２３０と、姿勢角補正量算出部２４０と、第１自己位置推定部３５０と、出力部２６０とを備えて構成される。
【０１２５】
取得部３１０は、速度センサにより検出された自車の速度と、角速度センサにより検出された自車の角速度と、ＧＰＳセンサにより検出された自車の位置及び姿勢角と、舵角センサにより検出された自車の操舵角とを取得する。
【０１２６】
具体的には、取得部３１０は、車輪速センサ１００により検出された自車の速度
2020112490.tif 000128
と、角速度センサ１１０により検出された自車のヨーレート
2020112490.tif 000129
と、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000130
及び姿勢角
2020112490.tif 000131
と、舵角センサ１３０により検出された自車の操舵角δｆとを取得する。
【０１２７】
そして、取得部３１０は、取得した自車の速度
2020112490.tif 000132
と、自車のヨーレート
2020112490.tif 000133
と、自車の位置
2020112490.tif 000134
及び姿勢角
2020112490.tif 000135
と、自車の操舵角δｆを第２自己位置推定部３２０、位置誤差算出部２３０及び第１自己位置推定部３５０に渡す。
【０１２８】
直進判定部３１２は、自車の操舵角δｆに基づいて自車が直進しているか否かを判定する。
【０１２９】
具体的には、直進判定部３１２は、自車の操舵角δｆの絶対値が予め定めた第１閾値以下である場合に、自車が直進していると判定し、自車の操舵角δｆの絶対値が予め定めた第１閾値よりも大きい場合、自車が直進していないと判定する。
【０１３０】
そして、直進判定部３１２は、自車が直進していると判定した場合、判定結果を滑り角設定部３１４に渡す。
【０１３１】
滑り角設定部３１４は、直進判定部３１２により自車が直進していると判定された場合に、自車の車速に対して予め求められた車体横滑り角β０を設定する。
【０１３２】
具体的には、滑り角設定部３１４は、自車の操舵角δｆ、自車の速度
2020112490.tif 000136
を入力として、予め定めたテーブルを参照することにより、車体横滑り角β０（計測値と同じ扱いとするため、以下
2020112490.tif 000137
と表記する）を求める。
【０１３３】
そして、滑り角設定部３１４は、車体横滑り角βを
2020112490.tif 000138
として設定し、車体横滑り角βを、第２自己位置推定部３２０及び第１自己位置推定部３５０に渡す。
【０１３４】
第２自己位置推定部３２０は、取得部３１０が取得した自車の速度と、自車の角速度と、予め求められた車体横滑り角とに基づいて、自車の位置である第２位置を推定する。
【０１３５】
具体的には、第２自己位置推定部３２０は、自己位置計測が可能な区間において、自車の速度
2020112490.tif 000139
と、自車のヨーレート
2020112490.tif 000140
と、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000141
及び姿勢角
2020112490.tif 000142
とに基づいて、姿勢角補正値を用いずに、誤差を含む自己位置である第２位置
2020112490.tif 000143
を、下記式（３３）〜（３６）を用いて推定する。
【０１３６】
【数１３】
2020112490.tif 000144
【０１３７】
ここで、添え字ｉは更新刻み、ΔＴは更新周期である。
【０１３８】
そして、第２自己位置推定部３２０は、推定した自己位置推定値
2020112490.tif 000145
を位置誤差算出部２３０に渡す。
【０１３９】
第１自己位置推定部３５０は、取得部３１０によって自車の位置が取得されなかった場合、取得部３１０が取得した自車の速度
2020112490.tif 000146
、自車のヨーレートＲ、及び取得部３１０によって前回取得された自車の位置
2020112490.tif 000147
及び姿勢角
2020112490.tif 000148
と、姿勢角補正量算出部２４０により算出された姿勢角補正値Δαと、車体横滑り角βとに基づいて自車の位置である第１位置
2020112490.tif 000149
を推定する。
【０１４０】
具体的には、第１自己位置推定部３５０は、姿勢角補正量算出部２４０により直前まで計算していた姿勢角誤差Δαと、車体横滑り角βとを用いて姿勢角
2020112490.tif 000150
を補正した補正後の姿勢角
2020112490.tif 000151
（下記式（３９））を用いて、推定した自己位置である第１位置
2020112490.tif 000152
を求める（下記式（３７）、（３８））。
【０１４１】
【数１４】
2020112490.tif 000153
【０１４２】
そして、第１自己位置推定部３５０は、推定した第１位置
2020112490.tif 000154
を、出力部２６０に渡す。
【０１４３】
＜本発明の第２の実施の形態に係る自己位置推定装置の作用＞
図１０は、本発明の第２の実施の形態に係る自己位置推定装置３００の姿勢角補正誤差推定処理ルーチンについて説明する。自己位置推定装置３００は、ＧＰＳセンサ１２０により自己位置が取得できる場合に、ＧＰＳセンサ１２０により自己位置が取得できなくなるまで、姿勢角補正誤差推定処理ルーチンを繰り返す。なお、第１の実施の形態に係る姿勢角補正誤差推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１４４】
ステップＳ４３２において、取得部３１０は、舵角センサ１３０により検出された自車の操舵角δｆを取得する。
【０１４５】
ステップＳ４３４において、自車の操舵角δｆに基づいて自車が直進しているか否かを判定する。
【０１４６】
自車の操舵角δｆが第１閾値以下でない場合（上記ステップＳ４３４のＮＯ）、処理を終了する。
【０１４７】
一方、自車の操舵角δｆが第１閾値以下である場合（上記ステップＳ４３４のＹＥＳ）、ステップＳ４３６において、滑り角設定部３１４は、自車の操舵角δｆ、自車の速度
2020112490.tif 000155
を入力として、予め定めたテーブルを参照することにより、車体横滑り角β０を求め、車体横滑り角βを
2020112490.tif 000156
として設定する。
【０１４８】
ｉ＝ｉＴでない場合（上記ステップＳ１４０のＮＯ）、ステップＳ４５０において、第２自己位置推定部３２０は、自己位置計測が可能な区間において、自車の速度
2020112490.tif 000157
と、自車のヨーレート
2020112490.tif 000158
と、車体横滑り角βに基づいて、誤差を含む自己位置である第２位置
2020112490.tif 000159
を、上記式（３３）〜（３６）を用いて推定する。
【０１４９】
次に、図１１を参照して、本発明の第２の実施の形態に係る自己位置推定装置３００の自己位置推定処理ルーチンについて説明する。自己位置推定装置３００は、ＧＰＳセンサ１２０により自己位置が取得できなくなった場合に、ＧＰＳセンサ１２０により自己位置が取得できるようになるまで、自己位置推定処理ルーチンを繰り返す。なお、第１の実施の形態に係る自己位置推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１５０】
ステップＳ５１５において、第１自己位置推定部３５０は、滑り角設定部３１４により設定された車体横滑り角βを取得する。
【０１５１】
ステップＳ５４０において、第１自己位置推定部３５０は、姿勢角補正量算出部２４０により直前まで計算していた姿勢角誤差Δαと、車体横滑り角βとを用いて姿勢角
2020112490.tif 000160
を補正した補正後の姿勢角
2020112490.tif 000161
を算出する（上記式（３９））。
【０１５２】
ステップＳ５５０において、第１自己位置推定部３５０は、上記ステップＳ５４０により算出した補正後の姿勢角
2020112490.tif 000162
（上記式（３９））を用いて、推定した自己位置である第１位置
2020112490.tif 000163
を求める（上記式（３７）、（３８））。
【０１５３】
以上説明したように、本発明の実施の形態に係る自己位置推定装置によれば、姿勢角補正値を用いずに、当該自車の速度と、当該自車の角速度と、自車が直進している場合に自車の速度に対して予め求められた車体横滑り角とに基づいて、当該自車の位置である第２位置を推定し、当該自車の位置が取得された場合に、推定された当該第２位置と、取得された当該自車の位置との偏差とに基づいて、姿勢角補正値を算出し、当該自車の位置が取得されなかった場合、当該姿勢角補正値を用いて当該自車の位置である第１位置を推定することにより、慣性航法時の自己位置推定精度を向上させることができる。
【０１５４】
＜本発明の第３の実施の形態に係る自己位置推定システムの構成＞
本発明の第３の実施の形態に係る自己位置推定システム３０の構成について説明する。なお、第１の実施の形態に係る自己位置推定システム１０及び第２の実施の形態に係る自己位置推定システム２０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１５５】
図１２に示すように、本発明の実施の形態に係る自己位置推定システム３０は、車輪速センサ１００と、角速度センサ１１０と、自己位置計測装置としてＧＰＳを使用した場合のＧＰＳセンサ１２０と、自己位置推定装置４００とを備えて構成される。
【０１５６】
自己位置推定装置４００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０１５７】
自己位置推定装置４００は、取得部４１０と、車速記憶部４１１と、車速比較部４１２と、滑り角設定部４１４と、第２自己位置推定部３２０と、位置誤差算出部２３０と、姿勢角補正量算出部２４０と、第１自己位置推定部３５０と、出力部２６０とを備えて構成される。
【０１５８】
取得部４１０は、速度センサにより検出された自車の速度と、角速度センサにより検出された自車の角速度と、ＧＰＳセンサにより検出された自車の位置及び姿勢角とを取得する。
【０１５９】
具体的には、取得部４１０は、車輪速センサ１００により検出された自車の速度
2020112490.tif 000164
と、角速度センサ１１０により検出された自車のヨーレート
2020112490.tif 000165
と、ＧＰＳセンサ１２０により検出された自車の位置
2020112490.tif 000166
及び姿勢角
2020112490.tif 000167
とを取得する。
【０１６０】
そして、取得部４１０は、取得した自車の速度
2020112490.tif 000168
と、自車のヨーレート
2020112490.tif 000169
と、自車の位置
2020112490.tif 000170
及び姿勢角
2020112490.tif 000171
とを第２自己位置推定部３２０、位置誤差算出部２３０及び第１自己位置推定部３５０に渡す。また、取得部４１０は、取得した自車の速度
2020112490.tif 000172
を車速比較部４１２に渡す。
【０１６１】
車速記憶部４１１には、車体横滑り角βが０となる車速Ｖβ０が格納されている。
【０１６２】
具体的には、操舵角δｆが一定値（＝δ０）の場合、車速Ｖにおける滑り角βの定常円旋回モデルは下記式（４１）で表される（参考文献１）。
［参考文献１］安部正人著、“車両の運動と制御（１９７９）”、山海堂
【０１６３】
【数１５】
2020112490.tif 000173
【０１６４】
ここで、ｍは車両質量［ｋｇ］であり、ｌはホイルベース［ｍ］であり、ｌｆ及びｌｒはそれぞれ車両重心に対する前輪、後輪の軸距離［ｍ］であり、Ｋｆ及びＫｒはそれぞれ前輪、後輪のコーナリングパワー［Ｎ／ｒａｄ］である。
【０１６５】
上記式（４１）より、分子が０となる車速（Ｖ＝Ｖβ０）では、自車の操舵角δｆの大きさに関わらず車体横滑り角βが０となり、車速Ｖβ０は次式となる。
【０１６６】
【数１６】
2020112490.tif 000174
【０１６７】
車速比較部４１２は、自車の速度と、車体横滑り角が０となるときの予め求められた速度との差の絶対値が所定の閾値以下であるか否かを判定する。
【０１６８】
具体的には、車速比較部４１２は、取得部４１０から取得した自車の速度
2020112490.tif 000175
と、車速記憶部４１１に格納されている車速Ｖβ０との差の絶対値を求め、当該絶対値が予め定めた第２閾値以下である場合に、Ｖ＝Ｖβ０と判定し、当該絶対値が予め定めた第２閾値以下でない場合、Ｖ≠Ｖβ０と判定する。
【０１６９】
そして、車速比較部４１２は、判定結果を、滑り角設定部４１４に渡す。
【０１７０】
滑り角設定部４１４は、自車の速度と、車体横滑り角が０となるときの予め求められた速度との差の絶対値が所定の閾値以下である場合に、車体横滑り角を０に設定する。
【０１７１】
具体的には、滑り角設定部４１４は、車速比較部４１２による判定結果が、Ｖ＝Ｖβ０である場合に、車体横滑り角βを０に設定する。
【０１７２】
そして、滑り角設定部４１４は、車体横滑り角βを、第２自己位置推定部３２０及び第１自己位置推定部３５０に渡す。
【０１７３】
＜本発明の第３の実施の形態に係る自己位置推定装置の作用＞
図１３は、本発明の第３の実施の形態に係る自己位置推定装置４００の姿勢角補正誤差推定処理ルーチンを示すフローチャートである。自己位置推定装置４００は、ＧＰＳセンサ１２０により自己位置が取得できる場合に、ＧＰＳセンサ１２０により自己位置が取得できなくなるまで、姿勢角補正誤差推定処理ルーチンを繰り返す。
【０１７４】
なお、第１の実施形態に係る姿勢角補正誤差推定処理ルーチン及び第２の実施の形態に係る姿勢角補正誤差推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１７５】
ステップＳ６３２において、車速比較部４１２は、車速記憶部４１１から予め求められた車体横滑り角βが０となる車速Ｖβ０を取得する。
【０１７６】
ステップＳ６３４において、車速比較部４１２は、上記ステップＳ１３０により取得した自車の速度
2020112490.tif 000176
と、上記ステップＳ６３２により取得した車速Ｖβ０との差の絶対値が所定の閾値以下であるか否かを判定する。
【０１７７】
所定の閾値以下でない場合（上記ステップＳ６３４のＮＯ）、処理を終了する。
【０１７８】
一方、所定の閾値以下である場合（上記ステップＳ６３４のＹＥＳ）、ステップＳ６３６において、滑り角設定部４１４は、車体横滑り角βを０に設定する。
【０１７９】
次に、本実施形態に係る自己位置推定装置４００の自己位置推定処理ルーチンについて説明する。自己位置推定装置４００は、ＧＰＳセンサ１２０により自己位置が取得できなくなった場合に、ＧＰＳセンサ１２０により再び自己位置が取得できるようになるまで、自己位置推定処理ルーチンを繰り返す。なお、具体的な処理は第２の実施の形態に係る自己位置推定処理ルーチンと同様であるため、詳細な説明は省略する。
【０１８０】
なお、本実施形態では、ステップＳ５１５において、第１自己位置推定部３５０は、滑り角設定部４１４により設定された車体横滑り角βを取得する。
【０１８１】
以上説明したように、本発明の実施の形態に係る自己位置推定装置によれば、当該自車の速度と、当該自車の角速度と、当該自車の速度と車体横滑り角βが０となる速度との差の絶対値が所定の閾値以下である場合に予め求められた車体横滑り角とに基づいて、当該自車の位置である第２位置を推定し、当該自車の位置が取得された場合に、推定された当該第２位置と、取得された当該自車の位置との偏差とに基づいて、姿勢角補正値を算出し、当該自車の位置が取得されなかった場合、当該姿勢角補正値を用いて当該自車の位置である第１位置を推定することにより、慣性航法時の自己位置推定精度を向上させることができる。
【０１８２】
＜本発明の第４の実施の形態に係る自己位置推定システムの構成＞
本発明の第４の実施の形態に係る自己位置推定システム４０の構成について説明する。なお、第１の実施の形態に係る自己位置推定システム１０及び第２の実施の形態に係る自己位置推定システム２０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１８３】
図１４に示すように、本発明の実施の形態に係る自己位置推定システム４０は、車輪速センサ１００と、角速度センサ１１０と、自己位置計測装置としてＧＰＳを使用した場合のＧＰＳセンサ１２０と、舵角センサ１３０と、自己位置推定装置５００とを備えて構成される。
【０１８４】
自己位置推定装置５００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０１８５】
自己位置推定装置５００は、取得部５１０と、滑り角設定部５１４と、第２自己位置推定部３２０と、位置誤差算出部２３０と、姿勢角補正量算出部２４０と、第１自己位置推定部３５０と、出力部２６０とを備えて構成される。
【０１８６】
取得部５１０は、取得部３１０と同様に、速度センサにより検出された自車の速度と、角速度センサにより検出された自車の角速度と、ＧＰＳセンサにより検出された自車の位置及び姿勢角と、舵角センサにより検出された自車の操舵角とを取得する。
【０１８７】
そして、取得部５１０は、取得部３１０は、取得した自車の速度
2020112490.tif 000177
と、自車のヨーレート
2020112490.tif 000178
と、自車の位置
2020112490.tif 000179
及び姿勢角
2020112490.tif 000180
と、自車の操舵角δｆを位置誤差算出部２３０、第２自己位置推定部３２０、第１自己位置推定部３５０及び滑り角設定部５１４に渡す。
【０１８８】
滑り角設定部５１４は、自車の操舵角と自車の速度とを入力として、車体横滑り角を推定する車両モデルを用いて車体横滑り角を推定し、車体横滑り角を推定した値に設定する。
【０１８９】
具体的には、上記式（４１）において、操舵角δｆ（＝δ０）を入力とし自車の速度Ｖを参照することで常時車体横滑り角βを算出することができる。このため、滑り角設定部５１４は、操舵角δｆと自車の速度Ｖとを上記式（４１）で表される車両モデルに入力することにより、車体横滑り角βを算出する。
【０１９０】
そして、滑り角設定部５１４は、車体横滑り角βを、第２自己位置推定部３２０及び第１自己位置推定部３５０に渡す。
【０１９１】
＜本発明の第４の実施の形態に係る自己位置推定装置の作用＞
図１５は、本発明の第４の実施の形態に係る自己位置推定装置５００の姿勢角補正誤差推定処理ルーチンを示すフローチャートである。自己位置推定装置５００は、ＧＰＳセンサ１２０により自己位置が取得できる場合に、ＧＰＳセンサ１２０により自己位置が取得できなくなるまで、姿勢角補正誤差推定処理ルーチンを繰り返す。
【０１９２】
なお、第１の実施形態に係る姿勢角補正誤差推定処理ルーチン及び第２の実施の形態に係る姿勢角補正誤差推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１９３】
ステップＳ７３５において、滑り角設定部５１４は、自車の操舵角と自車の速度とを入力として、車体横滑り角を推定する車両モデルを用いて車体横滑り角を推定し、車体横滑り角を推定した値に設定する。
【０１９４】
次に、本実施形態に係る自己位置推定装置５００の自己位置推定処理ルーチンについて説明する。自己位置推定装置５００は、ＧＰＳセンサ１２０により自己位置が取得できなくなった場合に、ＧＰＳセンサ１２０により再び自己位置が取得できるようになるまで、自己位置推定処理ルーチンを繰り返す。なお、具体的な処理は第２の実施の形態に係る自己位置推定処理ルーチンと同様であるため、詳細な説明は省略する。
【０１９５】
以上説明したように、本発明の実施の形態に係る自己位置推定装置によれば、当該自車の速度と、当該自車の角速度と、車体横滑り角を推定する車両モデルを用いて推定することにより予め求められた車体横滑り角とに基づいて、当該自車の位置である第２位置を推定し、当該自車の位置が取得された場合に、推定された当該第２位置と、取得された当該自車の位置との偏差とに基づいて、姿勢角補正値を算出し、当該自車の位置が取得されなかった場合、当該姿勢角補正値を用いて当該自車の位置である第１位置を推定することにより、慣性航法時の自己位置推定精度を向上させることができる。
【０１９６】
＜本発明の第５の実施の形態に係る自己位置推定システムの概要＞
第２〜第４の実施形態では、車体横滑り角βを既知とすることで、姿勢角誤差Δαから車体横滑り角βを除外することができた。
【０１９７】
本発明の第５の実施形態では、更に、姿勢角誤差Δαとして、ヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000181
を分離して検出する。
【０１９８】
走行条件として姿勢角
2020112490.tif 000182
が小さい走行状態、例えばヨー角計測値
2020112490.tif 000183
が小さい直進付近の走行状態とする。上記式（４）、（５）の微分値と上記式（１０）の姿勢角推定値
2020112490.tif 000184
に既知の車体横滑り角βを加えた値（＝
2020112490.tif 000185
＋β）を上記式（８）、（９）に代入し（下記式（４７））、それを微分すると自車の位置の真値
2020112490.tif 000186
及びこれらの推定値
2020112490.tif 000187
は下記式（４３）〜（４６）となる。
【０１９９】
【数１７】
2020112490.tif 000188
【０２００】
ここで、上記式（４５）、（４６）の推定値
2020112490.tif 000189
は、姿勢角誤差Δαの補正無しのため、右肩に’＊’（上付きアスタリスク）を付している。
【０２０１】
上記式（４３）、（４４）の姿勢角の真値をＡ（＝
2020112490.tif 000190
）、上記式（４７）のヨー角推定値
2020112490.tif 000191
における誤差を、上記式（１２）、（１３）を参照し、Ｂとして、それぞれ下記式（４８）、（４９）で表す。
【０２０２】
【数１８】
2020112490.tif 000192
【０２０３】
そして、上記Ａ及びＢを用いて、上記式（３５）〜（３８）を下記式（５０）〜（５３）に書き換える。
【０２０４】
【数１９】
2020112490.tif 000193
【０２０５】
ここで、
2020112490.tif 000194
は、既知の車体横滑り角である。
【０２０６】
Ｘ方向の速度の誤差
2020112490.tif 000195
と、Ｙ方向の速度の誤差
2020112490.tif 000196
は、それぞれ下記式（５４）、（５５）のようになる。
【０２０７】
【数２０】
2020112490.tif 000197
【０２０８】
ここで、Ｖ≒
2020112490.tif 000198
とする。
【０２０９】
短時間であればＢは１よりも十分に小さい値であり（Ｂ＜＜１）、ｃｏｓ（Ｂ）≒１、ｓｉｎ（Ｂ）≒Ｂが成り立つため、上記式（５４）、（５５）は下記式（５６）、（５７）で表すことができる。なお、上記式（５４）、（５５）中のＡを、
2020112490.tif 000199
として表す。
【０２１０】
【数２１】
2020112490.tif 000200
【０２１１】
さらに、姿勢角
2020112490.tif 000201
が１よりも十分に小さい（
2020112490.tif 000202
＜＜１）という条件が成り立つ走行条件であれば、上記式（５６）、（５７）は下記式（５８）、（５９）のように簡略化することができる。
【０２１２】
【数２２】
2020112490.tif 000203
【０２１３】
車両から見た横位置誤差の微分値
2020112490.tif 000204
は、上記式（２４）を参照し、上記式（５９）を用いて下記式（６０）となる。
【０２１４】
【数２３】
2020112490.tif 000205
【０２１５】
そして、上記式（６０）を積分すると、横位置誤差車両から見た時刻Ｔにおける横位置誤差は、下記式（６１）となる。
【０２１６】
【数２４】
2020112490.tif 000206
【０２１７】
図１７は、ＤＲ周期内で自己位置推定値
2020112490.tif 000207
をｉ１時点及びｉ２時点の２点で得た状態を示している。
【０２１８】
上記式（５１）で計算されるｉ１時点の横位置誤差
2020112490.tif 000208
、ｉ２時点の横位置誤差
2020112490.tif 000209
、及び上記式（１８）と同様に算出できるｉ１時点の移動距離の推定値
2020112490.tif 000210
、ｉ２時点の移動距離の推定値
2020112490.tif 000211
を用いると、下記式（６２）が成り立つ。
【０２１９】
【数２５】
2020112490.tif 000212
【０２２０】
ここで、上記式（６２）中のｔ１、ｔ２はＤＲｎ回目におけるＤＲ開始（図１７の自己位置初期値の時点）からｉ１、ｉ２点に至る時間である。
【０２２１】
ｉ１時点、ｉ２時点の２点の自己位置推定値を得ることで上記（５１）式が成り立ち、逆行列をとることで未知のヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000213
を求めることができる（下記式（６３））。
【０２２２】
【数２６】
2020112490.tif 000214
【０２２３】
すなわち、上記式（６３）により、ヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000215
を個別に推定することができる。
【０２２４】
＜本発明の第５の実施の形態に係る自己位置推定システムの構成＞
本発明の第５の実施の形態に係る自己位置推定システム５０の構成について説明する。なお、第１の実施の形態に係る自己位置推定システム１０及び第２の実施の形態に係る自己位置推定システム２０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０２２５】
図１６に示すように、本発明の実施の形態に係る自己位置推定システム５０は、車輪速センサ１００と、角速度センサ１１０と、自己位置計測装置としてＧＰＳを使用した場合のＧＰＳセンサ１２０と、舵角センサ１３０と、自己位置推定装置６００とを備えて構成される。
【０２２６】
自己位置推定装置６００は、ＣＰＵと、ＲＡＭと、後述する姿勢角補正誤差推定処理ルーチン及び自己位置推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【０２２７】
自己位置推定装置６００は、取得部３１０と、直進判定部６１２と、滑り角設定部３１４と、第２自己位置推定部３２０と、位置誤差算出部２３０と、位置誤差記憶部６３５と、姿勢角補正量算出部２４０と、第１自己位置推定部６５０と、出力部２６０とを備えて構成される。
【０２２８】
直進判定部６１２は、直進判定部３１２と同様に、自車の操舵角δｆに基づいて自車が直進しているか否かを判定する。
【０２２９】
そして、直進判定部６１２は、自車が直進していると判定した場合、判定結果を滑り角設定部３１４及び姿勢角補正量算出部６４０に渡す。
【０２３０】
位置誤差算出部６３０は、位置誤差算出部２３０と同様に、取得部３１０によって前回取得された自車の位置
2020112490.tif 000216
と第２位置
2020112490.tif 000217
との距離
2020112490.tif 000218
、及び取得部２１０が取得した自車の位置
2020112490.tif 000219
と第２位置
2020112490.tif 000220
との横位置誤差
2020112490.tif 000221
を算出する。
【０２３１】
そして、位置誤差算出部６３０は、算出した距離
2020112490.tif 000222
及び横位置誤差
2020112490.tif 000223
を、算出した時点と紐づけて、位置誤差記憶部６３５に格納する。
【０２３２】
位置誤差記憶部６３５には、位置誤差算出部６３０により算出された各時刻における距離
2020112490.tif 000224
及び横位置誤差
2020112490.tif 000225
が格納されている。
【０２３３】
姿勢角補正量算出部６４０は、自車の操舵角δｆに基づいて自車が直進していると判定された場合に、ｉ１時点での位置誤差算出部６３０により算出された距離
2020112490.tif 000226
及び横位置誤差
2020112490.tif 000227
と、ｉ２時点での位置誤差算出部６３０により算出された距離
2020112490.tif 000228
、及び横位置誤差
2020112490.tif 000229
とに基づいて、姿勢角補正値Δαとして、ヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000230
を算出する。
【０２３４】
具体的には、姿勢角補正量算出部６４０は、まず、位置誤差記憶部６３５から、ｉ１時点での距離
2020112490.tif 000231
及び横位置誤差
2020112490.tif 000232
と、ｉ２時点での距離
2020112490.tif 000233
、及び横位置誤差
2020112490.tif 000234
とを取得する。
【０２３５】
次に、姿勢角補正量算出部６４０は、ｉ１時点での距離
2020112490.tif 000235
及び横位置誤差
2020112490.tif 000236
と、ｉ２時点での距離
2020112490.tif 000237
、及び横位置誤差
2020112490.tif 000238
とを用いて、上記式（６３）により、姿勢角補正値Δαとして、ヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000239
を算出する。
【０２３６】
そして、姿勢角補正量算出部６４０は、算出したヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000240
を第１自己位置推定部６５０に渡す。
【０２３７】
第１自己位置推定部６５０は、取得部５１０によって自車の位置が取得されなかった場合、取得部５１０が取得した自車の速度
2020112490.tif 000241
、自車のヨーレートＲ、及び取得部３１０によって前回取得された自車の位置
2020112490.tif 000242
及び姿勢角
2020112490.tif 000243
と、姿勢角補正量算出部６４０により算出されたヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000244
と、予め求められた車体横滑り角βとに基づいて、自車の位置である第１位置
2020112490.tif 000245
を推定する。
【０２３８】
具体的には、第１自己位置推定部６５０は、姿勢角補正量算出部６４０により直前まで計算していた姿勢角誤差Δαとしてヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000246
と、車体横滑り角βとを用いて、推定した自己位置の微分
2020112490.tif 000247
を算出し（上記式（５２）、（５３））、自己位置の微分
2020112490.tif 000248
を積分して、推定した自己位置である第１位置
2020112490.tif 000249
を求める（上記式（４）、（５））。
【０２３９】
既知のヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000250
を与えてヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000251
を推定した結果を図１８及び図１９に示す。
【０２４０】
図１８及び図１９より精度よくヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000252
の推定ができていることが分かる。
【０２４１】
＜本発明の第５の実施の形態に係る自己位置推定装置の作用＞
図２０は、本発明の第５の実施の形態に係る自己位置推定装置６００の姿勢角補正誤差推定処理ルーチンについて説明する。自己位置推定装置６００は、ＧＰＳセンサ１２０により自己位置が取得できる場合に、ＧＰＳセンサ１２０により自己位置が取得できなくなるまで、姿勢角補正誤差推定処理ルーチンを繰り返す。なお、第１の実施の形態に係る姿勢角補正誤差推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０２４２】
ステップＳ７４０において、自己位置推定装置６００は、ｉ＝ｉ１であるか否かを判定する。
【０２４３】
ｉ＝ｉ１である場合（上記ステップＳ７４０のＹＥＳ）、ステップＳ７７０において、位置誤差算出部６３０は、上記Ｓ１１０により取得した自車の位置
2020112490.tif 000253
と上記ステップＳ４５０において推定した第２位置
2020112490.tif 000254
とのｉ１時点での距離
2020112490.tif 000255
を上記式（２３）により計算する。
【０２４４】
ステップＳ７８０において、位置誤差算出部６３０は、上記Ｓ１１０により取得した自車の位置
2020112490.tif 000256
と上記ステップＳ４５０において推定した第２位置
2020112490.tif 000257
とのｉ１時点での横位置誤差
2020112490.tif 000258
を、上記式（２４）を用いて算出する。
【０２４５】
一方、ｉ＝ｉ１でない場合（上記ステップＳ７４０のＮＯ）、ステップＳ７５０において、自己位置推定装置６００は、ｉ＝ｉ２であるか否かを判定する。
【０２４６】
ｉ＝ｉ２である場合（上記ステップＳ７５０のＹＥＳ）、ステップＳ７９０において、位置誤差算出部６３０は、上記Ｓ１１０により取得した自車の位置
2020112490.tif 000259
と上記ステップＳ４５０において推定した第２位置
2020112490.tif 000260
とのｉ２時点での距離
2020112490.tif 000261
を上記式（２３）により計算する。
【０２４７】
ステップＳ８００において、位置誤差算出部６３０は、上記Ｓ１１０により取得した自車の位置
2020112490.tif 000262
と上記ステップＳ４５０において推定した第２位置
2020112490.tif 000263
とのｉ２時点での横位置誤差
2020112490.tif 000264
を、上記式（２４）を用いて算出する。
【０２４８】
ステップＳ８１０において、姿勢角補正量算出部６４０は、上記ステップＳ７７０により算出されたｉ１時点での距離
2020112490.tif 000265
及び上記ステップＳ７８０により算出されたｉ１時点での横位置誤差
2020112490.tif 000266
と、上記ステップＳ７９０により算出されたｉ２時点での距離
2020112490.tif 000267
、及び上記ステップＳ８００により算出されたｉ２時点での横位置誤差
2020112490.tif 000268
とに基づいて、姿勢角補正値Δαとして、ヨーレートオフセット誤差ΔＲ及びヨー角オフセット誤差
2020112490.tif 000269
を、上記式（６３）により算出する。
【０２４９】
次に、図１１を参照して、本実施形態の自己位置推定装置６００の自己位置推定処理ルーチンについて説明する。自己位置推定装置６００は、ＧＰＳセンサ１２０により自己位置が取得できなくなった場合に、ＧＰＳセンサ１２０により再び自己位置が取得できるようになるまで、自己位置推定処理ルーチンを繰り返す。図１１は、本発明の第２の実施の形態に係る自己位置推定処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る自己位置推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０２５０】
以上説明したように、本発明の実施の形態に係る自己位置推定装置によれば、自車の操舵角に基づいて自車が直進していると判定された場合に、第１の時点での、前回取得された自車の位置と第２位置との距離、及び自車の位置と前記第２位置との横位置誤差と、第２の時点での、前回取得された自車の位置と第２位置との距離、及び自車の位置と前記第２位置との横位置誤差に基づいて、姿勢角補正値として、ヨーレートオフセット誤差及びヨー角オフセット誤差を算出し、自車の位置が取得されなかった場合、取得した自車の速度、自車の角速度、及び取得部によって取得された自車の位置と、算出されたヨーレートオフセット誤差及びヨー角オフセット誤差と、予め求められた車体横滑り角とに基づいて、自車の位置である第１位置を推定するため、慣性航法時の自己位置推定精度をより向上させることができる。
【０２５１】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０２５２】
上述の実施形態では、姿勢角誤差Δαを上記式（２５）により算出したがこれに限定されるものではない。
【０２５３】
例えば、下記式（６４）により姿勢角誤差Δαを算出することもできる。
【０２５４】
【数２７】
2020112490.tif 000270
【０２５５】
この場合、距離
2020112490.tif 000271
及び横位置誤差
2020112490.tif 000272
の算出が不要となるため、計算速度を速めることができる。
【０２５６】
また、上記式（２０）及び（２１）から、初期値である
2020112490.tif 000273
については上記式（６４）の値が０となる。このため、初期値における計算を省略することができる。
【０２５７】
また、第１の実施形態では、車体横滑り角βが不明である場合、第２及び第３の実施形態では、車体横滑り角βが既知である場合について説明したが、これを組み合わせることができる。
【０２５８】
例えば、滑り角設定部が、直進判定部により自車が直進していると判定された場合、自車の速度に対応して予め求められた車体横滑り角βを設定し、直進判定部により自車が直進していると判定されず、かつ、自車の速度と、車体横滑り角が０となるときの予め求められた速度との差の絶対値が所定の閾値以下である場合に、車体横滑り角βを０に設定し、滑り角設定部により車体横滑り角βが設定されていない場合には第１の実施形態における推定を行い、滑り角設定部により車体横滑り角βが設定された場合に、第２の実施形態又は第３の実施の形態における推定をするという構成を採用することができる。
【０２５９】
この場合、車体横滑り角βが未知の場合には慣性航法時に精度の高い自己位置推定をすることができ、車体横滑り角βが未知の場合には慣性航法時により精度の高い自己位置推定をすることができる。
【０２６０】
また、第２〜第４の実施形態を組み合わせることができる。
【０２６１】
例えば、滑り角設定部の構成を、直進判定部により自車が直進していると判定された場合、自車の速度に対応して予め求められた車体横滑り角βを設定し、直進判定部により自車が直進していると判定されず、かつ、自車の速度と、車体横滑り角が０となるときの予め求められた速度との差の絶対値が所定の閾値以下である場合、車体横滑り角βを０に設定し、直進判定部により自車が直進していると判定されず、かつ、当該差の絶対値が所定の閾値よりも上である場合に、自車の操舵角と自車の速度とを入力として、車体横滑り角を推定する車両モデルを用いて車体横滑り角βを推定し、車体横滑り角βを推定した値に設定する、という構成を採用することができる。
【０２６２】
この場合、より確度の高い車体横滑り角βを優先的に設定することができるため、慣性航法時により精度の高い自己位置推定をすることができる。
【０２６３】
また、第５の実施形態では、第２の実施形態に対して適用する場合を例に説明したが、第３の実施形態又は第４の実施形態と組み合わせることも可能である。
【０２６４】
また、第５の実施形態では、ｉ１及びｉ２が固定である場合を例に説明したが、これに限定されるものではない。
【０２６５】
例えば、ｉ１及びｉ２をそれぞれ複数用意しておき、今回のｉ２を次回のｉ１とし、次回のｉ２の時点での距離及び横位置誤差を求める構成としてもよい。
【０２６６】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０２６７】
１０   自己位置推定システム
２０   自己位置推定システム
３０   自己位置推定システム
４０   自己位置推定システム
５０   自己位置推定システム
１００ 車輪速センサ
１１０ 角速度センサ
１２０ ＧＰＳセンサ
１３０ 舵角センサ
２００ 自己位置推定装置
２１０ 取得部
２２０ 第２自己位置推定部
２３０ 位置誤差算出部
２４０ 姿勢角補正量算出部
２５０ 第１自己位置推定部
２６０ 出力部
３００ 自己位置推定装置
３１０ 取得部
３１２ 直進判定部
３１４ 滑り角設定部
３２０ 第２自己位置推定部
３５０ 第１自己位置推定部
４００ 自己位置推定装置
４１０ 取得部
４１１ 車速記憶部
４１２ 車速比較部
４１４ 滑り角設定部
５００ 自己位置推定装置
５１０ 取得部
５１４ 滑り角設定部
６００ 自己位置推定装置
６１２ 直進判定部
６３０ 位置誤差算出部
６３５ 位置誤差記憶部
６４０ 姿勢角補正量算出部
６５０ 第１自己位置推定部
【図１】
2020112490.tif 000274
【図２】
2020112490.tif 000275
【図３】
2020112490.tif 000276
【図４】
2020112490.tif 000277
【図５】
2020112490.tif 000278
【図６】
2020112490.tif 000279
【図７】
2020112490.tif 000280
【図８】
2020112490.tif 000281
【図９】
2020112490.tif 000282
【図１０】
2020112490.tif 000283
【図１１】
2020112490.tif 000284
【図１２】
2020112490.tif 000285
【図１３】
2020112490.tif 000286
【図１４】
2020112490.tif 000287
【図１５】
2020112490.tif 000288
【図１６】
2020112490.tif 000289
【図１７】
2020112490.tif 000290
【図１８】
2020112490.tif 000291
【図１９】
2020112490.tif 000292
【図２０】
2020112490.tif 000293