JPS63311115A - 位置検出装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、搬送車等の航走体の位置を、距離センサ及び
方位センサの情報を用いて、自立的に検出する位置検出
装置に関する。

〔従来の技術〕

第４図及び第５図を参照して、従来方式の航走体の自立
型位置検出装置の一例を説明する。

第４図において、（１）は、その航走時の位置が検出さ
れるべき、例えば搬送車の如き航走体である。

（２）は、角速度センサで、その入力軸が搬送車（１１
の旋回軸に平行となるように、搬送車＋１１に取付けら
れる。（３）は、距離センサで、搬送車（１１の車輪（
５）の回転軸に取付けた車輪（５）の回転に応じてパル
スを発生するパルス発生器が、その代表例である。（４
）。

（４′）・・・は、搬送車（１１の行動領域中の任意の
場所に夫々側々に設置した位置センサで、それ等の近傍
を搬送車（１）が通過した時に、この搬送車（１）のそ
の正しい位置を検出し、その検出位置を自立型位置検出
装置、即ち位置演算器（６）に入力し、その出力位置を
真の位置にリセットさせるに供する。位置センサ（４１
，（４’　”）・・・とじては、光学的な方式、超音波
、電波等各種の方式が考えられるが、それ等の詳細は本
発明とは関係がないのでこれ以上の説明は省略する。

位置演算器（６）を、第５図を参照して説明する。

同図に示す如く、角速度センサ（２）の出力杏は、位置
演算器（６）内の方位積分器（７）において積分され、
これより、搬送車＋１１の各時刻の方位角φが得られ、
この方位角φが、ｓｉｎ／ｃｏｓ発生器（８）に入力さ
れる。これよりの方位角φの正弦ｓｉｎφ及び余弦ｃｏ
ｓφを、それぞれＹ距離演算器（９）、Ｘ距離演算器ａ
ωへ入力する。尚、角速度センサ（２）と方位積分器（
７）とは積分ジャイロの如き１個の方位センサで代替す
ることも可能である。

ｙ、ｘ距離演算器＋９１．　Ｑｌは、このｓｉｎφ、　
ＣＯ３−と、距離センサ（３）よりの距離出力ΔＤとか
ら、積分により搬送車（１１の基準位置からの距離、即
ち位置を演算出力する、搬送車＋１１が例えば位置セン
サ（４）の近傍を通過すると、これよりの搬送車（１１
のその瞬間の正しい位置が、位置演算器（６）の位置補
正演算器αυに入力され、それに入力されているＹ及び
Ｘ距離演算器（９）及びα０）よりの位置出力を正しい
値にリセットする。

〔発明が解決しようとする問題点〕

しかしながら上述した従来の自立型位置検出装置におい
ては、位置センサｆ４１．　（４’　）・・・等の出力
により、位置演算器（６）の出力位置を正しい値にリセ
ットするのみであるため、方位積分器（７）の出力゛で
ある方位角φに誤差があった場合、時間の経過と共に位
置出力の誤差が増大していくこと、又、角速度センサ（
２）の出力−には、通常ジャイロドリフトと呼ばれる誤
差が存在し、これによっても、位置誤差が増大していく
という問題点があった。

又、方位角そのもののリセットには、位置センサ（４）
及び搬送車（１）に搬送車（１）の方位を検出する特殊
な部材を必要とし、且つその検出方位精度を十分良くし
ておくことが必須で、総合的にシステムコストの上昇を
きたす等の問題点があった。

従って、本発明は、上述の如き従来の装置の欠点を一掃
した位置検出装置を提供せんとするものである。

〔問題点を解決するための手段〕

本発明は、距離センサ（３）と、角速度センサ（２）及
び方位積分器（７）からなる方位センサと、位置演算装
置（６）と、位置センサｆ４Ｌ　（４’　）・・・とか
らなる航走体の位置検出装置に、上記距離センサ（３）
、上記方位積分器（７）及び上記位置センサ（４）の出
力を入力とするセンサ誤差演算器（３１）と、上記距離
センサの出力及び上記センサ誤差演算器のスケールファ
クタ修正値（Δ■）を入力としその出力をＸ、Ｙ距離演
算器０ω、（９）に出力する距離Ｓ／Ｆ修正器（３３）
と、上記角速度センサの出力と、上記センサ誤差演算器
のドリフト修正値（Δ■）とを入力とし、その出力を上
記方位積分器に出力するドリフト修正器（３４）と、上
記方位積分器の出力と上記センサ誤差演算器の方位修正
値（Δ■）とを入力として、その出力をｓｉｎ／ｃｏｓ
発生器（８）に出力する方位誤差修正器（３２）とを設
けると共に、上記センサ誤差演算器の演算にカルマンフ
ィルタを用いたものである。

