JP2012193965A - 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム - Google Patents
位置標定装置、位置標定装置の位置標定方法および位置標定プログラム Download PDFInfo
- Publication number
- JP2012193965A JP2012193965A JP2011056105A JP2011056105A JP2012193965A JP 2012193965 A JP2012193965 A JP 2012193965A JP 2011056105 A JP2011056105 A JP 2011056105A JP 2011056105 A JP2011056105 A JP 2011056105A JP 2012193965 A JP2012193965 A JP 2012193965A
- Authority
- JP
- Japan
- Prior art keywords
- calculated
- azimuth
- value
- azimuth angle
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Abstract
【課題】高精度でないＩＭＵと１台のＧＰＳ受信機とを用いて車両の位置を高精度に標定できるようにすることを目的とする。
【解決手段】初期値算出部１１０は、車両の出発時に３自由度２次元航法により方位角の初期値を算出する。走行時標定部１２１は、車両が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する。停車時標定部１２２は、車両が停車している間、方位角を拘束する拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する。
【選択図】図３
【解決手段】初期値算出部１１０は、車両の出発時に３自由度２次元航法により方位角の初期値を算出する。走行時標定部１２１は、車両が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する。停車時標定部１２２は、車両が停車している間、方位角を拘束する拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する。
【選択図】図３
Description
本発明は、例えば、車両の位置を標定する位置標定装置、位置標定装置の位置標定方法および位置標定プログラムに関するものである。
従来、６自由度の３次元航法は、高価で高精度なＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を利用する必要があったため、航空・宇宙関連分野など、限られた分野でしか使用できなかった。
そこで、低価格化のために複数台のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機と車速パルスとを利用して車両の位置・姿勢・方位を標定することも考えられる。
しかし、ＧＰＳアンテナ同士をある程度離して相対位置が変化しないように設置する必要がある。この制約は、一般の乗用車に適用するには大きな制約である。
しかし、ＧＰＳアンテナ同士をある程度離して相対位置が変化しないように設置する必要がある。この制約は、一般の乗用車に適用するには大きな制約である。
これらの制約が発生する具体的な要因は以下のとおりである。
（１）車両を停止した状態で方位を求める場合、中精度または低精度なＩＭＵでは、地球の自転を計測して方位を求めるといった計算が出来ない。
また、２台以上のＧＰＳ受信機を使用して方位を求める場合、ＧＰＳアンテナ間の基線解析を行うため、精度を出すためにはＧＰＳアンテナを可能な限り離して相対位置が変化しないよう設置する必要がある。
（２）３次元航法計算では、ある程度正確な初期値（位置・姿勢・方位）を設定しなければ計算結果が発散してしまう可能性が高い。
初期値のうち、姿勢角（ピッチ角、ロール角）は重力観測により得られ、位置はＧＰＳ観測により得られるが、方位に関しては上記（１）の理由により適切な値を得ることができない。
（３）正確な初期値が何らかの方法で与えられたとしても、その後の停車時にはジャイロの誤差等によって方位の値がドリフトしてしまう。
（１）車両を停止した状態で方位を求める場合、中精度または低精度なＩＭＵでは、地球の自転を計測して方位を求めるといった計算が出来ない。
また、２台以上のＧＰＳ受信機を使用して方位を求める場合、ＧＰＳアンテナ間の基線解析を行うため、精度を出すためにはＧＰＳアンテナを可能な限り離して相対位置が変化しないよう設置する必要がある。
（２）３次元航法計算では、ある程度正確な初期値（位置・姿勢・方位）を設定しなければ計算結果が発散してしまう可能性が高い。
初期値のうち、姿勢角（ピッチ角、ロール角）は重力観測により得られ、位置はＧＰＳ観測により得られるが、方位に関しては上記（１）の理由により適切な値を得ることができない。
（３）正確な初期値が何らかの方法で与えられたとしても、その後の停車時にはジャイロの誤差等によって方位の値がドリフトしてしまう。
今後、３次元地図のインフラ整備が急速に進むほか、低価格なＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）センサの性能向上によって一般のカーナビゲーションシステムでも車両の位置・姿勢・方位を標定する３次元航法のニーズが高まることが予想されている。
本発明は、例えば、高精度でないＩＭＵと１台のＧＰＳ受信機とを用いて車両の位置を高精度に標定できるようにすることを目的とする。
本発明の位置標定装置は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する。
前記位置標定装置は、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部とを備える。
前記位置標定装置は、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部とを備える。
本発明によれば、例えば、高精度でないＩＭＵ（加速度センサ、角速度センサ）と１台のＧＰＳ受信機とを用いて車両（移動体の一例）の位置を高精度に標定することができる。
実施の形態１．
車両の位置・姿勢・方位を標定する自己位置姿勢標定装置について説明する。
車両の位置・姿勢・方位を標定する自己位置姿勢標定装置について説明する。
図１は、実施の形態１における車両２００の概要図である。
図２は、実施の形態１における自己位置姿勢標定装置１００の概要図である。
実施の形態１における車両２００の概要と自己位置姿勢標定装置１００の概要とについて、図１と図２とに基づいて説明する。
図２は、実施の形態１における自己位置姿勢標定装置１００の概要図である。
実施の形態１における車両２００の概要と自己位置姿勢標定装置１００の概要とについて、図１と図２とに基づいて説明する。
図１において、車両２００（移動体の一例）は、ＧＰＳ受信機２１０（およびＧＰＳアンテナ２１１）と慣性センサ２２０、車速センサ２３０（速度センサの一例）と自己位置姿勢標定装置１００（位置標定装置の一例）を備える。
「ＧＰＳ」は「Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ」の略である。
「ＧＰＳ」は「Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ」の略である。
例えば、ＧＰＳ受信機２１０と慣性センサ２２０とを設置した天板２０１を車両２００の屋根部に設置する。また、自己位置姿勢標定装置１００と車速センサ２３０とを車両２００内に設置する。
但し、設置方法はこれに限らない。例えば、ＧＰＳ受信機２１０を屋根部以外に設置してもよいし、慣性センサ２２０を車両２００内に設置してもよく、天板２０１を設けることなく、ＧＰＳ受信機２１０と慣性センサ２２０を車両２００の内部または外部に取り付けてもよい。
また、自己位置姿勢標定装置１００は車両２００とは別個に設けた外部装置であっても構わない。
さらに、車両２００は他の移動体（例えば、飛行機、船舶、自動二輪車、自転車、列車）であっても構わない。
但し、設置方法はこれに限らない。例えば、ＧＰＳ受信機２１０を屋根部以外に設置してもよいし、慣性センサ２２０を車両２００内に設置してもよく、天板２０１を設けることなく、ＧＰＳ受信機２１０と慣性センサ２２０を車両２００の内部または外部に取り付けてもよい。
また、自己位置姿勢標定装置１００は車両２００とは別個に設けた外部装置であっても構わない。
さらに、車両２００は他の移動体（例えば、飛行機、船舶、自動二輪車、自転車、列車）であっても構わない。
ＧＰＳ受信機２１０は、ＧＰＳ衛星から発信される測位信号をＧＰＳアンテナ２１１を用いて観測し、観測結果に基づいて位置を測位する。
慣性センサ２２０は、３軸方向（車両２００の長さ方向と幅方向と高さ方向）の加速度と角速度とを計測する。
慣性センサ２２０は、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）と呼ばれる。
以下、３軸方向の加速度を「三次元加速度」といい、３軸方向の角速度を「三次元角速度」という。
慣性センサ２２０は、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）と呼ばれる。
以下、３軸方向の加速度を「三次元加速度」といい、３軸方向の角速度を「三次元角速度」という。
車速センサ２３０は、車軸の回転を車速パルスとして検出し、検出した車速パルスを用いて車両２００の速度を計測する。
図２に示すように、自己位置姿勢標定装置１００は、ＧＰＳ受信機２１０のデータと、慣性センサ２２０のデータと、車速センサ２３０のデータとに基づいて車両２００の位置と姿勢と方位とを標定する。
「位置」は、三次元の座標値（緯度、経度、高度）を意味する。
「姿勢」は、仰角（ピッチ角）と回転角（ロール角）とを意味する。
「方位」は、方位角を意味する。
「位置」は、三次元の座標値（緯度、経度、高度）を意味する。
「姿勢」は、仰角（ピッチ角）と回転角（ロール角）とを意味する。
「方位」は、方位角を意味する。
図３は、実施の形態１における自己位置姿勢標定装置１００の機能構成図である。
実施の形態１における自己位置姿勢標定装置１００の機能構成について、図３に基づいて説明する。
実施の形態１における自己位置姿勢標定装置１００の機能構成について、図３に基づいて説明する。
自己位置姿勢標定装置１００（位置標定装置の一例）は、初期値算出部１１０、自己位置姿勢標定部１２０、カルマンフィルタ部１３０、ストラップダウン演算部１４０および装置記憶部１９０を備える。
装置記憶部１９０は、自己位置姿勢標定装置１００で使用するデータを記憶する。
ＧＰＳデータ１９１、速度データ１９２、加速度データ１９３または角速度データ１９４は、装置記憶部１９０に記憶されるデータの一例である。
ＧＰＳデータ１９１、速度データ１９２、加速度データ１９３または角速度データ１９４は、装置記憶部１９０に記憶されるデータの一例である。
ＧＰＳデータ１９１は、ＧＰＳ受信機２１０により計測されたデータであり、時刻に対応付けて「位置」や「速度」を含んでいる。速度は、過去の位置と現在の位置と過去から現在までの経過時間とに基づいて計測される。
速度データ１９２は、車速センサ２３０により計測されたデータであり、時刻に対応付けて「速度」を含んでいる。
加速度データ１９３は、慣性センサ２２０を構成する加速度センサ２２１により計測されたデータであり、時刻に対応付けて三次元加速度を含んでいる。
角速度データ１９４は、慣性センサ２２０を構成する角速度センサ２２２により計測されたデータであり、時刻に対応付けて三次元角速度を含んでいる。つまり、角速度データ１９４は、「仰角」の角速度と「回転角」の角速度と「方位角」の角速度とを含んでいる。角速度センサ２２２は「ジャイロ」と呼ばれる。
速度データ１９２は、車速センサ２３０により計測されたデータであり、時刻に対応付けて「速度」を含んでいる。
加速度データ１９３は、慣性センサ２２０を構成する加速度センサ２２１により計測されたデータであり、時刻に対応付けて三次元加速度を含んでいる。
角速度データ１９４は、慣性センサ２２０を構成する角速度センサ２２２により計測されたデータであり、時刻に対応付けて三次元角速度を含んでいる。つまり、角速度データ１９４は、「仰角」の角速度と「回転角」の角速度と「方位角」の角速度とを含んでいる。角速度センサ２２２は「ジャイロ」と呼ばれる。
初期値算出部１１０は、カルマンフィルタ部１３０を制御して方位角の初期値を算出する（初期値算出処理）。
例えば、初期値算出部１１０は、以下のように方位角の初期値を算出する。
初期値算出部１１０は、角速度データ１９４に含まれる方位角の角速度に基づいて、車両２００の方位角を第一航法方位角として算出する（後述する方位更新部１１２の処理）。
初期値算出部１１０は、算出した第一航法方位角と速度データ１９２に含まれる速度とに基づいて、車両２００の座標値を第一航法座標値として算出する（後述する緯度経度更新部１１４の処理）。
初期値算出部１１０は、算出した第一航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第１のカルマンフィルタ１３１を用いて、第一航法方位角を補正する補正値を算出する。
初期値算出部１１０は、算出した補正値を用いて第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する（後述する出力補正部１１５の処理）。
初期値算出部１１０は、角速度データ１９４に含まれる方位角の角速度に基づいて、車両２００の方位角を第一航法方位角として算出する（後述する方位更新部１１２の処理）。
初期値算出部１１０は、算出した第一航法方位角と速度データ１９２に含まれる速度とに基づいて、車両２００の座標値を第一航法座標値として算出する（後述する緯度経度更新部１１４の処理）。
初期値算出部１１０は、算出した第一航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第１のカルマンフィルタ１３１を用いて、第一航法方位角を補正する補正値を算出する。
初期値算出部１１０は、算出した補正値を用いて第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する（後述する出力補正部１１５の処理）。
自己位置姿勢標定部１２０は、走行時標定部１２１（標定値算出部の一例）と停車時標定部１２２（方位角補正部の一例）と停車判定部１２３（停止判定部の一例）とを備える。
走行時標定部１２１は、車両２００が走行しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定（算出）する。
停車時標定部１２２は、車両２００が停車しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定する。
走行時標定部１２１は、車両２００が走行しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定（算出）する。
停車時標定部１２２は、車両２００が停車しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定する。
例えば、走行時標定部１２１は、以下のように車両２００の位置を標定する。
走行時標定部１２１は、角速度データ１９４に含まれる方位角の角速度に基づいて、方位角の変化量を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した方位角の変化量と初期値算出部１１０から出力された方位角初期値とに基づいて、車両２００の方位角を第二航法方位角として算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した第二航法方位角と加速度データ１９３に含まれる加速度とに基づいて、車両２００の座標値を第二航法座標値として算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した第二航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第２のカルマンフィルタ１３２を用いて、第二航法座標値を補正する補正値を算出する。
走行時標定部１２１は、算出した補正値を用いて第二航法座標値を補正し、車両２００の位置を標定した位置標定値として補正後の第二航法座標値を出力する（後述する出力補正部１２５の処理）。
走行時標定部１２１は、角速度データ１９４に含まれる方位角の角速度に基づいて、方位角の変化量を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した方位角の変化量と初期値算出部１１０から出力された方位角初期値とに基づいて、車両２００の方位角を第二航法方位角として算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した第二航法方位角と加速度データ１９３に含まれる加速度とに基づいて、車両２００の座標値を第二航法座標値として算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した第二航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第２のカルマンフィルタ１３２を用いて、第二航法座標値を補正する補正値を算出する。
走行時標定部１２１は、算出した補正値を用いて第二航法座標値を補正し、車両２００の位置を標定した位置標定値として補正後の第二航法座標値を出力する（後述する出力補正部１２５の処理）。
例えば、走行時標定部１２１と停車時標定部１２２とは、以下のように車両２００の位置を標定する。
停車時標定部１２２は、移動していた車両２００が移動を停止した場合、第３のカルマンフィルタ１３３を用いて第二航法方位角を補正する方位角補正値を算出する。
停車時標定部１２２は、算出した方位角補正値を用いて第二航法方位角を補正して補正後の第二航法方位角を算出する（後述する出力補正部１２５の処理）。
走行時標定部１２１は、停止した車両２００が移動を再開した場合、補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した新たな第二航法座標値を補正する位置補正値を第２のカルマンフィルタ１３２を用いて算出する。
走行時標定部１２１は、算出した位置補正値を用いて新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する（後述する出力補正部１２５の処理）。
停車時標定部１２２は、移動していた車両２００が移動を停止した場合、第３のカルマンフィルタ１３３を用いて第二航法方位角を補正する方位角補正値を算出する。
停車時標定部１２２は、算出した方位角補正値を用いて第二航法方位角を補正して補正後の第二航法方位角を算出する（後述する出力補正部１２５の処理）。
走行時標定部１２１は、停止した車両２００が移動を再開した場合、補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した新たな第二航法座標値を補正する位置補正値を第２のカルマンフィルタ１３２を用いて算出する。
走行時標定部１２１は、算出した位置補正値を用いて新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する（後述する出力補正部１２５の処理）。
停車判定部１２３は、車両２００が停車したか又は発車したかを判定する。
例えば、停車判定部１２３は、車両２００が停車したか又は発車したかを以下のように判定する。
停車判定部１２３は、車速センサ２３０により計測された速度が所定の速度閾値未満である場合、車両２００が移動を停止したと判定する。
停車判定部１２３は、車両２００が移動を停止したと判定した後、車速センサ２３０により計測された速度が速度閾値以上である場合、車両２００が移動を再開したと判定する。
停車判定部１２３は、車速センサ２３０により計測された速度が所定の速度閾値未満である場合、車両２００が移動を停止したと判定する。
停車判定部１２３は、車両２００が移動を停止したと判定した後、車速センサ２３０により計測された速度が速度閾値以上である場合、車両２００が移動を再開したと判定する。
カルマンフィルタ部１３０は、観測方程式や状態方程式を生成し、生成した観測方程式や状態方程式を用いて各種データの補正値を算出する。
カルマンフィルタ部１３０は、観測方程式が異なる複数のカルマンフィルタ（符号１３１−１３３）を備える。
カルマンフィルタ部１３０は、観測方程式が異なる複数のカルマンフィルタ（符号１３１−１３３）を備える。
例えば、第３のカルマンフィルタ１３３は、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を方位角補正値として算出する。
例えば、第３のカルマンフィルタ１３３は、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて方位角補正値を算出する。
図４は、実施の形態１におけるストラップダウン演算部１４０の機能構成図である。
実施の形態１におけるストラップダウン演算部１４０の機能構成について、図４に基づいて説明する。
実施の形態１におけるストラップダウン演算部１４０の機能構成について、図４に基づいて説明する。
ストラップダウン演算部１４０は、加速度データ１９３と角速度データ１９４とに基づいて速度と位置と姿勢と方位とを算出する。
ストラップダウン演算部１４０は、図４に示す構成（符号１４１−１４７、１４８Ａ−Ｃ）を備える。
角速度座標変換部１４１は、センサ座標系（車体座標系）の三次元角速度を入力する。
角速度座標変換部１４１は、姿勢方位更新部１４２により前回算出された姿勢と方位とに基づいて所定の座標変換式（例えば、方向余弦行列）を用いて三次元角速度をセンサ座標系の値から航法座標系（局所水平座標系）の値に変換する。
「センサ座標系」は車両２００を基準とする座標系であり、例えば、長さ方向を「ｘ」、幅方向を「ｙ」、高さ方向を「ｚ」として表される。
「航法座標系」は地面を基準とする座標系であり、例えば、「Ｎｏｒｔｈ（北）」「Ｅａｓｔ（東）」「Ｄｏｗｎ（下）」で表される。
角速度座標変換部１４１は、姿勢方位更新部１４２により前回算出された姿勢と方位とに基づいて所定の座標変換式（例えば、方向余弦行列）を用いて三次元角速度をセンサ座標系の値から航法座標系（局所水平座標系）の値に変換する。
「センサ座標系」は車両２００を基準とする座標系であり、例えば、長さ方向を「ｘ」、幅方向を「ｙ」、高さ方向を「ｚ」として表される。
「航法座標系」は地面を基準とする座標系であり、例えば、「Ｎｏｒｔｈ（北）」「Ｅａｓｔ（東）」「Ｄｏｗｎ（下）」で表される。
第一補正部１４８Ａは、角速度座標変換部１４１により変換された三次元角速度に航法系回転補正計算部１４７により算出される補正値を加算して三次元角速度を補正する。
姿勢方位更新部１４２は、第一補正部１４８Ａにより補正された方位角の角速度を１回積分して方位角の変化量を算出し、算出した方位角の変化量を方位角の初期値または前回標定された方位角に加算して新たな方位角を算出する。
同様に、姿勢方位更新部１４２は、新たな姿勢角を算出する。
ここで、姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
また、姿勢方位更新部１４２は、加速度センサ２２１により停車時に計測された加速度を用いて所定の姿勢角算出式を計算し、姿勢角の初期値を算出する。姿勢角算出式は、計測された加速度と重力加速度と姿勢角との関係を表した式である。
同様に、姿勢方位更新部１４２は、新たな姿勢角を算出する。
ここで、姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
また、姿勢方位更新部１４２は、加速度センサ２２１により停車時に計測された加速度を用いて所定の姿勢角算出式を計算し、姿勢角の初期値を算出する。姿勢角算出式は、計測された加速度と重力加速度と姿勢角との関係を表した式である。
加速度座標変換部１４３は、センサ座標系の三次元加速度を入力する。
加速度座標変換部１４３は、姿勢方位更新部１４２により算出された姿勢角と方位角とに基づいて、所定の座標変換式を用いて、三次元加速度をセンサ座標系の値から航法座標系の値に変換する。
加速度座標変換部１４３は、姿勢方位更新部１４２により算出された姿勢角と方位角とに基づいて、所定の座標変換式を用いて、三次元加速度をセンサ座標系の値から航法座標系の値に変換する。
第二補正部１４８Ｂは、加速度座標変換部１４３により変換された三次元加速度に重力補正計算部１４５により算出される補正値を加算して三次元加速度を補正する。
第三補正部１４８Ｃは、第二補正部１４８Ｂにより補正された三次元加速度にコリオリ補正計算部１４６により算出される補正値を加算して三次元加速度を補正する。
速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を１回積分して速度（速度ベクトル）を算出する。
また、速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を２回積分して位置の変化量を算出し、算出した位置の変化量を位置の初期値または前回標定された位置に加算して新たな位置を算出する。
例えば、速度位置更新部１４４は、ＧＰＳ受信機２１０により停車時に計測された位置を位置の初期値として用いる。
また、速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を２回積分して位置の変化量を算出し、算出した位置の変化量を位置の初期値または前回標定された位置に加算して新たな位置を算出する。
例えば、速度位置更新部１４４は、ＧＰＳ受信機２１０により停車時に計測された位置を位置の初期値として用いる。
重力補正計算部１４５は、速度位置更新部１４４により算出された位置を入力値として所定の重力補正計算式を計算し、三次元加速度の補正値を算出する。この補正値は計測された三次元加速度に含まれる重力加速度を取り除くための値である。
コリオリ補正計算部１４６は、速度位置更新部１４４により算出された速度と姿勢方位更新部１４２により算出された姿勢とを入力値として所定のコリオリ補正計算式を計算し、三次元加速度の補正値を算出する。この補正値はコリオリ力による計測誤差を三次元加速度から取り除くための値である。
航法系回転補正計算部１４７は、速度位置更新部１４４により算出された速度を入力値として所定の航法系回転補正計算式を計算し、三次元角速度の補正値を算出する。この補正値は地球の自転による計測誤差を三次元角速度から取り除くための値である。
図５は、実施の形態１における自己位置姿勢標定方法を示すフローチャートである。
実施の形態１における自己位置姿勢標定方法について、図５に基づいて説明する。
実施の形態１における自己位置姿勢標定方法について、図５に基づいて説明する。
まず、自己位置姿勢標定方法の概要について説明する。
初期値算出部１１０は、車両２００の出発時に３自由度２次元航法により方位角の初期値を算出する（Ｓ１１０）。
走行時標定部１２１は、車両２００が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１２０）。
停車時標定部１２２は、車両２００が停車している間、拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１４０）。
走行時標定部１２１は、車両２００が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１２０）。
停車時標定部１２２は、車両２００が停車している間、拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１４０）。
次に、自己位置姿勢標定方法の詳細について説明する。
Ｓ１１０において、初期値算出部１１０は、車両２００の出発時、つまり、自己位置姿勢標定装置１００の起動時に３自由度２次元航法（第一航法）により方位角の初期値を算出する。
「３自由度」は「緯度」と「経度」と「方位」とを意味し、「２次元」は「緯度、経度」または「方位」を意味する。
「３自由度」は「緯度」と「経度」と「方位」とを意味し、「２次元」は「緯度、経度」または「方位」を意味する。
図６は、実施の形態１における３自由度２次元航法のアルゴリズムを示す図である。
実施の形態１における３自由度２次元航法について、図６に基づいて説明する。
実施の形態１における３自由度２次元航法について、図６に基づいて説明する。
入力補正部１１１、方位更新部１１２、座標変換部１１３、緯度経度更新部１１４および出力補正部１１５は、初期値算出部１１０が備える機能構成である。
入力補正部１１１は、速度データ１９２に含まれる速度にカルマンフィルタ部１３０により算出される補正値を加算して速度（速度ベクトル）を補正する。
さらに、入力補正部１１１は、角速度データ１９４に含まれる「方位角」の角速度にカルマンフィルタ部１３０により算出される補正値を加算して方位角の角速度を補正する。
さらに、入力補正部１１１は、角速度データ１９４に含まれる「方位角」の角速度にカルマンフィルタ部１３０により算出される補正値を加算して方位角の角速度を補正する。
方位更新部１１２は、入力補正部１１１により補正された方位角の角速度を１回積分して方位角の変化量を算出する。
方位更新部１１２は、算出した方位角の変化量を方位角の初期値または出力補正部１１５により前回算出された方位角に加算して新たな方位角を算出する。
ここで、方位更新部１１２は方位角の初期値として所定の値（例えば、０度）を用いる。ここで用いる方位角の初期値は適当な値で構わない。つまり、方位角の初期値は、正確な方位角を示す適切な値であっても、不正確な方位角を示す不適切な値であっても構わない。
方位更新部１１２は、算出した方位角の変化量を方位角の初期値または出力補正部１１５により前回算出された方位角に加算して新たな方位角を算出する。
ここで、方位更新部１１２は方位角の初期値として所定の値（例えば、０度）を用いる。ここで用いる方位角の初期値は適当な値で構わない。つまり、方位角の初期値は、正確な方位角を示す適切な値であっても、不正確な方位角を示す不適切な値であっても構わない。
座標変換部１１３は、方位更新部１１２により算出された姿勢と方位とに基づいて、所定の座標変換式を用いて、入力補正部１１１により補正された速度をセンサ座標系の値から航法座標系の値に変換する。
これにより、航法座標系の二次元速度（緯度方向の速度、経度方向の速度）が得られる。
これにより、航法座標系の二次元速度（緯度方向の速度、経度方向の速度）が得られる。
緯度経度更新部１１４は、座標変換部１１３により得られた二次元速度を１回積分して二次元座標（緯度、経度）の変化量を算出する。
緯度経度更新部１１４は、算出した二次元座標の変化量を二次元座標の初期値または出力補正部１１５により前回算出された二次元座標に加算して新たな二次元座標を算出する。
例えば、緯度経度更新部１１４は、車両２００が出発する前、つまり、車両２００の停車時にＧＰＳ受信機２１０により測位された二次元座標値を二次元座標の初期値として用いる。
緯度経度更新部１１４は、算出した二次元座標の変化量を二次元座標の初期値または出力補正部１１５により前回算出された二次元座標に加算して新たな二次元座標を算出する。
例えば、緯度経度更新部１１４は、車両２００が出発する前、つまり、車両２００の停車時にＧＰＳ受信機２１０により測位された二次元座標値を二次元座標の初期値として用いる。
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元座標と緯度経度更新部１１４により算出された二次元座標との差を「座標残差」として算出する。
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元速度と座標変換部１１３により得られた二次元速度とを差を「速度残差」として算出する。
カルマンフィルタ部１３０は、座標残差と速度残差とを入力値として第１のカルマンフィルタ１３１を実行し、速度の補正値と角速度の補正値と二次元座標の補正値と方位角の補正値とを算出する。
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元速度と座標変換部１１３により得られた二次元速度とを差を「速度残差」として算出する。
カルマンフィルタ部１３０は、座標残差と速度残差とを入力値として第１のカルマンフィルタ１３１を実行し、速度の補正値と角速度の補正値と二次元座標の補正値と方位角の補正値とを算出する。
出力補正部１１５は、緯度経度更新部１１４により算出された二次元座標値にカルマンフィルタ部１３０により算出された補正値を加算して二次元座標値を補正し、補正した二次元座標値を出力する。
さらに、出力補正部１１５は、方位更新部１１２により算出された方位角にカルマンフィルタ部１３０により算出された補正値を加算して方位角を補正し、補正した方位角を出力する。
さらに、出力補正部１１５は、方位更新部１１２により算出された方位角にカルマンフィルタ部１３０により算出された補正値を加算して方位角を補正し、補正した方位角を出力する。
図７は、実施の形態１における３自由度２次元航法による方位角の推定結果を示すグラフである。
実施の形態１における３自由度２次元航法による方位角の推定結果について、図７に基づいて説明する。
実施の形態１における３自由度２次元航法による方位角の推定結果について、図７に基づいて説明する。
図７において、３自由度２次元航法（図６参照）により算出した方位角を実線で示し、正しい方位角を鎖線で示す。
（１）車両２００が「−１８０度」の方位を向いて停車しているときに方位角の初期値「０度」を初期値算出部１１０の方位更新部１１２に設定し、車両２００の走行を開始した。
（２）車両２００を左回りに回転させると、３自由度２次元航法により算出した方位角（実線）は数秒程度で正しい方位角（鎖線）に近い適切な値になった。
（３）車両２００を停車させると、角速度センサ２２２のバイアス（計測誤差）により方位角の値がドリフトを始めた。
（２）車両２００を左回りに回転させると、３自由度２次元航法により算出した方位角（実線）は数秒程度で正しい方位角（鎖線）に近い適切な値になった。
（３）車両２００を停車させると、角速度センサ２２２のバイアス（計測誤差）により方位角の値がドリフトを始めた。
図７（１）（２）に示すように、３自由度２次元航法は、方位角の初期値が不適切な値であってもすぐに適切な値を求めることができる。
これは、３自由度２次元航法において、加速度センサ２２１が計測した加速度ではなく、車速センサ２３０が計測した速度を用いているためである。
これにより、二次元座標を求める積分計算が１回で済み、速度の座標変換に用いる方位角（の初期値）の誤差の影響が比較的少なく、カルマンフィルタを用いて二次元座標と方位角とを適切に補正できる。
これは、３自由度２次元航法において、加速度センサ２２１が計測した加速度ではなく、車速センサ２３０が計測した速度を用いているためである。
これにより、二次元座標を求める積分計算が１回で済み、速度の座標変換に用いる方位角（の初期値）の誤差の影響が比較的少なく、カルマンフィルタを用いて二次元座標と方位角とを適切に補正できる。
図５に戻り、Ｓ１１０の説明を続ける。
カルマンフィルタ部１３０は、上記の３自由度２次元航法（図６参照）を所定の初期化時間（例えば、数秒）または所定の航法回数だけ実行し、実行を終了したときに算出した方位角を「方位角初期値」としてストラップダウン演算部１４０に出力する。
Ｓ１１０の後、Ｓ１２０に進む。
Ｓ１１０の後、Ｓ１２０に進む。
Ｓ１２０において、走行時標定部１２１は、方位角の初期値または前回標定した方位角を用いて６自由度３次元航法（第二航法）により位置と姿勢と方位とを標定する。
「６自由度」は「緯度」「経度」「高度」「仰角」「回転角」「方位角」を意味し、「３次元」は「緯度、経度、高度」または「仰角、回転角、方位角」を意味する。
「６自由度」は「緯度」「経度」「高度」「仰角」「回転角」「方位角」を意味し、「３次元」は「緯度、経度、高度」または「仰角、回転角、方位角」を意味する。
図８は、実施の形態１における６自由度３次元航法のアルゴリズムを示す図である。
実施の形態１における６自由度３次元航法のアルゴリズムについて、図８に基づいて説明する。
実施の形態１における６自由度３次元航法のアルゴリズムについて、図８に基づいて説明する。
停車判定部１２３、入力補正部１２４、出力補正部１２５は自己位置姿勢標定部１２０が備える機能構成である。
入力補正部１２４は、加速度データ１９３に含まれる三次元加速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元加速度を補正する。
さらに、入力補正部１２４は、角速度データ１９４に含まれる三次元角速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元角速度を補正する。
さらに、入力補正部１２４は、角速度データ１９４に含まれる三次元角速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元角速度を補正する。
ストラップダウン演算部１４０は、入力補正部１２４により補正された三次元加速度と入力補正部１２４により補正された三次元角速度とを用いて位置と姿勢と方位とを算出する（図４参照）。
ここで、ストラップダウン演算部１４０の姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
図７で説明したように、初期値算出部１１０により算出される方位角は正確な方位角に近い値であり、ストラップダウン演算に用いる方位角の初期値として適切な値である。例えば、初期値算出部１１０の代わりに方位磁石を用いた場合、ストラップダウン演算で用いる初期値として十分な精度の方位角を得ることができない。
ストラップダウン演算部１４０は、適切な方位角の初期値を用いることにより精度が高い位置と姿勢と方位とを算出することができる。
ここで、ストラップダウン演算部１４０の姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
図７で説明したように、初期値算出部１１０により算出される方位角は正確な方位角に近い値であり、ストラップダウン演算に用いる方位角の初期値として適切な値である。例えば、初期値算出部１１０の代わりに方位磁石を用いた場合、ストラップダウン演算で用いる初期値として十分な精度の方位角を得ることができない。
ストラップダウン演算部１４０は、適切な方位角の初期値を用いることにより精度が高い位置と姿勢と方位とを算出することができる。
出力補正部１２５は、ストラップダウン演算部１４０により算出された位置にカルマンフィルタ部１３０により算出される補正値を加算して位置を補正し、補正した位置を標定結果の位置として出力する。
また、出力補正部１２５は、ストラップダウン演算部１４０により算出された姿勢にカルマンフィルタ部１３０により算出される補正値を加算して姿勢を補正し、補正した姿勢を標定結果の姿勢角として出力する。
さらに、出力補正部１２５は、ストラップダウン演算部１４０により算出された方位にカルマンフィルタ部１３０により算出される補正値を加算して方位を補正し、補正した方位を標定結果の方位として出力する。
また、出力補正部１２５は、ストラップダウン演算部１４０により算出された姿勢にカルマンフィルタ部１３０により算出される補正値を加算して姿勢を補正し、補正した姿勢を標定結果の姿勢角として出力する。
さらに、出力補正部１２５は、ストラップダウン演算部１４０により算出された方位にカルマンフィルタ部１３０により算出される補正値を加算して方位を補正し、補正した方位を標定結果の方位として出力する。
停車判定部１２３は、車両２００が停車しているか否かを判定する。
カルマンフィルタ部１３０は、車両２００が停車していない場合、ＧＰＳデータ１９１に含まれる位置とストラップダウン演算部１４０により算出された位置との差を「位置残差」として算出する。
また、カルマンフィルタ部１３０は、速度データ１９２に含まれる速度とストラップダウン演算部１４０により算出された速度との差を「速度残差」として算出する。
そして、カルマンフィルタ部１３０は、位置残差と速度残差とを入力値として第２のカルマンフィルタ１３２を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
また、カルマンフィルタ部１３０は、速度データ１９２に含まれる速度とストラップダウン演算部１４０により算出された速度との差を「速度残差」として算出する。
そして、カルマンフィルタ部１３０は、位置残差と速度残差とを入力値として第２のカルマンフィルタ１３２を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
カルマンフィルタ部１３０は、車両２００が停車している場合、ストラップダウン演算部１４０により算出された方位に基づいて方位の変化量を「方位残差」として算出する。
そして、カルマンフィルタ部１３０は、方位残差を入力値として第３のカルマンフィルタ１３３を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
そして、カルマンフィルタ部１３０は、方位残差を入力値として第３のカルマンフィルタ１３３を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
図５に戻り、自己位置姿勢標定方法の説明を続ける。
Ｓ１２０の後、Ｓ１３０に進む。
Ｓ１３０において、停車判定部１２３は、速度データ１９２に含まれる速度を所定の速度閾値と比較し、速度が速度閾値未満である場合、車両２００が停車していると判定し、速度が速度閾値以上である場合、車両２００が停車していないと判定する。
但し、停車判定部１２３は、車両２００が停車しているか否かをその他のデータに基づいて判定してもよい。例えば、停車判定部１２３は、ＧＰＳデータ１９１に含まれる速度に基づいて判定を行ってもよい。また、停車判定部１２３は、ＧＰＳデータ１９１に含まれる前回の位置と今回の位置とに基づいて位置の変化量が所定の位置閾値未満である場合に車両２００が停車していると判定してもよい。また、停車判定部１２３は、加速度データ１９３（または角速度データ１９４）に基づいて加速度（または角速度）の変化量が所定の加速度閾値（または角速度閾値）未満である場合に車両２００が停車していると判定してもよい。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に進む。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
但し、停車判定部１２３は、車両２００が停車しているか否かをその他のデータに基づいて判定してもよい。例えば、停車判定部１２３は、ＧＰＳデータ１９１に含まれる速度に基づいて判定を行ってもよい。また、停車判定部１２３は、ＧＰＳデータ１９１に含まれる前回の位置と今回の位置とに基づいて位置の変化量が所定の位置閾値未満である場合に車両２００が停車していると判定してもよい。また、停車判定部１２３は、加速度データ１９３（または角速度データ１９４）に基づいて加速度（または角速度）の変化量が所定の加速度閾値（または角速度閾値）未満である場合に車両２００が停車していると判定してもよい。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に進む。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
Ｓ１４０において、停車時標定部１２２は、前回標定された方位角を用いて６自由度３次元航法（図８参照）により位置と姿勢と方位とを標定する。
但し、停車時標定部１２２は、６自由度３次元航法に「拡張ＺＵＰＴアルゴリズム」を適用する。
但し、停車時標定部１２２は、６自由度３次元航法に「拡張ＺＵＰＴアルゴリズム」を適用する。
拡張ＺＵＰＴアルゴリズムは、ＺＵＰＴ（Ｚｅｒｏ ｖｅｌｏｃｉｔｙ ＵＰ ｄａＴｅ）という従来技術を拡張した新規技術である。ＺＵＰＴとは、停止中の位置の計測値を固定することによりセンサ誤差による位置（計測値）の変化を防ぐ技術である。
拡張ＺＵＰＴアルゴリズムは、停止中の方位角の計測値を固定することによりセンサ誤差による方位角（計測値）の変化を防ぐ。
但し、拡張ＺＵＰＴアルゴリズムは、停止中の姿勢角の計測値を固定しない。車両２００が停車中であっても人の乗り降りや車内での人の移動によって、車両２００の姿勢角は変化するからである。もし、姿勢角の計測値を固定してしまうと姿勢角が実際に変化しているにも関わらず、カルマンフィルタが姿勢角の変化量をセンサ誤差として判断してしまい、間違った補正値を出力してしまう。また、姿勢角はレベリングによって正確な値を求めることができる。
拡張ＺＵＰＴアルゴリズムは、停止中の方位角の計測値を固定することによりセンサ誤差による方位角（計測値）の変化を防ぐ。
但し、拡張ＺＵＰＴアルゴリズムは、停止中の姿勢角の計測値を固定しない。車両２００が停車中であっても人の乗り降りや車内での人の移動によって、車両２００の姿勢角は変化するからである。もし、姿勢角の計測値を固定してしまうと姿勢角が実際に変化しているにも関わらず、カルマンフィルタが姿勢角の変化量をセンサ誤差として判断してしまい、間違った補正値を出力してしまう。また、姿勢角はレベリングによって正確な値を求めることができる。
図９は、実施の形態１における車両２００の停車時の方位角のドリフトを示すグラフである。
車両２００の停車時（但し、人の乗り降り及び車内での人の移動は無し）の回転角、仰角、方位角の変化を図９に示す。
図９に示すように、方位角の値は角速度センサ２２２のバイアス誤差により、時間の経過に伴ってドリフトする。
一方、回転角、仰角の値（姿勢角の値）は、重力加速度と加速度センサ２２１により計測された加速度との比較によって安定化することができる（レベリング）。
車両２００の停車時（但し、人の乗り降り及び車内での人の移動は無し）の回転角、仰角、方位角の変化を図９に示す。
図９に示すように、方位角の値は角速度センサ２２２のバイアス誤差により、時間の経過に伴ってドリフトする。
一方、回転角、仰角の値（姿勢角の値）は、重力加速度と加速度センサ２２１により計測された加速度との比較によって安定化することができる（レベリング）。
図１０は、実施の形態１における拡張ＺＵＰＴアルゴリズムの概要図である。
実施の形態１における拡張ＺＵＰＴアルゴリズムの概要について、図１０に基づいて説明する。
実施の形態１における拡張ＺＵＰＴアルゴリズムの概要について、図１０に基づいて説明する。
拡張ＺＵＰＴアルゴリズムでは、車両２００の左右に「仮想アンカポイント」を設定し、車両２００の長さ方向の２箇所に「仮想センシングポイント」を設定し、仮想アンカポイントと仮想センシングポイントとをワイヤーで結んだ状態を仮定する。
この場合、車両２００の姿勢角（ｘ軸またはｙ軸回りの回転）を変化させることは可能であるが、車両２００の方位角（ｚ軸回りの回転）を変化させることはできない。つまり、方位角について車両２００が拘束される。
この場合、車両２００の姿勢角（ｘ軸またはｙ軸回りの回転）を変化させることは可能であるが、車両２００の方位角（ｚ軸回りの回転）を変化させることはできない。つまり、方位角について車両２００が拘束される。
この状態において、仮想アンカポイントと仮想センシングポイントとの距離の二重差を観測値とする。
観測値を表す観測式（１）−（３）を以下に示す。式（３）に示すように観測値は「０」である。
観測値を表す観測式（１）−（３）を以下に示す。式（３）に示すように観測値は「０」である。
上記式（２）の行列Ａを表す式（６）および行列Ａの算出式（４）（５）を以下に示す。
上記式（４）の右辺の行列は、方位角を車体座標系（ｘｙｚ）から局所水平座標系（ＮＥＤ）に変換する方向余弦行列である。
また、上記式（４）の右辺の１つ目のベクトルは車両２００の右方向の単位行列であり、２つ目のベクトルは車両２００の左方向の単位行列である。
また、上記式（４）の右辺の１つ目のベクトルは車両２００の右方向の単位行列であり、２つ目のベクトルは車両２００の左方向の単位行列である。
上記式（２）に含まれる行列Ａと行列Ｃとベクトルｂとのうち、行列Ｃはセンサ誤差の影響による方位角の計測値の変化に応じて変化する。
そこで、予測値を以下の観測式（７）で表す。
そこで、予測値を以下の観測式（７）で表す。
式（２）に表した観測値と式（７）に表した予測値との差を方位角の観測誤差（方位残差）として以下の式（８）で表す。
図５に戻り、Ｓ１４０の説明を続ける。
６自由度３次元航法（図８）において、拡張ＺＵＰＴアルゴリズムを適用した第３のカルマンフィルタ１３３が実行される。
第３のカルマンフィルタ１３３は、上記式（８）を観測方程式として用いてカルマンフィルタ処理により各種の補正値を算出する。
Ｓ１４０の後、Ｓ１５０に進む。
第３のカルマンフィルタ１３３は、上記式（８）を観測方程式として用いてカルマンフィルタ処理により各種の補正値を算出する。
Ｓ１４０の後、Ｓ１５０に進む。
Ｓ１５０において、停車判定部１２３は、車両２００が停車しているか否かをＳ１３０と同様に判定する。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に戻る。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に戻る。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
図１１は、実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図である。
図１１において、自己位置姿勢標定装置１００は、ＣＰＵ９０１（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を備えている。ＣＰＵ９０１は、バス９０２を介してＲＯＭ９０３、ＲＡＭ９０４、通信ボード９０５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。
図１１において、自己位置姿勢標定装置１００は、ＣＰＵ９０１（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を備えている。ＣＰＵ９０１は、バス９０２を介してＲＯＭ９０３、ＲＡＭ９０４、通信ボード９０５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。
通信ボード９０５は、有線または無線で、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット、電話回線などの通信網に接続している。
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２２、ファイル群９２３が記憶されている。
プログラム群９２２には、実施の形態において「〜部」として説明する機能を実行するプログラムが含まれる。プログラムは、ＣＰＵ９０１により読み出され実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものであり、また「〜部」の手順や方法をコンピュータに実行させるものである。
ファイル群９２３には、実施の形態において説明する「〜部」で使用される各種データ（入力、出力、判定結果、計算結果、処理結果など）が含まれる。
実施の形態において構成図およびフローチャートに含まれている矢印は主としてデータや信号の入出力を示す。
実施の形態において「〜部」として説明するものは「〜回路」、「〜装置」、「〜機器」であってもよく、また「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ファームウェア、ソフトウェア、ハードウェアまたはこれらの組み合わせのいずれで実装されても構わない。
実施の形態１において、例えば、以下のような自己位置姿勢標定装置１００について説明した。
自己位置姿勢標定装置１００は、ＧＰＳとＩＭＵと車速パルスとを用いて（１）方位を求めるための初期演算と、（２）停車中に実施する方位に関するカルマンフィルタの観測更新アルゴリズム（拡張ＺＵＰＴ）とを実行する。
（１）（２）により、低価格で中精度なＩＭＵと設置場所に制約がない１台のＧＰＳとを用いて６自由度３次元航法を行うことが可能となる。
（１）（２）により、低価格で中精度なＩＭＵと設置場所に制約がない１台のＧＰＳとを用いて６自由度３次元航法を行うことが可能となる。
１００ 自己位置姿勢標定装置、１１０ 初期値算出部、１１１ 入力補正部、１１２ 方位更新部、１１３ 座標変換部、１１４ 緯度経度更新部、１１５ 出力補正部、１２０ 自己位置姿勢標定部、１２１ 走行時標定部、１２２ 停車時標定部、１２３ 停車判定部、１２４ 入力補正部、１２５ 出力補正部、１３０ カルマンフィルタ部、１３１ 第１のカルマンフィルタ、１３２ 第２のカルマンフィルタ、１３３ 第３のカルマンフィルタ、１４０ ストラップダウン演算部、１４１ 角速度座標変換部、１４２ 姿勢方位更新部、１４３ 加速度座標変換部、１４４ 速度位置更新部、１４５ 重力補正計算部、１４６ コリオリ補正計算部、１４７ 航法系回転補正計算部、１４８Ａ 第一補正部、１４８Ｂ 第二補正部、１４８Ｃ 第三補正部、１９０ 装置記憶部、１９１ ＧＰＳデータ、１９２ 速度データ、１９３ 加速度データ、１９４ 角速度データ、２００ 車両、２０１ 天板、２１０ ＧＰＳ受信機、２１１ ＧＰＳアンテナ、２２０ 慣性センサ、２２１ 加速度センサ、２２２ 角速度センサ、２３０ 車速センサ、９０１ ＣＰＵ、９０２ バス、９０３ ＲＯＭ、９０４ ＲＡＭ、９０５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２２ プログラム群、９２３ ファイル群。
Claims (11)
- ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部と
を備えたことを特徴とする位置標定装置。 - 前記標定値算出部は、
移動していた前記移動体が移動を停止した場合、第３のカルマンフィルタを用いて前記第二航法方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記第二航法方位角を補正して補正後の第二航法方位角を算出し、
停止した前記移動体が移動を再開した場合、前記補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出し、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出し、算出した新たな第二航法座標値を補正する位置補正値を前記第２のカルマンフィルタを用いて算出し、算出した位置補正値を用いて前記新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する
ことを特徴とする請求項１記載の位置標定装置。 - 前記標定値算出部は、
前記速度センサにより計測された速度が所定の速度閾値未満である場合、前記移動体が移動を停止したと判定し、
前記移動体が移動を停止したと判定した後、前記速度センサにより計測された速度が前記速度閾値以上である場合、前記移動体が移動を再開したと判定する
ことを特徴とする請求項２記載の位置標定装置。 - 前記第３のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を前記方位角補正値として算出する
ことを特徴とする請求項２または請求項３記載の位置標定装置。 - 前記第３のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて前記方位角補正値を算出する
ことを特徴とする請求項２から請求項４いずれかに記載の位置標定装置。 - ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力する標定値算出部と、
移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正する方位角補正部とを備え、
前記標定値算出部は、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力する
ことを特徴とする位置標定装置。 - ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
初期値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力し、
標定値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する
ことを特徴とする位置標定装置の位置標定方法。 - ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
標定値算出部は、前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力し、
方位角補正部は、移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正し、
前記標定値算出部は、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力する
ことを特徴とする位置標定装置の位置標定方法。 - ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部として
コンピュータを機能させることを特徴とする位置標定プログラム。 - ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力する標定値算出部と、
移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正する方位角補正部としてコンピュータを機能させ、
前記標定値算出部としてコンピュータに、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力させる
ことを特徴とする位置標定プログラム。 - 移動体の停止状態からの移動に応じて、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、速度センサ、加速度センサ、および角速度センサの計測データから自己の方位角を求め、移動後の再停止時に求めた方位角を初期方位角として確定するとともに、当該確定された初期方位角と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、速度センサ、加速度センサ、および角速度センサの計測データに基づいて、カルマンフィルタを用いた航法演算により自己位置を標定する演算装置を備えた位置標定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011056105A JP5602070B2 (ja) | 2011-03-15 | 2011-03-15 | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011056105A JP5602070B2 (ja) | 2011-03-15 | 2011-03-15 | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2012193965A true JP2012193965A (ja) | 2012-10-11 |
| JP5602070B2 JP5602070B2 (ja) | 2014-10-08 |
Family
ID=47086012
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011056105A Active JP5602070B2 (ja) | 2011-03-15 | 2011-03-15 | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5602070B2 (ja) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2015035165A (ja) * | 2013-08-09 | 2015-02-19 | 本田技研工業株式会社 | 標識情報出力装置 |
| CN105865450A (zh) * | 2016-04-19 | 2016-08-17 | 武汉理工大学 | 一种基于步态的零速更新方法及系统 |
| KR20170109646A (ko) * | 2015-12-21 | 2017-09-29 | 샹하이 화처 네비게이션 테크놀로지 엘티디. | 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법 |
| US10147005B2 (en) | 2013-07-31 | 2018-12-04 | Honda Motor Co., Ltd. | Sign display apparatus |
| CN109855617A (zh) * | 2019-02-28 | 2019-06-07 | 深圳市元征科技股份有限公司 | 一种车辆定位方法、车辆定位装置及终端设备 |
| CN111272170A (zh) * | 2020-03-17 | 2020-06-12 | 电子科技大学 | 一种基于SoC的实时行人定位系统及方法 |
| CN111344590A (zh) * | 2018-01-30 | 2020-06-26 | 古野电气株式会社 | 雷达天线装置以及方位测定方法 |
| CN112363196A (zh) * | 2020-10-20 | 2021-02-12 | 北京嘀嘀无限科技发展有限公司 | 车辆属性确定方法、装置、存储介质和电子设备 |
| WO2021027638A1 (zh) * | 2019-08-09 | 2021-02-18 | 深圳市道通智能航空技术有限公司 | 一种偏航角的融合方法、装置及飞行器 |
| CN113758502A (zh) * | 2021-09-24 | 2021-12-07 | 广东汇天航空航天科技有限公司 | 组合导航处理方法及装置 |
| WO2022018964A1 (ja) * | 2020-07-20 | 2022-01-27 | ソニーグループ株式会社 | 情報処理装置、情報処理方法およびプログラム |
| JP2022524634A (ja) * | 2019-03-12 | 2022-05-09 | ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング | 車両用の位置特定装置、及び、車両 |
Citations (37)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63151813A (ja) * | 1986-12-16 | 1988-06-24 | Matsushita Electric Ind Co Ltd | 移動体位置検出装置 |
| JPS63302317A (ja) * | 1987-06-03 | 1988-12-09 | Mitsubishi Heavy Ind Ltd | 移動体の位置速度測定装置 |
| JPH04198787A (ja) * | 1990-11-28 | 1992-07-20 | Matsushita Electric Ind Co Ltd | 航法システムの方位測定方法 |
| JPH04213083A (ja) * | 1990-12-06 | 1992-08-04 | Mazda Motor Corp | 車両用方位検出装置 |
| JPH0526681A (ja) * | 1991-07-25 | 1993-02-02 | Nissan Motor Co Ltd | ナビゲーシヨン装置 |
| JPH05157828A (ja) * | 1991-12-04 | 1993-06-25 | Maspro Denkoh Corp | 移動体の位置検出装置 |
| JPH05215564A (ja) * | 1992-02-05 | 1993-08-24 | Japan Aviation Electron Ind Ltd | 車両用位置測定装置 |
| JPH08271607A (ja) * | 1995-03-30 | 1996-10-18 | Aisin Seiki Co Ltd | 移動体測位装置 |
| JPH095106A (ja) * | 1995-06-22 | 1997-01-10 | Tamagawa Seiki Co Ltd | 慣性装置の真方位検出方法 |
| JPH095105A (ja) * | 1995-06-22 | 1997-01-10 | Tech Res & Dev Inst Of Japan Def Agency | 慣性航法装置の移動中における調定演算方法 |
| JPH09113293A (ja) * | 1995-10-17 | 1997-05-02 | Japan Radio Co Ltd | 車両方位推測方式 |
| JPH09189564A (ja) * | 1996-01-11 | 1997-07-22 | Matsushita Electric Ind Co Ltd | 移動体位置速度算出装置 |
| JPH09507581A (ja) * | 1994-10-24 | 1997-07-29 | キャタピラー インコーポレイテッド | 自律式車両の作動点を正確に求めるためのシステム及び方法 |
| JPH10176932A (ja) * | 1996-10-14 | 1998-06-30 | Yokogawa Denshi Kiki Kk | 慣性航法装置 |
| JPH10267650A (ja) * | 1997-03-27 | 1998-10-09 | Asia Kosoku Kk | 道路線形自動測量装置 |
| JPH10307032A (ja) * | 1997-05-02 | 1998-11-17 | Pioneer Electron Corp | ナビゲーション装置 |
| JPH1194573A (ja) * | 1997-09-16 | 1999-04-09 | Bio Oriented Technol Res Advancement Inst | 移動体用位置姿勢測定装置 |
| JP2000502803A (ja) * | 1995-12-28 | 2000-03-07 | マジェラン ディス インコーポレイテッド | 改良された車両ナビゲーションシステム用ゼロ運動検出システム |
| JP2000241179A (ja) * | 1999-02-19 | 2000-09-08 | Zenrin Co Ltd | ハイブリッド走行軌跡取得方法およびハイブリッド走行軌跡取得システム |
| JP2002303533A (ja) * | 2001-01-08 | 2002-10-18 | Motorola Inc | 車両ナビゲーション・システムの位置およびヘッディング・エラーを修正する方法および装置 |
| JP2003509671A (ja) * | 1999-09-15 | 2003-03-11 | シーフ、テクノロジー、インコーポレーテッド | 物体の位置を追跡するためのナビゲーションシステムおよび方法 |
| JP2003139499A (ja) * | 2001-10-31 | 2003-05-14 | Tamagawa Seiki Co Ltd | 飛翔体における初期化方法 |
| US20030216865A1 (en) * | 2002-04-12 | 2003-11-20 | Ensco, Inc. | Inertial navigation system for mobile objects with constraints |
| JP2007504448A (ja) * | 2003-09-05 | 2007-03-01 | ノヴァテル・インコーポレイテッド | 慣性システム用の入力された位置合わせデータを使用する慣性ｇｐｓナビゲーションシステム |
| JP2007156890A (ja) * | 2005-12-06 | 2007-06-21 | Tamagawa Seiki Co Ltd | 無人車輌の初期方位角設定方法 |
| JP2008002906A (ja) * | 2006-06-21 | 2008-01-10 | Toyota Motor Corp | 測位装置 |
| JP2008076389A (ja) * | 2006-09-19 | 2008-04-03 | Alpine Electronics Inc | ナビゲーション・システムおよびナビゲーション方法 |
| JP2008116370A (ja) * | 2006-11-06 | 2008-05-22 | Toyota Motor Corp | 移動体位置測位装置 |
| JP2008232867A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | 移動体の姿勢及び方位検出装置 |
| JP2008232869A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | Ｇｐｓ複合航法装置 |
| JP2009236532A (ja) * | 2008-03-26 | 2009-10-15 | Seiko Epson Corp | 測位方法、プログラム及び測位装置 |
| JP2010287156A (ja) * | 2009-06-15 | 2010-12-24 | Mitsubishi Electric Corp | モデル生成装置、モデル生成方法、モデル生成プログラム、点群画像生成方法および点群画像生成プログラム |
| JP2011017599A (ja) * | 2009-07-08 | 2011-01-27 | Fujitsu Ltd | 自律測位プログラム、自律測位装置および自律測位方法 |
| JP2011122921A (ja) * | 2009-12-10 | 2011-06-23 | Mitsubishi Electric Corp | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム |
| JP2011185899A (ja) * | 2010-03-11 | 2011-09-22 | Mitsubishi Electric Corp | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
| JP2011220844A (ja) * | 2010-04-09 | 2011-11-04 | Seiko Epson Corp | 位置算出方法及び位置算出装置 |
| JP2012047495A (ja) * | 2010-08-24 | 2012-03-08 | Ono Sokki Co Ltd | 姿勢角安定化装置及び方法 |
-
2011
- 2011-03-15 JP JP2011056105A patent/JP5602070B2/ja active Active
Patent Citations (37)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63151813A (ja) * | 1986-12-16 | 1988-06-24 | Matsushita Electric Ind Co Ltd | 移動体位置検出装置 |
| JPS63302317A (ja) * | 1987-06-03 | 1988-12-09 | Mitsubishi Heavy Ind Ltd | 移動体の位置速度測定装置 |
| JPH04198787A (ja) * | 1990-11-28 | 1992-07-20 | Matsushita Electric Ind Co Ltd | 航法システムの方位測定方法 |
| JPH04213083A (ja) * | 1990-12-06 | 1992-08-04 | Mazda Motor Corp | 車両用方位検出装置 |
| JPH0526681A (ja) * | 1991-07-25 | 1993-02-02 | Nissan Motor Co Ltd | ナビゲーシヨン装置 |
| JPH05157828A (ja) * | 1991-12-04 | 1993-06-25 | Maspro Denkoh Corp | 移動体の位置検出装置 |
| JPH05215564A (ja) * | 1992-02-05 | 1993-08-24 | Japan Aviation Electron Ind Ltd | 車両用位置測定装置 |
| JPH09507581A (ja) * | 1994-10-24 | 1997-07-29 | キャタピラー インコーポレイテッド | 自律式車両の作動点を正確に求めるためのシステム及び方法 |
| JPH08271607A (ja) * | 1995-03-30 | 1996-10-18 | Aisin Seiki Co Ltd | 移動体測位装置 |
| JPH095106A (ja) * | 1995-06-22 | 1997-01-10 | Tamagawa Seiki Co Ltd | 慣性装置の真方位検出方法 |
| JPH095105A (ja) * | 1995-06-22 | 1997-01-10 | Tech Res & Dev Inst Of Japan Def Agency | 慣性航法装置の移動中における調定演算方法 |
| JPH09113293A (ja) * | 1995-10-17 | 1997-05-02 | Japan Radio Co Ltd | 車両方位推測方式 |
| JP2000502803A (ja) * | 1995-12-28 | 2000-03-07 | マジェラン ディス インコーポレイテッド | 改良された車両ナビゲーションシステム用ゼロ運動検出システム |
| JPH09189564A (ja) * | 1996-01-11 | 1997-07-22 | Matsushita Electric Ind Co Ltd | 移動体位置速度算出装置 |
| JPH10176932A (ja) * | 1996-10-14 | 1998-06-30 | Yokogawa Denshi Kiki Kk | 慣性航法装置 |
| JPH10267650A (ja) * | 1997-03-27 | 1998-10-09 | Asia Kosoku Kk | 道路線形自動測量装置 |
| JPH10307032A (ja) * | 1997-05-02 | 1998-11-17 | Pioneer Electron Corp | ナビゲーション装置 |
| JPH1194573A (ja) * | 1997-09-16 | 1999-04-09 | Bio Oriented Technol Res Advancement Inst | 移動体用位置姿勢測定装置 |
| JP2000241179A (ja) * | 1999-02-19 | 2000-09-08 | Zenrin Co Ltd | ハイブリッド走行軌跡取得方法およびハイブリッド走行軌跡取得システム |
| JP2003509671A (ja) * | 1999-09-15 | 2003-03-11 | シーフ、テクノロジー、インコーポレーテッド | 物体の位置を追跡するためのナビゲーションシステムおよび方法 |
| JP2002303533A (ja) * | 2001-01-08 | 2002-10-18 | Motorola Inc | 車両ナビゲーション・システムの位置およびヘッディング・エラーを修正する方法および装置 |
| JP2003139499A (ja) * | 2001-10-31 | 2003-05-14 | Tamagawa Seiki Co Ltd | 飛翔体における初期化方法 |
| US20030216865A1 (en) * | 2002-04-12 | 2003-11-20 | Ensco, Inc. | Inertial navigation system for mobile objects with constraints |
| JP2007504448A (ja) * | 2003-09-05 | 2007-03-01 | ノヴァテル・インコーポレイテッド | 慣性システム用の入力された位置合わせデータを使用する慣性ｇｐｓナビゲーションシステム |
| JP2007156890A (ja) * | 2005-12-06 | 2007-06-21 | Tamagawa Seiki Co Ltd | 無人車輌の初期方位角設定方法 |
| JP2008002906A (ja) * | 2006-06-21 | 2008-01-10 | Toyota Motor Corp | 測位装置 |
| JP2008076389A (ja) * | 2006-09-19 | 2008-04-03 | Alpine Electronics Inc | ナビゲーション・システムおよびナビゲーション方法 |
| JP2008116370A (ja) * | 2006-11-06 | 2008-05-22 | Toyota Motor Corp | 移動体位置測位装置 |
| JP2008232867A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | 移動体の姿勢及び方位検出装置 |
| JP2008232869A (ja) * | 2007-03-22 | 2008-10-02 | Furuno Electric Co Ltd | Ｇｐｓ複合航法装置 |
| JP2009236532A (ja) * | 2008-03-26 | 2009-10-15 | Seiko Epson Corp | 測位方法、プログラム及び測位装置 |
| JP2010287156A (ja) * | 2009-06-15 | 2010-12-24 | Mitsubishi Electric Corp | モデル生成装置、モデル生成方法、モデル生成プログラム、点群画像生成方法および点群画像生成プログラム |
| JP2011017599A (ja) * | 2009-07-08 | 2011-01-27 | Fujitsu Ltd | 自律測位プログラム、自律測位装置および自律測位方法 |
| JP2011122921A (ja) * | 2009-12-10 | 2011-06-23 | Mitsubishi Electric Corp | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム |
| JP2011185899A (ja) * | 2010-03-11 | 2011-09-22 | Mitsubishi Electric Corp | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
| JP2011220844A (ja) * | 2010-04-09 | 2011-11-04 | Seiko Epson Corp | 位置算出方法及び位置算出装置 |
| JP2012047495A (ja) * | 2010-08-24 | 2012-03-08 | Ono Sokki Co Ltd | 姿勢角安定化装置及び方法 |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10147005B2 (en) | 2013-07-31 | 2018-12-04 | Honda Motor Co., Ltd. | Sign display apparatus |
| JP2015035165A (ja) * | 2013-08-09 | 2015-02-19 | 本田技研工業株式会社 | 標識情報出力装置 |
| KR20170109646A (ko) * | 2015-12-21 | 2017-09-29 | 샹하이 화처 네비게이션 테크놀로지 엘티디. | 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법 |
| KR102018450B1 (ko) * | 2015-12-21 | 2019-09-04 | 샹하이 화처 네비게이션 테크놀로지 엘티디. | 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법 |
| CN105865450A (zh) * | 2016-04-19 | 2016-08-17 | 武汉理工大学 | 一种基于步态的零速更新方法及系统 |
| CN111344590A (zh) * | 2018-01-30 | 2020-06-26 | 古野电气株式会社 | 雷达天线装置以及方位测定方法 |
| CN109855617A (zh) * | 2019-02-28 | 2019-06-07 | 深圳市元征科技股份有限公司 | 一种车辆定位方法、车辆定位装置及终端设备 |
| JP2022524634A (ja) * | 2019-03-12 | 2022-05-09 | ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング | 車両用の位置特定装置、及び、車両 |
| US11835634B2 (en) | 2019-03-12 | 2023-12-05 | Robert Bosch Gmbh | Position determining arrangement for a vehicle, and vehicle |
| WO2021027638A1 (zh) * | 2019-08-09 | 2021-02-18 | 深圳市道通智能航空技术有限公司 | 一种偏航角的融合方法、装置及飞行器 |
| CN111272170A (zh) * | 2020-03-17 | 2020-06-12 | 电子科技大学 | 一种基于SoC的实时行人定位系统及方法 |
| WO2022018964A1 (ja) * | 2020-07-20 | 2022-01-27 | ソニーグループ株式会社 | 情報処理装置、情報処理方法およびプログラム |
| CN112363196A (zh) * | 2020-10-20 | 2021-02-12 | 北京嘀嘀无限科技发展有限公司 | 车辆属性确定方法、装置、存储介质和电子设备 |
| CN112363196B (zh) * | 2020-10-20 | 2023-10-31 | 北京嘀嘀无限科技发展有限公司 | 车辆属性确定方法、装置、存储介质和电子设备 |
| CN113758502A (zh) * | 2021-09-24 | 2021-12-07 | 广东汇天航空航天科技有限公司 | 组合导航处理方法及装置 |
| CN113758502B (zh) * | 2021-09-24 | 2024-02-20 | 广东汇天航空航天科技有限公司 | 组合导航处理方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP5602070B2 (ja) | 2014-10-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5602070B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
| EP3109589B1 (en) | A unit and method for improving positioning accuracy | |
| JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
| JP6094026B2 (ja) | 姿勢判定方法、位置算出方法及び姿勢判定装置 | |
| JP4964047B2 (ja) | 位置検出装置及び位置検出方法 | |
| JP5419665B2 (ja) | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム | |
| JP4989035B2 (ja) | 慣性ナビゲーションシステムの誤差補正 | |
| JP6083279B2 (ja) | 移動状況情報算出方法及び移動状況情報算出装置 | |
| JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
| JP5742450B2 (ja) | 位置算出方法及び位置算出装置 | |
| KR101394546B1 (ko) | Ｇｐｓ와 ｉｍｕ의 데이터 융합에 의한 위치 정보 획득장치 및 그의 위치 정보 획득방법 | |
| JP2016033473A (ja) | 位置算出方法及び位置算出装置 | |
| JP5164645B2 (ja) | カルマンフィルタ処理における繰り返し演算制御方法及び装置 | |
| CN111854740A (zh) | 能够在交通工具中进行航位推算的惯性导航系统 | |
| KR100525517B1 (ko) | 차량 항법 시스템 및 그 제어방법 | |
| JP6221295B2 (ja) | 位置算出方法及び位置算出装置 | |
| KR20190116500A (ko) | 차재기, 연산 장치 및 프로그램 | |
| JP2013228318A (ja) | キャリブレーション良否判定装置及び方法 | |
| JP4527171B2 (ja) | 単一ｇｐｓと慣性データ（加速度，角速度）によるビークル姿勢角の計測方法 | |
| KR101340158B1 (ko) | 고정 표적을 이용한 무인항공기의 표적 위치 보정 방법 및 컴퓨터 판독 가능한 기록매체 | |
| WO2017039000A1 (ja) | 移動体の走行軌跡計測システム、移動体、及び計測プログラム | |
| WO2020021867A1 (ja) | ジオイド測定方法、ジオイド測定装置、ジオイド推定装置、ジオイド計算用データ収集装置 | |
| KR101639152B1 (ko) | 도로 구배를 이용한 차량 위치 추정 방법 및 장치 | |
| KR20120062519A (ko) | 관성 항법 시스템의 바이어스 추정치를 이용한 정렬 장치 및 그 항법 시스템 | |
| JP5753026B2 (ja) | 移動体横流れ量計測装置及び方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20130927 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20140425 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20140507 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20140703 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20140722 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140819 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5602070 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |