位置検出装置
【課題】車速パルス信号を用いずに、移動体の現在位置を精度良く検出できる位置検出装置を提供する。
【解決手段】本発明に係る位置検出装置は、ロール角算出部１９は右側方向加速度センサ１１とヨー方向角速度センサ１６の出力信号に基づいてロール角を算出し、姿勢角算出処理部１３は、ピッチ方向角速度センサ１５、ヨー方向角速度センサ１６、ロール角算出部１９の出力信号に基づいて移動体の姿勢角を算出する。進行方向加速度センサ１０は移動体の進行方向の加速度を検出し、走行距離算出部１２は加速度センサ１０において検出された加速度と、姿勢角算出処理部１３において算出された移動体の姿勢角に基づいて移動体の走行距離を算出する。位置算出部１４は姿勢角算出処理部１３において算出された移動体の姿勢角と、走行距離算出部１２において算出された移動体の走行距離に基づいて移動体の現在位置を算出する。
【解決手段】本発明に係る位置検出装置は、ロール角算出部１９は右側方向加速度センサ１１とヨー方向角速度センサ１６の出力信号に基づいてロール角を算出し、姿勢角算出処理部１３は、ピッチ方向角速度センサ１５、ヨー方向角速度センサ１６、ロール角算出部１９の出力信号に基づいて移動体の姿勢角を算出する。進行方向加速度センサ１０は移動体の進行方向の加速度を検出し、走行距離算出部１２は加速度センサ１０において検出された加速度と、姿勢角算出処理部１３において算出された移動体の姿勢角に基づいて移動体の走行距離を算出する。位置算出部１４は姿勢角算出処理部１３において算出された移動体の姿勢角と、走行距離算出部１２において算出された移動体の走行距離に基づいて移動体の現在位置を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、加速度センサと角速度センサを用い、車両などの移動体の位置を検出する位置検出装置に関する。
【背景技術】
【０００２】
近年、車両に搭載され、出発地から目的地までの推奨経路に従って案内を行うナビゲーション装置が広く普及している。このナビゲーション装置では、トンネル、市街地、高架下等においてＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機が衛星からの電波を受信できない場合、加速度センサ、角速度センサおよび速度センサを含む自立センサと、走行速度に応じてパルス周期が変化する車速パルス信号とを用いて、現在位置を算出する。そして、水平面を基準とした２次元の地図上に現在位置を表示することが一般的である。
【０００３】
しかしながら、ＰＮＤ（Ｐｅｒｓｏｎａｌ／Ｐｏｒｔａｂｌｅ Ｎａｖｉｇａｔｉｏｎ
Ｄｅｖｉｃｅ）のようなポータブル型のナビゲーション装置は、通常、車外へ持ち運び可能に構成されるので、その構成上、車両への取付けは容易であるが、煩雑な取付け作業を必要とする車速パルス信号の出力は接続されないことが多く、車速パルス信号を得ることが難しい。さらに、ポータブル型のナビゲーション装置は、車外で利用される場合もあり、車速パルス信号が全く得られないこともある。従って、水平面を基準とした２次元の地図上に正確な現在位置を表示するためには、車速パルス信号なしで現在位置を算出する必要がある。
【０００４】
これを実現する従来技術として、図６に示すように、移動体１の進行方向の加速度を検出する第１の加速度センサ２と、移動体の右側方向を示す軸回りの角速度を検出する第１の角速度センサ３と、移動体の垂直下方向を示す軸回りの角速度を検出する第２の角速度センサ４とを備えるセンサ構成で現在位置を算出する技術が提案されている（例えば、特許文献１参照）。より具体的には、図７に示すように、車両１に設けられた第１の加速度センサ２を用いて車両進行方向の加速度Ａｘを検出する。そして、第１の角速度センサ３で検出した角速度Ｗｙの時間積分で求めた車両の傾斜角αを用いて重力加速度Ｇの進行方向成分の加速度Ａｇ（＝Ｇｓｉｎα）を算出する。車両の移動に伴う加速度Ａｃｃは、ベクトル換算でＡｃｃ＝Ａｘ＋Ａｇと表される。このため、Ａｃｃを時間で２階積分すれば、車両の走行距離を算出することができる。
そして、図８に示すように、第２の角速度センサ４で検出した角速度Ｗｚの時間積分で求めた車両１の方位角βを算出すれば、水平面を基準とした２次元の地図上における車両１の進行方向が分かり、上述した走行距離と合わせて車両の現在位置を算出することができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−２９２２４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、第１の角速度センサ３で検出した角速度Ｗｙで車両の傾斜角αを算出する場合、その他の２軸の角度（車両１の垂直下方向軸回り及び進行方向軸回りの角度）に伴う角速度を考慮する必要がある。同様に、第２の角速度センサ４で検出した角速度Ｗｚで車両１の方位角βを算出する場合も、その他の２軸の角度（車両１の右側方向回り及び
進行方向軸回りの角度）に伴う角速度を考慮する必要がある。これを角速度センサの他軸感度と呼ぶ。
【０００７】
より具体的には、図９に示すように、水平面に対して車両１が傾斜角αで傾くことによって、第２の角速度センサ４で検出した角速度Ｗｚは、Ｗｚ×ｃｏｓαが方位角βを算出するための角速度となり、Ｗｚ×ｓｉｎαは車両進行方向軸回りの角度を算出するための角速度に影響を及ぼす。
そのため、車両に設けられた角速度センサで正確に車両の角度（以下、姿勢角と称す）を算出するためには、３軸方向の角速度センサを必要とし、それぞれ検出した角速度と算出した姿勢角を用いて、水平面に対する角速度に変換し、他軸感度の影響を取り除く必要がある。
このため、上記特許文献１に開示された技術では、車両進行方向軸回りの角速度を検出する第３の角速度センサを備えていないため、その車両進行方向軸回りの角度（以下、ロール角と称す）に伴う他軸感度が誤差として残り、正確に車両の傾斜角αや方位角βを算出できなかった。
加速度センサと角速度センサのみでナビゲーション装置の位置検出装置を構成する場合、特に、ロール角に伴う角速度によって生じる傾斜角αの誤差が、最終的には、現在位置の誤差の大きな要因となる。より具体的には、傾斜角αに１［ｄｅｇ］の誤差が含まれると、車両の移動に伴う加速度Ａｃｃに約０．１７［ｍ／ｓ・ｓ］（＝Ｇｓｉｎ（１°））の誤差が生じることになり、２０秒間で約３４［ｍ］も現在位置がズレることになる。
従って、図１０に示すように、この現在位置の誤差によって、特に、交差点が連続するような場所では、車両１の右左折するタイミングを正確に案内できないため、ナビゲーション装置としては致命的なものとなる。
それ故、本発明は、従来の問題を解決するためになされたものであり、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角αを算出し、算出した傾斜角αを利用して移動体の現在位置を精度良く算出できる位置検出装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る位置検出装置は、移動体のピッチ方向の角速度を検出するピッチ方向角速度検出部と、前記移動体のヨー方向の角速度を検出するヨー方向角速度検出部と、前記移動体の進行方向の加速度を検出する進行方向加速度検出部と、前記移動体の右側方向の加速度を検出する右側方向加速度検出部と、前記ヨー方向角速度検出部が検出した角速度と前記右側方向加速度検出部が検出した加速度に基づいてロール角を算出するロール角算出部と、前記ピッチ方向角速度検出部と前記ヨー方向角速度検出部が検出した角速度と前記ロール角算出部が算出したロール角に基づいて前記移動体の姿勢角を算出する姿勢角算出処理部と、前記進行方向加速度検出部が検出した加速度と前記姿勢角算出処理部が算出した前記移動体の姿勢角に基づいて前記移動体の走行距離を算出する走行距離算出部と、前記姿勢角算出処理部が算出した前記移動体の姿勢角と前記走行距離算出部が算出した前記移動体の走行距離に基づいて前記移動体の現在位置を算出する位置算出部とを備える。
【発明の効果】
【０００９】
本発明によれば、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角を算出し、算出した傾斜角を利用して移動体の現在位置を精度良く算出する位置検出装置を提供できる。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施形態に係る位置検出装置の構成を示すブロック図
【図２】車両座標系を示した図
【図３】車両が走行する一般的な傾斜道路の傾斜角の一例を示した図
【図４】車両が一般的な傾斜道路を走行している様子と、そのときの傾斜角（ピッチ角、ロール角）の一例を示した図
【図５】位置検出装置の位置検出処理の流れを示したフロー図
【図６】従来の位置検出装置における構成を示す図
【図７】従来の位置検出装置の処理を説明するための図
【図８】従来の位置検出装置の処理を説明するための図
【図９】傾斜角に伴う角速度センサの他軸感度の一例を示す図
【図１０】従来の位置検出装置の車両位置を示す図
【発明を実施するための形態】
【００１１】
以下、図面を参照しながら、本発明の実施形態に係る位置検出装置について具体的に説明する。なお、本実施形態では、位置検出装置を備えるナビゲーション装置が、例えば移動体である自動車などの車両に搭載される場合を例に挙げて説明する。
【００１２】
まず図１を参照して、本発明の実施形態に係る位置検出装置の構成について説明する。図１は、本発明の実施形態に係る位置検出装置の構成を示すブロック図である。
【００１３】
図１において、進行方向加速度センサ１０と、右側方向加速度センサ１１は、それぞれ加速度を検出する加速度検出部であり、進行方向加速度センサ１０は進行方向の加速度を走行距離算出部１２に出力し、右側方向加速度センサ１１は右側方向の加速度をロール角算出部１９へ出力する。ピッチ方向角速度センサ１５と、ヨー方向角速度センサ１６は、それぞれ角速度を検出する角速度検出部であり、例えば振動型ジャイロや光ファイバジャイロ等のジャイロ装置によって構成されている。なお、車両用ナビゲーション装置に搭載される角速度センサとしては、振動型ジャイロ（安価のため）を用いる。ピッチ方向角速度センサ１５、ヨー方向角速度センサ１６は、姿勢角算出処理部１３に信号を出力する。また、ヨー方向角速度センサ１６は、ロール角算出部１９にも信号を出力する。姿勢角算出処理部１３は、ピッチ方向角速度センサ１５、ヨー方向角速度センサ１６、ロール角算出部１９の入力信号に基づいて、姿勢角を算出し、走行距離算出部１２と位置算出部１４に信号を出力する。走行距離算出部１２は、進行方向加速度センサ１０から検出される加速度に、姿勢角算出処理部１３から得られる姿勢角を用いて、走行距離を算出し、位置算出部１４へ出力している。
図２は、車両２０の車両座標系を示した図である。図２において、Ｘｂ軸は車両の進行方向を示す軸であり、Ｙｂ軸は車両の右側方向を示す軸であり、Ｚｂ軸はＸｂ−Ｙｂ軸平面に対して垂直下方向を示す軸である。そしてこれらＸｂ軸、Ｙｂ軸、Ｚｂ軸からなる座標系を、車両座標系とする。ピッチ方向角速度センサ１５は、図２に示すように、車両２０の車両座標系におけるＹｂ軸回りの角速度を検出する。また、ヨー方向角速度センサ１６は、車両座標系におけるＺｂ軸回りの角速度を検出する。以下では、ピッチ方向角速度センサ１５で検出された角速度をＷｂｙとし、ヨー方向角速度センサ１６で検出された角速度をＷｂｚとする。角速度Ｗｂｙと、角速度Ｗｂｚは、常に姿勢角算出処理部１３へ出力されている。また、角速度Ｗｂｚは、常にロール角算出部１９へも出力されている。
【００１４】
進行方向加速度センサ１０と、右側方向加速度センサ１１は、加速度を検出する加速度検出部であり、例えば圧電型、静電容量型やサーボ型等の加速度検出装置によって構成されている。進行方向加速度センサ１０は、図２に示すように、車両座標系におけるＸｂ軸方向の加速度を検出する。また、右側方向加速度センサ１１は、車両座標系におけるＹｂ軸方向の加速度を検出する。以下では、進行方向加速度センサ１０で検出された加速度をＡｘとし、右側方向加速度センサ１１で検出された加速度をＡｙとする。加速度Ａｘは、常に走行距離算出部１２に出力されている。また、加速度Ａｙは、常にロール角算出部１９に出力されている。
ロール角算出部１９は、右側方向加速度センサ１１で検出された加速度Ａｙから、車両の
姿勢角θｔ（ロール角θｘｔ、ピッチ角θｙｔ、ヨー角θｚｔ）の１つであるロール角θｘｔを算出する。加速度Ａｙは、式（１）に示すように、遠心加速度Ａｗと、重力加速度Ｇの右側方向成分Ａｙｇ（＝Ｇｓｉｎθｘｔｃｏｓθｙｔ）のベクトル和で表される。ここで、車両はスリップ走行しないと仮定し、車両の移動に伴う右側方向加速度は無視している。遠心加速度Ａｗは、車両が回転したときに生じる加速度であり、走行距離算出部１２から得られる車両座標系におけるＸｂ軸方向の速度Ｖｂｘと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚとから算出する。なお、車両座標系におけるＸｂ軸方向の速度Ｖｂｘの詳細は、後述する。ここで、もし車両が停止していれば、この遠心加速度Ａｗは生じない。また、車両は道路から跳ね上がることはないと仮定し、車両座標系におけるＺｂ軸方向の速度Ｖｂｚをゼロとするため、進行方向軸の回転に伴う遠心加速度は無視している。
（式１）
Ａｙ＝Ａｗ＋Ａｙｇ
Ａｗ＝ｋ（Ｖｂｘ、Ｗｂｚ）
ところで、図３に示すように、車両２０が走行する一般的な傾斜道路の傾斜角は小さく、車両の姿勢角θｔのうち、車両進行方向軸回り（Ｘｂ軸回り）の角度であるロール角θｘｔは１０［ｄｅｇ］未満であり、車両右側方向軸回り（Ｙｂ軸回り）の角度であるピッチ角θｙｔは２０[ｄｅｇ]未満である。
ここで、本発明者は、車両が走行する一般的な道路では、ロール角θｘｔおよびピッチ角θｙｔが小さいことから、ロール角θｘｔもしくはピッチ角θｙｔの三角関数を数値的に近似しても、誤差が生じないことに着目した。ロール角θｘｔもしくはピッチ角θｙｔの三角関数の数値的近似とは、ロール角θｘｔの例でいえば、ｃｏｓθｘｔ＝１、ｓｉｎθｙｔ＝θｙｔである。
ロール角算出部１９は、この着目点に基づいて構築されてたものであり、式（２）に示すように、加速度Ａｙから遠心加速度Ａｗを減算した結果を、重力加速度Ｇで除算することによってロール角θｘｔを算出する。
（式２）
θｘｔ＝（Ａｙ−ｋ（Ｖｂｘ、Ｗｂｚ））／Ｇ
姿勢角算出処理部１３は、姿勢角制御部１７と、姿勢角演算部１８とで構成されている。姿勢角演算部１８は、通常、式（３）に示すように、ピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、ロール角算出部１９から得られるロール角θｘ０（＝θｘｔ）と、自身に設定されたピッチ角θｙ０およびヨー角θｚ０を用いて、水平面に対する角速度Ｗｌにそれぞれ変換し、その角速度Ｗｌを更新時間Δｔで時間積分して、ピッチ角およびヨー角の変化量をぞれぞれ算出する。そして、ピッチ角θ０に算出したピッチ角の変化量Δθｙを加え、ピッチ角θｙｔを算出する。また、ヨー角θｚ０に算出したヨー角の変化量Δθｚを加え、ヨー角θｚｔを算出する。
さらに更新時間Δｔ後、ロール角算出部１９から得られるロール角θｘｔと、前回算出したピッチ角θｙｔおよびヨー角θｚｔをそれぞれ姿勢角θ０（ロール角θｘ０、ピッチ角θｙ０、ヨー角θｚ０）として、上述したように、角速度Ｗｂｙと角速度Ｗｂｚを姿勢角θ０を用いて角速度Ｗ１に変換し、ピッチ角およびヨー角の変化量を算出する。そして、ピッチ角θ０に算出したピッチ角の変化量Δθｙを加え、ピッチ角θｙｔを算出する。また、ヨー角θｚ０に算出したヨー角の変化量Δθｚを加え、ヨー角θｚｔを算出する。この処理は、更新時間Δｔ毎に繰り返し行われる。
（式３）
Ｗｌｙ＝ｆ（Ｗｂｙ、Ｗｂｚ、θｘ０）
Ｗｌｚ＝ｇ（Ｗｂｙ、Ｗｂｚ、θｘ０、θｙ０）
Δθｙ＝（Ｗｌｙ×Δｔ）
Δθｚ＝（Ｗｌｚ×Δｔ）
θｙｔ＝θｙ０＋Δθｙ
θｚｔ＝θｚ０＋Δθｚ
ところで、図４に示すように、車両２０が走行する一般的な傾斜道路は傾斜角一定である複数の所定区間で構成されており、その隣り合う所定区間の傾斜角度の差は小さく、その差は１[ｄｅｇ]未満である。さらに、ピッチ方向角速度センサ１５で検出される角速度Ｗｂｙは、３[ｄｅｇ／ｓ]未満である。図４は、車両２０が一般的な傾斜道路を走行している様子の一例を示した図であり、（ａ）は側面図、（ｂ）は正面図である。図４の例では、傾斜道路は位置Ｘ１から位置Ｘ２までの第１区間、位置Ｘ２から位置Ｘ３までの第２区間、位置Ｘ３から位置Ｘ４までの第３区間で構成されている。第１区間におけるロール角はθｘ１、ピッチ角はθｙ１で一定となり、第２区間におけるロール角はθｘ２、ピッチ角はθｙ２で一定となり、第３区間におけるロール角はθｘ１、ピッチ角はθｙ１で一定となっている。従って、図４の例でいえば、ロール角θｘ１とロール角θｘ２との差およびピッチ角θｙ１とピッチ角θｙ２がそれぞれ１［ｄｅｇ］未満となっており、位置Ｘ２および位置Ｘ３の傾斜道路の傾斜角の変化により、車両右側方向軸（Ｙｂ軸）回りに角速度Ｗｂｙが出力されるが、その絶対値は小さいことが分かる。
また、上述したように、車両２０が走行する一般的な傾斜道路の傾斜角は小さく、車両２０の姿勢角θｔのうち、車両進行方向軸回り（Ｘｂ軸回り）の角度であるロール角θｘｔは１０［ｄｅｇ］未満であり、車両右側方向軸回り（Ｙｂ軸回り）の角度であるピッチ角θｙｔは２０[ｄｅｇ]未満である。
ここで、本発明者は、車両２０が走行する一般的な道路では、ピッチ方向角速度センサ１５で検出される角速度Ｗｂｙ、ロール角θｘｔおよびピッチ角θｙｔが小さいことから、姿勢角演算部１８でピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、姿勢角θ０を用いて角速度Ｗｌに変換する演算式において、角速度Ｗｂｙ、ピッチ角θｙ０およびロール角θｘ０のうち、何れかを２つ以上含む項をゼロとしても、ピッチ角θｙｔおよびヨー角θｚｔの算出に誤差が生じないことに着目した。
また、本発明者は、車両２０が走行する一般的な道路では、ロール角θｘｔおよびピッチ角θｙｔが小さいことから、姿勢角演算部１８でピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、姿勢角θ０を用いて角速度Ｗ１に変換する演算式において、ロール角θｘ０もしくはピッチ角θｙ０の三角関数を数値的に近似しても、ピッチ角θｙｔおよびヨー角θｚｔの算出に誤差が生じないことに着目した。ロール角θｘ０もしくはピッチ角θｙ０の三角関数の数値的近似とは、ロール角θｘ０の例でいえば、ｃｏｓθｘ０＝１、ｓｉｎθｘ０＝θｘ０である。姿勢角制御部１７は、これらの着目点に基づいて構築されたものであり、姿勢角演算部１８でピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂzを、姿勢角θ０を用いて角速度Ｗｌに変換する演算式において、ピッチ方向角速度センサ１５で検出された角速度Ｗｂｙ、ロール角算出部１９から得られるロール角θｘ０、もしくは、姿勢角演算部１８で算出されるピッチ角θｙ０を含む項を、予め数値的に近似する。そして、姿勢角制御部１７で数値的近似によって制御された結果は、姿勢角演算部１８へ出力されている。
また、姿勢角制御部１７は、ロール角算出部１９から得られるロール角θｘｔと、姿勢角演算部１８で算出されるピッチ角θｔがそれぞれの上限値を超えた場合、その上限値をロール角θｘｔとピッチ角θｙｔの算出結果とするように制御する。ここで、ロール角θｘｔの上限値は、例えば１０［ｄｅｇ］であり、ピッチ角θｙｔの上限値は、例えば２０［ｄｅｇ］である。具体的な姿勢角算出処理部１３の算出処理については、後述する。
なお、姿勢角算出処理部１３の算出結果は、走行距離算出部１２と、位置算出部１４へ出力されている。
【００１５】
走行距離算出部１２は、式（４）に示すように、進行方向加速度センサ１０から検出される加速度Ａｘに、姿勢角算出処理部１３から得られるピッチ角θｙｔを用いて算出した重力加速度Ｇの進行方向成分の加速度Ａｇ（＝Ｇｓｉｎθｙｔ）を加え、更新時間Δｔで
時間積分して、車両座標系におけるＸｂ軸方向の速度Ｖｂｘを算出する。速度Ｖｂｘは、更新時間Δｔ毎に繰り返し算出され、ロール角算出部１９へ出力されている。
（式４）
Ｖｂｘ＝（Ａｘ＋Ａｇ）×Δｔ
また、走行距離算出部１２は、式（５）に示すように、車両座標系におけるＸｂ軸方向の速度Ｖｂｘを、更新時間Δｔで時間積分して、走行距離ΔＸを算出する。走行距離ΔＸは、更新時間Δｔ毎に繰り返し算出され、位置算出部１４へ出力されている。
（式５）
ΔＸ＝Ｖｂｘ×Δｔ
位置算出部１４は、式（６）に示すように、姿勢角算出処理部１３から得られる姿勢角θｔのうち、垂直下方向軸回り（Ｚｂ軸回り）の角度であるヨー角θｚｔ（＝方位角）を用いて、走行距離算出部１２から得られる走行距離ΔＸを各成分に分解する。そして、走行距離ΔＸの各成分を、自身に設定された位置Ｒ０（Ｒｘ０、Ｒｙ０）に加え、位置Ｒｔ（Ｒｘｔ、Ｒｙｔ）を算出する。
更新時間Δｔ後、位置Ｒｔを位置Ｒ０として、上述したように、位置Ｒ０にヨー角θｚｔを用いて分解した走行距離ΔＸの各成分を加え、位置Ｒｔを算出する。この処理は、更新時間Δｔ毎に繰り返し行われる。
（式６）
Ｒｘｔ＝Ｒｘ０＋ΔＸｃｏｓθｚｔ
Ｒｙｔ＝Ｒｙ０＋ΔＸｓｉｎθｚｔ
＜位置検出装置の処理動作＞
図５を参照して、位置検出装置の位置検出処理の動作について説明する。図５は、位置検出装置の位置検出処理の流れを示したフロー図である。
図５において、位置検出装置の位置検出処理を開始すると、姿勢角算出処理部１３は、ピッチ方向角速度センサ１５で検出されたＷｂｙと、ヨー方向角速度センサ１６で検出されたＷｂｚを例えば１０［ｍｓ］でサンプリングする（ステップ１１）。これが、車両の姿勢角θｔ、車両座標系におけるＸｂ軸方向の速度Ｖｂｘ、走行距離ΔＸおよび位置Ｒｔの更新時間Δｔである。
ステップＳ１１の次に、姿勢角算出処理部１３は、姿勢角制御部１７において、ロール角算出部１９から得られるロール角θｘ０もしくは姿勢角演算部１８から得られるピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似する。そして、角速度Ｗｂｙと角速度Ｗｂｙ、ロール角θｘ０、ピッチ角θｙ０の乗算、ロール角θｘ０とロール角θｘ０、ピッチ角θｙ０の乗算、ピッチ角θｙ０とピッチ角θｙ０の乗算をゼロに近似する。そして、姿勢角演算部１８は、姿勢角制御部１７で出力された数値的近似値を用いて、式（３）の角速度Ｗｂｙと角速度Ｗｂｚから角速度Ｗｌｙと角速度Ｗｌｚへの変換式を簡略化し、式（７）に基づいて、水平面に対する角速度Ｗｌをそれぞれ算出する（ステップ１２）。
(式７)
Ｗｌｙ＝ｉ（Ｗｂｙ、Ｗｂｚ、θｘ０）
Ｗｌｚ＝ｊ（Ｗｂｚ）
そして姿勢角算出処理部１３は、姿勢角演算部１８において、式（３）に基づいて、変換した水平面の角速度Ｗｌを更新時間Δｔで時間積分して、ピッチ角およびヨー角の変化量をそれぞれ算出する（ステップＳ１３）。
ステップＳ１３の次に、姿勢角算出処理部１３は、姿勢角演算部１８において、式（３）に基づいて、自身に設定されたピッチ角θｙ０に算出したピッチ角の変化量Δθｙを加え、ピッチ角を算出する。また、姿勢角算出処理部１３は、姿勢角演算部１８において、自身に設定されたヨー角θｚｔに算出したヨー角の変化量Δθｚを算出する（ステップＳ１４）。
ステップＳ１４の次に、走行距離算出部１２は、進行方向加速度センサ１０で検出された加速度Ａｘを、車両の姿勢角θｔ（ロール角θｘｔ、ピッチ角θｙｔ、ヨー角θｚｔ）と
同様の更新時間Δｔでサンプリングする（ステップＳ１５）。
ステップＳ１５の次に、走行距離算出部１２は、式（４）に基づいて、加速度Ａｘに姿勢角算出処理部１３から得られるピッチ角θｙｔを用いて算出した重力加速度Ｇの進行方向成分の加速度Ａｇを加え、更新時間Δｔで時間積分して、車両座標系におけるＸｂ軸方向の速度を算出する（ステップＳ１６）。
ステップＳ１６の次に、走行距離算出部１２は、式（５）に基づいて、車両座標系におけるＸｂ軸方向の速度Ｖｂｘを、更新時間Δｔで時間積分して、走行距離ΔＸを算出する（ステップＳ１７）。
【００１６】
ステップＳ１７の次に、位置算出部１４は、式（６）に基づいて、姿勢角算出処理部１３から得られるヨー角θｚを用いて、走行距離算出部１２から得られる走行距離ΔＸを各成分に分解する。そして、走行距離ΔＸの各成分を、自身に設定された位置Ｒ０（Ｒｘ０、Ｒｙ０）に加え、位置Ｒｔ（Ｒｘｔ、Ｒｙｔ）を算出する（ステップＳ１８）。
ステップＳ１８の次に、ロール角算出部１９は、右側方向加速度センサ１１で検出された加速度Ａｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、車両の姿勢角θｔと同様の更新時間Δｔでサンプリングする（ステップＳ１９）。
ロール角算出部１９は、角速度Ｗｂｚと、走行距離算出部１２から得られる車両座標系におけるＸｂ軸方向の速度Ｖｂｘとから、遠心加速度Ａｗを算出した後、加速度Ａｙから遠心加速度Ａｗを減算した結果を、重力加速度Ｇで除算することによってロール角θｘｔを算出する（ステップＳ２０）。ロール角算出部１９は、算出したロール角θｘｔをθｘ０として、姿勢角算出処理部１３へ出力する。その後、処理はステップＳ１１へ戻る。
【００１７】
以上の説明より明らかなように、本発明の実施の形態では、姿勢角算出処理部１３は、ピッチ方向角速度センサ１５とヨー方向角速度センサ１６が検出した角速度とロール角算出部１９が算出したロール角に基づいて移動体の姿勢角を算出するので、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角を算出し、算出した傾斜角を利用して移動体の現在位置を精度良く算出することができる。
【００１８】
また、ロール角算出部１９は、移動体の移動に伴う右側方向加速度及び進行方向軸の回転に伴う遠心加速度を０とし、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、演算しているので、進行方向軸回りの角速度センサがなくても、移動体の現在位置を精度良く算出することができる。
【００１９】
また、姿勢角算出処理部１３は、移動体の進行方向軸回り（ロール方向）と右側方向軸回り（ピッチ方向）の角度と角速度が小さいことを利用し、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、姿勢角を算出するので、演算処理を複雑に行わなくても移動体の現在位置を精度良く算出することができる。
【００２０】
また、姿勢角算出処理部１３は、角速度Ｗｂｙと角速度Ｗｂｙ、ロール角θｘ０、ピッチ角θｙ０の乗算、ロール角θｘ０とロール角θｘ０、ピッチ角θｙ０の乗算、ピッチ角θｙ０とピッチ角θｙ０の乗算をゼロに近似して、演算するので、演算処理数を少なくしても、移動体の現在位置を精度良く算出することができる。
【００２１】
また、姿勢角算出処理部１３は移動体の姿勢角を算出するが、入力値である角速度は振動型ジャイロで検出しているので、誤差が大きくなる可能性があり、ロール角θｘ０とピッチ角θｙ０が上限値を超えた場合、誤差が含まれていると判断し、上限値を走行距離算出部１２と位置算出部１４に出力するので、移動体の現在位置を精度良く算出することができる。
【産業上の利用可能性】
【００２２】
本発明に係る位置検出装置は、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角を算出し、算出した傾斜角を利用して移動体の現在位置を精度良く算出し、車両に搭載されるナビゲーション装置等に適用される。
【符号の説明】
【００２３】
１０ 進行方向加速度センサ
１１ 右側方向加速度センサ
１２ 走行距離算出部
１３ 姿勢角算出処理部
１４ 位置算出部
１５ ピッチ方向角速度センサ
１６ ヨー方向角速度センサ
１９ ロール角算出部
【技術分野】
【０００１】
本発明は、加速度センサと角速度センサを用い、車両などの移動体の位置を検出する位置検出装置に関する。
【背景技術】
【０００２】
近年、車両に搭載され、出発地から目的地までの推奨経路に従って案内を行うナビゲーション装置が広く普及している。このナビゲーション装置では、トンネル、市街地、高架下等においてＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機が衛星からの電波を受信できない場合、加速度センサ、角速度センサおよび速度センサを含む自立センサと、走行速度に応じてパルス周期が変化する車速パルス信号とを用いて、現在位置を算出する。そして、水平面を基準とした２次元の地図上に現在位置を表示することが一般的である。
【０００３】
しかしながら、ＰＮＤ（Ｐｅｒｓｏｎａｌ／Ｐｏｒｔａｂｌｅ Ｎａｖｉｇａｔｉｏｎ
Ｄｅｖｉｃｅ）のようなポータブル型のナビゲーション装置は、通常、車外へ持ち運び可能に構成されるので、その構成上、車両への取付けは容易であるが、煩雑な取付け作業を必要とする車速パルス信号の出力は接続されないことが多く、車速パルス信号を得ることが難しい。さらに、ポータブル型のナビゲーション装置は、車外で利用される場合もあり、車速パルス信号が全く得られないこともある。従って、水平面を基準とした２次元の地図上に正確な現在位置を表示するためには、車速パルス信号なしで現在位置を算出する必要がある。
【０００４】
これを実現する従来技術として、図６に示すように、移動体１の進行方向の加速度を検出する第１の加速度センサ２と、移動体の右側方向を示す軸回りの角速度を検出する第１の角速度センサ３と、移動体の垂直下方向を示す軸回りの角速度を検出する第２の角速度センサ４とを備えるセンサ構成で現在位置を算出する技術が提案されている（例えば、特許文献１参照）。より具体的には、図７に示すように、車両１に設けられた第１の加速度センサ２を用いて車両進行方向の加速度Ａｘを検出する。そして、第１の角速度センサ３で検出した角速度Ｗｙの時間積分で求めた車両の傾斜角αを用いて重力加速度Ｇの進行方向成分の加速度Ａｇ（＝Ｇｓｉｎα）を算出する。車両の移動に伴う加速度Ａｃｃは、ベクトル換算でＡｃｃ＝Ａｘ＋Ａｇと表される。このため、Ａｃｃを時間で２階積分すれば、車両の走行距離を算出することができる。
そして、図８に示すように、第２の角速度センサ４で検出した角速度Ｗｚの時間積分で求めた車両１の方位角βを算出すれば、水平面を基準とした２次元の地図上における車両１の進行方向が分かり、上述した走行距離と合わせて車両の現在位置を算出することができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−２９２２４８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、第１の角速度センサ３で検出した角速度Ｗｙで車両の傾斜角αを算出する場合、その他の２軸の角度（車両１の垂直下方向軸回り及び進行方向軸回りの角度）に伴う角速度を考慮する必要がある。同様に、第２の角速度センサ４で検出した角速度Ｗｚで車両１の方位角βを算出する場合も、その他の２軸の角度（車両１の右側方向回り及び
進行方向軸回りの角度）に伴う角速度を考慮する必要がある。これを角速度センサの他軸感度と呼ぶ。
【０００７】
より具体的には、図９に示すように、水平面に対して車両１が傾斜角αで傾くことによって、第２の角速度センサ４で検出した角速度Ｗｚは、Ｗｚ×ｃｏｓαが方位角βを算出するための角速度となり、Ｗｚ×ｓｉｎαは車両進行方向軸回りの角度を算出するための角速度に影響を及ぼす。
そのため、車両に設けられた角速度センサで正確に車両の角度（以下、姿勢角と称す）を算出するためには、３軸方向の角速度センサを必要とし、それぞれ検出した角速度と算出した姿勢角を用いて、水平面に対する角速度に変換し、他軸感度の影響を取り除く必要がある。
このため、上記特許文献１に開示された技術では、車両進行方向軸回りの角速度を検出する第３の角速度センサを備えていないため、その車両進行方向軸回りの角度（以下、ロール角と称す）に伴う他軸感度が誤差として残り、正確に車両の傾斜角αや方位角βを算出できなかった。
加速度センサと角速度センサのみでナビゲーション装置の位置検出装置を構成する場合、特に、ロール角に伴う角速度によって生じる傾斜角αの誤差が、最終的には、現在位置の誤差の大きな要因となる。より具体的には、傾斜角αに１［ｄｅｇ］の誤差が含まれると、車両の移動に伴う加速度Ａｃｃに約０．１７［ｍ／ｓ・ｓ］（＝Ｇｓｉｎ（１°））の誤差が生じることになり、２０秒間で約３４［ｍ］も現在位置がズレることになる。
従って、図１０に示すように、この現在位置の誤差によって、特に、交差点が連続するような場所では、車両１の右左折するタイミングを正確に案内できないため、ナビゲーション装置としては致命的なものとなる。
それ故、本発明は、従来の問題を解決するためになされたものであり、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角αを算出し、算出した傾斜角αを利用して移動体の現在位置を精度良く算出できる位置検出装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る位置検出装置は、移動体のピッチ方向の角速度を検出するピッチ方向角速度検出部と、前記移動体のヨー方向の角速度を検出するヨー方向角速度検出部と、前記移動体の進行方向の加速度を検出する進行方向加速度検出部と、前記移動体の右側方向の加速度を検出する右側方向加速度検出部と、前記ヨー方向角速度検出部が検出した角速度と前記右側方向加速度検出部が検出した加速度に基づいてロール角を算出するロール角算出部と、前記ピッチ方向角速度検出部と前記ヨー方向角速度検出部が検出した角速度と前記ロール角算出部が算出したロール角に基づいて前記移動体の姿勢角を算出する姿勢角算出処理部と、前記進行方向加速度検出部が検出した加速度と前記姿勢角算出処理部が算出した前記移動体の姿勢角に基づいて前記移動体の走行距離を算出する走行距離算出部と、前記姿勢角算出処理部が算出した前記移動体の姿勢角と前記走行距離算出部が算出した前記移動体の走行距離に基づいて前記移動体の現在位置を算出する位置算出部とを備える。
【発明の効果】
【０００９】
本発明によれば、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角を算出し、算出した傾斜角を利用して移動体の現在位置を精度良く算出する位置検出装置を提供できる。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施形態に係る位置検出装置の構成を示すブロック図
【図２】車両座標系を示した図
【図３】車両が走行する一般的な傾斜道路の傾斜角の一例を示した図
【図４】車両が一般的な傾斜道路を走行している様子と、そのときの傾斜角（ピッチ角、ロール角）の一例を示した図
【図５】位置検出装置の位置検出処理の流れを示したフロー図
【図６】従来の位置検出装置における構成を示す図
【図７】従来の位置検出装置の処理を説明するための図
【図８】従来の位置検出装置の処理を説明するための図
【図９】傾斜角に伴う角速度センサの他軸感度の一例を示す図
【図１０】従来の位置検出装置の車両位置を示す図
【発明を実施するための形態】
【００１１】
以下、図面を参照しながら、本発明の実施形態に係る位置検出装置について具体的に説明する。なお、本実施形態では、位置検出装置を備えるナビゲーション装置が、例えば移動体である自動車などの車両に搭載される場合を例に挙げて説明する。
【００１２】
まず図１を参照して、本発明の実施形態に係る位置検出装置の構成について説明する。図１は、本発明の実施形態に係る位置検出装置の構成を示すブロック図である。
【００１３】
図１において、進行方向加速度センサ１０と、右側方向加速度センサ１１は、それぞれ加速度を検出する加速度検出部であり、進行方向加速度センサ１０は進行方向の加速度を走行距離算出部１２に出力し、右側方向加速度センサ１１は右側方向の加速度をロール角算出部１９へ出力する。ピッチ方向角速度センサ１５と、ヨー方向角速度センサ１６は、それぞれ角速度を検出する角速度検出部であり、例えば振動型ジャイロや光ファイバジャイロ等のジャイロ装置によって構成されている。なお、車両用ナビゲーション装置に搭載される角速度センサとしては、振動型ジャイロ（安価のため）を用いる。ピッチ方向角速度センサ１５、ヨー方向角速度センサ１６は、姿勢角算出処理部１３に信号を出力する。また、ヨー方向角速度センサ１６は、ロール角算出部１９にも信号を出力する。姿勢角算出処理部１３は、ピッチ方向角速度センサ１５、ヨー方向角速度センサ１６、ロール角算出部１９の入力信号に基づいて、姿勢角を算出し、走行距離算出部１２と位置算出部１４に信号を出力する。走行距離算出部１２は、進行方向加速度センサ１０から検出される加速度に、姿勢角算出処理部１３から得られる姿勢角を用いて、走行距離を算出し、位置算出部１４へ出力している。
図２は、車両２０の車両座標系を示した図である。図２において、Ｘｂ軸は車両の進行方向を示す軸であり、Ｙｂ軸は車両の右側方向を示す軸であり、Ｚｂ軸はＸｂ−Ｙｂ軸平面に対して垂直下方向を示す軸である。そしてこれらＸｂ軸、Ｙｂ軸、Ｚｂ軸からなる座標系を、車両座標系とする。ピッチ方向角速度センサ１５は、図２に示すように、車両２０の車両座標系におけるＹｂ軸回りの角速度を検出する。また、ヨー方向角速度センサ１６は、車両座標系におけるＺｂ軸回りの角速度を検出する。以下では、ピッチ方向角速度センサ１５で検出された角速度をＷｂｙとし、ヨー方向角速度センサ１６で検出された角速度をＷｂｚとする。角速度Ｗｂｙと、角速度Ｗｂｚは、常に姿勢角算出処理部１３へ出力されている。また、角速度Ｗｂｚは、常にロール角算出部１９へも出力されている。
【００１４】
進行方向加速度センサ１０と、右側方向加速度センサ１１は、加速度を検出する加速度検出部であり、例えば圧電型、静電容量型やサーボ型等の加速度検出装置によって構成されている。進行方向加速度センサ１０は、図２に示すように、車両座標系におけるＸｂ軸方向の加速度を検出する。また、右側方向加速度センサ１１は、車両座標系におけるＹｂ軸方向の加速度を検出する。以下では、進行方向加速度センサ１０で検出された加速度をＡｘとし、右側方向加速度センサ１１で検出された加速度をＡｙとする。加速度Ａｘは、常に走行距離算出部１２に出力されている。また、加速度Ａｙは、常にロール角算出部１９に出力されている。
ロール角算出部１９は、右側方向加速度センサ１１で検出された加速度Ａｙから、車両の
姿勢角θｔ（ロール角θｘｔ、ピッチ角θｙｔ、ヨー角θｚｔ）の１つであるロール角θｘｔを算出する。加速度Ａｙは、式（１）に示すように、遠心加速度Ａｗと、重力加速度Ｇの右側方向成分Ａｙｇ（＝Ｇｓｉｎθｘｔｃｏｓθｙｔ）のベクトル和で表される。ここで、車両はスリップ走行しないと仮定し、車両の移動に伴う右側方向加速度は無視している。遠心加速度Ａｗは、車両が回転したときに生じる加速度であり、走行距離算出部１２から得られる車両座標系におけるＸｂ軸方向の速度Ｖｂｘと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚとから算出する。なお、車両座標系におけるＸｂ軸方向の速度Ｖｂｘの詳細は、後述する。ここで、もし車両が停止していれば、この遠心加速度Ａｗは生じない。また、車両は道路から跳ね上がることはないと仮定し、車両座標系におけるＺｂ軸方向の速度Ｖｂｚをゼロとするため、進行方向軸の回転に伴う遠心加速度は無視している。
（式１）
Ａｙ＝Ａｗ＋Ａｙｇ
Ａｗ＝ｋ（Ｖｂｘ、Ｗｂｚ）
ところで、図３に示すように、車両２０が走行する一般的な傾斜道路の傾斜角は小さく、車両の姿勢角θｔのうち、車両進行方向軸回り（Ｘｂ軸回り）の角度であるロール角θｘｔは１０［ｄｅｇ］未満であり、車両右側方向軸回り（Ｙｂ軸回り）の角度であるピッチ角θｙｔは２０[ｄｅｇ]未満である。
ここで、本発明者は、車両が走行する一般的な道路では、ロール角θｘｔおよびピッチ角θｙｔが小さいことから、ロール角θｘｔもしくはピッチ角θｙｔの三角関数を数値的に近似しても、誤差が生じないことに着目した。ロール角θｘｔもしくはピッチ角θｙｔの三角関数の数値的近似とは、ロール角θｘｔの例でいえば、ｃｏｓθｘｔ＝１、ｓｉｎθｙｔ＝θｙｔである。
ロール角算出部１９は、この着目点に基づいて構築されてたものであり、式（２）に示すように、加速度Ａｙから遠心加速度Ａｗを減算した結果を、重力加速度Ｇで除算することによってロール角θｘｔを算出する。
（式２）
θｘｔ＝（Ａｙ−ｋ（Ｖｂｘ、Ｗｂｚ））／Ｇ
姿勢角算出処理部１３は、姿勢角制御部１７と、姿勢角演算部１８とで構成されている。姿勢角演算部１８は、通常、式（３）に示すように、ピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、ロール角算出部１９から得られるロール角θｘ０（＝θｘｔ）と、自身に設定されたピッチ角θｙ０およびヨー角θｚ０を用いて、水平面に対する角速度Ｗｌにそれぞれ変換し、その角速度Ｗｌを更新時間Δｔで時間積分して、ピッチ角およびヨー角の変化量をぞれぞれ算出する。そして、ピッチ角θ０に算出したピッチ角の変化量Δθｙを加え、ピッチ角θｙｔを算出する。また、ヨー角θｚ０に算出したヨー角の変化量Δθｚを加え、ヨー角θｚｔを算出する。
さらに更新時間Δｔ後、ロール角算出部１９から得られるロール角θｘｔと、前回算出したピッチ角θｙｔおよびヨー角θｚｔをそれぞれ姿勢角θ０（ロール角θｘ０、ピッチ角θｙ０、ヨー角θｚ０）として、上述したように、角速度Ｗｂｙと角速度Ｗｂｚを姿勢角θ０を用いて角速度Ｗ１に変換し、ピッチ角およびヨー角の変化量を算出する。そして、ピッチ角θ０に算出したピッチ角の変化量Δθｙを加え、ピッチ角θｙｔを算出する。また、ヨー角θｚ０に算出したヨー角の変化量Δθｚを加え、ヨー角θｚｔを算出する。この処理は、更新時間Δｔ毎に繰り返し行われる。
（式３）
Ｗｌｙ＝ｆ（Ｗｂｙ、Ｗｂｚ、θｘ０）
Ｗｌｚ＝ｇ（Ｗｂｙ、Ｗｂｚ、θｘ０、θｙ０）
Δθｙ＝（Ｗｌｙ×Δｔ）
Δθｚ＝（Ｗｌｚ×Δｔ）
θｙｔ＝θｙ０＋Δθｙ
θｚｔ＝θｚ０＋Δθｚ
ところで、図４に示すように、車両２０が走行する一般的な傾斜道路は傾斜角一定である複数の所定区間で構成されており、その隣り合う所定区間の傾斜角度の差は小さく、その差は１[ｄｅｇ]未満である。さらに、ピッチ方向角速度センサ１５で検出される角速度Ｗｂｙは、３[ｄｅｇ／ｓ]未満である。図４は、車両２０が一般的な傾斜道路を走行している様子の一例を示した図であり、（ａ）は側面図、（ｂ）は正面図である。図４の例では、傾斜道路は位置Ｘ１から位置Ｘ２までの第１区間、位置Ｘ２から位置Ｘ３までの第２区間、位置Ｘ３から位置Ｘ４までの第３区間で構成されている。第１区間におけるロール角はθｘ１、ピッチ角はθｙ１で一定となり、第２区間におけるロール角はθｘ２、ピッチ角はθｙ２で一定となり、第３区間におけるロール角はθｘ１、ピッチ角はθｙ１で一定となっている。従って、図４の例でいえば、ロール角θｘ１とロール角θｘ２との差およびピッチ角θｙ１とピッチ角θｙ２がそれぞれ１［ｄｅｇ］未満となっており、位置Ｘ２および位置Ｘ３の傾斜道路の傾斜角の変化により、車両右側方向軸（Ｙｂ軸）回りに角速度Ｗｂｙが出力されるが、その絶対値は小さいことが分かる。
また、上述したように、車両２０が走行する一般的な傾斜道路の傾斜角は小さく、車両２０の姿勢角θｔのうち、車両進行方向軸回り（Ｘｂ軸回り）の角度であるロール角θｘｔは１０［ｄｅｇ］未満であり、車両右側方向軸回り（Ｙｂ軸回り）の角度であるピッチ角θｙｔは２０[ｄｅｇ]未満である。
ここで、本発明者は、車両２０が走行する一般的な道路では、ピッチ方向角速度センサ１５で検出される角速度Ｗｂｙ、ロール角θｘｔおよびピッチ角θｙｔが小さいことから、姿勢角演算部１８でピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、姿勢角θ０を用いて角速度Ｗｌに変換する演算式において、角速度Ｗｂｙ、ピッチ角θｙ０およびロール角θｘ０のうち、何れかを２つ以上含む項をゼロとしても、ピッチ角θｙｔおよびヨー角θｚｔの算出に誤差が生じないことに着目した。
また、本発明者は、車両２０が走行する一般的な道路では、ロール角θｘｔおよびピッチ角θｙｔが小さいことから、姿勢角演算部１８でピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、姿勢角θ０を用いて角速度Ｗ１に変換する演算式において、ロール角θｘ０もしくはピッチ角θｙ０の三角関数を数値的に近似しても、ピッチ角θｙｔおよびヨー角θｚｔの算出に誤差が生じないことに着目した。ロール角θｘ０もしくはピッチ角θｙ０の三角関数の数値的近似とは、ロール角θｘ０の例でいえば、ｃｏｓθｘ０＝１、ｓｉｎθｘ０＝θｘ０である。姿勢角制御部１７は、これらの着目点に基づいて構築されたものであり、姿勢角演算部１８でピッチ方向角速度センサ１５で検出された角速度Ｗｂｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂzを、姿勢角θ０を用いて角速度Ｗｌに変換する演算式において、ピッチ方向角速度センサ１５で検出された角速度Ｗｂｙ、ロール角算出部１９から得られるロール角θｘ０、もしくは、姿勢角演算部１８で算出されるピッチ角θｙ０を含む項を、予め数値的に近似する。そして、姿勢角制御部１７で数値的近似によって制御された結果は、姿勢角演算部１８へ出力されている。
また、姿勢角制御部１７は、ロール角算出部１９から得られるロール角θｘｔと、姿勢角演算部１８で算出されるピッチ角θｔがそれぞれの上限値を超えた場合、その上限値をロール角θｘｔとピッチ角θｙｔの算出結果とするように制御する。ここで、ロール角θｘｔの上限値は、例えば１０［ｄｅｇ］であり、ピッチ角θｙｔの上限値は、例えば２０［ｄｅｇ］である。具体的な姿勢角算出処理部１３の算出処理については、後述する。
なお、姿勢角算出処理部１３の算出結果は、走行距離算出部１２と、位置算出部１４へ出力されている。
【００１５】
走行距離算出部１２は、式（４）に示すように、進行方向加速度センサ１０から検出される加速度Ａｘに、姿勢角算出処理部１３から得られるピッチ角θｙｔを用いて算出した重力加速度Ｇの進行方向成分の加速度Ａｇ（＝Ｇｓｉｎθｙｔ）を加え、更新時間Δｔで
時間積分して、車両座標系におけるＸｂ軸方向の速度Ｖｂｘを算出する。速度Ｖｂｘは、更新時間Δｔ毎に繰り返し算出され、ロール角算出部１９へ出力されている。
（式４）
Ｖｂｘ＝（Ａｘ＋Ａｇ）×Δｔ
また、走行距離算出部１２は、式（５）に示すように、車両座標系におけるＸｂ軸方向の速度Ｖｂｘを、更新時間Δｔで時間積分して、走行距離ΔＸを算出する。走行距離ΔＸは、更新時間Δｔ毎に繰り返し算出され、位置算出部１４へ出力されている。
（式５）
ΔＸ＝Ｖｂｘ×Δｔ
位置算出部１４は、式（６）に示すように、姿勢角算出処理部１３から得られる姿勢角θｔのうち、垂直下方向軸回り（Ｚｂ軸回り）の角度であるヨー角θｚｔ（＝方位角）を用いて、走行距離算出部１２から得られる走行距離ΔＸを各成分に分解する。そして、走行距離ΔＸの各成分を、自身に設定された位置Ｒ０（Ｒｘ０、Ｒｙ０）に加え、位置Ｒｔ（Ｒｘｔ、Ｒｙｔ）を算出する。
更新時間Δｔ後、位置Ｒｔを位置Ｒ０として、上述したように、位置Ｒ０にヨー角θｚｔを用いて分解した走行距離ΔＸの各成分を加え、位置Ｒｔを算出する。この処理は、更新時間Δｔ毎に繰り返し行われる。
（式６）
Ｒｘｔ＝Ｒｘ０＋ΔＸｃｏｓθｚｔ
Ｒｙｔ＝Ｒｙ０＋ΔＸｓｉｎθｚｔ
＜位置検出装置の処理動作＞
図５を参照して、位置検出装置の位置検出処理の動作について説明する。図５は、位置検出装置の位置検出処理の流れを示したフロー図である。
図５において、位置検出装置の位置検出処理を開始すると、姿勢角算出処理部１３は、ピッチ方向角速度センサ１５で検出されたＷｂｙと、ヨー方向角速度センサ１６で検出されたＷｂｚを例えば１０［ｍｓ］でサンプリングする（ステップ１１）。これが、車両の姿勢角θｔ、車両座標系におけるＸｂ軸方向の速度Ｖｂｘ、走行距離ΔＸおよび位置Ｒｔの更新時間Δｔである。
ステップＳ１１の次に、姿勢角算出処理部１３は、姿勢角制御部１７において、ロール角算出部１９から得られるロール角θｘ０もしくは姿勢角演算部１８から得られるピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似する。そして、角速度Ｗｂｙと角速度Ｗｂｙ、ロール角θｘ０、ピッチ角θｙ０の乗算、ロール角θｘ０とロール角θｘ０、ピッチ角θｙ０の乗算、ピッチ角θｙ０とピッチ角θｙ０の乗算をゼロに近似する。そして、姿勢角演算部１８は、姿勢角制御部１７で出力された数値的近似値を用いて、式（３）の角速度Ｗｂｙと角速度Ｗｂｚから角速度Ｗｌｙと角速度Ｗｌｚへの変換式を簡略化し、式（７）に基づいて、水平面に対する角速度Ｗｌをそれぞれ算出する（ステップ１２）。
(式７)
Ｗｌｙ＝ｉ（Ｗｂｙ、Ｗｂｚ、θｘ０）
Ｗｌｚ＝ｊ（Ｗｂｚ）
そして姿勢角算出処理部１３は、姿勢角演算部１８において、式（３）に基づいて、変換した水平面の角速度Ｗｌを更新時間Δｔで時間積分して、ピッチ角およびヨー角の変化量をそれぞれ算出する（ステップＳ１３）。
ステップＳ１３の次に、姿勢角算出処理部１３は、姿勢角演算部１８において、式（３）に基づいて、自身に設定されたピッチ角θｙ０に算出したピッチ角の変化量Δθｙを加え、ピッチ角を算出する。また、姿勢角算出処理部１３は、姿勢角演算部１８において、自身に設定されたヨー角θｚｔに算出したヨー角の変化量Δθｚを算出する（ステップＳ１４）。
ステップＳ１４の次に、走行距離算出部１２は、進行方向加速度センサ１０で検出された加速度Ａｘを、車両の姿勢角θｔ（ロール角θｘｔ、ピッチ角θｙｔ、ヨー角θｚｔ）と
同様の更新時間Δｔでサンプリングする（ステップＳ１５）。
ステップＳ１５の次に、走行距離算出部１２は、式（４）に基づいて、加速度Ａｘに姿勢角算出処理部１３から得られるピッチ角θｙｔを用いて算出した重力加速度Ｇの進行方向成分の加速度Ａｇを加え、更新時間Δｔで時間積分して、車両座標系におけるＸｂ軸方向の速度を算出する（ステップＳ１６）。
ステップＳ１６の次に、走行距離算出部１２は、式（５）に基づいて、車両座標系におけるＸｂ軸方向の速度Ｖｂｘを、更新時間Δｔで時間積分して、走行距離ΔＸを算出する（ステップＳ１７）。
【００１６】
ステップＳ１７の次に、位置算出部１４は、式（６）に基づいて、姿勢角算出処理部１３から得られるヨー角θｚを用いて、走行距離算出部１２から得られる走行距離ΔＸを各成分に分解する。そして、走行距離ΔＸの各成分を、自身に設定された位置Ｒ０（Ｒｘ０、Ｒｙ０）に加え、位置Ｒｔ（Ｒｘｔ、Ｒｙｔ）を算出する（ステップＳ１８）。
ステップＳ１８の次に、ロール角算出部１９は、右側方向加速度センサ１１で検出された加速度Ａｙと、ヨー方向角速度センサ１６で検出された角速度Ｗｂｚを、車両の姿勢角θｔと同様の更新時間Δｔでサンプリングする（ステップＳ１９）。
ロール角算出部１９は、角速度Ｗｂｚと、走行距離算出部１２から得られる車両座標系におけるＸｂ軸方向の速度Ｖｂｘとから、遠心加速度Ａｗを算出した後、加速度Ａｙから遠心加速度Ａｗを減算した結果を、重力加速度Ｇで除算することによってロール角θｘｔを算出する（ステップＳ２０）。ロール角算出部１９は、算出したロール角θｘｔをθｘ０として、姿勢角算出処理部１３へ出力する。その後、処理はステップＳ１１へ戻る。
【００１７】
以上の説明より明らかなように、本発明の実施の形態では、姿勢角算出処理部１３は、ピッチ方向角速度センサ１５とヨー方向角速度センサ１６が検出した角速度とロール角算出部１９が算出したロール角に基づいて移動体の姿勢角を算出するので、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角を算出し、算出した傾斜角を利用して移動体の現在位置を精度良く算出することができる。
【００１８】
また、ロール角算出部１９は、移動体の移動に伴う右側方向加速度及び進行方向軸の回転に伴う遠心加速度を０とし、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、演算しているので、進行方向軸回りの角速度センサがなくても、移動体の現在位置を精度良く算出することができる。
【００１９】
また、姿勢角算出処理部１３は、移動体の進行方向軸回り（ロール方向）と右側方向軸回り（ピッチ方向）の角度と角速度が小さいことを利用し、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、姿勢角を算出するので、演算処理を複雑に行わなくても移動体の現在位置を精度良く算出することができる。
【００２０】
また、姿勢角算出処理部１３は、角速度Ｗｂｙと角速度Ｗｂｙ、ロール角θｘ０、ピッチ角θｙ０の乗算、ロール角θｘ０とロール角θｘ０、ピッチ角θｙ０の乗算、ピッチ角θｙ０とピッチ角θｙ０の乗算をゼロに近似して、演算するので、演算処理数を少なくしても、移動体の現在位置を精度良く算出することができる。
【００２１】
また、姿勢角算出処理部１３は移動体の姿勢角を算出するが、入力値である角速度は振動型ジャイロで検出しているので、誤差が大きくなる可能性があり、ロール角θｘ０とピッチ角θｙ０が上限値を超えた場合、誤差が含まれていると判断し、上限値を走行距離算出部１２と位置算出部１４に出力するので、移動体の現在位置を精度良く算出することができる。
【産業上の利用可能性】
【００２２】
本発明に係る位置検出装置は、車速パルス信号を用いずに角速度センサの他軸感度の影響を取り除き、移動体の傾斜角を算出し、算出した傾斜角を利用して移動体の現在位置を精度良く算出し、車両に搭載されるナビゲーション装置等に適用される。
【符号の説明】
【００２３】
１０ 進行方向加速度センサ
１１ 右側方向加速度センサ
１２ 走行距離算出部
１３ 姿勢角算出処理部
１４ 位置算出部
１５ ピッチ方向角速度センサ
１６ ヨー方向角速度センサ
１９ ロール角算出部
【特許請求の範囲】
【請求項１】
移動体のピッチ方向の角速度を検出するピッチ方向角速度検出部と、
前記移動体のヨー方向の角速度を検出するヨー方向角速度検出部と、
前記移動体の進行方向の加速度を検出する進行方向加速度検出部と、
前記移動体の右側方向の加速度を検出する右側方向加速度検出部と、
前記ヨー方向角速度検出部が検出した角速度と前記右側方向加速度検出部が検出した加速度に基づいてロール角を算出するロール角算出部と、
前記ピッチ方向角速度検出部と前記ヨー方向角速度検出部が検出した角速度と前記ロール角算出部が算出したロール角に基づいて前記移動体の姿勢角を算出する姿勢角算出処理部と、
前記進行方向加速度検出部が検出した加速度と前記姿勢角算出処理部が算出した前記移動体の姿勢角に基づいて前記移動体の走行距離を算出する走行距離算出部と、
前記姿勢角算出処理部が算出した前記移動体の姿勢角と前記走行距離算出部が算出した前記移動体の走行距離に基づいて前記移動体の現在位置を算出する位置算出部とを備える位置検出装置。
【請求項２】
前記ロール角算出部は、前記移動体の移動に伴う右側方向加速度及び進行方向軸の回転に伴う遠心加速度を０とし、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、演算することを特徴とする請求項１記載の位置検出装置。
【請求項３】
前記姿勢角算出処理部は、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、演算することを特徴とする請求項１記載の位置検出装置。
【請求項４】
前記姿勢角算出処理部は、角速度Ｗｂｙと角速度Ｗｂｙ、ロール角θｘ０、ピッチ角θｙ０の乗算、ロール角θｘ０とロール角θｘ０、ピッチ角θｙ０の乗算、ピッチ角θｙ０とピッチ角θｙ０の乗算をゼロに近似して、演算することを特徴とする請求項１記載の位置検出装置。
【請求項５】
前記姿勢角算出処理部は、前記移動体の姿勢角を算出し、ロール角とピッチ角が上限値を超えた場合、前記上限値を前記走行距離算出部と前記位置算出部に出力することを特徴とする請求項１記載の位置検出装置。
【請求項１】
移動体のピッチ方向の角速度を検出するピッチ方向角速度検出部と、
前記移動体のヨー方向の角速度を検出するヨー方向角速度検出部と、
前記移動体の進行方向の加速度を検出する進行方向加速度検出部と、
前記移動体の右側方向の加速度を検出する右側方向加速度検出部と、
前記ヨー方向角速度検出部が検出した角速度と前記右側方向加速度検出部が検出した加速度に基づいてロール角を算出するロール角算出部と、
前記ピッチ方向角速度検出部と前記ヨー方向角速度検出部が検出した角速度と前記ロール角算出部が算出したロール角に基づいて前記移動体の姿勢角を算出する姿勢角算出処理部と、
前記進行方向加速度検出部が検出した加速度と前記姿勢角算出処理部が算出した前記移動体の姿勢角に基づいて前記移動体の走行距離を算出する走行距離算出部と、
前記姿勢角算出処理部が算出した前記移動体の姿勢角と前記走行距離算出部が算出した前記移動体の走行距離に基づいて前記移動体の現在位置を算出する位置算出部とを備える位置検出装置。
【請求項２】
前記ロール角算出部は、前記移動体の移動に伴う右側方向加速度及び進行方向軸の回転に伴う遠心加速度を０とし、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、演算することを特徴とする請求項１記載の位置検出装置。
【請求項３】
前記姿勢角算出処理部は、ロール角θｘ０もしくはピッチ角θｙ０のｓｉｎ関数をそれぞれθｘ０、θｙ０、ｃｏｓ関数を１に近似して、演算することを特徴とする請求項１記載の位置検出装置。
【請求項４】
前記姿勢角算出処理部は、角速度Ｗｂｙと角速度Ｗｂｙ、ロール角θｘ０、ピッチ角θｙ０の乗算、ロール角θｘ０とロール角θｘ０、ピッチ角θｙ０の乗算、ピッチ角θｙ０とピッチ角θｙ０の乗算をゼロに近似して、演算することを特徴とする請求項１記載の位置検出装置。
【請求項５】
前記姿勢角算出処理部は、前記移動体の姿勢角を算出し、ロール角とピッチ角が上限値を超えた場合、前記上限値を前記走行距離算出部と前記位置算出部に出力することを特徴とする請求項１記載の位置検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１０−２７６５６２（Ｐ２０１０−２７６５６２Ａ）
【公開日】平成２２年１２月９日（２０１０．１２．９）
【国際特許分類】
【出願番号】特願２００９−１３１７４４（Ｐ２００９−１３１７４４）
【出願日】平成２１年６月１日（２００９．６．１）
【出願人】（０００００５８２１）パナソニック株式会社 (73,050)
【Ｆターム（参考）】
【公開日】平成２２年１２月９日（２０１０．１２．９）
【国際特許分類】
【出願日】平成２１年６月１日（２００９．６．１）
【出願人】（０００００５８２１）パナソニック株式会社 (73,050)
【Ｆターム（参考）】
[ Back to top ]