〔作　用〕

距離センサ（３）からの速度成分、方位積分器（７）か
らの方位を用いて、センサ誤差修正システム（３０）に
より常時システム誤差の発生状態を予測し、位置センサ
（４）からの位置が入力されると、それと位置補正演算
器αυのシステム位置との比較を行い、この比較結果を
観測誤差として、カルマンフィルタを用いて誤差の発生
原因、位置誤差、方位誤差、距離センサのスケールファ
クタ、ジャイロドリフトを推定し補償する。

〔実施例〕

以下、第１図乃至第３図を参照して、本発明の一実施例
を説明する。尚、同図において、第４図及び第５図と同
一部材は同一の符号を付し、これ等の説明は省略する。

本発明は、第５図に示した従来装置に、第１図に於て破
線で囲ったセンサ誤差修正システム（３ｏ）を付加した
ものである。このセンサ誤差修正システム（３０）は、
位置センサ（４Ｌ（４’）・・・位置補正演算器αＢ、
距離センサ（３）、方位積分器（７）の出力を入力とす
るセンサ誤差演算器（３１）と、該センサ誤差演算器（
３１）の出力を入力とする、方位積分器（７）及びｓｉ
ｎ／ｃｏｓ発生器（８）間に設けた方位誤差修正器（３
２）と、距離センサ（３）の出力側に設けた距離Ｓ／Ｆ
（スケールファクタ）修正器（３３）とから、主として
構成される。尚、（３４）は、角速度センサ（２）と方
位積分器（７）との間に設けたドリフト修正器で、セン
サ誤差演算器（３１）からのドリフト修正信号Δ■を入
力とし、角速度センサ（２）のドリフト誤差を修正する
のに供する。

ここで、本発明の基本となるセンサ誤差演算器（３１）
に用いられるカルマンフィルタの概要を説明する。シス
テムを１次の微分方程式で記述すると次式となる。

１（ｔ）＝Ａ（ｔ）　・ｘ（ｔ）＋Ｗ　　　　　　　　
［ＩＸ　（ｔ）　＝状態変数マトリクス Ａ　（ｔ）　＝システムマトリクス Ｗ　　−ランダムベクトル （１）式を遷移行列法で離散値形に変形すると、ｘ（ｔ
＋Δｔ）−＠　・ｘ（ｔ）　＋　Ｇ−Ｗ　　　　　　　
（２）又、観測系は次式で表わせる。

ｙ（ｔ）　＝Ｈ−ｘ（ｔ）　＋ｖ　　　　　　　　　　
（３１ｙ（ｔ）　　＝観測値 システム力（２１，＋３）式にて記述されると、カルマ
ンフィルタによる最適推定値ｘ（ｔ＋Δ１，１＋Δｔ）
は次式により求められる。

Ｐ（ｔ＋Ａｔ、ｔ）−〇　・Ｐ（ｔ、ｔ）　・ｏ”＋Ｇ
　・−・ＧＴΔｔ ・・・・・・（４） Ｋ（ｔ＋Δｔ、　ｔ）　＝　Ｐ　（ｔ＋Δ１，１）・Ｈ
７・ＣＩ（−Ｐ　（ｔ＋Δｔ、　ｔ）　ｆｌ（”十Ｖ）
　　−’−ｆ５）ｘ（を十Δｔ＋　ｔ）＝　ｅ　　・　
ｘ（ｔ、ｔ）　　　　　　　　　　　　　　　　　　＝
＋６）ｘ　（ｔ４Δ１，１＋Δｔ）−ｘ（ｔ＋Δｔ、ｔ
）＋　　Ｋ（ｔ＋Δ１．１）・（ｙ−Ｈｘ（ｔ＋Δｔ、
ｔ））　　　　　・（７）Ｐ（ｔ＋Δ１．１＋Δｔ）＝
　　（ＩＩ−Ｋ（ｔ÷Δｔ）・Ｈ〕　・Ｐ（ｔ＋Δｔ、
ｔ）　　　　　　　　・・・（８）Δｔ Ｇ−０・Δｔ　　（ｎ−□　・Ａ）　　　　　・・・α
０）ここで、■は単位マトリクス。

