JP5602070B2 - 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム - Google Patents
位置標定装置、位置標定装置の位置標定方法および位置標定プログラム Download PDFInfo
- Publication number
- JP5602070B2 JP5602070B2 JP2011056105A JP2011056105A JP5602070B2 JP 5602070 B2 JP5602070 B2 JP 5602070B2 JP 2011056105 A JP2011056105 A JP 2011056105A JP 2011056105 A JP2011056105 A JP 2011056105A JP 5602070 B2 JP5602070 B2 JP 5602070B2
- Authority
- JP
- Japan
- Prior art keywords
- calculated
- navigation
- azimuth
- value
- dimensional
- 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.)
- Active
Links
Images
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Description
しかし、GPSアンテナ同士をある程度離して相対位置が変化しないように設置する必要がある。この制約は、一般の乗用車に適用するには大きな制約である。
(1)車両を停止した状態で方位を求める場合、中精度または低精度なIMUでは、地球の自転を計測して方位を求めるといった計算が出来ない。
また、2台以上のGPS受信機を使用して方位を求める場合、GPSアンテナ間の基線解析を行うため、精度を出すためにはGPSアンテナを可能な限り離して相対位置が変化しないよう設置する必要がある。
(2)3次元航法計算では、ある程度正確な初期値(位置・姿勢・方位)を設定しなければ計算結果が発散してしまう可能性が高い。
初期値のうち、姿勢角(ピッチ角、ロール角)は重力観測により得られ、位置はGPS観測により得られるが、方位に関しては上記(1)の理由により適切な値を得ることができない。
(3)正確な初期値が何らかの方法で与えられたとしても、その後の停車時にはジャイロの誤差等によって方位の値がドリフトしてしまう。
前記位置標定装置は、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角と前記速度センサにより計測された速度とに基づいて前記移動体の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記GPS受信機により測位された座標値とに基づいて第1のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角と前記加速度センサにより計測された加速度とに基づいて前記移動体の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記GPS受信機により測位された座標値とに基づいて第2のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部とを備える。
車両の位置・姿勢・方位を標定する自己位置姿勢標定装置について説明する。
図2は、実施の形態1における自己位置姿勢標定装置100の概要図である。
実施の形態1における車両200の概要と自己位置姿勢標定装置100の概要とについて、図1と図2とに基づいて説明する。
「GPS」は「Global Positioning System」の略である。
但し、設置方法はこれに限らない。例えば、GPS受信機210を屋根部以外に設置してもよいし、慣性センサ220を車両200内に設置してもよく、天板201を設けることなく、GPS受信機210と慣性センサ220を車両200の内部または外部に取り付けてもよい。
また、自己位置姿勢標定装置100は車両200とは別個に設けた外部装置であっても構わない。
さらに、車両200は他の移動体(例えば、飛行機、船舶、自動二輪車、自転車、列車)であっても構わない。
慣性センサ220は、IMU(Inertial Measurement Unit)と呼ばれる。
以下、3軸方向の加速度を「三次元加速度」といい、3軸方向の角速度を「三次元角速度」という。
「位置」は、三次元の座標値(緯度、経度、高度)を意味する。
「姿勢」は、仰角(ピッチ角)と回転角(ロール角)とを意味する。
「方位」は、方位角を意味する。
実施の形態1における自己位置姿勢標定装置100の機能構成について、図3に基づいて説明する。
GPSデータ191、速度データ192、加速度データ193または角速度データ194は、装置記憶部190に記憶されるデータの一例である。
速度データ192は、車速センサ230により計測されたデータであり、時刻に対応付けて「速度」を含んでいる。
加速度データ193は、慣性センサ220を構成する加速度センサ221により計測されたデータであり、時刻に対応付けて三次元加速度を含んでいる。
角速度データ194は、慣性センサ220を構成する角速度センサ222により計測されたデータであり、時刻に対応付けて三次元角速度を含んでいる。つまり、角速度データ194は、「仰角」の角速度と「回転角」の角速度と「方位角」の角速度とを含んでいる。角速度センサ222は「ジャイロ」と呼ばれる。
初期値算出部110は、角速度データ194に含まれる方位角の角速度に基づいて、車両200の方位角を第一航法方位角として算出する(後述する方位更新部112の処理)。
初期値算出部110は、算出した第一航法方位角と速度データ192に含まれる速度とに基づいて、車両200の座標値を第一航法座標値として算出する(後述する緯度経度更新部114の処理)。
初期値算出部110は、算出した第一航法座標値とGPSデータ191に含まれる座標値とに基づいて、第1のカルマンフィルタ131を用いて、第一航法方位角を補正する補正値を算出する。
初期値算出部110は、算出した補正値を用いて第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する(後述する出力補正部115の処理)。
走行時標定部121は、車両200が走行しているときに、カルマンフィルタ部130とストラップダウン演算部140とを制御して車両200の位置と姿勢と方位とを標定(算出)する。
停車時標定部122は、車両200が停車しているときに、カルマンフィルタ部130とストラップダウン演算部140とを制御して車両200の位置と姿勢と方位とを標定する。
走行時標定部121は、角速度データ194に含まれる方位角の角速度に基づいて、方位角の変化量を算出する(後述する姿勢方位更新部142の処理)。
走行時標定部121は、算出した方位角の変化量と初期値算出部110から出力された方位角初期値とに基づいて、車両200の方位角を第二航法方位角として算出する(後述する姿勢方位更新部142の処理)。
走行時標定部121は、算出した第二航法方位角と加速度データ193に含まれる加速度とに基づいて、車両200の座標値を第二航法座標値として算出する(後述する速度位置更新部144の処理)。
走行時標定部121は、算出した第二航法座標値とGPSデータ191に含まれる座標値とに基づいて、第2のカルマンフィルタ132を用いて、第二航法座標値を補正する補正値を算出する。
走行時標定部121は、算出した補正値を用いて第二航法座標値を補正し、車両200の位置を標定した位置標定値として補正後の第二航法座標値を出力する(後述する出力補正部125の処理)。
停車時標定部122は、移動していた車両200が移動を停止した場合、第3のカルマンフィルタ133を用いて第二航法方位角を補正する方位角補正値を算出する。
停車時標定部122は、算出した方位角補正値を用いて第二航法方位角を補正して補正後の第二航法方位角を算出する(後述する出力補正部125の処理)。
走行時標定部121は、停止した車両200が移動を再開した場合、補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出する(後述する姿勢方位更新部142の処理)。
走行時標定部121は、算出した新たな第二航法方位角に基づいて新たな第二航法座標値を算出する(後述する速度位置更新部144の処理)。
走行時標定部121は、算出した新たな第二航法座標値を補正する位置補正値を第2のカルマンフィルタ132を用いて算出する。
走行時標定部121は、算出した位置補正値を用いて新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する(後述する出力補正部125の処理)。
停車判定部123は、車速センサ230により計測された速度が所定の速度閾値未満である場合、車両200が移動を停止したと判定する。
停車判定部123は、車両200が移動を停止したと判定した後、車速センサ230により計測された速度が速度閾値以上である場合、車両200が移動を再開したと判定する。
カルマンフィルタ部130は、観測方程式が異なる複数のカルマンフィルタ(符号131−133)を備える。
実施の形態1におけるストラップダウン演算部140の機能構成について、図4に基づいて説明する。
角速度座標変換部141は、姿勢方位更新部142により前回算出された姿勢と方位とに基づいて所定の座標変換式(例えば、方向余弦行列)を用いて三次元角速度をセンサ座標系の値から航法座標系(局所水平座標系)の値に変換する。
「センサ座標系」は車両200を基準とする座標系であり、例えば、長さ方向を「x」、幅方向を「y」、高さ方向を「z」として表される。
「航法座標系」は地面を基準とする座標系であり、例えば、「North(北)」「East(東)」「Down(下)」で表される。
同様に、姿勢方位更新部142は、新たな姿勢角を算出する。
ここで、姿勢方位更新部142は、初期値算出部110により算出された方位角を方位角の初期値として用いる。
また、姿勢方位更新部142は、加速度センサ221により停車時に計測された加速度を用いて所定の姿勢角算出式を計算し、姿勢角の初期値を算出する。姿勢角算出式は、計測された加速度と重力加速度と姿勢角との関係を表した式である。
加速度座標変換部143は、姿勢方位更新部142により算出された姿勢角と方位角とに基づいて、所定の座標変換式を用いて、三次元加速度をセンサ座標系の値から航法座標系の値に変換する。
また、速度位置更新部144は、第三補正部148Cにより補正された三次元加速度を2回積分して位置の変化量を算出し、算出した位置の変化量を位置の初期値または前回標定された位置に加算して新たな位置を算出する。
例えば、速度位置更新部144は、GPS受信機210により停車時に計測された位置を位置の初期値として用いる。
実施の形態1における自己位置姿勢標定方法について、図5に基づいて説明する。
走行時標定部121は、車両200が停車するまで、方位角の初期値または前回の方位角を用いて6自由度3次元航法により位置と姿勢と方位とを標定する(S120)。
停車時標定部122は、車両200が停車している間、拡張ZUPTアルゴリズムに基づいて6自由度3次元航法により位置と姿勢と方位とを標定する(S140)。
「3自由度」は「緯度」と「経度」と「方位」とを意味し、「2次元」は「緯度、経度」または「方位」を意味する。
実施の形態1における3自由度2次元航法について、図6に基づいて説明する。
さらに、入力補正部111は、角速度データ194に含まれる「方位角」の角速度にカルマンフィルタ部130により算出される補正値を加算して方位角の角速度を補正する。
方位更新部112は、算出した方位角の変化量を方位角の初期値または出力補正部115により前回算出された方位角に加算して新たな方位角を算出する。
ここで、方位更新部112は方位角の初期値として所定の値(例えば、0度)を用いる。ここで用いる方位角の初期値は適当な値で構わない。つまり、方位角の初期値は、正確な方位角を示す適切な値であっても、不正確な方位角を示す不適切な値であっても構わない。
これにより、航法座標系の二次元速度(緯度方向の速度、経度方向の速度)が得られる。
緯度経度更新部114は、算出した二次元座標の変化量を二次元座標の初期値または出力補正部115により前回算出された二次元座標に加算して新たな二次元座標を算出する。
例えば、緯度経度更新部114は、車両200が出発する前、つまり、車両200の停車時にGPS受信機210により測位された二次元座標値を二次元座標の初期値として用いる。
カルマンフィルタ部130は、GPSデータ191に含まれる二次元速度と座標変換部113により得られた二次元速度とを差を「速度残差」として算出する。
カルマンフィルタ部130は、座標残差と速度残差とを入力値として第1のカルマンフィルタ131を実行し、速度の補正値と角速度の補正値と二次元座標の補正値と方位角の補正値とを算出する。
さらに、出力補正部115は、方位更新部112により算出された方位角にカルマンフィルタ部130により算出された補正値を加算して方位角を補正し、補正した方位角を出力する。
実施の形態1における3自由度2次元航法による方位角の推定結果について、図7に基づいて説明する。
(2)車両200を左回りに回転させると、3自由度2次元航法により算出した方位角(実線)は数秒程度で正しい方位角(鎖線)に近い適切な値になった。
(3)車両200を停車させると、角速度センサ222のバイアス(計測誤差)により方位角の値がドリフトを始めた。
これは、3自由度2次元航法において、加速度センサ221が計測した加速度ではなく、車速センサ230が計測した速度を用いているためである。
これにより、二次元座標を求める積分計算が1回で済み、速度の座標変換に用いる方位角(の初期値)の誤差の影響が比較的少なく、カルマンフィルタを用いて二次元座標と方位角とを適切に補正できる。
S110の後、S120に進む。
「6自由度」は「緯度」「経度」「高度」「仰角」「回転角」「方位角」を意味し、「3次元」は「緯度、経度、高度」または「仰角、回転角、方位角」を意味する。
実施の形態1における6自由度3次元航法のアルゴリズムについて、図8に基づいて説明する。
さらに、入力補正部124は、角速度データ194に含まれる三次元角速度にカルマンフィルタ部130により算出される補正値を加算して三次元角速度を補正する。
ここで、ストラップダウン演算部140の姿勢方位更新部142は、初期値算出部110により算出された方位角を方位角の初期値として用いる。
図7で説明したように、初期値算出部110により算出される方位角は正確な方位角に近い値であり、ストラップダウン演算に用いる方位角の初期値として適切な値である。例えば、初期値算出部110の代わりに方位磁石を用いた場合、ストラップダウン演算で用いる初期値として十分な精度の方位角を得ることができない。
ストラップダウン演算部140は、適切な方位角の初期値を用いることにより精度が高い位置と姿勢と方位とを算出することができる。
また、出力補正部125は、ストラップダウン演算部140により算出された姿勢にカルマンフィルタ部130により算出される補正値を加算して姿勢を補正し、補正した姿勢を標定結果の姿勢角として出力する。
さらに、出力補正部125は、ストラップダウン演算部140により算出された方位にカルマンフィルタ部130により算出される補正値を加算して方位を補正し、補正した方位を標定結果の方位として出力する。
また、カルマンフィルタ部130は、速度データ192に含まれる速度とストラップダウン演算部140により算出された速度との差を「速度残差」として算出する。
そして、カルマンフィルタ部130は、位置残差と速度残差とを入力値として第2のカルマンフィルタ132を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
そして、カルマンフィルタ部130は、方位残差を入力値として第3のカルマンフィルタ133を実行し、三次元加速度の補正値と三次元角速度の補正値と位置の補正値と姿勢の補正値と方位の補正値とを算出する。
但し、停車判定部123は、車両200が停車しているか否かをその他のデータに基づいて判定してもよい。例えば、停車判定部123は、GPSデータ191に含まれる速度に基づいて判定を行ってもよい。また、停車判定部123は、GPSデータ191に含まれる前回の位置と今回の位置とに基づいて位置の変化量が所定の位置閾値未満である場合に車両200が停車していると判定してもよい。また、停車判定部123は、加速度データ193(または角速度データ194)に基づいて加速度(または角速度)の変化量が所定の加速度閾値(または角速度閾値)未満である場合に車両200が停車していると判定してもよい。
車両200が停車している場合(YES)、S140に進む。
車両200が停車していない場合(NO)、S120に戻る。
但し、停車時標定部122は、6自由度3次元航法に「拡張ZUPTアルゴリズム」を適用する。
拡張ZUPTアルゴリズムは、停止中の方位角の計測値を固定することによりセンサ誤差による方位角(計測値)の変化を防ぐ。
但し、拡張ZUPTアルゴリズムは、停止中の姿勢角の計測値を固定しない。車両200が停車中であっても人の乗り降りや車内での人の移動によって、車両200の姿勢角は変化するからである。もし、姿勢角の計測値を固定してしまうと姿勢角が実際に変化しているにも関わらず、カルマンフィルタが姿勢角の変化量をセンサ誤差として判断してしまい、間違った補正値を出力してしまう。また、姿勢角はレベリングによって正確な値を求めることができる。
車両200の停車時(但し、人の乗り降り及び車内での人の移動は無し)の回転角、仰角、方位角の変化を図9に示す。
図9に示すように、方位角の値は角速度センサ222のバイアス誤差により、時間の経過に伴ってドリフトする。
一方、回転角、仰角の値(姿勢角の値)は、重力加速度と加速度センサ221により計測された加速度との比較によって安定化することができる(レベリング)。
実施の形態1における拡張ZUPTアルゴリズムの概要について、図10に基づいて説明する。
この場合、車両200の姿勢角(x軸またはy軸回りの回転)を変化させることは可能であるが、車両200の方位角(z軸回りの回転)を変化させることはできない。つまり、方位角について車両200が拘束される。
観測値を表す観測式(1)−(3)を以下に示す。式(3)に示すように観測値は「0」である。
また、上記式(4)の右辺の1つ目のベクトルは車両200の右方向の単位行列であり、2つ目のベクトルは車両200の左方向の単位行列である。
そこで、予測値を以下の観測式(7)で表す。
第3のカルマンフィルタ133は、上記式(8)を観測方程式として用いてカルマンフィルタ処理により各種の補正値を算出する。
S140の後、S150に進む。
車両200が停車している場合(YES)、S140に戻る。
車両200が停車していない場合(NO)、S120に戻る。
図11において、自己位置姿勢標定装置100は、CPU901(Central Processing Unit)を備えている。CPU901は、バス902を介してROM903、RAM904、通信ボード905、磁気ディスク装置920と接続され、これらのハードウェアデバイスを制御する。
(1)(2)により、低価格で中精度なIMUと設置場所に制約がない1台のGPSとを用いて6自由度3次元航法を行うことが可能となる。
Claims (7)
- GPS(Global Positioning System)を利用して測位を行うGPS受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置において、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角に基づいて、前記速度センサにより計測された速さを二次元の速度に変換し、変換後の二次元の速度を積分して二次元座標における変化量を算出し、算出した前記変化量を二次元の位置初期値に加えることにより前記移動体の二次元の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記GPS受信機により測位された座標値に含まれる二次元の座標値とに基づいて第1のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角に基づいて、前記加速度センサにより計測された三次元の加速度を航法座標系の三次元の加速度に変換し、変換後の三次元の加速度を積分して三次元座標における変化量を算出し、算出した前記変化量を三次元の位置初期値に加えることにより前記移動体の三次元の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記GPS受信機により測位された三次元の座標値とに基づいて第2のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部とを備えたことを特徴とする位置標定装置。 - 前記標定値算出部は、
移動していた前記移動体が移動を停止した場合、前回算出された第二航法方位角と今回算出された第二航法方位角とに基づいて第3のカルマンフィルタを用いて今回算出された前記第二航法方位角を補正する方位角補正値を算出し、算出した方位角補正値を用いて今回算出された前記第二航法方位角を補正して補正後の第二航法方位角を算出し、
停止した前記移動体が移動を再開した場合、前記補正後の第二航法方位角と新たな方位角の変化量とに基づいて新たな第二航法方位角を算出し、算出した新たな第二航法方位角に基づいて、前記加速度センサにより計測された新たな三次元の加速度を航法座標系の三次元の加速度に変換し、変換後の三次元の加速度を積分して三次元座標における変化量を算出し、算出した変化量を前回算出した第二航法座標値に加えることにより、新たな第二航法座標値を算出し、算出した新たな第二航法座標値と前記GPS受信機により測位された新たな三次元の座標値とに基づいて新たな第二航法座標値を補正する位置補正値を前記第2のカルマンフィルタを用いて算出し、算出した位置補正値を用いて前記新たな第二航法座標値を補正し、補正後の新たな第二航法座標値を新たな位置標定値として出力する
ことを特徴とする請求項1記載の位置標定装置。 - 前記標定値算出部は、
前記速度センサにより計測された速度が所定の速度閾値未満である場合、前記移動体が移動を停止したと判定し、
前記移動体が移動を停止したと判定した後、前記速度センサにより計測された速度が前記速度閾値以上である場合、前記移動体が移動を再開したと判定する
ことを特徴とする請求項2記載の位置標定装置。 - 前記第3のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角とを一致させる補正値を前記方位角補正値として算出する
ことを特徴とする請求項2または請求項3記載の位置標定装置。 - 前記第3のカルマンフィルタは、前回算出された第二航法方位角と今回算出された第二航法方位角との差分を表す観測方程式を生成し、生成した観測方程式を用いて前記方位角補正値を算出する
ことを特徴とする請求項2から請求項4いずれかに記載の位置標定装置。 - GPS(Global Positioning System)を利用して測位を行うGPS受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定装置の位置標定方法において、
初期値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角に基づいて、前記速度センサにより計測された速さを二次元の速度に変換し、変換後の二次元の速度を積分して二次元座標における変化量を算出し、算出した前記変化量を二次元の位置初期値に加えることにより前記移動体の二次元の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記GPS受信機により測位された座標値に含まれる二次元の座標値とに基づいて第1のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力し、標定値算出部は、前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角に基づいて、前記加速度センサにより計測された三次元の加速度を航法座標系の三次元の加速度に変換し、変換後の三次元の加速度を積分して三次元座標における変化量を算出し、算出した前記変化量を三次元の位置初期値に加えることにより前記移動体の三次元の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記GPS受信機により測位された三次元の座標値とに基づいて第2のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する
ことを特徴とする位置標定装置の位置標定方法。 - GPS(Global Positioning System)を利用して測位を行うGPS受信機と、速度を計測する速度センサと、加速度を計測する加速度センサと、方位角の角速度を計測する角速度センサとが取り付けられた移動体の位置を標定する位置標定プログラムにおいて、
前記角速度センサにより計測された方位角の角速度に基づいて前記移動体の方位角を第一航法方位角として算出し、算出した第一航法方位角に基づいて、前記速度センサにより計測された速さを二次元の速度に変換し、変換後の二次元の速度を積分して二次元座標における変化量を算出し、算出した前記変化量を二次元の位置初期値に加えることにより前記移動体の二次元の座標値を第一航法座標値として算出し、算出した第一航法座標値と前記GPS受信機により測位された座標値に含まれる二次元の座表示とに基づいて第1のカルマンフィルタを用いて前記第一航法方位角を補正する補正値を算出し、算出した補正値を用いて前記第一航法方位角を補正し、補正後の第一航法方位角を方位角初期値として出力する初期値算出部と、
前記角速度センサにより計測された方位角の角速度に基づいて方位角の変化量を算出し、算出した方位角の変化量と前記初期値算出部から出力された方位角初期値とに基づいて前記移動体の方位角を第二航法方位角として算出し、算出した第二航法方位角に基づいて、前記加速度センサにより計測された三次元の加速度を航法座標系の三次元の加速度に変換し、変換後の三次元の加速度を積分して三次元座標における変化量を算出し、算出した前記変化量を三次元の位置初期値に加えることにより前記移動体の三次元の座標値を第二航法座標値として算出し、算出した第二航法座標値と前記GPS受信機により測位された三次元の座標値とに基づいて第2のカルマンフィルタを用いて前記第二航法座標値を補正する補正値を算出し、算出した補正値を用いて前記第二航法座標値を補正し、前記移動体の位置を標定した位置標定値として補正後の第二航法座標値を出力する標定値算出部として
コンピュータを機能させることを特徴とする位置標定プログラム。
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 JP2012193965A (ja) | 2012-10-11 |
JP5602070B2 true 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) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5896962B2 (ja) * | 2013-08-09 | 2016-03-30 | 本田技研工業株式会社 | 標識情報出力装置 |
EP3217321B1 (en) | 2013-07-31 | 2019-08-28 | Honda Motor Co., Ltd. | Sign information output apparatus |
CN105823481B (zh) * | 2015-12-21 | 2019-02-12 | 上海华测导航技术股份有限公司 | 一种基于单天线的gnss-ins车辆定姿方法 |
CN105865450A (zh) * | 2016-04-19 | 2016-08-17 | 武汉理工大学 | 一种基于步态的零速更新方法及*** |
WO2019150848A1 (ja) * | 2018-01-30 | 2019-08-08 | 古野電気株式会社 | レーダアンテナ装置及び方位測定方法 |
CN109855617A (zh) * | 2019-02-28 | 2019-06-07 | 深圳市元征科技股份有限公司 | 一种车辆定位方法、车辆定位装置及终端设备 |
DE102019203332A1 (de) | 2019-03-12 | 2020-09-17 | Robert Bosch Gmbh | Positionsermittlungsanordnung für ein Fahrzeug, Fahrzeug |
CN110440805B (zh) * | 2019-08-09 | 2021-09-21 | 深圳市道通智能航空技术股份有限公司 | 一种偏航角的融合方法、装置及飞行器 |
CN111272170A (zh) * | 2020-03-17 | 2020-06-12 | 电子科技大学 | 一种基于SoC的实时行人定位***及方法 |
WO2022018964A1 (ja) * | 2020-07-20 | 2022-01-27 | ソニーグループ株式会社 | 情報処理装置、情報処理方法およびプログラム |
CN112363196B (zh) * | 2020-10-20 | 2023-10-31 | 北京嘀嘀无限科技发展有限公司 | 车辆属性确定方法、装置、存储介质和电子设备 |
CN113758502B (zh) * | 2021-09-24 | 2024-02-20 | 广东汇天航空航天科技有限公司 | 组合导航处理方法及装置 |
Family Cites Families (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 | 車両用位置測定装置 |
ZA957639B (en) * | 1994-10-24 | 1996-05-24 | Caterpillar Inc | System and method for precisely determining an operating point for an autonomous vehicle |
JP3533745B2 (ja) * | 1995-03-30 | 2004-05-31 | アイシン精機株式会社 | 移動体測位装置 |
JP3354353B2 (ja) * | 1995-06-22 | 2002-12-09 | 防衛庁技術研究本部長 | 飛行体に設けられた慣性航法装置の移動中における調定演算方法 |
JPH095106A (ja) * | 1995-06-22 | 1997-01-10 | Tamagawa Seiki Co Ltd | 慣性装置の真方位検出方法 |
JPH09113293A (ja) * | 1995-10-17 | 1997-05-02 | Japan Radio Co Ltd | 車両方位推測方式 |
US5991692A (en) * | 1995-12-28 | 1999-11-23 | Magellan Dis, Inc. | Zero motion detection system for improved vehicle navigation system |
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 | 慣性航法装置 |
JP3753833B2 (ja) * | 1997-03-27 | 2006-03-08 | アジア航測株式会社 | 道路線形自動測量装置 |
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 | 移動体用位置姿勢測定装置 |
JP3013309B1 (ja) * | 1999-02-19 | 2000-02-28 | 株式会社ゼンリン | ハイブリッド走行軌跡取得方法およびハイブリッド走行軌跡取得システム |
JP3907170B2 (ja) * | 1999-09-16 | 2007-04-18 | サーフ テクノロジー インコーポレイテッド | 物体の位置を追跡するためのナビゲーションシステムおよび方法 |
US6577952B2 (en) * | 2001-01-08 | 2003-06-10 | Motorola, Inc. | Position and heading error-correction method and apparatus for vehicle navigation systems |
JP3914413B2 (ja) * | 2001-10-31 | 2007-05-16 | 多摩川精機株式会社 | 飛翔体における初期化方法 |
US6826478B2 (en) * | 2002-04-12 | 2004-11-30 | Ensco, Inc. | Inertial navigation system for mobile objects with constraints |
US7346452B2 (en) * | 2003-09-05 | 2008-03-18 | Novatel, Inc. | Inertial GPS navigation system using injected alignment data for the inertial system |
JP2007156890A (ja) * | 2005-12-06 | 2007-06-21 | Tamagawa Seiki Co Ltd | 無人車輌の初期方位角設定方法 |
JP4600357B2 (ja) * | 2006-06-21 | 2010-12-15 | トヨタ自動車株式会社 | 測位装置 |
US20080071476A1 (en) * | 2006-09-19 | 2008-03-20 | Takayuki Hoshizaki | Vehicle dynamics conditioning method on MEMS based integrated INS/GPS vehicle navigation system |
JP2008116370A (ja) * | 2006-11-06 | 2008-05-22 | Toyota Motor Corp | 移動***置測位装置 |
JP5022747B2 (ja) * | 2007-03-22 | 2012-09-12 | 古野電気株式会社 | 移動体の姿勢及び方位検出装置 |
JP5398120B2 (ja) * | 2007-03-22 | 2014-01-29 | 古野電気株式会社 | Gps複合航法装置 |
JP2009236532A (ja) * | 2008-03-26 | 2009-10-15 | Seiko Epson Corp | 測位方法、プログラム及び測位装置 |
JP5322789B2 (ja) * | 2009-06-15 | 2013-10-23 | 三菱電機株式会社 | モデル生成装置、モデル生成方法、モデル生成プログラム、点群画像生成方法および点群画像生成プログラム |
JP5521416B2 (ja) * | 2009-07-08 | 2014-06-11 | 富士通株式会社 | 自律測位プログラム、自律測位装置および自律測位方法 |
JP5419665B2 (ja) * | 2009-12-10 | 2014-02-19 | 三菱電機株式会社 | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム |
JP5586994B2 (ja) * | 2010-03-11 | 2014-09-10 | 三菱電機株式会社 | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
JP2011220844A (ja) * | 2010-04-09 | 2011-11-04 | Seiko Epson Corp | 位置算出方法及び位置算出装置 |
JP5405417B2 (ja) * | 2010-08-24 | 2014-02-05 | 株式会社小野測器 | 姿勢角安定化装置及び方法 |
-
2011
- 2011-03-15 JP JP2011056105A patent/JP5602070B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2012193965A (ja) | 2012-10-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5602070B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
EP3109589B1 (en) | A unit and method for improving positioning accuracy | |
JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
US9494428B2 (en) | Attitude determination method, position calculation method, and attitude determination device | |
JP5419665B2 (ja) | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム | |
JP4964047B2 (ja) | 位置検出装置及び位置検出方法 | |
JP4989035B2 (ja) | 慣性ナビゲーションシステムの誤差補正 | |
US9026263B2 (en) | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis | |
JP7073052B2 (ja) | ビークルの角度位置を測定するシステムおよび方法 | |
JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
KR101394546B1 (ko) | Gps와 imu의 데이터 융합에 의한 위치 정보 획득장치 및 그의 위치 정보 획득방법 | |
JP6083279B2 (ja) | 移動状況情報算出方法及び移動状況情報算出装置 | |
CN111854740B (zh) | 能够在交通工具中进行航位推算的惯性导航*** | |
JP2008058187A (ja) | ナビゲーション装置、位置検出方法及び位置検出プログラム | |
US11408735B2 (en) | Positioning system and positioning method | |
KR100525517B1 (ko) | 차량 항법 시스템 및 그 제어방법 | |
JP2016033473A (ja) | 位置算出方法及び位置算出装置 | |
JP5164645B2 (ja) | カルマンフィルタ処理における繰り返し演算制御方法及び装置 | |
KR20190116500A (ko) | 차재기, 연산 장치 및 프로그램 | |
JP2013228318A (ja) | キャリブレーション良否判定装置及び方法 | |
KR101639152B1 (ko) | 도로 구배를 이용한 차량 위치 추정 방법 및 장치 | |
WO2010030565A1 (en) | Magnetic sensing device for navigation and detecting inclination | |
KR101340158B1 (ko) | 고정 표적을 이용한 무인항공기의 표적 위치 보정 방법 및 컴퓨터 판독 가능한 기록매체 | |
WO2020021867A1 (ja) | ジオイド測定方法、ジオイド測定装置、ジオイド推定装置、ジオイド計算用データ収集装置 | |
CN112649001A (zh) | 一种小型无人机姿态与位置解算方法 |
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 |