JP6488860B2

JP6488860B2 - 勾配推定装置及びプログラム

Info

Publication number: JP6488860B2
Application number: JP2015091909A
Authority: JP
Inventors: 洪二郎武山; 祥子小島
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2015-04-28
Filing date: 2015-04-28
Publication date: 2019-03-27
Anticipated expiration: 2035-04-28
Also published as: JP2016206149A

Description

本発明は、勾配推定装置及びプログラムに関する。

従来、車輪速で取得した車速に基づいた加速度と、加速度センサで取得した加速度に基づき路面勾配を推定する技術が提案されている（例えば、特許文献１参照）。

また、衛星から受信した信号をもとに測位された対象物の測位情報と、対象物の速度情報と、対象物の加速度情報とに基づき、加速度情報を補正するための補正値を逐次算出し、補正値と、速度情報と、加速度情報とに基づいて、対象物が位置する場所の傾斜角を算出する技術が提案されている（例えば、特許文献２参照）。特許文献２に記載の技術では、車速と加速度センサに基づき算出した路面勾配（オフセット誤差等含む）に対し、衛星情報から得た測位位置等に基づきオフセット誤差等の補正を行い、路面勾配を算出する。

特開平２−１６１３０８号公報特開２０１３−１３０４１６号公報

しかし、上記の特許文献１に記載の技術では、車体に対する加速度センサ設置角のずれが考慮されていないため、算出される路面勾配には誤差が生じる、という問題がある。

また、特許文献２に記載の技術では、衛星から受信した信号をそのまま利用した場合、建物による遮蔽や反射などにより、図５に示すように、可視衛星数不足により測位情報や速度情報が得られないおそれや、反射による誤差が生じることにより計測値が劣化するおそれがあり、路面勾配の誤差が増加するおそれがある、という問題がある。

本発明は、上記の事情に鑑みてなされたもので、各時刻における観測衛星数が少ない環境でも、移動体が走行する走路の勾配を精度良く推定することができる勾配推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の勾配推定装置は、各測位衛星から送信された衛星データの時系列、移動体に搭載された速度センサによって検出された速度データの時系列、及び前記移動体に搭載された加速度センサによって検出された加速度の時系列を取得する取得手段と、前記取得手段により取得された加速度の時系列、及び前記速度データの時系列に基づいて算出される前記移動体の進行方向の加速度の時系列から、前記加速度センサのピッチ角の時系列を算出するピッチ角算出手段と、前記取得手段により取得された各測位衛星からの衛星データの時系列と、前記速度データの時系列と、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、前記３次元速度ベクトルの高さ方向の速度成分が、前記移動体の速度データ、現時刻の前記移動体のピッチ角、及び前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の前記移動体のピッチ角を、前記移動体が走行する走路の勾配として推定する勾配推定手段と、を含んで構成されている。

また、本発明のプログラムは、コンピュータを各測位衛星から送信された衛星データの時系列、移動体に搭載された速度センサによって検出された速度データの時系列、及び前記移動体に搭載された加速度センサによって検出された加速度の時系列を取得する取得手段、前記取得手段により取得された加速度の時系列、及び前記速度データの時系列に基づいて算出される前記移動体の進行方向の加速度の時系列から、前記加速度センサのピッチ角の時系列を算出するピッチ角算出手段、及び前記取得手段により取得された各測位衛星からの衛星データの時系列と、前記速度データの時系列と、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、前記３次元速度ベクトルの高さ方向の速度成分が、前記移動体の速度データ、現時刻の前記移動体のピッチ角、及び前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の前記移動体のピッチ角を、前記移動体が走行する走路の勾配として推定する勾配推定手段として機能させるためのプログラムである。

本発明によれば、取得手段が、各測位衛星から送信された衛星データの時系列、移動体に搭載された速度センサによって検出された速度データの時系列、及び移動体に搭載された加速度センサによって検出された加速度の時系列を取得する。

そして、ピッチ角算出手段が、取得手段により取得された加速度の時系列、及び速度データの時系列に基づいて算出される移動体の進行方向の加速度の時系列から、加速度センサのピッチ角の時系列を算出する。

そして、勾配推定手段が、取得手段により取得された各測位衛星からの衛星データの時系列と、速度データの時系列と、ピッチ角算出手段によって算出された加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における移動体の３次元速度ベクトルを推定するための方程式であって、３次元速度ベクトルの高さ方向の速度成分が、移動体の速度データ、現時刻の移動体のピッチ角、及びピッチ角算出手段によって算出された加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の移動体のピッチ角を、移動体が走行する走路の勾配として推定する。

このように、移動体に搭載された加速度センサによって検出された加速度の時系列、及び移動体の速度データの時系列に基づいて算出される移動体の進行方向の加速度の時系列から、加速度センサのピッチ角の時系列を算出し、各測位衛星からの衛星データの時系列と、速度データの時系列と、加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における自車両の３次元速度ベクトルを推定するための方程式であって、３次元速度ベクトルの高さ方向の速度成分が、移動体の速度データ、現時刻の移動体のピッチ角、及加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の移動体のピッチ角を、移動体が走行する走路の勾配として推定することにより、各時刻における観測衛星数が少ない環境でも、移動体が走行する走路の勾配を精度良く推定することができる。

