JPS61155813A - 移動体位置推定装置 - Google Patents
移動体位置推定装置Info
- Publication number
- JPS61155813A JPS61155813A JP27494484A JP27494484A JPS61155813A JP S61155813 A JPS61155813 A JP S61155813A JP 27494484 A JP27494484 A JP 27494484A JP 27494484 A JP27494484 A JP 27494484A JP S61155813 A JPS61155813 A JP S61155813A
- Authority
- JP
- Japan
- Prior art keywords
- vector
- moving body
- value
- detecting means
- stored
- 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.)
- Pending
Links
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。
め要約のデータは記録されません。
Description
【発明の詳細な説明】
［産業上の利用分野］
この発明は車両等移動体の位置推定装置に関する。
［従来技術］
従来より、航空機等移動体の位置を推定する装置として
ハイブリッド航法システムと呼ばれているものがある（
１９７４年７月本航空宇宙学会誌Ｖｏ１．２２．２４６
号）。
ハイブリッド航法システムと呼ばれているものがある（
１９７４年７月本航空宇宙学会誌Ｖｏ１．２２．２４６
号）。
この概要を第４図に示した。電波航法装置１はドツプラ
ーレーダ、ロラン受信機、又は、ＧＰＳ受信機等からな
り、航空機等の移動体の絶対位置を測定する。慣性航法
装置３はジャイロ、速度センサ等を備え移動体の速度及
び移動方位を求める７電波航法装＠１により測定された
移動体の位置情報と、慣性航法装置３により求まる移動
体の速度、方位情報（ベクトル情報）は移動体位置の最
適推定手段としてのカルマンフィルタ５に入力される。
ーレーダ、ロラン受信機、又は、ＧＰＳ受信機等からな
り、航空機等の移動体の絶対位置を測定する。慣性航法
装置３はジャイロ、速度センサ等を備え移動体の速度及
び移動方位を求める７電波航法装＠１により測定された
移動体の位置情報と、慣性航法装置３により求まる移動
体の速度、方位情報（ベクトル情報）は移動体位置の最
適推定手段としてのカルマンフィルタ５に入力される。
カルマンフィルタ５はこれら情報に基いて逐次の演算を
行い移動体位置の最適推定を行う。なお、本例は帰還補
正方式の例を示し、カルマンフィルタ５の出力は慣性航
法装置３に帰還されている。
行い移動体位置の最適推定を行う。なお、本例は帰還補
正方式の例を示し、カルマンフィルタ５の出力は慣性航
法装置３に帰還されている。
しかしながら、このような従来の移動体位置推定装置に
あっては、移動体位置の最適推定手段としてのカルマン
フィルタの出力値をそのままベクトル検出手段としての
慣性航法装置に帰還させる方式であったため、絶対位置
検出手段としての電波航法装置の測位点がばらつきこれ
によってカルマンフィルタの出力値もばらつくような場
合にはそのばらついた値が慣性航法装置にそのまま帰還
されることになり測位精度を低下させているという問題
点があった。
あっては、移動体位置の最適推定手段としてのカルマン
フィルタの出力値をそのままベクトル検出手段としての
慣性航法装置に帰還させる方式であったため、絶対位置
検出手段としての電波航法装置の測位点がばらつきこれ
によってカルマンフィルタの出力値もばらつくような場
合にはそのばらついた値が慣性航法装置にそのまま帰還
されることになり測位精度を低下させているという問題
点があった。
［発明の目的］
この発明は、上記問題点を改善し、例え絶対位置検出手
段の測位点がばらついたとしても、ベクトル検出手段の
誤差補正を良好に行うことができる移動体位置測定装置
を提供することを目的とする。
段の測位点がばらついたとしても、ベクトル検出手段の
誤差補正を良好に行うことができる移動体位置測定装置
を提供することを目的とする。
［発明の構成］
上記目的を達成するためのこの発明は、第１図に示した
ように、移動体位置を絶対位置で検出する絶対位置検出
手段７と、移動体゛の移動量を移動方位及び移動速度等
移動１成分で検出するベクトル検出手段９と、前記絶対
位置検出手段７から絶対位置情報を受【プると共に前記
ベクトル検出手段９から移動方位及び移動速度等移動量
成分の情報を受け移動体の位置を最適推定する移動体位
置推定手段１１と、前記ベクトル検出手段９の順次の検
出成分と前記移動体位置推定手段が最適推定した移動体
位置に基いて前記順次の検出成分に対応させて求めた順
次の演算成分との平均的差で前記ベクトル検出手段９の
補正を行うベクトル補正手段１３と、を具備して成る移
動体位置推定装置である。
ように、移動体位置を絶対位置で検出する絶対位置検出
手段７と、移動体゛の移動量を移動方位及び移動速度等
移動１成分で検出するベクトル検出手段９と、前記絶対
位置検出手段７から絶対位置情報を受【プると共に前記
ベクトル検出手段９から移動方位及び移動速度等移動量
成分の情報を受け移動体の位置を最適推定する移動体位
置推定手段１１と、前記ベクトル検出手段９の順次の検
出成分と前記移動体位置推定手段が最適推定した移動体
位置に基いて前記順次の検出成分に対応させて求めた順
次の演算成分との平均的差で前記ベクトル検出手段９の
補正を行うベクトル補正手段１３と、を具備して成る移
動体位置推定装置である。
［実施例］
以下、この発明の実施例を詳細に説明する。
第２図はこの発明の一実施例に係る車両用の移動体位置
推定装置の構成ブロック図である。図において第４図に
示した従来の移動体位置推定装置と同一の機能を果す要
素には同一の参照符号をイラして示しである。
推定装置の構成ブロック図である。図において第４図に
示した従来の移動体位置推定装置と同一の機能を果す要
素には同一の参照符号をイラして示しである。
絶対位置検出手段としての電波航法装置Ｆ１は現在位置
の検出値（Ｘ、Ｙ）を出力する。ベクトル検出手段とし
ての慣性航法袋！１１３は検出値θ′に補正値δθ′を
減算して求めた移動方位θ（方位はＹ軸よりの偏角で示
す）と検出した移動速度■（ＶＸ 、 ＶＶ ）を出力
する。カルマンフィルタ５はこれらの入力を得て車両の
現在位１ （Ｘｏ 、 ＹＯ）を最適推定し、推定した
現在位置を出力する。
の検出値（Ｘ、Ｙ）を出力する。ベクトル検出手段とし
ての慣性航法袋！１１３は検出値θ′に補正値δθ′を
減算して求めた移動方位θ（方位はＹ軸よりの偏角で示
す）と検出した移動速度■（ＶＸ 、 ＶＶ ）を出力
する。カルマンフィルタ５はこれらの入力を得て車両の
現在位１ （Ｘｏ 、 ＹＯ）を最適推定し、推定した
現在位置を出力する。
ベクトル補正手段１３は、メモリ１５と、平均Ｔｌ１８
Ｉｉ算部１７と、補正値演算部１９並びに成分演算手段
２１とを有する。
Ｉｉ算部１７と、補正値演算部１９並びに成分演算手段
２１とを有する。
メモリ１５はメモリ１５ａとメモリ１５ｂとを有し、メ
モリ１５ａは慣性航法袋［３の出力する移動方位θを入
力しこれを検出成分として記憶する。記憶は順次の入力
を所定回（ｊ回とする）記憶できるようにしている。
モリ１５ａは慣性航法袋［３の出力する移動方位θを入
力しこれを検出成分として記憶する。記憶は順次の入力
を所定回（ｊ回とする）記憶できるようにしている。
成分演算手段２１は、まず、カルマンフィルタ５の出力
値（Ｘｏ 、 Ｙｏ ）を得て、これと前の出力との差
△Ｘｏ、ΔＹｏから演算成分たる最適方位θ０を次式で
求める。
値（Ｘｏ 、 Ｙｏ ）を得て、これと前の出力との差
△Ｘｏ、ΔＹｏから演算成分たる最適方位θ０を次式で
求める。
θＯ−ｊａｎ −１（ΔＸｏ／ΔＹ ｏ ） −−■そ
して、メモリ１５ｂは上記０式で求められる最適方位θ
０を演算成分として記憶する。記憶は前記検出成分の場
合と同様に順次の入力を所定回（ｊ回）記憶できるよう
にしている。
して、メモリ１５ｂは上記０式で求められる最適方位θ
０を演算成分として記憶する。記憶は前記検出成分の場
合と同様に順次の入力を所定回（ｊ回）記憶できるよう
にしている。
平均値演算部１７は、メモリ１５ａに対応する演算部１
７ａと、メモリ１５ｂに対応する演算部１７ｂとを有す
る。演算部１７ａは、メモリ１５ａに現在記憶されてい
るｊ個の方位角θの平均値θＡＹを求める。演算部１７
ｂは、メモリ１５ｂに現在記憶されているｊｌの方位角
θ０の平均値θＱＡＶを求める。、この差θＡＹ−θＱ
ＡＶをΔθとする。
７ａと、メモリ１５ｂに対応する演算部１７ｂとを有す
る。演算部１７ａは、メモリ１５ａに現在記憶されてい
るｊ個の方位角θの平均値θＡＹを求める。演算部１７
ｂは、メモリ１５ｂに現在記憶されているｊｌの方位角
θ０の平均値θＱＡＶを求める。、この差θＡＹ−θＱ
ＡＶをΔθとする。
補正値演算部１９は補正値δθを次式で求め、これを慣
性航法装置３に与える。
性航法装置３に与える。
δθ＝Δθ＋δθ′・・・・・・■
ここに、δθ′は前に求めた補正値である。
、次に第３図のフローチャートに基づいて上記位置推定
装置で行われる位置推定方式を説明する。
装置で行われる位置推定方式を説明する。
ステップ３０１において処理が開始され、ステップ３０
３で以後の計算に必要な初期値の設定が行われる。処理
回数をｋとする。
３で以後の計算に必要な初期値の設定が行われる。処理
回数をｋとする。
ステップ３０５で電波航法袋Ｍ１により車両の現在位置
Ｘ（ｋ）、Ｙ（ｋ）を計算する。
Ｘ（ｋ）、Ｙ（ｋ）を計算する。
ステップ３０７．３０９．３１１は慣性航法装置３の処
理を示す。即ち、ステップ３０７はジャイロ等方位計測
装置の計測結果に基づいて車両の移動方位θ′（ｋ）を
算出する。
理を示す。即ち、ステップ３０７はジャイロ等方位計測
装置の計測結果に基づいて車両の移動方位θ′（ｋ）を
算出する。
ステップ３０９は上記移動方位θ′（ｋ）から前に求め
られている補正値δ′θ＝δθ（ｋ−１）を減算し補正
された方位角θ（ｋ ）を求める。なお、この値θ（ｋ
）はメモリ１５ａに記憶する。
られている補正値δ′θ＝δθ（ｋ−１）を減算し補正
された方位角θ（ｋ ）を求める。なお、この値θ（ｋ
）はメモリ１５ａに記憶する。
ステップ３１１は車速センサの検出速度■に上記方位角
θ（ｋ）を対応させ移動速度のＸＹ成分ｖｘ ｖｙを求
める。
θ（ｋ）を対応させ移動速度のＸＹ成分ｖｘ ｖｙを求
める。
ステップ３１３はカルマンフィルタ５の処理を示し、ス
テップ３０５及びステップ３１１で求めた位置データ（
Ｘ（ｋ）、Ｙ（ｋ））及び速度データ（Ｖｘ （ｋ
＞、 Ｖｙ （ｋ ） ）を用いてカルマンフィルタ
特有の計算を行い、車両の現在位置に対する最適推定位
置（Ｘｏ （ｋ ＞、 Ｙｏ （ｋ ） ）を求め
る。
テップ３０５及びステップ３１１で求めた位置データ（
Ｘ（ｋ）、Ｙ（ｋ））及び速度データ（Ｖｘ （ｋ
＞、 Ｖｙ （ｋ ） ）を用いてカルマンフィルタ
特有の計算を行い、車両の現在位置に対する最適推定位
置（Ｘｏ （ｋ ＞、 Ｙｏ （ｋ ） ）を求め
る。
ステップ３１５．３１７．３１９はベクトル補正手段１
３の行う補正処理を示す。
３の行う補正処理を示す。
ステップ３１５は成分演算手段２１の処理を示す。即ち
、繰り返し数（ｋ−１）回目の最適推定位＠（Ｘｏ
（ｋ −１）、 Ｙｏ （ｋ −１））と、繰り返し
数に回目の最適推定位ｆｉ（Ｘｏ（ｋ）、Ｙｏ（ｋ））
に基づいて０式により最適方位角θ０（ｋ ）を求める
。この値θ０ （ｋ）はメモリ１５ｂに記憶する。
、繰り返し数（ｋ−１）回目の最適推定位＠（Ｘｏ
（ｋ −１）、 Ｙｏ （ｋ −１））と、繰り返し
数に回目の最適推定位ｆｉ（Ｘｏ（ｋ）、Ｙｏ（ｋ））
に基づいて０式により最適方位角θ０（ｋ ）を求める
。この値θ０ （ｋ）はメモリ１５ｂに記憶する。
ステップ３１７は平均値演算部１７ａ及び１７ｂの処理
を示し、方位角θ＜ｒ ＞及び最適方位角θｏ （ｉ
） ［ｉ ＝（ｋ −ｊ ）〜にで、ｊは任意の整数
］の平均値θＡｖ（ｉ）、θ□Ａｖ（ｉ）を第３図図示
の数式でそれぞれ求める。即ち、平均値は処理回数ｊ回
の平均値として求める。
を示し、方位角θ＜ｒ ＞及び最適方位角θｏ （ｉ
） ［ｉ ＝（ｋ −ｊ ）〜にで、ｊは任意の整数
］の平均値θＡｖ（ｉ）、θ□Ａｖ（ｉ）を第３図図示
の数式でそれぞれ求める。即ち、平均値は処理回数ｊ回
の平均値として求める。
ステップ３１９は補正値演算部１９の処理を示している
。ここでは、ステップ３１７で求めた平均値θ０Ａｙ（
ｋ）と平均値θ□Ａｖ（ｋ）との差に、繰り返し回数（
ｋ−１）の次点における誤差補正値δθ（ｋ−１）を加
算して次回の補正値δθ（＝δθ（ｋ））を算出する。
。ここでは、ステップ３１７で求めた平均値θ０Ａｙ（
ｋ）と平均値θ□Ａｖ（ｋ）との差に、繰り返し回数（
ｋ−１）の次点における誤差補正値δθ（ｋ−１）を加
算して次回の補正値δθ（＝δθ（ｋ））を算出する。
ステップ３２１では繰り返し回数にの値を１増加させ、
しかる後ステップ３０５に返り、ステップ３０５からス
テップ３２１の処理を繰り返す。
しかる後ステップ３０５に返り、ステップ３０５からス
テップ３２１の処理を繰り返す。
以上により、ステップ３１９で補正値δθ（ｋ）が求め
られるが、この補正値δθ（ｋ）は検出値と演算値との
平均値差として求められたものであるから、演算値に多
少のばらつきがあったとしてもこれは平均されて吸収さ
れ、より正確な補正値を得ることができるのである。
られるが、この補正値δθ（ｋ）は検出値と演算値との
平均値差として求められたものであるから、演算値に多
少のばらつきがあったとしてもこれは平均されて吸収さ
れ、より正確な補正値を得ることができるのである。
なお、上記実施例の説明においては、検出値と演算値と
を平均値演算部１７ａ、１７ｂでそれぞれ平均してから
補正値演算部１９でその差を求めるようにしたが、ｋ回
、ｋ−１、・・・・・・ｋ−ｊ回ごとの差を求めてから
、この差を平均しても同様の結果を得ることができるこ
とは勿論である。
を平均値演算部１７ａ、１７ｂでそれぞれ平均してから
補正値演算部１９でその差を求めるようにしたが、ｋ回
、ｋ−１、・・・・・・ｋ−ｊ回ごとの差を求めてから
、この差を平均しても同様の結果を得ることができるこ
とは勿論である。
［発明の効果］
以上詳細に説明したように、この発明によれば、例え絶
対位置検出手段の出力値がばらついたとしてもベクトル
検出手段の誤差補正を正確に行うことができるので、よ
り正確な推定位置を最適推定することのできる移動体位
置推定装置を提供することができる。
対位置検出手段の出力値がばらついたとしてもベクトル
検出手段の誤差補正を正確に行うことができるので、よ
り正確な推定位置を最適推定することのできる移動体位
置推定装置を提供することができる。
第１図はこの発明の概要を示すブロック図、第２図は移
動体位置推定装置の実施例ブロック図、 第３図は第２図に示した位置推定装置の処理フローチャ
ート、 第４図は従来の移動体位置推定装置のブロック図、 ７・・・絶対位置検出手段 ９・・・ベクトル検出手段 １１・・・移動体位置推定手段 １３・・・ベクトル補正手段 特許出願人 日産自動車株式会社；略テモ５ 代理人 弁理士 三 好 保 男７−ｊ；、−二
！第１図 第２図 第３
動体位置推定装置の実施例ブロック図、 第３図は第２図に示した位置推定装置の処理フローチャ
ート、 第４図は従来の移動体位置推定装置のブロック図、 ７・・・絶対位置検出手段 ９・・・ベクトル検出手段 １１・・・移動体位置推定手段 １３・・・ベクトル補正手段 特許出願人 日産自動車株式会社；略テモ５ 代理人 弁理士 三 好 保 男７−ｊ；、−二
！第１図 第２図 第３
Claims (1)
- 移動体位置を絶対位置で検出する絶対位置検出手段と
、移動体の移動量を移動方位及び移動速度等移動量成分
で検出するベクトル検出手段と、前記絶対位置検出手段
から絶対位置情報を受けると共に前記ベクトル検出手段
から移動方位及び移動速度等移動量成分の情報を受け移
動体の位置を最適推定する移動体位置推定手段と、前記
ベクトル検出手段の順次の検出成分と前記移動体位置推
定手段が最適推定した移動体位置に基いて前記順次の検
出成分に対応させて求めた順次の演算成分との平均的差
で前記ベクトル検出手段の補正を行うベクトル補正手段
と、を具備して成る移動体位置推定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP27494484A JPS61155813A (ja) | 1984-12-28 | 1984-12-28 | 移動体位置推定装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP27494484A JPS61155813A (ja) | 1984-12-28 | 1984-12-28 | 移動体位置推定装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPS61155813A true JPS61155813A (ja) | 1986-07-15 |
Family
ID=17548726
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP27494484A Pending JPS61155813A (ja) | 1984-12-28 | 1984-12-28 | 移動体位置推定装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPS61155813A (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63311116A (ja) * | 1987-06-15 | 1988-12-19 | Tokyo Keiki Co Ltd | 位置検出装置 |
| FR2684213A1 (fr) * | 1991-11-26 | 1993-05-28 | Renault | Procede et dispositif de localisation d'un vehicule terrestre par couplage d'informations de navigations a l'estime et d'informations de localisation captees par radio. |
| DE112018008222T5 (de) | 2018-12-20 | 2021-09-09 | Mitsubishi Electric Corporation | Bewegungsplan-erzeugungseinrichtung und autonomes fahrsystem |
-
1984
- 1984-12-28 JP JP27494484A patent/JPS61155813A/ja active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63311116A (ja) * | 1987-06-15 | 1988-12-19 | Tokyo Keiki Co Ltd | 位置検出装置 |
| FR2684213A1 (fr) * | 1991-11-26 | 1993-05-28 | Renault | Procede et dispositif de localisation d'un vehicule terrestre par couplage d'informations de navigations a l'estime et d'informations de localisation captees par radio. |
| DE112018008222T5 (de) | 2018-12-20 | 2021-09-09 | Mitsubishi Electric Corporation | Bewegungsplan-erzeugungseinrichtung und autonomes fahrsystem |
| US12097886B2 (en) | 2018-12-20 | 2024-09-24 | Mitsubishi Electric Corporation | Travel plan generation device and autonomous driving system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US4416067A (en) | Correction method and device for a magnetic field probe | |
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| US7983842B2 (en) | Vehicular present position detection apparatus and program storage medium | |
| US7076342B2 (en) | Attitude sensing apparatus for determining the attitude of a mobile unit | |
| US8527201B2 (en) | Azimuth detecting device and azimuth detecting method | |
| JP3164659B2 (ja) | 航法装置 | |
| CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
| US20110202204A1 (en) | System and Method of Navigation based on State Estimation Using a Stepped Filter | |
| WO1997008514A1 (en) | Movement detector | |
| JPH04127016A (ja) | 車両用ナビゲーション装置 | |
| JP5022747B2 (ja) | 移動体の姿勢及び方位検出装置 | |
| CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
| JPH03293517A (ja) | 地磁気センサの着磁補正法 | |
| KR101964059B1 (ko) | 실내 주차장 안내 시스템을 위한 바퀴 속력 센서 기반 차량 측위 시스템 | |
| JPH0944244A (ja) | 無人車の走行制御装置 | |
| JP2004045385A (ja) | 移動体の姿勢検出装置 | |
| JPH03285110A (ja) | 角速度計測装置 | |
| JPH0949737A (ja) | 航法信号出力方法 | |
| CN114397480B (zh) | 声学多普勒测速仪误差估计方法、装置及系统 | |
| CN112525143B (zh) | 一种设备安装角的确定方法及车载终端 | |
| JP3503204B2 (ja) | 車両用現在位置検出装置 | |
| JP2000321069A (ja) | 方位角検出装置 | |
| JP3746851B2 (ja) | 宇宙航行体の姿勢推定装置 | |
| JP2002286829A (ja) | 絶対位置推定装置 | |
| JPH0390815A (ja) | 移動体用ナビゲーション装置 |