位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
【課題】高精度でないＩＭＵと１台のＧＰＳ受信機とを用いて車両の位置を高精度に標定できるようにすることを目的とする。
【解決手段】初期値算出部１１０は、車両の出発時に３自由度２次元航法により方位角の初期値を算出する。走行時標定部１２１は、車両が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する。停車時標定部１２２は、車両が停車している間、方位角を拘束する拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する。
【解決手段】初期値算出部１１０は、車両の出発時に３自由度２次元航法により方位角の初期値を算出する。走行時標定部１２１は、車両が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する。停車時標定部１２２は、車両が停車している間、方位角を拘束する拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、車両の位置を標定する位置標定装置、位置標定装置の位置標定方法および位置標定プログラムに関するものである。
【背景技術】
【０００２】
従来、６自由度の３次元航法は、高価で高精度なＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を利用する必要があったため、航空・宇宙関連分野など、限られた分野でしか使用できなかった。
【０００３】
そこで、低価格化のために複数台のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機と車速パルスとを利用して車両の位置・姿勢・方位を標定することも考えられる。
しかし、ＧＰＳアンテナ同士をある程度離して相対位置が変化しないように設置する必要がある。この制約は、一般の乗用車に適用するには大きな制約である。
【０００４】
これらの制約が発生する具体的な要因は以下のとおりである。
（１）車両を停止した状態で方位を求める場合、中精度または低精度なＩＭＵでは、地球の自転を計測して方位を求めるといった計算が出来ない。
また、２台以上のＧＰＳ受信機を使用して方位を求める場合、ＧＰＳアンテナ間の基線解析を行うため、精度を出すためにはＧＰＳアンテナを可能な限り離して相対位置が変化しないよう設置する必要がある。
（２）３次元航法計算では、ある程度正確な初期値（位置・姿勢・方位）を設定しなければ計算結果が発散してしまう可能性が高い。
初期値のうち、姿勢角（ピッチ角、ロール角）は重力観測により得られ、位置はＧＰＳ観測により得られるが、方位に関しては上記（１）の理由により適切な値を得ることができない。
（３）正確な初期値が何らかの方法で与えられたとしても、その後の停車時にはジャイロの誤差等によって方位の値がドリフトしてしまう。
【０００５】
今後、３次元地図のインフラ整備が急速に進むほか、低価格なＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）センサの性能向上によって一般のカーナビゲーションシステムでも車両の位置・姿勢・方位を標定する３次元航法のニーズが高まることが予想されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２０１０−２８７１５６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明は、例えば、高精度でないＩＭＵと１台のＧＰＳ受信機とを用いて車両の位置を高精度に標定できるようにすることを目的とする。
【課題を解決するための手段】
【０００８】
本発明の位置標定装置は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する。
前記位置標定装置は、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部とを備える。
【発明の効果】
【０００９】
本発明によれば、例えば、高精度でないＩＭＵ（加速度センサ、角速度センサ）と１台のＧＰＳ受信機とを用いて車両（移動体の一例）の位置を高精度に標定することができる。
【図面の簡単な説明】
【００１０】
【図１】実施の形態１における車両２００の概要図。
【図２】実施の形態１における自己位置姿勢標定装置１００の概要図。
【図３】実施の形態１における自己位置姿勢標定装置１００の機能構成図。
【図４】実施の形態１におけるストラップダウン演算部１４０の機能構成図。
【図５】実施の形態１における自己位置姿勢標定方法を示すフローチャート。
【図６】実施の形態１における３自由度２次元航法のアルゴリズムを示す図。
【図７】実施の形態１における３自由度２次元航法による方位角の推定結果を示すグラフ。
【図８】実施の形態１における６自由度３次元航法のアルゴリズムを示す図。
【図９】実施の形態１における車両２００の停車時の方位角のドリフトを示すグラフ。
【図１０】実施の形態１における拡張ＺＵＰＴアルゴリズムの概要図。
【図１１】実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図。
【発明を実施するための形態】
【００１１】
実施の形態１．
車両の位置・姿勢・方位を標定する自己位置姿勢標定装置について説明する。
【００１２】
図１は、実施の形態１における車両２００の概要図である。
図２は、実施の形態１における自己位置姿勢標定装置１００の概要図である。
実施の形態１における車両２００の概要と自己位置姿勢標定装置１００の概要とについて、図１と図２とに基づいて説明する。
【００１３】
図１において、車両２００（移動体の一例）は、ＧＰＳ受信機２１０（およびＧＰＳアンテナ２１１）と慣性センサ２２０、車速センサ２３０（速度センサの一例）と自己位置姿勢標定装置１００（位置標定装置の一例）を備える。
「ＧＰＳ」は「Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ」の略である。
【００１４】
例えば、ＧＰＳ受信機２１０と慣性センサ２２０とを設置した天板２０１を車両２００の屋根部に設置する。また、自己位置姿勢標定装置１００と車速センサ２３０とを車両２００内に設置する。
但し、設置方法はこれに限らない。例えば、ＧＰＳ受信機２１０を屋根部以外に設置してもよいし、慣性センサ２２０を車両２００内に設置してもよく、天板２０１を設けることなく、ＧＰＳ受信機２１０と慣性センサ２２０を車両２００の内部または外部に取り付けてもよい。
また、自己位置姿勢標定装置１００は車両２００とは別個に設けた外部装置であっても構わない。
さらに、車両２００は他の移動体（例えば、飛行機、船舶、自動二輪車、自転車、列車）であっても構わない。
【００１５】
ＧＰＳ受信機２１０は、ＧＰＳ衛星から発信される測位信号をＧＰＳアンテナ２１１を用いて観測し、観測結果に基づいて位置を測位する。
【００１６】
慣性センサ２２０は、３軸方向（車両２００の長さ方向と幅方向と高さ方向）の加速度と角速度とを計測する。
慣性センサ２２０は、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）と呼ばれる。
以下、３軸方向の加速度を「三次元加速度」といい、３軸方向の角速度を「三次元角速度」という。
【００１７】
車速センサ２３０は、車軸の回転を車速パルスとして検出し、検出した車速パルスを用いて車両２００の速度を計測する。
【００１８】
図２に示すように、自己位置姿勢標定装置１００は、ＧＰＳ受信機２１０のデータと、慣性センサ２２０のデータと、車速センサ２３０のデータとに基づいて車両２００の位置と姿勢と方位とを標定する。
「位置」は、三次元の座標値（緯度、経度、高度）を意味する。
「姿勢」は、仰角（ピッチ角）と回転角（ロール角）とを意味する。
「方位」は、方位角を意味する。
【００１９】
図３は、実施の形態１における自己位置姿勢標定装置１００の機能構成図である。
実施の形態１における自己位置姿勢標定装置１００の機能構成について、図３に基づいて説明する。
【００２０】
自己位置姿勢標定装置１００（位置標定装置の一例）は、初期値算出部１１０、自己位置姿勢標定部１２０、カルマンフィルタ部１３０、ストラップダウン演算部１４０および装置記憶部１９０を備える。
【００２１】
装置記憶部１９０は、自己位置姿勢標定装置１００で使用するデータを記憶する。
ＧＰＳデータ１９１、速度データ１９２、加速度データ１９３または角速度データ１９４は、装置記憶部１９０に記憶されるデータの一例である。
【００２２】
ＧＰＳデータ１９１は、ＧＰＳ受信機２１０により計測されたデータであり、時刻に対応付けて「位置」や「速度」を含んでいる。速度は、過去の位置と現在の位置と過去から現在までの経過時間とに基づいて計測される。
速度データ１９２は、車速センサ２３０により計測されたデータであり、時刻に対応付けて「速度」を含んでいる。
加速度データ１９３は、慣性センサ２２０を構成する加速度センサ２２１により計測されたデータであり、時刻に対応付けて三次元加速度を含んでいる。
角速度データ１９４は、慣性センサ２２０を構成する角速度センサ２２２により計測されたデータであり、時刻に対応付けて三次元角速度を含んでいる。つまり、角速度データ１９４は、「仰角」の角速度と「回転角」の角速度と「方位角」の角速度とを含んでいる。角速度センサ２２２は「ジャイロ」と呼ばれる。
【００２３】
初期値算出部１１０は、カルマンフィルタ部１３０を制御して方位角の初期値を算出する（初期値算出処理）。
【００２４】
例えば、初期値算出部１１０は、以下のように方位角の初期値を算出する。
初期値算出部１１０は、角速度データ１９４に含まれる方位角の角速度に基づいて、車両２００の方位角を第一航法方位角として算出する（後述する方位更新部１１２の処理）。
初期値算出部１１０は、算出した第一航法方位角と速度データ１９２に含まれる速度とに基づいて、車両２００の座標値を第一航法座標値として算出する（後述する緯度経度更新部１１４の処理）。
初期値算出部１１０は、算出した第一航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第１のカルマンフィルタ１３１を用いて、第一航法方位角を補正する補正値を算出する。
初期値算出部１１０は、算出した補正値を用いて第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する（後述する出力補正部１１５の処理）。
【００２５】
自己位置姿勢標定部１２０は、走行時標定部１２１（標定値算出部の一例）と停車時標定部１２２（方位角補正部の一例）と停車判定部１２３（停止判定部の一例）とを備える。
走行時標定部１２１は、車両２００が走行しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定（算出）する。
停車時標定部１２２は、車両２００が停車しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定する。
【００２６】
例えば、走行時標定部１２１は、以下のように車両２００の位置を標定する。
走行時標定部１２１は、角速度データ１９４に含まれる方位角の角速度に基づいて、方位角の変化量を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した方位角の変化量と初期値算出部１１０から出力された方位角初期値とに基づいて、車両２００の方位角を第二航法方位角として算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した第二航法方位角と加速度データ１９３に含まれる加速度とに基づいて、車両２００の座標値を第二航法座標値として算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した第二航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第２のカルマンフィルタ１３２を用いて、第二航法座標値を補正する補正値を算出する。
走行時標定部１２１は、算出した補正値を用いて第二航法座標値を補正し、車両２００の位置を標定した位置標定値として補正後の第二航法座標値を出力する（後述する出力補正部１２５の処理）。
【００２７】
例えば、走行時標定部１２１と停車時標定部１２２とは、以下のように車両２００の位置を標定する。
停車時標定部１２２は、移動していた車両２００が移動を停止した場合、第３のカルマンフィルタ１３３を用いて第二航法方位角を補正する方位角補正値を算出する。
停車時標定部１２２は、算出した方位角補正値を用いて第二航法方位角を補正して補正後の第二航法方位角を算出する（後述する出力補正部１２５の処理）。
走行時標定部１２１は、停止した車両２００が移動を再開した場合、補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した新たな第二航法座標値を補正する位置補正値を第２のカルマンフィルタ１３２を用いて算出する。
走行時標定部１２１は、算出した位置補正値を用いて新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する（後述する出力補正部１２５の処理）。
【００２８】
停車判定部１２３は、車両２００が停車したか又は発車したかを判定する。
【００２９】
例えば、停車判定部１２３は、車両２００が停車したか又は発車したかを以下のように判定する。
停車判定部１２３は、車速センサ２３０により計測された速度が所定の速度閾値未満である場合、車両２００が移動を停止したと判定する。
停車判定部１２３は、車両２００が移動を停止したと判定した後、車速センサ２３０により計測された速度が速度閾値以上である場合、車両２００が移動を再開したと判定する。
【００３０】
カルマンフィルタ部１３０は、観測方程式や状態方程式を生成し、生成した観測方程式や状態方程式を用いて各種データの補正値を算出する。
カルマンフィルタ部１３０は、観測方程式が異なる複数のカルマンフィルタ（符号１３１−１３３）を備える。
【００３１】
例えば、第３のカルマンフィルタ１３３は、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を方位角補正値として算出する。
【００３２】
例えば、第３のカルマンフィルタ１３３は、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて方位角補正値を算出する。
【００３３】
図４は、実施の形態１におけるストラップダウン演算部１４０の機能構成図である。
実施の形態１におけるストラップダウン演算部１４０の機能構成について、図４に基づいて説明する。
【００３４】
ストラップダウン演算部１４０は、加速度データ１９３と角速度データ１９４とに基づいて速度と位置と姿勢と方位とを算出する。
【００３５】
ストラップダウン演算部１４０は、図４に示す構成（符号１４１−１４７、１４８Ａ−Ｃ）を備える。
【００３６】
角速度座標変換部１４１は、センサ座標系（車体座標系）の三次元角速度を入力する。
角速度座標変換部１４１は、姿勢方位更新部１４２により前回算出された姿勢と方位とに基づいて所定の座標変換式（例えば、方向余弦行列）を用いて三次元角速度をセンサ座標系の値から航法座標系（局所水平座標系）の値に変換する。
「センサ座標系」は車両２００を基準とする座標系であり、例えば、長さ方向を「ｘ」、幅方向を「ｙ」、高さ方向を「ｚ」として表される。
「航法座標系」は地面を基準とする座標系であり、例えば、「Ｎｏｒｔｈ（北）」「Ｅａｓｔ（東）」「Ｄｏｗｎ（下）」で表される。
【００３７】
第一補正部１４８Ａは、角速度座標変換部１４１により変換された三次元角速度に航法系回転補正計算部１４７により算出される補正値を加算して三次元角速度を補正する。
【００３８】
姿勢方位更新部１４２は、第一補正部１４８Ａにより補正された方位角の角速度を１回積分して方位角の変化量を算出し、算出した方位角の変化量を方位角の初期値または前回標定された方位角に加算して新たな方位角を算出する。
同様に、姿勢方位更新部１４２は、新たな姿勢角を算出する。
ここで、姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
また、姿勢方位更新部１４２は、加速度センサ２２１により停車時に計測された加速度を用いて所定の姿勢角算出式を計算し、姿勢角の初期値を算出する。姿勢角算出式は、計測された加速度と重力加速度と姿勢角との関係を表した式である。
【００３９】
加速度座標変換部１４３は、センサ座標系の三次元加速度を入力する。
加速度座標変換部１４３は、姿勢方位更新部１４２により算出された姿勢角と方位角とに基づいて、所定の座標変換式を用いて、三次元加速度をセンサ座標系の値から航法座標系の値に変換する。
【００４０】
第二補正部１４８Ｂは、加速度座標変換部１４３により変換された三次元加速度に重力補正計算部１４５により算出される補正値を加算して三次元加速度を補正する。
【００４１】
第三補正部１４８Ｃは、第二補正部１４８Ｂにより補正された三次元加速度にコリオリ補正計算部１４６により算出される補正値を加算して三次元加速度を補正する。
【００４２】
速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を１回積分して速度（速度ベクトル）を算出する。
また、速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を２回積分して位置の変化量を算出し、算出した位置の変化量を位置の初期値または前回標定された位置に加算して新たな位置を算出する。
例えば、速度位置更新部１４４は、ＧＰＳ受信機２１０により停車時に計測された位置を位置の初期値として用いる。
【００４３】
重力補正計算部１４５は、速度位置更新部１４４により算出された位置を入力値として所定の重力補正計算式を計算し、三次元加速度の補正値を算出する。この補正値は計測された三次元加速度に含まれる重力加速度を取り除くための値である。
【００４４】
コリオリ補正計算部１４６は、速度位置更新部１４４により算出された速度と姿勢方位更新部１４２により算出された姿勢とを入力値として所定のコリオリ補正計算式を計算し、三次元加速度の補正値を算出する。この補正値はコリオリ力による計測誤差を三次元加速度から取り除くための値である。
【００４５】
航法系回転補正計算部１４７は、速度位置更新部１４４により算出された速度を入力値として所定の航法系回転補正計算式を計算し、三次元角速度の補正値を算出する。この補正値は地球の自転による計測誤差を三次元角速度から取り除くための値である。
【００４６】
図５は、実施の形態１における自己位置姿勢標定方法を示すフローチャートである。
実施の形態１における自己位置姿勢標定方法について、図５に基づいて説明する。
【００４７】
まず、自己位置姿勢標定方法の概要について説明する。
【００４８】
初期値算出部１１０は、車両２００の出発時に３自由度２次元航法により方位角の初期値を算出する（Ｓ１１０）。
走行時標定部１２１は、車両２００が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１２０）。
停車時標定部１２２は、車両２００が停車している間、拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１４０）。
【００４９】
次に、自己位置姿勢標定方法の詳細について説明する。
【００５０】
Ｓ１１０において、初期値算出部１１０は、車両２００の出発時、つまり、自己位置姿勢標定装置１００の起動時に３自由度２次元航法（第一航法）により方位角の初期値を算出する。
「３自由度」は「緯度」と「経度」と「方位」とを意味し、「２次元」は「緯度、経度」または「方位」を意味する。
【００５１】
図６は、実施の形態１における３自由度２次元航法のアルゴリズムを示す図である。
実施の形態１における３自由度２次元航法について、図６に基づいて説明する。
【００５２】
入力補正部１１１、方位更新部１１２、座標変換部１１３、緯度経度更新部１１４および出力補正部１１５は、初期値算出部１１０が備える機能構成である。
【００５３】
入力補正部１１１は、速度データ１９２に含まれる速度にカルマンフィルタ部１３０により算出される補正値を加算して速度（速度ベクトル）を補正する。
さらに、入力補正部１１１は、角速度データ１９４に含まれる「方位角」の角速度にカルマンフィルタ部１３０により算出される補正値を加算して方位角の角速度を補正する。
【００５４】
方位更新部１１２は、入力補正部１１１により補正された方位角の角速度を１回積分して方位角の変化量を算出する。
方位更新部１１２は、算出した方位角の変化量を方位角の初期値または出力補正部１１５により前回算出された方位角に加算して新たな方位角を算出する。
ここで、方位更新部１１２は方位角の初期値として所定の値（例えば、０度）を用いる。ここで用いる方位角の初期値は適当な値で構わない。つまり、方位角の初期値は、正確な方位角を示す適切な値であっても、不正確な方位角を示す不適切な値であっても構わない。
【００５５】
座標変換部１１３は、方位更新部１１２により算出された姿勢と方位とに基づいて、所定の座標変換式を用いて、入力補正部１１１により補正された速度をセンサ座標系の値から航法座標系の値に変換する。
これにより、航法座標系の二次元速度（緯度方向の速度、経度方向の速度）が得られる。
【００５６】
緯度経度更新部１１４は、座標変換部１１３により得られた二次元速度を１回積分して二次元座標（緯度、経度）の変化量を算出する。
緯度経度更新部１１４は、算出した二次元座標の変化量を二次元座標の初期値または出力補正部１１５により前回算出された二次元座標に加算して新たな二次元座標を算出する。
例えば、緯度経度更新部１１４は、車両２００が出発する前、つまり、車両２００の停車時にＧＰＳ受信機２１０により測位された二次元座標値を二次元座標の初期値として用いる。
【００５７】
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元座標と緯度経度更新部１１４により算出された二次元座標との差を「座標残差」として算出する。
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元速度と座標変換部１１３により得られた二次元速度とを差を「速度残差」として算出する。
カルマンフィルタ部１３０は、座標残差と速度残差とを入力値として第１のカルマンフィルタ１３１を実行し、速度の補正値と角速度の補正値と二次元座標の補正値と方位角の補正値とを算出する。
【００５８】
出力補正部１１５は、緯度経度更新部１１４により算出された二次元座標値にカルマンフィルタ部１３０により算出された補正値を加算して二次元座標値を補正し、補正した二次元座標値を出力する。
さらに、出力補正部１１５は、方位更新部１１２により算出された方位角にカルマンフィルタ部１３０により算出された補正値を加算して方位角を補正し、補正した方位角を出力する。
【００５９】
図７は、実施の形態１における３自由度２次元航法による方位角の推定結果を示すグラフである。
実施の形態１における３自由度２次元航法による方位角の推定結果について、図７に基づいて説明する。
【００６０】
図７において、３自由度２次元航法（図６参照）により算出した方位角を実線で示し、正しい方位角を鎖線で示す。
【００６１】
（１）車両２００が「−１８０度」の方位を向いて停車しているときに方位角の初期値「０度」を初期値算出部１１０の方位更新部１１２に設定し、車両２００の走行を開始した。
（２）車両２００を左回りに回転させると、３自由度２次元航法により算出した方位角（実線）は数秒程度で正しい方位角（鎖線）に近い適切な値になった。
（３）車両２００を停車させると、角速度センサ２２２のバイアス（計測誤差）により方位角の値がドリフトを始めた。
【００６２】
図７（１）（２）に示すように、３自由度２次元航法は、方位角の初期値が不適切な値であってもすぐに適切な値を求めることができる。
これは、３自由度２次元航法において、加速度センサ２２１が計測した加速度ではなく、車速センサ２３０が計測した速度を用いているためである。
これにより、二次元座標を求める積分計算が１回で済み、速度の座標変換に用いる方位角（の初期値）の誤差の影響が比較的少なく、カルマンフィルタを用いて二次元座標と方位角とを適切に補正できる。
【００６３】
図５に戻り、Ｓ１１０の説明を続ける。
【００６４】
カルマンフィルタ部１３０は、上記の３自由度２次元航法（図６参照）を所定の初期化時間（例えば、数秒）または所定の航法回数だけ実行し、実行を終了したときに算出した方位角を「方位角初期値」としてストラップダウン演算部１４０に出力する。
Ｓ１１０の後、Ｓ１２０に進む。
【００６５】
Ｓ１２０において、走行時標定部１２１は、方位角の初期値または前回標定した方位角を用いて６自由度３次元航法（第二航法）により位置と姿勢と方位とを標定する。
「６自由度」は「緯度」「経度」「高度」「仰角」「回転角」「方位角」を意味し、「３次元」は「緯度、経度、高度」または「仰角、回転角、方位角」を意味する。
【００６６】
図８は、実施の形態１における６自由度３次元航法のアルゴリズムを示す図である。
実施の形態１における６自由度３次元航法のアルゴリズムについて、図８に基づいて説明する。
【００６７】
停車判定部１２３、入力補正部１２４、出力補正部１２５は自己位置姿勢標定部１２０が備える機能構成である。
【００６８】
入力補正部１２４は、加速度データ１９３に含まれる三次元加速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元加速度を補正する。
さらに、入力補正部１２４は、角速度データ１９４に含まれる三次元角速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元角速度を補正する。
【００６９】
ストラップダウン演算部１４０は、入力補正部１２４により補正された三次元加速度と入力補正部１２４により補正された三次元角速度とを用いて位置と姿勢と方位とを算出する（図４参照）。
ここで、ストラップダウン演算部１４０の姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
図７で説明したように、初期値算出部１１０により算出される方位角は正確な方位角に近い値であり、ストラップダウン演算に用いる方位角の初期値として適切な値である。例えば、初期値算出部１１０の代わりに方位磁石を用いた場合、ストラップダウン演算で用いる初期値として十分な精度の方位角を得ることができない。
ストラップダウン演算部１４０は、適切な方位角の初期値を用いることにより精度が高い位置と姿勢と方位とを算出することができる。
【００７０】
出力補正部１２５は、ストラップダウン演算部１４０により算出された位置にカルマンフィルタ部１３０により算出される補正値を加算して位置を補正し、補正した位置を標定結果の位置として出力する。
また、出力補正部１２５は、ストラップダウン演算部１４０により算出された姿勢にカルマンフィルタ部１３０により算出される補正値を加算して姿勢を補正し、補正した姿勢を標定結果の姿勢角として出力する。
さらに、出力補正部１２５は、ストラップダウン演算部１４０により算出された方位にカルマンフィルタ部１３０により算出される補正値を加算して方位を補正し、補正した方位を標定結果の方位として出力する。
【００７１】
停車判定部１２３は、車両２００が停車しているか否かを判定する。
【００７２】
カルマンフィルタ部１３０は、車両２００が停車していない場合、ＧＰＳデータ１９１に含まれる位置とストラップダウン演算部１４０により算出された位置との差を「位置残差」として算出する。
また、カルマンフィルタ部１３０は、速度データ１９２に含まれる速度とストラップダウン演算部１４０により算出された速度との差を「速度残差」として算出する。
そして、カルマンフィルタ部１３０は、位置残差と速度残差とを入力値として第２のカルマンフィルタ１３２を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
【００７３】
カルマンフィルタ部１３０は、車両２００が停車している場合、ストラップダウン演算部１４０により算出された方位に基づいて方位の変化量を「方位残差」として算出する。
そして、カルマンフィルタ部１３０は、方位残差を入力値として第３のカルマンフィルタ１３３を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
【００７４】
図５に戻り、自己位置姿勢標定方法の説明を続ける。
【００７５】
Ｓ１２０の後、Ｓ１３０に進む。
【００７６】
Ｓ１３０において、停車判定部１２３は、速度データ１９２に含まれる速度を所定の速度閾値と比較し、速度が速度閾値未満である場合、車両２００が停車していると判定し、速度が速度閾値以上である場合、車両２００が停車していないと判定する。
但し、停車判定部１２３は、車両２００が停車しているか否かをその他のデータに基づいて判定してもよい。例えば、停車判定部１２３は、ＧＰＳデータ１９１に含まれる速度に基づいて判定を行ってもよい。また、停車判定部１２３は、ＧＰＳデータ１９１に含まれる前回の位置と今回の位置とに基づいて位置の変化量が所定の位置閾値未満である場合に車両２００が停車していると判定してもよい。また、停車判定部１２３は、加速度データ１９３（または角速度データ１９４）に基づいて加速度（または角速度）の変化量が所定の加速度閾値（または角速度閾値）未満である場合に車両２００が停車していると判定してもよい。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に進む。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
【００７７】
Ｓ１４０において、停車時標定部１２２は、前回標定された方位角を用いて６自由度３次元航法（図８参照）により位置と姿勢と方位とを標定する。
但し、停車時標定部１２２は、６自由度３次元航法に「拡張ＺＵＰＴアルゴリズム」を適用する。
【００７８】
拡張ＺＵＰＴアルゴリズムは、ＺＵＰＴ（Ｚｅｒｏ ｖｅｌｏｃｉｔｙ ＵＰ ｄａＴｅ）という従来技術を拡張した新規技術である。ＺＵＰＴとは、停止中の位置の計測値を固定することによりセンサ誤差による位置（計測値）の変化を防ぐ技術である。
拡張ＺＵＰＴアルゴリズムは、停止中の方位角の計測値を固定することによりセンサ誤差による方位角（計測値）の変化を防ぐ。
但し、拡張ＺＵＰＴアルゴリズムは、停止中の姿勢角の計測値を固定しない。車両２００が停車中であっても人の乗り降りや車内での人の移動によって、車両２００の姿勢角は変化するからである。もし、姿勢角の計測値を固定してしまうと姿勢角が実際に変化しているにも関わらず、カルマンフィルタが姿勢角の変化量をセンサ誤差として判断してしまい、間違った補正値を出力してしまう。また、姿勢角はレベリングによって正確な値を求めることができる。
【００７９】
図９は、実施の形態１における車両２００の停車時の方位角のドリフトを示すグラフである。
車両２００の停車時（但し、人の乗り降り及び車内での人の移動は無し）の回転角、仰角、方位角の変化を図９に示す。
図９に示すように、方位角の値は角速度センサ２２２のバイアス誤差により、時間の経過に伴ってドリフトする。
一方、回転角、仰角の値（姿勢角の値）は、重力加速度と加速度センサ２２１により計測された加速度との比較によって安定化することができる（レベリング）。
【００８０】
図１０は、実施の形態１における拡張ＺＵＰＴアルゴリズムの概要図である。
実施の形態１における拡張ＺＵＰＴアルゴリズムの概要について、図１０に基づいて説明する。
【００８１】
拡張ＺＵＰＴアルゴリズムでは、車両２００の左右に「仮想アンカポイント」を設定し、車両２００の長さ方向の２箇所に「仮想センシングポイント」を設定し、仮想アンカポイントと仮想センシングポイントとをワイヤーで結んだ状態を仮定する。
この場合、車両２００の姿勢角（ｘ軸またはｙ軸回りの回転）を変化させることは可能であるが、車両２００の方位角（ｚ軸回りの回転）を変化させることはできない。つまり、方位角について車両２００が拘束される。
【００８２】
この状態において、仮想アンカポイントと仮想センシングポイントとの距離の二重差を観測値とする。
観測値を表す観測式（１）−（３）を以下に示す。式（３）に示すように観測値は「０」である。
【００８３】
【数１】
【００８４】
上記式（２）の行列Ａを表す式（６）および行列Ａの算出式（４）（５）を以下に示す。
【００８５】
【数２】
【００８６】
上記式（４）の右辺の行列は、方位角を車体座標系（ｘｙｚ）から局所水平座標系（ＮＥＤ）に変換する方向余弦行列である。
また、上記式（４）の右辺の１つ目のベクトルは車両２００の右方向の単位行列であり、２つ目のベクトルは車両２００の左方向の単位行列である。
【００８７】
上記式（２）に含まれる行列Ａと行列Ｃとベクトルｂとのうち、行列Ｃはセンサ誤差の影響による方位角の計測値の変化に応じて変化する。
そこで、予測値を以下の観測式（７）で表す。
【００８８】
【数３】
【００８９】
式（２）に表した観測値と式（７）に表した予測値との差を方位角の観測誤差（方位残差）として以下の式（８）で表す。
【００９０】
【数４】
【００９１】
図５に戻り、Ｓ１４０の説明を続ける。
【００９２】
６自由度３次元航法（図８）において、拡張ＺＵＰＴアルゴリズムを適用した第３のカルマンフィルタ１３３が実行される。
第３のカルマンフィルタ１３３は、上記式（８）を観測方程式として用いてカルマンフィルタ処理により各種の補正値を算出する。
Ｓ１４０の後、Ｓ１５０に進む。
【００９３】
Ｓ１５０において、停車判定部１２３は、車両２００が停車しているか否かをＳ１３０と同様に判定する。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に戻る。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
【００９４】
図１１は、実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図である。
図１１において、自己位置姿勢標定装置１００は、ＣＰＵ９０１（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を備えている。ＣＰＵ９０１は、バス９０２を介してＲＯＭ９０３、ＲＡＭ９０４、通信ボード９０５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。
【００９５】
通信ボード９０５は、有線または無線で、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット、電話回線などの通信網に接続している。
【００９６】
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２２、ファイル群９２３が記憶されている。
【００９７】
プログラム群９２２には、実施の形態において「〜部」として説明する機能を実行するプログラムが含まれる。プログラムは、ＣＰＵ９０１により読み出され実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものであり、また「〜部」の手順や方法をコンピュータに実行させるものである。
【００９８】
ファイル群９２３には、実施の形態において説明する「〜部」で使用される各種データ（入力、出力、判定結果、計算結果、処理結果など）が含まれる。
【００９９】
実施の形態において構成図およびフローチャートに含まれている矢印は主としてデータや信号の入出力を示す。
【０１００】
実施の形態において「〜部」として説明するものは「〜回路」、「〜装置」、「〜機器」であってもよく、また「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ファームウェア、ソフトウェア、ハードウェアまたはこれらの組み合わせのいずれで実装されても構わない。
【０１０１】
実施の形態１において、例えば、以下のような自己位置姿勢標定装置１００について説明した。
【０１０２】
自己位置姿勢標定装置１００は、ＧＰＳとＩＭＵと車速パルスとを用いて（１）方位を求めるための初期演算と、（２）停車中に実施する方位に関するカルマンフィルタの観測更新アルゴリズム（拡張ＺＵＰＴ）とを実行する。
（１）（２）により、低価格で中精度なＩＭＵと設置場所に制約がない１台のＧＰＳとを用いて６自由度３次元航法を行うことが可能となる。
【符号の説明】
【０１０３】
１００ 自己位置姿勢標定装置、１１０ 初期値算出部、１１１ 入力補正部、１１２ 方位更新部、１１３ 座標変換部、１１４ 緯度経度更新部、１１５ 出力補正部、１２０ 自己位置姿勢標定部、１２１ 走行時標定部、１２２ 停車時標定部、１２３ 停車判定部、１２４ 入力補正部、１２５ 出力補正部、１３０ カルマンフィルタ部、１３１ 第１のカルマンフィルタ、１３２ 第２のカルマンフィルタ、１３３ 第３のカルマンフィルタ、１４０ ストラップダウン演算部、１４１ 角速度座標変換部、１４２ 姿勢方位更新部、１４３ 加速度座標変換部、１４４ 速度位置更新部、１４５ 重力補正計算部、１４６ コリオリ補正計算部、１４７ 航法系回転補正計算部、１４８Ａ 第一補正部、１４８Ｂ 第二補正部、１４８Ｃ 第三補正部、１９０ 装置記憶部、１９１ ＧＰＳデータ、１９２ 速度データ、１９３ 加速度データ、１９４ 角速度データ、２００ 車両、２０１ 天板、２１０ ＧＰＳ受信機、２１１ ＧＰＳアンテナ、２２０ 慣性センサ、２２１ 加速度センサ、２２２ 角速度センサ、２３０ 車速センサ、９０１ ＣＰＵ、９０２ バス、９０３ ＲＯＭ、９０４ ＲＡＭ、９０５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２２ プログラム群、９２３ ファイル群。
【技術分野】
【０００１】
本発明は、例えば、車両の位置を標定する位置標定装置、位置標定装置の位置標定方法および位置標定プログラムに関するものである。
【背景技術】
【０００２】
従来、６自由度の３次元航法は、高価で高精度なＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）を利用する必要があったため、航空・宇宙関連分野など、限られた分野でしか使用できなかった。
【０００３】
そこで、低価格化のために複数台のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機と車速パルスとを利用して車両の位置・姿勢・方位を標定することも考えられる。
しかし、ＧＰＳアンテナ同士をある程度離して相対位置が変化しないように設置する必要がある。この制約は、一般の乗用車に適用するには大きな制約である。
【０００４】
これらの制約が発生する具体的な要因は以下のとおりである。
（１）車両を停止した状態で方位を求める場合、中精度または低精度なＩＭＵでは、地球の自転を計測して方位を求めるといった計算が出来ない。
また、２台以上のＧＰＳ受信機を使用して方位を求める場合、ＧＰＳアンテナ間の基線解析を行うため、精度を出すためにはＧＰＳアンテナを可能な限り離して相対位置が変化しないよう設置する必要がある。
（２）３次元航法計算では、ある程度正確な初期値（位置・姿勢・方位）を設定しなければ計算結果が発散してしまう可能性が高い。
初期値のうち、姿勢角（ピッチ角、ロール角）は重力観測により得られ、位置はＧＰＳ観測により得られるが、方位に関しては上記（１）の理由により適切な値を得ることができない。
（３）正確な初期値が何らかの方法で与えられたとしても、その後の停車時にはジャイロの誤差等によって方位の値がドリフトしてしまう。
【０００５】
今後、３次元地図のインフラ整備が急速に進むほか、低価格なＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）センサの性能向上によって一般のカーナビゲーションシステムでも車両の位置・姿勢・方位を標定する３次元航法のニーズが高まることが予想されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２０１０−２８７１５６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明は、例えば、高精度でないＩＭＵと１台のＧＰＳ受信機とを用いて車両の位置を高精度に標定できるようにすることを目的とする。
【課題を解決するための手段】
【０００８】
本発明の位置標定装置は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する。
前記位置標定装置は、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部とを備える。
【発明の効果】
【０００９】
本発明によれば、例えば、高精度でないＩＭＵ（加速度センサ、角速度センサ）と１台のＧＰＳ受信機とを用いて車両（移動体の一例）の位置を高精度に標定することができる。
【図面の簡単な説明】
【００１０】
【図１】実施の形態１における車両２００の概要図。
【図２】実施の形態１における自己位置姿勢標定装置１００の概要図。
【図３】実施の形態１における自己位置姿勢標定装置１００の機能構成図。
【図４】実施の形態１におけるストラップダウン演算部１４０の機能構成図。
【図５】実施の形態１における自己位置姿勢標定方法を示すフローチャート。
【図６】実施の形態１における３自由度２次元航法のアルゴリズムを示す図。
【図７】実施の形態１における３自由度２次元航法による方位角の推定結果を示すグラフ。
【図８】実施の形態１における６自由度３次元航法のアルゴリズムを示す図。
【図９】実施の形態１における車両２００の停車時の方位角のドリフトを示すグラフ。
【図１０】実施の形態１における拡張ＺＵＰＴアルゴリズムの概要図。
【図１１】実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図。
【発明を実施するための形態】
【００１１】
実施の形態１．
車両の位置・姿勢・方位を標定する自己位置姿勢標定装置について説明する。
【００１２】
図１は、実施の形態１における車両２００の概要図である。
図２は、実施の形態１における自己位置姿勢標定装置１００の概要図である。
実施の形態１における車両２００の概要と自己位置姿勢標定装置１００の概要とについて、図１と図２とに基づいて説明する。
【００１３】
図１において、車両２００（移動体の一例）は、ＧＰＳ受信機２１０（およびＧＰＳアンテナ２１１）と慣性センサ２２０、車速センサ２３０（速度センサの一例）と自己位置姿勢標定装置１００（位置標定装置の一例）を備える。
「ＧＰＳ」は「Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ」の略である。
【００１４】
例えば、ＧＰＳ受信機２１０と慣性センサ２２０とを設置した天板２０１を車両２００の屋根部に設置する。また、自己位置姿勢標定装置１００と車速センサ２３０とを車両２００内に設置する。
但し、設置方法はこれに限らない。例えば、ＧＰＳ受信機２１０を屋根部以外に設置してもよいし、慣性センサ２２０を車両２００内に設置してもよく、天板２０１を設けることなく、ＧＰＳ受信機２１０と慣性センサ２２０を車両２００の内部または外部に取り付けてもよい。
また、自己位置姿勢標定装置１００は車両２００とは別個に設けた外部装置であっても構わない。
さらに、車両２００は他の移動体（例えば、飛行機、船舶、自動二輪車、自転車、列車）であっても構わない。
【００１５】
ＧＰＳ受信機２１０は、ＧＰＳ衛星から発信される測位信号をＧＰＳアンテナ２１１を用いて観測し、観測結果に基づいて位置を測位する。
【００１６】
慣性センサ２２０は、３軸方向（車両２００の長さ方向と幅方向と高さ方向）の加速度と角速度とを計測する。
慣性センサ２２０は、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）と呼ばれる。
以下、３軸方向の加速度を「三次元加速度」といい、３軸方向の角速度を「三次元角速度」という。
【００１７】
車速センサ２３０は、車軸の回転を車速パルスとして検出し、検出した車速パルスを用いて車両２００の速度を計測する。
【００１８】
図２に示すように、自己位置姿勢標定装置１００は、ＧＰＳ受信機２１０のデータと、慣性センサ２２０のデータと、車速センサ２３０のデータとに基づいて車両２００の位置と姿勢と方位とを標定する。
「位置」は、三次元の座標値（緯度、経度、高度）を意味する。
「姿勢」は、仰角（ピッチ角）と回転角（ロール角）とを意味する。
「方位」は、方位角を意味する。
【００１９】
図３は、実施の形態１における自己位置姿勢標定装置１００の機能構成図である。
実施の形態１における自己位置姿勢標定装置１００の機能構成について、図３に基づいて説明する。
【００２０】
自己位置姿勢標定装置１００（位置標定装置の一例）は、初期値算出部１１０、自己位置姿勢標定部１２０、カルマンフィルタ部１３０、ストラップダウン演算部１４０および装置記憶部１９０を備える。
【００２１】
装置記憶部１９０は、自己位置姿勢標定装置１００で使用するデータを記憶する。
ＧＰＳデータ１９１、速度データ１９２、加速度データ１９３または角速度データ１９４は、装置記憶部１９０に記憶されるデータの一例である。
【００２２】
ＧＰＳデータ１９１は、ＧＰＳ受信機２１０により計測されたデータであり、時刻に対応付けて「位置」や「速度」を含んでいる。速度は、過去の位置と現在の位置と過去から現在までの経過時間とに基づいて計測される。
速度データ１９２は、車速センサ２３０により計測されたデータであり、時刻に対応付けて「速度」を含んでいる。
加速度データ１９３は、慣性センサ２２０を構成する加速度センサ２２１により計測されたデータであり、時刻に対応付けて三次元加速度を含んでいる。
角速度データ１９４は、慣性センサ２２０を構成する角速度センサ２２２により計測されたデータであり、時刻に対応付けて三次元角速度を含んでいる。つまり、角速度データ１９４は、「仰角」の角速度と「回転角」の角速度と「方位角」の角速度とを含んでいる。角速度センサ２２２は「ジャイロ」と呼ばれる。
【００２３】
初期値算出部１１０は、カルマンフィルタ部１３０を制御して方位角の初期値を算出する（初期値算出処理）。
【００２４】
例えば、初期値算出部１１０は、以下のように方位角の初期値を算出する。
初期値算出部１１０は、角速度データ１９４に含まれる方位角の角速度に基づいて、車両２００の方位角を第一航法方位角として算出する（後述する方位更新部１１２の処理）。
初期値算出部１１０は、算出した第一航法方位角と速度データ１９２に含まれる速度とに基づいて、車両２００の座標値を第一航法座標値として算出する（後述する緯度経度更新部１１４の処理）。
初期値算出部１１０は、算出した第一航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第１のカルマンフィルタ１３１を用いて、第一航法方位角を補正する補正値を算出する。
初期値算出部１１０は、算出した補正値を用いて第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する（後述する出力補正部１１５の処理）。
【００２５】
自己位置姿勢標定部１２０は、走行時標定部１２１（標定値算出部の一例）と停車時標定部１２２（方位角補正部の一例）と停車判定部１２３（停止判定部の一例）とを備える。
走行時標定部１２１は、車両２００が走行しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定（算出）する。
停車時標定部１２２は、車両２００が停車しているときに、カルマンフィルタ部１３０とストラップダウン演算部１４０とを制御して車両２００の位置と姿勢と方位とを標定する。
【００２６】
例えば、走行時標定部１２１は、以下のように車両２００の位置を標定する。
走行時標定部１２１は、角速度データ１９４に含まれる方位角の角速度に基づいて、方位角の変化量を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した方位角の変化量と初期値算出部１１０から出力された方位角初期値とに基づいて、車両２００の方位角を第二航法方位角として算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した第二航法方位角と加速度データ１９３に含まれる加速度とに基づいて、車両２００の座標値を第二航法座標値として算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した第二航法座標値とＧＰＳデータ１９１に含まれる座標値とに基づいて、第２のカルマンフィルタ１３２を用いて、第二航法座標値を補正する補正値を算出する。
走行時標定部１２１は、算出した補正値を用いて第二航法座標値を補正し、車両２００の位置を標定した位置標定値として補正後の第二航法座標値を出力する（後述する出力補正部１２５の処理）。
【００２７】
例えば、走行時標定部１２１と停車時標定部１２２とは、以下のように車両２００の位置を標定する。
停車時標定部１２２は、移動していた車両２００が移動を停止した場合、第３のカルマンフィルタ１３３を用いて第二航法方位角を補正する方位角補正値を算出する。
停車時標定部１２２は、算出した方位角補正値を用いて第二航法方位角を補正して補正後の第二航法方位角を算出する（後述する出力補正部１２５の処理）。
走行時標定部１２１は、停止した車両２００が移動を再開した場合、補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出する（後述する姿勢方位更新部１４２の処理）。
走行時標定部１２１は、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出する（後述する速度位置更新部１４４の処理）。
走行時標定部１２１は、算出した新たな第二航法座標値を補正する位置補正値を第２のカルマンフィルタ１３２を用いて算出する。
走行時標定部１２１は、算出した位置補正値を用いて新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する（後述する出力補正部１２５の処理）。
【００２８】
停車判定部１２３は、車両２００が停車したか又は発車したかを判定する。
【００２９】
例えば、停車判定部１２３は、車両２００が停車したか又は発車したかを以下のように判定する。
停車判定部１２３は、車速センサ２３０により計測された速度が所定の速度閾値未満である場合、車両２００が移動を停止したと判定する。
停車判定部１２３は、車両２００が移動を停止したと判定した後、車速センサ２３０により計測された速度が速度閾値以上である場合、車両２００が移動を再開したと判定する。
【００３０】
カルマンフィルタ部１３０は、観測方程式や状態方程式を生成し、生成した観測方程式や状態方程式を用いて各種データの補正値を算出する。
カルマンフィルタ部１３０は、観測方程式が異なる複数のカルマンフィルタ（符号１３１−１３３）を備える。
【００３１】
例えば、第３のカルマンフィルタ１３３は、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を方位角補正値として算出する。
【００３２】
例えば、第３のカルマンフィルタ１３３は、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて方位角補正値を算出する。
【００３３】
図４は、実施の形態１におけるストラップダウン演算部１４０の機能構成図である。
実施の形態１におけるストラップダウン演算部１４０の機能構成について、図４に基づいて説明する。
【００３４】
ストラップダウン演算部１４０は、加速度データ１９３と角速度データ１９４とに基づいて速度と位置と姿勢と方位とを算出する。
【００３５】
ストラップダウン演算部１４０は、図４に示す構成（符号１４１−１４７、１４８Ａ−Ｃ）を備える。
【００３６】
角速度座標変換部１４１は、センサ座標系（車体座標系）の三次元角速度を入力する。
角速度座標変換部１４１は、姿勢方位更新部１４２により前回算出された姿勢と方位とに基づいて所定の座標変換式（例えば、方向余弦行列）を用いて三次元角速度をセンサ座標系の値から航法座標系（局所水平座標系）の値に変換する。
「センサ座標系」は車両２００を基準とする座標系であり、例えば、長さ方向を「ｘ」、幅方向を「ｙ」、高さ方向を「ｚ」として表される。
「航法座標系」は地面を基準とする座標系であり、例えば、「Ｎｏｒｔｈ（北）」「Ｅａｓｔ（東）」「Ｄｏｗｎ（下）」で表される。
【００３７】
第一補正部１４８Ａは、角速度座標変換部１４１により変換された三次元角速度に航法系回転補正計算部１４７により算出される補正値を加算して三次元角速度を補正する。
【００３８】
姿勢方位更新部１４２は、第一補正部１４８Ａにより補正された方位角の角速度を１回積分して方位角の変化量を算出し、算出した方位角の変化量を方位角の初期値または前回標定された方位角に加算して新たな方位角を算出する。
同様に、姿勢方位更新部１４２は、新たな姿勢角を算出する。
ここで、姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
また、姿勢方位更新部１４２は、加速度センサ２２１により停車時に計測された加速度を用いて所定の姿勢角算出式を計算し、姿勢角の初期値を算出する。姿勢角算出式は、計測された加速度と重力加速度と姿勢角との関係を表した式である。
【００３９】
加速度座標変換部１４３は、センサ座標系の三次元加速度を入力する。
加速度座標変換部１４３は、姿勢方位更新部１４２により算出された姿勢角と方位角とに基づいて、所定の座標変換式を用いて、三次元加速度をセンサ座標系の値から航法座標系の値に変換する。
【００４０】
第二補正部１４８Ｂは、加速度座標変換部１４３により変換された三次元加速度に重力補正計算部１４５により算出される補正値を加算して三次元加速度を補正する。
【００４１】
第三補正部１４８Ｃは、第二補正部１４８Ｂにより補正された三次元加速度にコリオリ補正計算部１４６により算出される補正値を加算して三次元加速度を補正する。
【００４２】
速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を１回積分して速度（速度ベクトル）を算出する。
また、速度位置更新部１４４は、第三補正部１４８Ｃにより補正された三次元加速度を２回積分して位置の変化量を算出し、算出した位置の変化量を位置の初期値または前回標定された位置に加算して新たな位置を算出する。
例えば、速度位置更新部１４４は、ＧＰＳ受信機２１０により停車時に計測された位置を位置の初期値として用いる。
【００４３】
重力補正計算部１４５は、速度位置更新部１４４により算出された位置を入力値として所定の重力補正計算式を計算し、三次元加速度の補正値を算出する。この補正値は計測された三次元加速度に含まれる重力加速度を取り除くための値である。
【００４４】
コリオリ補正計算部１４６は、速度位置更新部１４４により算出された速度と姿勢方位更新部１４２により算出された姿勢とを入力値として所定のコリオリ補正計算式を計算し、三次元加速度の補正値を算出する。この補正値はコリオリ力による計測誤差を三次元加速度から取り除くための値である。
【００４５】
航法系回転補正計算部１４７は、速度位置更新部１４４により算出された速度を入力値として所定の航法系回転補正計算式を計算し、三次元角速度の補正値を算出する。この補正値は地球の自転による計測誤差を三次元角速度から取り除くための値である。
【００４６】
図５は、実施の形態１における自己位置姿勢標定方法を示すフローチャートである。
実施の形態１における自己位置姿勢標定方法について、図５に基づいて説明する。
【００４７】
まず、自己位置姿勢標定方法の概要について説明する。
【００４８】
初期値算出部１１０は、車両２００の出発時に３自由度２次元航法により方位角の初期値を算出する（Ｓ１１０）。
走行時標定部１２１は、車両２００が停車するまで、方位角の初期値または前回の方位角を用いて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１２０）。
停車時標定部１２２は、車両２００が停車している間、拡張ＺＵＰＴアルゴリズムに基づいて６自由度３次元航法により位置と姿勢と方位とを標定する（Ｓ１４０）。
【００４９】
次に、自己位置姿勢標定方法の詳細について説明する。
【００５０】
Ｓ１１０において、初期値算出部１１０は、車両２００の出発時、つまり、自己位置姿勢標定装置１００の起動時に３自由度２次元航法（第一航法）により方位角の初期値を算出する。
「３自由度」は「緯度」と「経度」と「方位」とを意味し、「２次元」は「緯度、経度」または「方位」を意味する。
【００５１】
図６は、実施の形態１における３自由度２次元航法のアルゴリズムを示す図である。
実施の形態１における３自由度２次元航法について、図６に基づいて説明する。
【００５２】
入力補正部１１１、方位更新部１１２、座標変換部１１３、緯度経度更新部１１４および出力補正部１１５は、初期値算出部１１０が備える機能構成である。
【００５３】
入力補正部１１１は、速度データ１９２に含まれる速度にカルマンフィルタ部１３０により算出される補正値を加算して速度（速度ベクトル）を補正する。
さらに、入力補正部１１１は、角速度データ１９４に含まれる「方位角」の角速度にカルマンフィルタ部１３０により算出される補正値を加算して方位角の角速度を補正する。
【００５４】
方位更新部１１２は、入力補正部１１１により補正された方位角の角速度を１回積分して方位角の変化量を算出する。
方位更新部１１２は、算出した方位角の変化量を方位角の初期値または出力補正部１１５により前回算出された方位角に加算して新たな方位角を算出する。
ここで、方位更新部１１２は方位角の初期値として所定の値（例えば、０度）を用いる。ここで用いる方位角の初期値は適当な値で構わない。つまり、方位角の初期値は、正確な方位角を示す適切な値であっても、不正確な方位角を示す不適切な値であっても構わない。
【００５５】
座標変換部１１３は、方位更新部１１２により算出された姿勢と方位とに基づいて、所定の座標変換式を用いて、入力補正部１１１により補正された速度をセンサ座標系の値から航法座標系の値に変換する。
これにより、航法座標系の二次元速度（緯度方向の速度、経度方向の速度）が得られる。
【００５６】
緯度経度更新部１１４は、座標変換部１１３により得られた二次元速度を１回積分して二次元座標（緯度、経度）の変化量を算出する。
緯度経度更新部１１４は、算出した二次元座標の変化量を二次元座標の初期値または出力補正部１１５により前回算出された二次元座標に加算して新たな二次元座標を算出する。
例えば、緯度経度更新部１１４は、車両２００が出発する前、つまり、車両２００の停車時にＧＰＳ受信機２１０により測位された二次元座標値を二次元座標の初期値として用いる。
【００５７】
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元座標と緯度経度更新部１１４により算出された二次元座標との差を「座標残差」として算出する。
カルマンフィルタ部１３０は、ＧＰＳデータ１９１に含まれる二次元速度と座標変換部１１３により得られた二次元速度とを差を「速度残差」として算出する。
カルマンフィルタ部１３０は、座標残差と速度残差とを入力値として第１のカルマンフィルタ１３１を実行し、速度の補正値と角速度の補正値と二次元座標の補正値と方位角の補正値とを算出する。
【００５８】
出力補正部１１５は、緯度経度更新部１１４により算出された二次元座標値にカルマンフィルタ部１３０により算出された補正値を加算して二次元座標値を補正し、補正した二次元座標値を出力する。
さらに、出力補正部１１５は、方位更新部１１２により算出された方位角にカルマンフィルタ部１３０により算出された補正値を加算して方位角を補正し、補正した方位角を出力する。
【００５９】
図７は、実施の形態１における３自由度２次元航法による方位角の推定結果を示すグラフである。
実施の形態１における３自由度２次元航法による方位角の推定結果について、図７に基づいて説明する。
【００６０】
図７において、３自由度２次元航法（図６参照）により算出した方位角を実線で示し、正しい方位角を鎖線で示す。
【００６１】
（１）車両２００が「−１８０度」の方位を向いて停車しているときに方位角の初期値「０度」を初期値算出部１１０の方位更新部１１２に設定し、車両２００の走行を開始した。
（２）車両２００を左回りに回転させると、３自由度２次元航法により算出した方位角（実線）は数秒程度で正しい方位角（鎖線）に近い適切な値になった。
（３）車両２００を停車させると、角速度センサ２２２のバイアス（計測誤差）により方位角の値がドリフトを始めた。
【００６２】
図７（１）（２）に示すように、３自由度２次元航法は、方位角の初期値が不適切な値であってもすぐに適切な値を求めることができる。
これは、３自由度２次元航法において、加速度センサ２２１が計測した加速度ではなく、車速センサ２３０が計測した速度を用いているためである。
これにより、二次元座標を求める積分計算が１回で済み、速度の座標変換に用いる方位角（の初期値）の誤差の影響が比較的少なく、カルマンフィルタを用いて二次元座標と方位角とを適切に補正できる。
【００６３】
図５に戻り、Ｓ１１０の説明を続ける。
【００６４】
カルマンフィルタ部１３０は、上記の３自由度２次元航法（図６参照）を所定の初期化時間（例えば、数秒）または所定の航法回数だけ実行し、実行を終了したときに算出した方位角を「方位角初期値」としてストラップダウン演算部１４０に出力する。
Ｓ１１０の後、Ｓ１２０に進む。
【００６５】
Ｓ１２０において、走行時標定部１２１は、方位角の初期値または前回標定した方位角を用いて６自由度３次元航法（第二航法）により位置と姿勢と方位とを標定する。
「６自由度」は「緯度」「経度」「高度」「仰角」「回転角」「方位角」を意味し、「３次元」は「緯度、経度、高度」または「仰角、回転角、方位角」を意味する。
【００６６】
図８は、実施の形態１における６自由度３次元航法のアルゴリズムを示す図である。
実施の形態１における６自由度３次元航法のアルゴリズムについて、図８に基づいて説明する。
【００６７】
停車判定部１２３、入力補正部１２４、出力補正部１２５は自己位置姿勢標定部１２０が備える機能構成である。
【００６８】
入力補正部１２４は、加速度データ１９３に含まれる三次元加速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元加速度を補正する。
さらに、入力補正部１２４は、角速度データ１９４に含まれる三次元角速度にカルマンフィルタ部１３０により算出される補正値を加算して三次元角速度を補正する。
【００６９】
ストラップダウン演算部１４０は、入力補正部１２４により補正された三次元加速度と入力補正部１２４により補正された三次元角速度とを用いて位置と姿勢と方位とを算出する（図４参照）。
ここで、ストラップダウン演算部１４０の姿勢方位更新部１４２は、初期値算出部１１０により算出された方位角を方位角の初期値として用いる。
図７で説明したように、初期値算出部１１０により算出される方位角は正確な方位角に近い値であり、ストラップダウン演算に用いる方位角の初期値として適切な値である。例えば、初期値算出部１１０の代わりに方位磁石を用いた場合、ストラップダウン演算で用いる初期値として十分な精度の方位角を得ることができない。
ストラップダウン演算部１４０は、適切な方位角の初期値を用いることにより精度が高い位置と姿勢と方位とを算出することができる。
【００７０】
出力補正部１２５は、ストラップダウン演算部１４０により算出された位置にカルマンフィルタ部１３０により算出される補正値を加算して位置を補正し、補正した位置を標定結果の位置として出力する。
また、出力補正部１２５は、ストラップダウン演算部１４０により算出された姿勢にカルマンフィルタ部１３０により算出される補正値を加算して姿勢を補正し、補正した姿勢を標定結果の姿勢角として出力する。
さらに、出力補正部１２５は、ストラップダウン演算部１４０により算出された方位にカルマンフィルタ部１３０により算出される補正値を加算して方位を補正し、補正した方位を標定結果の方位として出力する。
【００７１】
停車判定部１２３は、車両２００が停車しているか否かを判定する。
【００７２】
カルマンフィルタ部１３０は、車両２００が停車していない場合、ＧＰＳデータ１９１に含まれる位置とストラップダウン演算部１４０により算出された位置との差を「位置残差」として算出する。
また、カルマンフィルタ部１３０は、速度データ１９２に含まれる速度とストラップダウン演算部１４０により算出された速度との差を「速度残差」として算出する。
そして、カルマンフィルタ部１３０は、位置残差と速度残差とを入力値として第２のカルマンフィルタ１３２を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
【００７３】
カルマンフィルタ部１３０は、車両２００が停車している場合、ストラップダウン演算部１４０により算出された方位に基づいて方位の変化量を「方位残差」として算出する。
そして、カルマンフィルタ部１３０は、方位残差を入力値として第３のカルマンフィルタ１３３を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
【００７４】
図５に戻り、自己位置姿勢標定方法の説明を続ける。
【００７５】
Ｓ１２０の後、Ｓ１３０に進む。
【００７６】
Ｓ１３０において、停車判定部１２３は、速度データ１９２に含まれる速度を所定の速度閾値と比較し、速度が速度閾値未満である場合、車両２００が停車していると判定し、速度が速度閾値以上である場合、車両２００が停車していないと判定する。
但し、停車判定部１２３は、車両２００が停車しているか否かをその他のデータに基づいて判定してもよい。例えば、停車判定部１２３は、ＧＰＳデータ１９１に含まれる速度に基づいて判定を行ってもよい。また、停車判定部１２３は、ＧＰＳデータ１９１に含まれる前回の位置と今回の位置とに基づいて位置の変化量が所定の位置閾値未満である場合に車両２００が停車していると判定してもよい。また、停車判定部１２３は、加速度データ１９３（または角速度データ１９４）に基づいて加速度（または角速度）の変化量が所定の加速度閾値（または角速度閾値）未満である場合に車両２００が停車していると判定してもよい。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に進む。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
【００７７】
Ｓ１４０において、停車時標定部１２２は、前回標定された方位角を用いて６自由度３次元航法（図８参照）により位置と姿勢と方位とを標定する。
但し、停車時標定部１２２は、６自由度３次元航法に「拡張ＺＵＰＴアルゴリズム」を適用する。
【００７８】
拡張ＺＵＰＴアルゴリズムは、ＺＵＰＴ（Ｚｅｒｏ ｖｅｌｏｃｉｔｙ ＵＰ ｄａＴｅ）という従来技術を拡張した新規技術である。ＺＵＰＴとは、停止中の位置の計測値を固定することによりセンサ誤差による位置（計測値）の変化を防ぐ技術である。
拡張ＺＵＰＴアルゴリズムは、停止中の方位角の計測値を固定することによりセンサ誤差による方位角（計測値）の変化を防ぐ。
但し、拡張ＺＵＰＴアルゴリズムは、停止中の姿勢角の計測値を固定しない。車両２００が停車中であっても人の乗り降りや車内での人の移動によって、車両２００の姿勢角は変化するからである。もし、姿勢角の計測値を固定してしまうと姿勢角が実際に変化しているにも関わらず、カルマンフィルタが姿勢角の変化量をセンサ誤差として判断してしまい、間違った補正値を出力してしまう。また、姿勢角はレベリングによって正確な値を求めることができる。
【００７９】
図９は、実施の形態１における車両２００の停車時の方位角のドリフトを示すグラフである。
車両２００の停車時（但し、人の乗り降り及び車内での人の移動は無し）の回転角、仰角、方位角の変化を図９に示す。
図９に示すように、方位角の値は角速度センサ２２２のバイアス誤差により、時間の経過に伴ってドリフトする。
一方、回転角、仰角の値（姿勢角の値）は、重力加速度と加速度センサ２２１により計測された加速度との比較によって安定化することができる（レベリング）。
【００８０】
図１０は、実施の形態１における拡張ＺＵＰＴアルゴリズムの概要図である。
実施の形態１における拡張ＺＵＰＴアルゴリズムの概要について、図１０に基づいて説明する。
【００８１】
拡張ＺＵＰＴアルゴリズムでは、車両２００の左右に「仮想アンカポイント」を設定し、車両２００の長さ方向の２箇所に「仮想センシングポイント」を設定し、仮想アンカポイントと仮想センシングポイントとをワイヤーで結んだ状態を仮定する。
この場合、車両２００の姿勢角（ｘ軸またはｙ軸回りの回転）を変化させることは可能であるが、車両２００の方位角（ｚ軸回りの回転）を変化させることはできない。つまり、方位角について車両２００が拘束される。
【００８２】
この状態において、仮想アンカポイントと仮想センシングポイントとの距離の二重差を観測値とする。
観測値を表す観測式（１）−（３）を以下に示す。式（３）に示すように観測値は「０」である。
【００８３】
【数１】
【００８４】
上記式（２）の行列Ａを表す式（６）および行列Ａの算出式（４）（５）を以下に示す。
【００８５】
【数２】
【００８６】
上記式（４）の右辺の行列は、方位角を車体座標系（ｘｙｚ）から局所水平座標系（ＮＥＤ）に変換する方向余弦行列である。
また、上記式（４）の右辺の１つ目のベクトルは車両２００の右方向の単位行列であり、２つ目のベクトルは車両２００の左方向の単位行列である。
【００８７】
上記式（２）に含まれる行列Ａと行列Ｃとベクトルｂとのうち、行列Ｃはセンサ誤差の影響による方位角の計測値の変化に応じて変化する。
そこで、予測値を以下の観測式（７）で表す。
【００８８】
【数３】
【００８９】
式（２）に表した観測値と式（７）に表した予測値との差を方位角の観測誤差（方位残差）として以下の式（８）で表す。
【００９０】
【数４】
【００９１】
図５に戻り、Ｓ１４０の説明を続ける。
【００９２】
６自由度３次元航法（図８）において、拡張ＺＵＰＴアルゴリズムを適用した第３のカルマンフィルタ１３３が実行される。
第３のカルマンフィルタ１３３は、上記式（８）を観測方程式として用いてカルマンフィルタ処理により各種の補正値を算出する。
Ｓ１４０の後、Ｓ１５０に進む。
【００９３】
Ｓ１５０において、停車判定部１２３は、車両２００が停車しているか否かをＳ１３０と同様に判定する。
車両２００が停車している場合（ＹＥＳ）、Ｓ１４０に戻る。
車両２００が停車していない場合（ＮＯ）、Ｓ１２０に戻る。
【００９４】
図１１は、実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図である。
図１１において、自己位置姿勢標定装置１００は、ＣＰＵ９０１（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を備えている。ＣＰＵ９０１は、バス９０２を介してＲＯＭ９０３、ＲＡＭ９０４、通信ボード９０５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。
【００９５】
通信ボード９０５は、有線または無線で、ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、インターネット、電話回線などの通信網に接続している。
【００９６】
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２２、ファイル群９２３が記憶されている。
【００９７】
プログラム群９２２には、実施の形態において「〜部」として説明する機能を実行するプログラムが含まれる。プログラムは、ＣＰＵ９０１により読み出され実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものであり、また「〜部」の手順や方法をコンピュータに実行させるものである。
【００９８】
ファイル群９２３には、実施の形態において説明する「〜部」で使用される各種データ（入力、出力、判定結果、計算結果、処理結果など）が含まれる。
【００９９】
実施の形態において構成図およびフローチャートに含まれている矢印は主としてデータや信号の入出力を示す。
【０１００】
実施の形態において「〜部」として説明するものは「〜回路」、「〜装置」、「〜機器」であってもよく、また「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ファームウェア、ソフトウェア、ハードウェアまたはこれらの組み合わせのいずれで実装されても構わない。
【０１０１】
実施の形態１において、例えば、以下のような自己位置姿勢標定装置１００について説明した。
【０１０２】
自己位置姿勢標定装置１００は、ＧＰＳとＩＭＵと車速パルスとを用いて（１）方位を求めるための初期演算と、（２）停車中に実施する方位に関するカルマンフィルタの観測更新アルゴリズム（拡張ＺＵＰＴ）とを実行する。
（１）（２）により、低価格で中精度なＩＭＵと設置場所に制約がない１台のＧＰＳとを用いて６自由度３次元航法を行うことが可能となる。
【符号の説明】
【０１０３】
１００ 自己位置姿勢標定装置、１１０ 初期値算出部、１１１ 入力補正部、１１２ 方位更新部、１１３ 座標変換部、１１４ 緯度経度更新部、１１５ 出力補正部、１２０ 自己位置姿勢標定部、１２１ 走行時標定部、１２２ 停車時標定部、１２３ 停車判定部、１２４ 入力補正部、１２５ 出力補正部、１３０ カルマンフィルタ部、１３１ 第１のカルマンフィルタ、１３２ 第２のカルマンフィルタ、１３３ 第３のカルマンフィルタ、１４０ ストラップダウン演算部、１４１ 角速度座標変換部、１４２ 姿勢方位更新部、１４３ 加速度座標変換部、１４４ 速度位置更新部、１４５ 重力補正計算部、１４６ コリオリ補正計算部、１４７ 航法系回転補正計算部、１４８Ａ 第一補正部、１４８Ｂ 第二補正部、１４８Ｃ 第三補正部、１９０ 装置記憶部、１９１ ＧＰＳデータ、１９２ 速度データ、１９３ 加速度データ、１９４ 角速度データ、２００ 車両、２０１ 天板、２１０ ＧＰＳ受信機、２１１ ＧＰＳアンテナ、２２０ 慣性センサ、２２１ 加速度センサ、２２２ 角速度センサ、２３０ 車速センサ、９０１ ＣＰＵ、９０２ バス、９０３ ＲＯＭ、９０４ ＲＡＭ、９０５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２２ プログラム群、９２３ ファイル群。
【特許請求の範囲】
【請求項１】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部と
を備えたことを特徴とする位置標定装置。
【請求項２】
前記標定値算出部は、
移動していた前記移動体が移動を停止した場合、第３のカルマンフィルタを用いて前記第二航法方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記第二航法方位角を補正して補正後の第二航法方位角を算出し、
停止した前記移動体が移動を再開した場合、前記補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出し、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出し、算出した新たな第二航法座標値を補正する位置補正値を前記第２のカルマンフィルタを用いて算出し、算出した位置補正値を用いて前記新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する
ことを特徴とする請求項１記載の位置標定装置。
【請求項３】
前記標定値算出部は、
前記速度センサにより計測された速度が所定の速度閾値未満である場合、前記移動体が移動を停止したと判定し、
前記移動体が移動を停止したと判定した後、前記速度センサにより計測された速度が前記速度閾値以上である場合、前記移動体が移動を再開したと判定する
ことを特徴とする請求項２記載の位置標定装置。
【請求項４】
前記第３のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を前記方位角補正値として算出する
ことを特徴とする請求項２または請求項３記載の位置標定装置。
【請求項５】
前記第３のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて前記方位角補正値を算出する
ことを特徴とする請求項２から請求項４いずれかに記載の位置標定装置。
【請求項６】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力する標定値算出部と、
移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正する方位角補正部とを備え、
前記標定値算出部は、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力する
ことを特徴とする位置標定装置。
【請求項７】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
初期値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力し、
標定値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する
ことを特徴とする位置標定装置の位置標定方法。
【請求項８】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
標定値算出部は、前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力し、
方位角補正部は、移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正し、
前記標定値算出部は、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力する
ことを特徴とする位置標定装置の位置標定方法。
【請求項９】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部として
コンピュータを機能させることを特徴とする位置標定プログラム。
【請求項１０】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力する標定値算出部と、
移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正する方位角補正部としてコンピュータを機能させ、
前記標定値算出部としてコンピュータに、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力させる
ことを特徴とする位置標定プログラム。
【請求項１１】
移動体の停止状態からの移動に応じて、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、速度センサ、加速度センサ、および角速度センサの計測データから自己の方位角を求め、移動後の再停止時に求めた方位角を初期方位角として確定するとともに、当該確定された初期方位角と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、速度センサ、加速度センサ、および角速度センサの計測データに基づいて、カルマンフィルタを用いた航法演算により自己位置を標定する演算装置を備えた位置標定装置。
【請求項１】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部と
を備えたことを特徴とする位置標定装置。
【請求項２】
前記標定値算出部は、
移動していた前記移動体が移動を停止した場合、第３のカルマンフィルタを用いて前記第二航法方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記第二航法方位角を補正して補正後の第二航法方位角を算出し、
停止した前記移動体が移動を再開した場合、前記補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出し、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出し、算出した新たな第二航法座標値を補正する位置補正値を前記第２のカルマンフィルタを用いて算出し、算出した位置補正値を用いて前記新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する
ことを特徴とする請求項１記載の位置標定装置。
【請求項３】
前記標定値算出部は、
前記速度センサにより計測された速度が所定の速度閾値未満である場合、前記移動体が移動を停止したと判定し、
前記移動体が移動を停止したと判定した後、前記速度センサにより計測された速度が前記速度閾値以上である場合、前記移動体が移動を再開したと判定する
ことを特徴とする請求項２記載の位置標定装置。
【請求項４】
前記第３のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を前記方位角補正値として算出する
ことを特徴とする請求項２または請求項３記載の位置標定装置。
【請求項５】
前記第３のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて前記方位角補正値を算出する
ことを特徴とする請求項２から請求項４いずれかに記載の位置標定装置。
【請求項６】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力する標定値算出部と、
移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正する方位角補正部とを備え、
前記標定値算出部は、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力する
ことを特徴とする位置標定装置。
【請求項７】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
初期値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力し、
標定値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する
ことを特徴とする位置標定装置の位置標定方法。
【請求項８】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
標定値算出部は、前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力し、
方位角補正部は、移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正し、
前記標定値算出部は、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力する
ことを特徴とする位置標定装置の位置標定方法。
【請求項９】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第１のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記ＧＰＳ受信機により測位された座標値とに基づいて第２のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部として
コンピュータを機能させることを特徴とする位置標定プログラム。
【請求項１０】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を利用して測位を行うＧＰＳ受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記移動体が移動している場合、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記移動体の特定の方位角とに基づいて前記移動体の方位角を算出し、算出した前記移動体の方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を算出し、算出した前記移動体の座標値と前記ＧＰＳ受信機により測位された座標値とに基づいてカルマンフィルタＡを用いて前記移動体の座標値を補正する位置補正値を算出し、算出した位置補正値を用いて前記移動体の座標値を補正し、前記移動体の位置を標定した位置標定値として前記移動体の補正後の座標値を出力する標定値算出部と、
移動していた前記移動体が移動を停止した場合、カルマンフィルタＢを用いて前記移動体の方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて前記移動体の方位角を補正する方位角補正部としてコンピュータを機能させ、
前記標定値算出部としてコンピュータに、
停止した前記移動体が移動を再開した場合、前記方位角補正部によって補正された補正後の方位角と新たな方位角の変化量とに基づいて前記移動体の新たな方位角を算出し、算出した前記移動体の新たな方位角に基づいて前記移動体の新たな座標値を算出し、算出した前記移動体の新たな座標値に基づいて前記第２のカルマンフィルタを用いて新たな位置補正値を算出し、算出した新たな位置補正値を用いて前記移動体の新たな座標値を補正し、補正後の新たな座標値を新たな位置標定値として出力させる
ことを特徴とする位置標定プログラム。
【請求項１１】
移動体の停止状態からの移動に応じて、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、速度センサ、加速度センサ、および角速度センサの計測データから自己の方位角を求め、移動後の再停止時に求めた方位角を初期方位角として確定するとともに、当該確定された初期方位角と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機、速度センサ、加速度センサ、および角速度センサの計測データに基づいて、カルマンフィルタを用いた航法演算により自己位置を標定する演算装置を備えた位置標定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−１９３９６５（Ｐ２０１２−１９３９６５Ａ）
【公開日】平成２４年１０月１１日（２０１２．１０．１１）
【国際特許分類】
【出願番号】特願２０１１−５６１０５（Ｐ２０１１−５６１０５）
【出願日】平成２３年３月１５日（２０１１．３．１５）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年１０月１１日（２０１２．１０．１１）
【国際特許分類】
【出願日】平成２３年３月１５日（２０１１．３．１５）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]