また、本発明は、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列とに基づいて、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角を補正することにより、前記走路の勾配を推定する補正手段を更に含むようにすることができる。

また、本発明は、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列とに基づいて、前記勾配推定手段によって推定された前記走路の勾配に関する信頼度を算出する信頼度算出手段を更に含み、前記補正手段は、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列と、前記信頼度算出手段によって算出された信頼度の時系列とに基づいて、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角を補正するようにすることができる。

また、本発明は、方位変化算出手段を更に含み、前記取得手段は、前記移動体に搭載された角速度センサによって検出された前記移動体のヨーレイトの時系列を更に取得し、前記方位変化算出手段は、前記取得手段によって取得された前記移動体のヨーレイトに基づいて、前記移動体の進行方向の方位変化データの時系列を算出し、前記勾配推定手段は、前記取得手段により取得された各測位衛星からの衛星データの時系列と、前記速度データの時系列と、前記ヨーレイトの時系列と、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記方位変化算出手段によって算出された前記方位変化データの時系列とに基づいて、前記３次元速度ベクトルが、前記移動体の速度データ、現時刻の前記移動体のピッチ角、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時間変化、及び前記方位変化算出手段によって算出された前記移動体の前記方位変化データの時間変化により拘束された前記方程式に従って、前記移動体が走行する走路の勾配を推定するようにすることができる。

また、本発明の前記勾配推定手段は、クロックドリフトの時間変化の線形化により更に拘束された前記方程式に従って、前記移動体が走行する走路の勾配を推定するようにすることができる。

また、本発明の前記勾配推定手段は、前記方程式に従って、前記移動体が走行する走路の勾配及び前記移動体の３次元速度ベクトルを推定するようにすることができる。

また、本発明は、前記移動体に搭載された速度センサによって検出された速度データの時系列と、前記勾配推定手段によって推定された前記移動体が走行した走路の勾配の時系列とに基づいて、前記移動体の高さ方向の走行軌跡を算出する高さ方向走行軌跡算出手段を更に含むようにすることができる。

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

以上説明したように、本発明の勾配推定装置及びプログラムによれば、移動体に搭載された加速度センサによって検出された加速度の時系列、及び移動体の速度データの時系列に基づいて算出される移動体の進行方向の加速度の時系列から、加速度センサのピッチ角の時系列を算出し、各測位衛星からの衛星データの時系列と、速度データの時系列と、加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における自車両の３次元速度ベクトルを推定するための方程式であって、３次元速度ベクトルの高さ方向の速度成分が、移動体の速度データ、現時刻の移動体のピッチ角、及加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の移動体のピッチ角を、移動体が走行する走路の勾配として推定することにより、各時刻における観測衛星数が少ない環境でも、移動体が走行する走路の勾配を精度良く推定することができる、という効果が得られる。

本発明の実施の形態の概要を説明するための説明図である。本発明の実施の形態に係る勾配推定装置を示すブロック図である。ＧＰＳデータに基づく３次元速度ベクトルの推定を説明するための図である。本発明の実施の形態における勾配推定処理ルーチンの内容を示すフローチャートである。従来技術を説明するための説明図である。

［本発明の実施の形態の概要］
本発明の実施の形態では、車両が走行する道路の勾配を精度良く推定し、高さ方向の車両の走行軌跡を精度良く求めることを目的とする。図１に本実施の形態の概要を説明するための説明図を示す。

本実施の形態では、車両が走行する道路の勾配を推定する方法として、まず、図１に示すように、車両の速度と、加速度センサによって検出された車両の加速度に基づき道路勾配の変化を算出する。次に、測位衛星のドップラー周波数と、道路勾配の変化の時系列データを統合することで現在の道路勾配を推定する。そして、推定した道路勾配と車輪速を組み合わせることで自車の高さ方向の走行軌跡を算出する。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、自車両が走行する道路の勾配を推定する勾配推定装置に、本発明を適用した場合を例に説明する。また、移動体が車両である場合を例に説明する。

＜本実施の形態に係る勾配推定装置１０の構成＞
図２に示すように、第１の実施の形態に係る勾配推定装置１０は、各測位衛星から送信される衛星データを逐次受信するＧＰＳ装置１２と、自車両の角速度を逐次検出する角速度センサ１４と、自車両の加速度を逐次検出する加速度センサ１６と、自車両の速度を逐次検出する速度センサ１８と、自車両が走行する走路の一例としての道路の勾配を推定し、自車両の高さ方向の走行軌跡を算出するコンピュータ２０とを備えている。

ＧＰＳ装置１２は、各時刻について、自車両の測位及び速度推定に必要となる擬似距離、ドップラー周波数、衛星番号、衛星位置、衛星速度等を含む衛星データを、各測位衛星から受信する。

