(57)【要約】【課題】  従来技術によれば、演算精度が悪化するような状態でも同一の処理を継続するため、演算精度が悪化する状態である部分の演算結果の影響により全体の精度が悪化する、という課題がある。【解決手段】  位置演算装置は、移動体状態を計測する状態計測部と、移動体状態に基づいて移動体の状態量を計算する状態量計算部と、移動体の位置を計算する位置計算部と、移動体状態の推定値を計算する状態推定部と、移動体が進行方向に対して横方向にふらついているか否かを判定するふらつき判定部と、ふらつき判定部が移動体はふらついていると判定した場合、推定値を選択し、ふらつき判定部が移動体はふらついていないと判定した場合、状態量を選択し、選択した何れか一方を位置計算部に出力する選択部を備える。【選択図】図１
2009115624.tif 000002
【特許請求の範囲】
【請求項１】
移動体状態を計測する状態計測部と、
前記移動体状態に基づいて、前記移動体の状態量を計算する状態量計算部と、
前記移動体の位置を計算する位置計算部と、
前記移動体状態の推定値を計算する状態推定部と、
前記移動体が進行方向に対して横方向にふらついているか否かを判定するふらつき判定部と、
前記ふらつき判定部が前記移動体はふらついていると判定した場合、前記推定値を選択し、前記ふらつき判定部が前記移動体はふらついていないと判定した場合、前記状態量を選択し、選択した何れか一方を前記位置計算部に出力する選択部を備える、位置演算装置。
【請求項２】
前記移動体状態と前記移動体状態量の少なくとも一方を記憶するメモリを更に備え、
前記状態推定部は、前記メモリが記憶しているデータに基づいて前記推定値を計算する、請求項１記載の位置演算装置。
【請求項３】
前記ふらつき判定部は、更に、前記移動体のふらつき度合いを算出し、
前記選択部は、前記ふらつき度合いに基づいて、前記移動体状態及び前記推定値を混合する、請求項１又は２記載の位置演算装置。
【請求項４】
前記選択部は、前記ふらつき度合いが所定値より高い場合、前記推定値の割合を多くして混合し、前記ふらつき度合いが前記所定値より低い場合、前記移動体状態の割合を多くして混合し、当該混合結果を前記位置計算部に出力する、請求項３記載の位置演算装置。
【請求項５】
前記移動体の運転状態を表す指標を示す運転状態ファクタを取得する運転状態ファクタ取得部を更に備え、
前記ふらつき判定部は、前記運転状態ファクタに基づいて、前記移動体がふらついているか否かの判定基準を変動させる、請求項１乃至４記載の位置演算装置。
【請求項６】
前記移動体の運転状態を表す指標を示す運転状態ファクタを取得する運転状態ファクタ取得部を更に備え、
前記ふらつき判定部は、前記運転状態ファクタに基づいて、前記推定値と前記移動体状態の混合の割合を変動させる、請求項１乃至４記載の位置演算装置。
【請求項７】
前記運転状態ファクタは、ハンドルの操作角度，運転者の状態、及び、外界からの情報のうち、少なくとも一つを示す、請求項５又は６記載の位置演算装置。
【請求項８】
前記運転者の特性を取得する運転者特性取得部を更に備え、
前記ふらつき判定部は、運転者の特性に基づいて、前記移動体がふらついているか否かの判定基準を変動させる、請求項１乃至４記載の位置演算装置。
【請求項９】
前記運転者の特性を取得する運転者特性取得部を更に備え、
前記ふらつき判定部は、運転者の特性に基づいて、前記推定値と前記移動体状態の混合の割合を変動させる、請求項１乃至４記載の位置演算装置。
【請求項１０】
前記運転者の特性は、ハンドル操作の癖を示す、請求項８又は９記載の位置演算装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置を演算する技術に関する。
【背景技術】
【０００２】
移動体（車両等）の位置を内界センサの情報をもとに演算する技術に関し、内界センサの値からフィルタを用いて真値を推定し、位置を演算する手法がある。この方式で精度を向上させるために、移動体の静止時にセンサ値のバイアス量を計算して記憶し、センサ値を補正する移動体測位装置がある（特許文献１参照）。
【０００３】
【特許文献１】特開２００５−１９５３９５号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
特許文献１では、移動体の特性を考慮しておらず、画一的に処理を実行するため、演算精度が悪化するような状態（例えば、移動体がふらつく場合）でも同一の処理を継続する。即ち、演算精度が悪化する状態である部分の演算結果の影響により全体の精度が悪化する、という課題がある。
【０００５】
そこで、本発明の目的は、移動体において演算精度が悪化するような状態であっても、高精度な位置演算を行うことができる装置を提供することにある。
【課題を解決するための手段】
【０００６】
上記課題を解決するため、本発明の望ましい態様の一つは次の通りである。
【０００７】
位置演算装置は、移動体状態を計測する状態計測部と、移動体状態に基づいて移動体の状態量を計算する状態量計算部と、移動体の位置を計算する位置計算部と、移動体状態の推定値を計算する状態推定部と、移動体が進行方向に対して横方向にふらついているか否かを判定するふらつき判定部と、ふらつき判定部が移動体はふらついていると判定した場合、推定値を選択し、ふらつき判定部が移動体はふらついていないと判定した場合、状態量を選択し、選択した何れか一方を位置計算部に出力する選択部を備える。
【発明の効果】
【０００８】
本発明によれば、移動体において演算精度が悪化するような状態であっても、高精度な位置演算を行うことができる装置を提供することができる。
【発明を実施するための最良の形態】
【０００９】
以下、実施例を、図を用いて説明する。尚、本稿では、移動体を車両であるとして説明するが、これに限定されるものではない。
【実施例１】
【００１０】
図１は、位置演算装置の構成を示す図である。内部情報計測部１０（内界センサ等）により、ヨーレート，加速度，ハンドルの操作角度が測定され、これらのデータが状態量計算部１１に入力されている。状態量計算部１１は、車両の状態量を計算する。例えば、センサ計測値の電圧等の値から物理量への変換，センサのオフセット量の除去処理や直進性の補償，座標系の修正等を行い、車両の横方向Ｇ（加速度），前後方向Ｇ，ヨー角等の車両の状態変数を車両の状態量として出力する。尚、センサデータや車両の状態量は一例であって、本実施例に必要なセンサデータの種類や数，装置内の信号の種類等を特定するものではない。
【００１１】
又、センサデータと車両の状態量はメモリ２１に入力される。メモリは過去数周期にわたる入力されたデータを記憶し、必要に応じて記憶しているデータを出力する。ここで、メモリ２１に記憶し出力するデータはセンサデータのみであっても良いし、車両の状態量のみであっても良いし、センサデータと車両の状態量の両方であっても良い。又、一定の時間毎に記憶する方式であっても良いし、一定の走行距離毎に記憶する方式であっても良い。
【００１２】
状態推定部２２は、メモリ２１からのデータを受け、過去からのデータを用いて現在の車両の状態量を推定し車両の状態の推定値として計算し、出力する。車両状態の推定値は、例えば、仮想車両の横方向Ｇ，仮想前後方向Ｇ，仮想ヨー角等となる。
【００１３】
又、運転状態ファクタ取得部２６は、移動体の運転状態を表す指標を示す運転状態ファクタ（例えば、ハンドル操作角度や方向指示器の操作状況やワイパースイッチの操作状況等）を取得し、ふらつき判定部２５に入力する。本実施例では、運転状態ファクタとしてハンドルの操作角の振れ量を用いるが、他のセンサデータであっても良いし、車両の状態変数であっても良いし、その他の装置からの信号であっても良いし、これらの組み合わせであっても良い。又、運転状態ファクタは、運転者の状態を監視した結果であっても良い。例えば、運転者の着座姿勢を感知する着座圧センサや運転者の注視方向を検出する視線センサや携帯電話等の車内で使用する機材の操作状況やドリンクホルダの飲物の状況検出結果等である。更に、運転状態ファクタは、運転者の車両のスイッチ類（オーディオのスイッチ等）の操作状況の検出結果や運転者のアクセルぺダルやブレーキペダルや変速装置の操作状況の検出結果等であっても良い。
【００１４】
運転者特性取得部２７は、運転者の特性（ハンドル操作の癖等）を取得し、ふらつき判定部２５に入力する。
【００１５】
ふらつき判定部２５は、ハンドルの操作角の振れ量、又は、運転者の特性に基づいて、車両状態がふらつきであるのか否かを判定し、判定結果を切り換え信号２３Ｃとして、選択部２３へ出力する。
【００１６】
選択部２３は、当該判定結果に応じて、車両状態の入力２３Ａと、車両状態の推定値の入力２３Ｂとを選択し、選択結果を位置計算部１２へ出力する。尚、選択部２３は、切り換え信号に応じて、アナログ的に車両状態と車両状態の推定値とを混合して出力しても良い。又、切り換え信号の大きさに選択結果が比例する必要もなく、目的に沿って意図した選択を行っても良い。
【００１７】
位置計算部１２は、選択部２３の出力から、積分や、方向の修正等の必要な演算を実施して車両位置を算出し、当該車両位置を出力する。
【００１８】
尚、本実施例の位置演算装置の出力は、ナビゲーション装置や安全のための運動制御装置や警報装置、利便のための走行案内装置や車庫入れ装置等の車両の位置が必要なアプリケーションに用いられる。
【００１９】
又、ふらつき度合いの値の判断として、例えば、一般的ドライバの運転を計測し、ふらついていない場合のデータを所定値として記憶させておき、この所定値より計算したふらつき度合いが高い場合は、その高さに応じて推定値と移動体状態を混合して、選択結果として出力しても良い。
【００２０】
混合に関しては、例えば、ふらつき度合いが所定値からの高さが第１の範囲内の場合には推定値３割、移動体状態７割として算術平均により混合し、所定値からの高さが第１の範囲内よりも高い第２の範囲内の場合には推定値６割、移動体状態４割として算術平均により混合し、所定値からの高さが第２の範囲内よりも高い場合には推定値１０割、移動体状態０割として算術平均により混合するように算術平均を用いて混合しても良いし、他の演算手法で混合する方式であっても良い。
【００２１】
図２は、車両が直進走行から途中で蛇行運転を行い、再び直進走行に戻る様子を示す図である。途中、蛇行運転を行っているが、最終的に車両の走行軌跡は直線路の中心線２１００上に留まる走行である。
【００２２】
図３は、位置演算装置の動作を示すフロー図である。
【００２３】
まず、状態量計算部１１は、内部情報計測部１０の計測値を収集する（ステップ３０１）。例えば、センサからのアナログ信号を計測して計測値として取り込んだり、ネットワークに接続されたセンサから通信される計測値をネットワーク経由で計測値を取り込んだりする。
【００２４】
次に、状態量計算部１１は、車両の状態量を計算する（ステップ３０２）。
【００２５】
ここで、図４にハンドル角度の取得値、図５に計算した車両の横方向Ｇのデータを示す。図において、時間０秒から１９秒が直進走行、１９秒から２９秒が蛇行運転、時間２９秒から３５秒が直進走行である。従来技術である横方向Ｇの二階積分を実施して横方向への移動距離を求める手法の結果を図６に示す。
【００２６】
従来技術では、時間０秒から１９秒の直進走行の間に実際の車両は直進しているにもかかわらず、車両位置が横方向に移動するように演算されてしまい、最終的には５ｍもの大きな変位になっている。これは、直進時のふらつきにより発生する微小な横方向加速度を積分してしまい累積誤差として大きな変位となってしまったためである。
【００２７】
本稿では、これを避けるため、車両の横方向Ｇをそのまま積分せずに以下の処理を行う。まず、メモリ２１にて、内部情報計測部１０の計測値と車両の状態量を取得し、これを記憶する（ステップ３０３）。メモリ２１は、複数のデータを記憶することができ、データが入力されると記憶している古い情報を消去して空きを作り、ここに新しい情報を保存する。即ち、メモリ２１には現在から一定時間過去までの情報が連続的に保存されている。
【００２８】
次に、状態推定部２２は、メモリ２１から情報を取得し車両の状態量を推定する（ステップ３０４）。ここでは、ふらつき判定部２５からのふらつきの判定結果を取得し、ふらつきの有無により車両の状態量を推定するための計算方法を調整しても良い。状態推定部２２は過去のデータから推移を抽出して現在の値を計算するが、計算方法については限定しない。本実施例では、移動平均フィルタを用いて平均値を算出し、これを現在の値の推定値とした。結果として状態推定部２２は、仮想車両の横方向Ｇ，仮想前後方向Ｇ，仮想ヨー角を計算して出力する。
【００２９】
次に、ふらつき判定部２５は、運転状態ファクタであるハンドル操作角の触れ量から、ふらつきが有るか否かを判定する（ステップ３０５）。
【００３０】
図７は、ステップ３０５の詳細を示すフロー図である。
【００３１】
まず、ふらつき判定部２５内のハンドルの操作角度の判定値を格納するバッファの格納位置を示すポインタをシフトしてバッファの現時刻のデータが入る領域を空にする（ステップ７０１）。次に、ハンドルの操作角度に最小二乗法等を用いて曲線をフィッティングする（ステップ７０２）。次に、フィッティングした曲線と現時刻のハンドルの操作角度との差を算出する（ステップ７０３）。次に、差が所定値より大きいか否かを判断し（ステップ７０４）、大きい場合にはバッファに１を格納し（ステップ７０５）、小さい場合にはバッファに０を格納する（ステップ７０６）。バッファの過去２００ｍｓ間のデータを参照し（ステップ７０７）、全て０か否かを判定し（ステップ７０８）、全て０ならばふらつき有りとし（ステップ７０９）、一つでも１があれば、ハンドルの操舵が行われていると判定し、ふらつき無し（操舵が行われている）と判定する（ステップ７１０）。
【００３２】
図８は、ふらつきが有る場合の操作角度の変化の例を示す図である。図はハンドルの操作角度８１とフィッティングした曲線８２、又、フィッティング曲線と現時刻のハンドルの操作角度との差が大きいか否かを判定する判定曲線８３，８４を示してある。図８の示す例の場合、ハンドルの操作角度はバッファ内全てのデータにおいて差が小さい。よって、この場合、ふらつきが有ると判定される。一方、図９に示す例の場合、ハンドルの別の操作角度５１Ｂは、差が大きいか否かを判定する判定曲線５４から外れるデータがある。よって、この場合、ふらつきが無いと判定される。ハンドルの操作角度からふらつきの判定を行った結果の例を図１０に示す。
【００３３】
次に、ふらつき判定部の出力を参照し（ステップ３０６）、ふらつきの無い場合は車両の状態量を選択し（ステップ３０７）、ふらつきの有る場合は車両の状態量の推定値を選択する（ステップ３０８）。
【００３４】
次に、選択された値から、積分や、方向の修正等の必要な演算を実施し、車両位置を算出し出力する（ステップ３０９）。ハンドルの操作角度から、ふらつきか否かを判断して演算を切り換える手法を用いた場合の車両位置の算出結果を図１１に示す。図６の従来の手法による演算結果と比較すると、直進中は正しく直進状態であることを演算できていて、車両の走行軌跡と演算結果が良く一致しており、車両位置の演算精度を向上できていることが分かる。
【００３５】
以上、本実施例によれば、内部情報計測部の情報から移動体の位置を演算する際、演算精度が悪化する原因となる移動体の進行方向に対して横方向の動きであるふらつきを検出し、ふらついている場合は移動体の状態の推定値をもとに移動体位置を演算し、ふらついていない場合は移動体の状態量をもとに移動体位置を演算する。即ち、移動体の特性を考慮して演算精度が悪化するような状態である部分の処理を別に実行し、移動体位置の演算精度を向上させることができる。
【００３６】
又、ふらつきを度合いとして算出し、ふらつき度合いに応じて、移動体の状態と、移動体の状態の推定値とを混合することで、ふらつきでは無い移動時の位置計算を迅速に行うことができる。又、ハンドルの操作角度を参照することで、ふらつきの一つの原因である運転者により近い部分での計測データをもとにふらつきを判断でき、より正確にふらつきを検出できる。又、運転者の状態を監視し、運転者の状態により、ふらついているか否かを判定する基準を変動させることで、より正確にふらつきを検出できる。更に、運転者の特性を記憶するメモリを有し、運転者の特性により、ふらついているか否かの判定の基準を変動させることで、より正確にふらつきを検出できる。
【実施例２】
【００３７】
図１２は、第二の位置演算装置の構成を示す図である。以下、第一の実施例と異なる部分について説明する。
【００３８】
第二の実施例では、外部情報計測部３０（外界センサ等）により、ＧＰＳデータ，カメラデータ，レーダデータが計測され、これらのデータが外部情報計測部３０からの位置演算部３１に入力される。又、ふらつき判定部２５にもこれら外部からのデータが入力される。外部情報計測部からの位置演算部３１の出力はメモリ２１に入力されるとともに位置選択部３３に入力される。位置計算部１２の出力と外部情報計測部からの位置演算器３１の出力を監視し、確からしいデータを選択するデータ選択器３２があり、データ選択器３２に出力が位置選択部３３の切り換えに信号３３Ｃに入力され、データ選択器の切り換えに信号３３Ｃに応じて、移動体の位置３３Ａと、外部情報計測部からの移動体の位置３３Ｂとを選択し、移動体位置として出力する構成となっている。
【００３９】
第二の実施例では、ふらつき判定部２５に運転状態ファクタとともに、外部情報計測部のデータも入力される。これにより、ふらつき判定する際に、例えば、道路の状況（曲率，傾斜等）を考慮してふらつきか否かを判定することができ、より一層、判定の精度を向上することができる。
【００４０】
又、メモリ２１にも外部情報計測部のデータが入力される。これにより状態推定器の計算も外部情報計測部から得られる情報を用いることができ、より一層、推定値の精度を向上することができる。
【００４１】
位置計算部１２の出力と外部情報計測部からの位置演算部３１の出力は位置選択部により、確からしいデータが選択されて車両位置として出力される。
【００４２】
以上、本実施例によれば、外部情報計測部からの情報も運転状態ファクタとして監視し、これにより、ふらついているか否かを判定する基準を変動させることで、より正確にふらつきを検出できる。
【実施例３】
【００４３】
図１３は、位置演算装置を衝突回避システムに搭載した例を示す図である。本実施例では、ハンドル角センサ２０１，ヨーレートセンサ２０２，加速度センサ２０３の内界センサとレーザセンサ２０４，ミリ波レーダ２０５の外界センサの値から制御装置２１０内の位置計算部２１１と外界情報計算部２１２で車両の位置や障害物の位置を計算し、衝突の可能性が高い場合に操舵装置やブレーキ装置を動作させ、衝突を回避するように車両を制御するシステムである。このシステムの制御装置２１０内の位置計算部２１１に位置演算装置を用いている。車両の位置や障害物の位置は行動決定部２１３に引き渡され、衝突回避動作の必要性が計算に用いられる。衝突回避動作の必要性がある場合には回避指令が出力され、運転支援部２１４を経て、操舵制御装置２３１，ブレーキ制御装置２３３，警報装置２３２を動作させて操舵アクチュエータ２３４，ブレーキ２３５を動かし、車両３００に衝突回避運動を行わせる。
【００４４】
衝突回避システムの動作において、車両に位置を計算する際に、位置演算装置を用いることで、ふらつきが発生している状況下においても車両の位置を精度良く計算できる。よって、衝突回避システムの衝突可能性の計算精度を向上させることができ、より信頼性の高いシステムを構築することができる。
【実施例４】
【００４５】
図１４は、位置演算装置をＧＰＳナビゲーション装置４０１に搭載した例を示す図である。本実施例では、外部情報計測部としてＧＰＳ２０６を持つ。ＧＰＳナビゲーション装置４０１は、ＧＰＳ情報からＧＰＳ情報処理部４０２により車両位置を計算する。又、ＧＰＳの電波が届かない場所や、交差点での車両の転回時などにおいては車両位置を把握することが難しくなるため、内部情報計測部１０による自律航法を用いている、この自律航法の位置計算部２１１に位置演算装置を用いる。計算した移動体の位置はナビゲーション部４０３に入力され、ナビに用いる情報に用いられ、用いられた位置情報を用いて表示制御部４０４は運転者の提示する情報を計算し、ディスプレイ４０５を通じて運転者に提供する。
【００４６】
ＧＰＳナビゲーション装置の動作において自律航法が必要である場合に、位置演算装置を用いることで、ふらつきが発生している状況下においても車両の位置を精度良く計算できる。よって、ＧＰＳナビゲーション装置の精度を向上させることができ、より信頼性のナビゲーション装置を構築することができる。
【図面の簡単な説明】
【００４７】
【図１】位置演算装置の構成を示す図。
【図２】車両の走行状態を説明する図。
【図３】移動体位置演算の処理フローを示す図。
【図４】ハンドル角度の取得値の一例を示す図。
【図５】移動体の横方向加速度の取得値の一例を示す図。
【図６】従来技術の、横方向の移動距離を求める手法の結果の一例を示す図。
【図７】ふらつき有無を判定する詳細な処理フローを示す図。
【図８】ふらつきが有る場合のハンドル操作角度の変化の一例を示す図。
【図９】ふらつきが無い場合のハンドル操作角度の変化の一例を示す図。
【図１０】ハンドル操作角度からふらつきの判定を行った結果の一例を示す図。
【図１１】移動体位置の算出結果一例を示す図。
【図１２】別の位置演算装置の構成を示す図。
【図１３】位置演算装置を衝突回避システムに搭載した構成を示す図。
【図１４】位置演算装置をＧＰＳナビゲーション装置に搭載した構成を示す図。
【符号の説明】
【００４８】
１０  内部情報計測部
１１  状態量計算部
１２  位置計算部
２１  メモリ
２２  状態推定部
２３  選択部
２５  ふらつき判定部
２６  運転状態ファクタ取得部
２７  運転者特性取得部
【図１】
2009115624.tif 000003
【図２】
2009115624.tif 000004
【図３】
2009115624.tif 000005
【図４】
2009115624.tif 000006
【図５】
2009115624.tif 000007
【図６】
2009115624.tif 000008
【図７】
2009115624.tif 000009
【図８】
2009115624.tif 000010
【図９】
2009115624.tif 000011
【図１０】
2009115624.tif 000012
【図１１】
2009115624.tif 000013
【図１２】
2009115624.tif 000014
【図１３】
2009115624.tif 000015
【図１４】
2009115624.tif 000016