（４）式のＰは誤差共分散行列であり、Ｐ（ｔ＋Δ１，
１）は現時刻ｔでのシステム誤差の統計量がΔを秒後に
どの位変化するのかを予想するものである。（５）式の
Ｋはカルマンゲインであり、位置情報が得られた時に計
算され、システム誤差と観測雑音との統計量を比較し、
システムと観測値のどちらに重みをかけるかを決定する
量である。（７）式は最適推定値であり、時刻ｔでのシ
ステム誤差の推定値を用いΔを秒後の値を予測するが、
この予測値は外乱の影響で正しく予測されているとは限
らないので、観測値が得られた段階で観測値と上記予測
値との偏差を用い、最適推定値を得るため、カルマンゲ
インで修正する。（８）式は誤差共分散行列であり、観
測値が得られて、システム誤差が修正された時、システ
ム誤差の統計量も修正する必要かあり、（８）弐を用い
て修正される。

第１図をラプラス演算子Ｓを用いてモデル化すす ると、第２図となる。尚、同図に於て、−は積分を示し
、φ’、Ｘ’、Ｙ’は各々誤差を含む方位角ＸＹ距離を
表わすものとする。

角速度センサ（２）は旋回角速度ωを計測すると共に、
ジャイロドリフトによる誤差をも計測し、それを積分し
たものが方位角φ′である。距離センサ（３）からの速
度■は、スケールファクタ誤差Δ■を持って乗ぜられ、
上記方位角のｓｉｎ、　ｃｏｓをとり、積分することに
より、Ｘ方向の距離Ｘ’、Ｙ方向の距離Ｙ′となる。

ところで、Ｘ方向の距離誤差６文は、 φ′＝φ＋Δ■とすると、 Δ文−文′−文 −■　（１＋Δ■）　　ｓｉｎφ’　−Ｖｓｉｎφ＝Ｖ
ｓｉｎφ・Δ■十Ｖｃｏｓφ・Δ■同様に、Ｙ方向の距
離誤差Δ♀は、 Δ９−９′−♀ ＝■（１＋Δ■）　ｃｏｓφ’　　Ｖｃｏｓφ＝Ｖｃｏ
ｓφ・Δ５Ｆ−Ｖｓｉｎφ・Δ■方位角誤差Δ■は、 Δさ＝φ′−８ −（ω十Ｕ）−ω Ｕ スケールファクタ誤差Δ■及びジャイロドリフトＵが時
間と共に変化しないとすると、 ΔＳ’Ｆ　＝　Ｏ、Δ０＝０ 以上により、本システムを１次の微分方程式で書くと、 となる。尚、この時の状態変数マトリクスは以下で表わ
せる。

ｘｌ−（ΔＸ、ΔＹ、Δ■、Δ■、Δ■）第３図は第１
図に示すセンサ誤差演算器（３１）の−具体例のブロッ
ク図で、これは、（１１）式中のシステムマトリクスへ
の演算をするシステム誤差演算器（４１）と、（９１，
００１式の遷移行列の０．Ｇの計算をする遷移行列演算
器（４２）と、（４）式の誤差共分散行列Ｐの計算をす
る誤差共分散行列演算器（４３）と、位置センサ（４）
からの情報の有無を判定する位置情報スイッチ（４４）
と、位置情報が入力されない時に（６）式を計算する最
適予測値演算器（４５）と、位置情報が入力された時に
、位置センサ（４）と位置補正演、１２ 算器０υとの出力から現在の位置誤差を計算する位置誤
差演算器（４６）と、（５）式を計算するカルマンゲイ
ン演算器（４７）と、（６）、　（７１式を計算する最
適推定値演算器（４８）と、（８）式を計算する誤差共
分散行列修正演算器（４９）とからなっている。

次に、センサ誤差演算器（３１）の位置情報スイッチ（
４４）の入出力関連を更に詳細に説明する。位置センサ
（４）からの位置情報が無い時には、遷移行列演算器（
４２）の出力が、位置情報スイッチ（４４）を介して最
適予測値演算器（４５）に供給され、これが（６）式の
演算を行う。一方、位置センサ（４）からの位置情報が
有る時には、誤差共分散行列演算器（４３）の出力を、
位置情報スイッチ（４４）を介して、カルマンゲイン演
算器（４７）に供給し、ここで（５）式の演算を実施す
ると共に、該カルマンゲイン演算器（４７）、位置誤差
演算器（４６）、最適予測値演算器（４５）の各各の出
力を最適推定値演算器（４８）に入力し、ここで、＋６
１．　（７）式の演算を行い、誤差修正値出力ΔＸ。