角速度センサ１４は、各時刻について、自車両の車両運動として、自車両の角速度を検出する。本実施の形態では、角速度センサ１４は、ヨーレイトを角速度として検出する。なお、本実施の形態では、角速度センサ１４はヨーレイトを検出するが、ピッチ角速度またはロール角速度を併せて検出してもよい。角速度センサ１４としては、ジャイロセンサなどを用いることができる。また、角速度センサ１４として、ジャイロセンサ以外のセンサを用いてもよい。

加速度センサ１６は、各時刻について、自車両の加速度を検出する。加速度センサ１６としては、Ｇセンサなどの車載加速度計を用いることができる。加速度センサ１６によって検出された加速度は車体の移動に伴う加速度と、重力により発生する加速度を含む値である。

速度センサ１８は、各時刻について、自車両の車輪速から出力された速度データを検出する。なお、速度センサ１８としては、レーザレーダ、ミリ波レーダ、ステレオカメラ等、ＧＰＳ装置１２以外であれば、車輪速を検出するセンサ以外を用いてもよい。

コンピュータ２０は、ＣＰＵ、後述する勾配推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。このコンピュータ２０を以下で説明する勾配推定処理ルーチンに従って機能ブロックで表すと、図２に示すように、衛星データを取得する衛星データ取得部２００と、衛星データ取得部２００によって取得された衛星データが時系列に蓄積される衛星データ蓄積部２０２と、角速度センサ１４によって検出された各時刻の角速度を取得する角速度取得部２０４と、角速度取得部２０４によって取得されたヨーレイトに基づいて、自車両の進行方向の方位変化データを算出する方位変化算出部２０６と、方位変化算出部２０６によって算出された方位変化データが時系列に蓄積される方位変化データ蓄積部２０８と、加速度センサ１６によって検出された各時刻の加速度を取得する加速度取得部２１０と、速度センサ１８によって検出された各時刻の速度データを取得する速度データ取得部２１２と、速度データ取得部２１２によって取得された速度データが時系列に蓄積される速度データ蓄積部２１４と、速度データ取得部２１２によって取得された速度データに基づいて、自車両の進行方向の加速度を各時刻について算出する加速度算出部２１６と、加速度取得部２１０によって取得された加速度、及び加速度算出部２１６によって算出された加速度から、水平面に対する加速度センサ１６のピッチ角を算出するセンサピッチ角算出部２１８と、センサピッチ角算出部２１８によって算出された水平面に対する加速度センサ１６のピッチ角が時系列に蓄積されるセンサピッチ角データ蓄積部２２０と、衛星データ蓄積部２０２に蓄積された各測位衛星からの衛星データの時系列と、方位変化データ蓄積部２０８に蓄積された方位変化データの時系列と、速度データ蓄積部２１４に格納された速度データの時系列と、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列とに基づいて、自車両が走行する現時刻の道路の勾配を推定する道路勾配推定部２２２と、道路勾配推定部２２２によって推定された道路の勾配が時系列に蓄積される道路勾配データ蓄積部２２４と、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列と、道路勾配データ蓄積部２２４に蓄積された道路の勾配の時系列とに基づいて、推定された道路の勾配に関する信頼度を算出する信頼度算出部２２６と、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列と、道路勾配データ蓄積部２２４に蓄積された道路の勾配の時系列と、信頼度算出部２２６によって算出された信頼度とに基づいて、センサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角を補正し、道路の勾配を推定する道路勾配補正部２２８と、速度データ蓄積部２１４に蓄積された速度データの時系列と、道路勾配補正部２２８によって推定された自車両が走行した道路の勾配に基づいて、自車両の高さ方向の走行軌跡を算出する高さ方向走行軌跡算出部２３０とを含んだ構成で表すことができる。衛星データ取得部２００、角速度取得部２０４、加速度取得部２１０、及び速度データ取得部２１２は、取得手段の一例である。また、センサピッチ角算出部２１８はピッチ角算出手段の一例であり、道路勾配推定部２２２は勾配推定手段の一例であり、道路勾配補正部２２８は補正手段の一例である。

衛星データ取得部２００は、ＧＰＳ装置１２によって受信された各時刻の衛星データを取得する。

衛星データ蓄積部２０２には、衛星データ取得部２００によって取得された衛星データが時系列に保存される。データ保存のトリガは、一定時間経過時でもよいし、一定距離走行時でもよい。

角速度取得部２０４は、角速度センサ１４によって検出された各時刻のヨーレイトを取得する。

方位変化算出部２０６は、角速度取得部２０４によって取得された自車両のヨーレイトに基づいて、自車両の進行方向の方位変化データの時系列を算出する。
具体的には、方位変化算出部２０６は、角速度取得部２０４によって取得されたヨーレイトを時間積分することにより、自車両の進行方向の方位変化データを算出する。なお、方位変化算出部２０６は、ヨーレイトを時間積分する前処理として、角速度センサ１４によって検出されたヨーレイトに対し零点補正処理を行っても良い。

方位変化データ蓄積部２０８には、方位変化算出部２０６によって算出された方位変化データが時系列に保存される。データ保存のトリガは、衛星データ蓄積部２０２と同じタイミングとする。

加速度取得部２１０は、加速度センサ１６によって検出された各時刻の加速度を取得する。

速度データ取得部２１２は、速度センサ１８によって検出された各時刻の速度データを取得する。

速度データ蓄積部２１４には、速度データ取得部２１２によって取得された速度データが時系列に保存される。データ保存のトリガは、衛星データ蓄積部２０２と同じタイミングとする。

加速度算出部２１６は、速度データ取得部２１２によって取得された速度データを時間微分し、自車両の進行方向の加速度を各時刻について算出する。

センサピッチ角算出部２１８は、各時刻について、加速度取得部２１０によって取得された加速度、及び加速度算出部２１６によって算出された加速度から、水平面に対する加速度センサ１６のピッチ角を算出する。

具体的には、センサピッチ角算出部２１８は、以下の式（１）に従って、ピッチ角φを算出する。なお、加速度算出部２１６によって算出された加速度をＡ_１、加速度取得部２１０によって取得された加速度をＡ_２で表す。なお、ｇは重力加速度を表す。

上記図１に示されるように、加速度センサ１６によって検出された加速度をＡ_２と、加速度算出部２１６によって算出された加速度をＡ_１との差分が重力成分Ｇに対応し、上記式（１）によって、水平面に対する加速度センサ１６のピッチ角が算出される。

センサピッチ角データ蓄積部２２０には、センサピッチ角算出部２１８によって算出された、水平面に対する加速度センサ１６のピッチ角が時系列に保存される。データ保存のトリガは、衛星データ蓄積部２０２と同じタイミングとする。

道路勾配推定部２２２は、衛星データ蓄積部２０２に蓄積された各測位衛星からの衛星データの時系列と、方位変化データ蓄積部２０８に蓄積された方位変化データの時系列と、速度データ蓄積部２１４に格納された速度データの時系列と、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列とに基づいて、自車両が走行する走路の一例としての道路の勾配を推定する。

なお、速度推定に用いる時系列データの範囲は、現時刻からの経過時間や走行距離などに基づき決定する。以下では時刻インデックスｔは現時刻よりｔ秒前に蓄積されたデータであることを示しており、ｔ＝０を現時刻として扱う。

具体的には、道路勾配推定部２２２は、衛星データ蓄積部２０２に蓄積された各測位衛星からの衛星データの時系列に基づき立式される各時刻における自車両の３次元速度ベクトルを推定するための方程式に従って、自車両の３次元速度ベクトルを推定するときに求められる、現時刻の自車両のピッチ角を、自車両が走行する道路の勾配として推定する。

本実施の形態では、自車両の３次元速度ベクトルを推定するための方程式は、３次元速度ベクトルの速度成分が、速度データ蓄積部２１４に格納された速度データ、現時刻の自車両のピッチ角、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時間変化、現時刻の自車両の方位変化データ、方位変化データ蓄積部２０８に蓄積された自車両の方位変化データの時間変化、及びクロックドリフトの時間変化の線形化により拘束されている。

図３に衛星ドップラーを用いた自車速度ベクトル算出の概要を示す。衛星ドップラーを用いた速度推定では、観測された各衛星信号のドップラー周波数に基づき、各測位衛星方向への速度成分をそれぞれ算出し、それらを合成することで自車両の３次元速度ベクトルを求める。ここではまず、各測位衛星方向への速度成分を求める方法を示す。以下の式（２）に、自車両の３次元速度ベクトルを推定するための方程式を示す。

ここで、Ｖｓ^t _iは自車両から測位衛星ｉ方向に投影した衛星速度、Ｄ^t _iは衛星信号のドップラー周波数、ｆ１は衛星データの搬送波の周波数、Ｃは光速、ｒ^t _iは自車両とＧＰＳ衛星ｉ間の擬似距離、Ｇｘ^t _i、Ｇｙ^t _i、Ｇｚ^t _iは自車両から測位衛星ｉ方向への単位ベクトル、Ｖｘ^t、Ｖｙ^t、Ｖｚ^tは自車速度ベクトル、Ｘ^t、Ｙ^t、Ｚ^tは自車両位置、Ｘ^tｓ_i、Ｙ^tｓ_i、Ｚ^tｓ_iは衛星位置を示す。添え字ｔはｔ時刻前のデータであることを表し、添え字ｉはｉ番目の測位衛星のデータであることを表す。自車両位置は、擬似距離を用いた一般的な測位方法またはその他の測位方法を用いて算出した値を用いることができる。Ｘ，Ｙは水平方向を示し、Ｚは鉛直方向を示す。

道路勾配推定部２２２は、後述する拘束条件で拘束された、自車両の３次元速度ベクトルを推定するための方程式に基づいて、自車両の３次元速度ベクトルを推定するときに求められる、現時刻の自車両のピッチ角を、自車両が走行する道路の勾配として推定する。勾配推定に用いる各観測値の時系列データの範囲は、現時刻からの経過時間や走行距離などに基づき決定する。