第１図に示す位置補正演算器（１１）、方位誤差修正器
（３２）、距離Ｓ／Ｆ修正器（３３）、ドリフト修正器
（３４）に夫々供給すると共に、位置情報スイッチ（４
４）を介した誤差共分散行列演算器（４３）及びカルマ
ンゲイン演算器（４７）の出力を、誤差共分散行列修正
演算器（４９）に入力し、（８）式の演算を行う。該、
誤差共分散行列修正演算器（４９）の出力Ｐは、次回、
誤差共分散行列演算器（４３）の入力として用いられる
。

尚、以上は角速度センサ（２）の信号を方位積分器（７
）により積分した方位角φを用いた位置検出装置を示し
たが、本発明はこれ等に限定されるものではな（、レー
ト積分ジャイロ、リングレーザ−ジャイロ、チューンド
ドライジャイロ等のように直接方位角を出力するタイプ
のジャイロ、又、ジャイロコンパスのように直接真北か
らの絶対方位角を出力するセンサも使用可能である。

〔発明の効果〕

角速度センサと距離センサとを用いた位置検出装置にお
いて、離散的な外部位置情報のみを用いて、位置誤差、
方位誤差、ジャイロドリフト等のシステム誤差をカルマ
ンフィルタを用いて検出演算するようにしたことにより
、特に角速度センサのドリフトによって生ずる方位誤差
に起因する位置誤差を大巾に減少させることができ、高
精度の位置検出装置を得ることができる。又、位置情報
により方位誤差を修正するようにしたことにより、低精
度、低コストの角速度センサの使用が可能となる。更に
、位置情報により、距離センサのスケールファクタ誤差
を検出修正するようにしたことにより、高精度の位置検
出装置を得ることができる。又、方位角を修正する装置
を用いることなく、位置情報のみにより、方位角と、角
速度センサのドリフトとを修正することが可能となった
ことにより、位置計測システム全体のコストを低減させ
ることができる。

【図面の簡単な説明】

第１図は本発明によるセンサ誤差修正システムを有する
位置検出装置のブロック図、第２図は第１図をモデル化
したブロック図、第３図は第１図のセンサ誤差修正シス
テムのブロック図、第４図は従来の航走体の位置検出装
置の外観図、第５図は従来方式の位置検出装置のブロッ
ク図である。 図において（１１は搬送車、（２）は角速度センサ、（
３）は距離センサ、（４）、　（４’）、・・・は位置
センサ、（６）は位置演算器、（３１）はセンサ誤差演
算器、（３２）は方位誤差修正器、（３３）は距離Ｓ／
Ｆ修正器、（３４）はドリフト修正器、（４１）はシス
テム誤差演算器、（４２）は遷移行列演算器、（４３）
は誤差共分散行列演算器、（４４）は位置情報スイッチ
、（４５）は最適予測値演算器、（４６）は位置誤差演
算器、（４７）はカルマンゲイン演算器、（４８）は最
適推定値演算器、（４９）は誤差共分散行列修正演算器
を夫々示す。

Claims (1)

1. 【特許請求の範囲】 距離センサと、角速度センサ及び方位積分器からなる方
   位センサと、位置演算装置と、位置センサとからなる航
   走体の位置検出装置において、上記距離センサ、上記方
   位積分器、上記位置センサの出力を入力とするセンサ誤
   差演算器と、上記距離センサの出力及び上記センサ誤差
   演算器のスケールファクタ修正値（Δ■）を入力としそ
   の出力をＸ、Ｙ距離演算器に出力する距離Ｓ／Ｆ修正器
   と、 上記角速度センサの出力と、上記センサ誤差演算器のド
   リフト修正値（Δ■）とを入力とし、その出力を上記方
   位積分器に出力するドリフト修正器と、 上記方位積分器の出力と上記センサ誤差演算器の方位修
   正値（Δ■）とを入力として、その出力をｓｉｎ／ｃｏ
   ｓ発生器に出力する方位誤差修正器とを設けると共に、 上記センサ誤差演算器の演算にカルマンフィルタを用い
   たことを特徴とする位置検出装置。