ここで、本実施の形態の原理について説明する。

上記式（２）の連立方程式を解くにあたり、解を得るための条件は、通常「未知パラメータ数≦立式数」である、上記式（２）では、Ｖｘ^t、Ｖｙ^t、Ｖｚ^t、及びＣｂｖ^tの４つが未知パラメータであるため、時系列データの全時刻における未知パラメータ数は、「未知パラメータ数＝時刻数×４」となる。これに対して、「立式数＝時刻数×各時刻での観測衛星数」であるため、各時刻での観測衛星数が平均４つ以上ない場合には、速度推定結果が得られないことになる。特に、都市部のように建物の遮蔽により観測衛星数が少ない環境では、上記条件を満たすのは困難である。

そこで、本実施の形態では、以下の拘束条件を取り入れることで、未知パラメータ数を削減し、観測衛星数が少ない環境でも速度推定の解が得られる可能性を向上させる。第１の実施の形態で用いる拘束条件は、下記に示す拘束条件１〜４である。

上記式（３）の拘束条件１内のθ^ｔは自車両の時系列の方位角であり、φ^ｔは自車両の時系列のピッチ角である。

また、上記式（４）の拘束条件２内のθ^０は現時刻における自車両の進行方向の方位角であり、θ_ｇｙｒｏ ^０は方位変化データ蓄積部２０８に格納された現時刻の自車両の進行方向の方位角であり、θ_ｇｙｒｏ ^ｔは方位変化データ蓄積部２０８に格納されたｔ時刻前の自車両の進行方向の方位角である。

また、上記式（５）の拘束条件３内のφ^０は現時刻におけるピッチ角であり、φ_Ｇ ^０はセンサピッチ角データ蓄積部２２０に格納された現時刻のピッチ角であり、φ_Ｇ ^ｔはセンサピッチ角データ蓄積部２２０に格納されたｔ時刻前のピッチ角である。

また、上記式（６）の拘束条件４内のＣｂｖ^０は現時刻におけるクロックドリフト、及びＡはクロックドリフトの時間変化の傾きを示す。

上記式（３）〜（６）を、上記（２）式に代入することにより、「時刻数×４」個あった未知パラメータは、θ^０，φ^０，Ｃｂｖ^０，Ａの合計「４」個に低減される。これにより都心部など建物の遮蔽により衛星数が低減した場合でも、現時刻の自車両のピッチ角（φ^０）を求めることができる。

道路勾配データ蓄積部２２４には、道路勾配推定部２２２によって推定された道路の勾配が時系列に保存される。データ保存のトリガは、衛星データ蓄積部２０２と同じタイミングとする。

信頼度算出部２２６は、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列と、道路勾配データ蓄積部２２４に蓄積された道路の勾配の時系列とに基づいて、推定された現時刻の道路の勾配に関する信頼度を算出する。

信頼度算出部２２６は、各時刻について、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角と、道路勾配データ蓄積部２２４に蓄積されたピッチ角との比較を行うことで、道路勾配推定部２２２で算出したピッチ角の信頼性を判断する。信頼度Ｒは以下の式（７）に従い算出する。

上記式（７）中のφ_Ｇ ^ｔはセンサピッチ角データ蓄積部２２０に蓄積されたｔ時刻前の加速度センサ１６のピッチ角を表し、φ_ＧＰＳ ^ｔは道路勾配データ蓄積部２２４に蓄積されたｔ時刻前のピッチ角を示す。また、ｖａｒ（）は（）内の分散値を算出する関数であると定義する。本実施の形態では、信頼度の算出に用いる時系列データ長は過去５秒とするが、その他の値でもよい。

道路勾配補正部２２８は、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列と、道路勾配データ蓄積部２２４に蓄積された道路の勾配の時系列と、信頼度算出部２２６によって算出された信頼度の時系列とに基づいて、センサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角を補正することにより、道路の勾配を推定する。

センサピッチ角データ蓄積部２２０に蓄積されている、センサピッチ角算出部２１８によって算出されたピッチ角は、水平面に対する自車両のピッチ角であるが、これは車体に対する加速度センサ１６の設置角の傾きや、加速度センサ１６のオフセット誤差などが乗った値である。従って、正しい道路勾配の値を推定するためにはこれらの補正が必要である。

一方、道路勾配推定部２２２では衛星情報を用いて道路勾配（ピッチ角）を随時算出することができるが、衛星の受信状況などにより値が得られない場合も多くある。そこで、道路勾配推定部２２２で算出したピッチ角に対し、信頼度算出部２２６で算出した信頼度を用いて信頼性の高いピッチ角のみを抽出し、抽出されたピッチ角を用いてセンサピッチ角算出部２１８によって算出されたピッチ角の補正を行う。以下の式（８）に補正式を示す。

上記式（８）中のφ_Ｇ ^０はセンサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角を表し、φ_ＧＰＳ ^ｔは道路勾配データ蓄積部２２４に蓄積されたｔ時刻前のピッチ角であって、信頼性の高いピッチ角を表し、φ_Ｇ ^ｔはセンサピッチ角データ蓄積部２２０に蓄積されたｔ時刻前の加速度センサ１６のピッチ角を表す。またｍｅａｎ（）は（）内の平均値を算出する関数であると定義する。本実施の形態では、ピッチ角の補正に用いる時系列データ長を過去５００秒分とするが、その他の値でもよい。

高さ方向走行軌跡算出部２３０は、速度データ蓄積部２１４に蓄積された速度データの時系列と、道路勾配補正部２２８によって推定された自車両が走行した道路の勾配の時系列に基づいて、自車両の高さ方向の走行軌跡を算出する。なお、自車両の走行軌跡の推定方法については、従来既知の手法を用いればよく、詳細な説明を省略する。

＜本実施の形態に係る勾配推定装置１０の作用＞
次に、第１の実施の形態に係る勾配推定装置１０の作用について説明する。

各時刻において、ＧＰＳ装置１２が各測位衛星から衛星データを受信し、角速度センサ１４が自車両のヨーレイトを検出し、加速度センサ１６が自車両の加速度を検出し、速度センサ１８が自車両の速度データを検出しているときに、コンピュータ２０において、図４に示す勾配推定処理ルーチンが実行される。

ステップＳ１００において、衛星データ取得部２００は、ＧＰＳ装置１２によって各測位衛星から受信された衛星データを取得し、衛星データ蓄積部２０２に格納する。角速度取得部２０４は、角速度センサ１４によって検出された自車両のヨーレイトを取得する。加速度取得部２１０は、加速度センサ１６によって検出された自車両の加速度を取得する。速度データ取得部２１２は、速度センサ１８によって検出された速度データを取得し、速度データ蓄積部２１４に格納する。

ステップＳ１０２において、方位変化算出部２０６は、上記ステップＳ１００で取得された自車両のヨーレイトに基づいて、自車両の進行方向の方位変化データを算出する。そして、方位変化算出部２０６は、算出された自車両の進行方向の方位変化データを方位変化データ蓄積部２０８に格納する。

ステップＳ１０４において、加速度算出部２１６は、上記ステップＳ１００で取得された速度データを時間微分し、自車両の進行方向の加速度を算出する。

ステップＳ１０６において、センサピッチ角算出部２１８は、上記ステップＳ１００で取得された加速度、及び上記ステップＳ１０４で算出された加速度から、水平面に対する加速度センサ１６のピッチ角を算出する。そして、センサピッチ角算出部２１８は、算出された加速度センサ１６のピッチ角を、センサピッチ角データ蓄積部２２０に格納する。

ステップＳ１０８において、道路勾配推定部２２２は、衛星データ蓄積部２０２に蓄積された各測位衛星からの衛星データの時系列と、方位変化データ蓄積部２０８に蓄積された方位変化データの時系列と、速度データ蓄積部２１４に格納された速度データの時系列と、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列とに基づいて、上記式（２）〜（６）に従って、自車両の３次元速度ベクトルを推定し、現時刻の自車両のピッチ角を、自車両が走行する現時刻の道路の勾配として推定する。そして、道路勾配推定部２２２は、推定された自車両が走行する現時刻の道路の勾配を、道路勾配データ蓄積部２２４に格納する。

ステップＳ１１０において、信頼度算出部２２６は、予め定められた時間区間における各時刻についてセンサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列と、予め定められた時間区間における各時刻について道路勾配データ蓄積部２２４に蓄積された道路の勾配の時系列とに基づいて、上記式（７）に従って、推定された現時刻の道路の勾配に関する信頼度Ｒを算出する。

ステップＳ１１２において、道路勾配補正部２２８は、予め定められた時間区間における各時刻についてセンサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角の時系列と、予め定められた時間区間における各時刻について道路勾配データ蓄積部２２４に蓄積された道路の勾配の時系列と、上記ステップＳ１１０で算出された信頼度の時系列とに基づいて、センサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角を補正することにより、道路の勾配を推定する。

ステップＳ１１４において、高さ方向走行軌跡算出部２３０は、速度データ蓄積部２１４に蓄積された速度データの時系列と、上記ステップＳ１１２で推定された自車両が走行した道路の勾配の時系列に基づいて、自車両の高さ方向の走行軌跡を算出する。

ステップＳ１１６において、上記ステップＳ１１４で算出された自車両の高さ方向の走行軌跡を結果として出力する。

以上説明したように、本発明の実施の形態に係る勾配推定装置によれば、自車両に搭載された加速度センサによって検出された加速度の時系列、及び自車両の速度データの時系列に基づいて算出される自車両の進行方向の加速度の時系列から、加速度センサのピッチ角の時系列を算出し、各測位衛星からの衛星データの時系列と、速度データの時系列と、加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における自車両の３次元速度ベクトルを推定するための方程式であって、３次元速度ベクトルの高さ方向の速度成分が、自車両の速度データ、現時刻の自車両のピッチ角、及加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の自車両のピッチ角を、自車両が走行する道路の勾配として推定することにより、各時刻における観測衛星数が少ない環境でも、自車両が走行する道路の勾配を精度良く推定することができる。

また、加速度センサの設置角やオフセット誤差を補正できるため、衛星データと慣性センサデータ（加速度センサによって検出された加速度）を時系列で統合することにより、衛星信号の遮蔽又は反射が生じた場合でも、加速度センサの設置角やオフセット誤差を精度良く補正することができる。

また、本実施の形態では、車速から得られた加速度及び加速度センサによって検出された加速度に基づき算出した道路勾配の変化と、ドップラー周波数の時系列データとを統合することにより、ドップラー周波数のみを用いて道路勾配を推定する場合に比べて少ない衛星数であっても道路勾配を推定することができる。また、誤差の要因となるマルチパス信号を除去しやすくなる。

また、本実施の形態では、車速から得られた加速度及び加速度センサによって検出された加速度に基づき算出した道路勾配の変化と、ドップラー周波数とを時系列で統合することにより、観測値を時系列方向に増加させることができる。その結果、ドップラー周波数のみを用いてピッチ角を推定する場合に比べて、外れ値であるマルチパスを除去しやすくなる。また、過去の衛星数を合わせて利用できることで瞬間的に衛星数が減少する場合でも、ピッチ角の推定が可能となる。これにより都心部のように衛星数の減少やマルチパス誤差が発生しやすい環境においても精度良く道路勾配を推定でき、その結果、高さ方向の自車両の走行軌跡を精度良く求めることができる。

なお、上記実施の形態では、３次元速度ベクトルの高さ方向の速度成分Ｖｚ^ｔが、上記式（５）に示す時系列のピッチ角に関する拘束条件３及び上記式（３）に示す拘束条件１の速度データＶ_{ｗｈｅｅｌ} ^ｔに関する拘束条件によって拘束されると共に、上記式（３）に示す拘束条件１の３次元速度ベクトルの速度成分Ｖｘ^ｔ及びＶｙ^ｔについての速度データＶ_{ｗｈｅｅｌ} ^ｔに関する拘束条件、上記式（４）に示す時系列の方位変化データに関する拘束条件２、及び上記式（６）に示すクロックドリフトの時間変化による拘束条件４の全てを用いる場合について説明したが、これに限定されない。例えば、３次元速度ベクトルの高さ方向の速度成分Ｖｚ^ｔについて、上記式（５）に示す時系列のピッチ角に関する拘束条件３及び上記式（３）に示す速度データＶ_{ｗｈｅｅｌ} ^ｔに関する拘束条件のみを用いてもよいし、上記式（３）に示す３次元速度ベクトルの速度成分Ｖｘ^ｔ及びＶｙ^ｔについての速度データＶ_{ｗｈｅｅｌ} ^ｔに関する拘束条件、上記式（４）に示す時系列の方位変化データに関する拘束条件２、及び上記式（６）に示すクロックドリフトの時間変化による拘束条件４の少なくとも１つの拘束条件を組み合わせて用いてもよい。

また、上記実施の形態において、信頼度算出部２２６は、上記式（７）に従って、センサピッチ角データ蓄積部２２０に蓄積された加速度センサ１６のピッチ角と、道路勾配データ蓄積部２２４に蓄積されたピッチ角との比較を行うことで、道路勾配推定部２２２で算出したピッチ角の信頼性を判断する場合を例に説明したが、これに限定されるものではなく、例えば信頼度の算出に用いる時系列データ内のピッチ角の最大値が、予め定められた値以上である場合には、低い信頼度を与えるようにしてもよい。

また、上記実施の形態において、道路勾配補正部２２８は、上記式（８）に従って、道路勾配データ蓄積部２２４に蓄積されたｔ時刻前のピッチ角であって、信頼性の高いピッチ角φ_ＧＰＳ ^ｔと、センサピッチ角データ蓄積部２２０に蓄積されたｔ時刻前の加速度センサ１６のピッチ角φ_Ｇ ^ｔとの差分の平均値に基づいて、センサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角φ_Ｇ ^０の補正を行う場合を例に説明したがこれに限定されるものではない。例えば、ピッチ角φ_ＧＰＳ ^ｔと、ピッチ角φ_Ｇ ^ｔとから最小二乗法による最尤推定によって補正量を推定し、推定された補正量に基づいて、センサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角φ_Ｇ ^０の補正を行ってもよい。または、信頼性の高いピッチ角φ_ＧＰＳ ^ｔのみを用いるのではなく、信頼度算出部２２６で算出した信頼度の時系列を重みとして、信頼度の時系列と、ピッチ角φ_ＧＰＳ ^ｔとピッチ角φ_Ｇ ^ｔとの差分との重み付き和によって、センサピッチ角データ蓄積部２２０に記憶された現時刻の加速度センサ１６のピッチ角φ_Ｇ ^０の補正を行ってもよい。

また、上記実施の形態では、車両に搭載される勾配推定装置について説明したが、本発明の勾配推定装置が搭載される移動体は車両に限定されない。例えば、勾配推定装置をロボットに搭載してもよいし、歩行者が携帯できるように勾配推定装置をポータブル端末として構成するようにしてもよい。

１０勾配推定装置
１２ＧＰＳ装置
１４角速度センサ
１６加速度センサ
１８速度センサ
２０コンピュータ
２００衛星データ取得部
２０２衛星データ蓄積部
２０４角速度取得部
２０６方位変化算出部
２０８方位変化データ蓄積部
２１０加速度取得部
２１２速度データ取得部
２１４速度データ蓄積部
２１６加速度算出部
２１８センサピッチ角算出部
２２０センサピッチ角データ蓄積部
２２２道路勾配推定部
２２４道路勾配データ蓄積部
２２６信頼度算出部
２２８道路勾配補正部
２３０高さ方向走行軌跡算出部

Claims

各測位衛星から送信された衛星データの時系列、移動体に搭載された速度センサによって検出された速度データの時系列、及び前記移動体に搭載された加速度センサによって検出された加速度の時系列を取得する取得手段と、
前記取得手段により取得された加速度の時系列、及び前記速度データの時系列に基づいて算出される前記移動体の進行方向の加速度の時系列から、前記加速度センサのピッチ角の時系列を算出するピッチ角算出手段と、
前記取得手段により取得された各測位衛星からの衛星データの時系列と、前記速度データの時系列と、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、前記３次元速度ベクトルの高さ方向の速度成分が、前記移動体の速度データ、現時刻の前記移動体のピッチ角、及び前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の前記移動体のピッチ角を、前記移動体が走行する走路の勾配として推定する勾配推定手段と、
前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列とに基づいて、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角を補正することにより、前記走路の勾配を推定する補正手段と、
を含む勾配推定装置。
前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列とに基づいて、前記勾配推定手段によって推定された前記走路の勾配に関する信頼度を算出する信頼度算出手段を更に含み、
前記補正手段は、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列と、前記信頼度算出手段によって算出された信頼度の時系列とに基づいて、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角を補正する
請求項１に記載の勾配推定装置。
方位変化算出手段を更に含み、
前記取得手段は、前記移動体に搭載された角速度センサによって検出された前記移動体のヨーレイトの時系列を更に取得し、
前記方位変化算出手段は、前記取得手段によって取得された前記移動体のヨーレイトに基づいて、前記移動体の進行方向の方位変化データの時系列を算出し、
前記勾配推定手段は、前記取得手段により取得された各測位衛星からの衛星データの時系列と、前記速度データの時系列と、前記ヨーレイトの時系列と、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記方位変化算出手段によって算出された前記方位変化データの時系列とに基づいて、前記３次元速度ベクトルが、前記移動体の速度データ、現時刻の前記移動体のピッチ角、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時間変化、及び前記方位変化算出手段によって算出された前記移動体の前記方位変化データの時間変化により拘束された前記方程式に従って、前記移動体が走行する走路の勾配を推定する
請求項１又は請求項２に記載の勾配推定装置。
前記勾配推定手段は、クロックドリフトの時間変化の線形化により更に拘束された前記方程式に従って、前記移動体が走行する走路の勾配を推定する
請求項１〜請求項３の何れか１項に記載の勾配推定装置。
前記勾配推定手段は、前記方程式に従って、前記移動体が走行する走路の勾配及び前記移動体の３次元速度ベクトルを推定する
請求項１〜請求項４の何れか１項に記載の勾配推定装置。
前記移動体に搭載された速度センサによって検出された速度データの時系列と、前記勾配推定手段によって推定された前記移動体が走行した走路の勾配の時系列とに基づいて、前記移動体の高さ方向の走行軌跡を算出する高さ方向走行軌跡算出手段を更に含む
請求項１〜請求項５の何れか１項に記載の勾配推定装置。
コンピュータを
各測位衛星から送信された衛星データの時系列、移動体に搭載された速度センサによって検出された速度データの時系列、及び前記移動体に搭載された加速度センサによって検出された加速度の時系列を取得する取得手段、
前記取得手段により取得された加速度の時系列、及び前記速度データの時系列に基づいて算出される前記移動体の進行方向の加速度の時系列から、前記加速度センサのピッチ角の時系列を算出するピッチ角算出手段、
前記取得手段により取得された各測位衛星からの衛星データの時系列と、前記速度データの時系列と、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列とに基づいて、各測位衛星からの衛星データの時系列に基づき立式される各時刻における前記移動体の３次元速度ベクトルを推定するための方程式であって、前記３次元速度ベクトルの高さ方向の速度成分が、前記移動体の速度データ、現時刻の前記移動体のピッチ角、及び前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時間変化により拘束された方程式に従って、現時刻の前記移動体のピッチ角を、前記移動体が走行する走路の勾配として推定する勾配推定手段、及び
前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角の時系列と、前記勾配推定手段によって推定された前記走路の勾配の時系列とに基づいて、前記ピッチ角算出手段によって算出された前記加速度センサのピッチ角を補正することにより、前記走路の勾配を推定する補正手段
として機能させるためのプログラム。