JPS63311116A - 位置検出装置 - Google Patents

Abstract

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

Description

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

〔従来の技術〕

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

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

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

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

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

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

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

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

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

又、方位角φそのもののリセットには、搬送車（１）を
、所定のりセント位置に移動させると共に、外部に搬送
車（１）の方位を検出する装置を必要とし、且つその検
出方位精度を十分良くしておくことが必須で、総合的に
システムコストの上昇をきたす等の問題点があった。

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

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

本発明によれば、距離センサ（３）と、方位センサｆ２
）、　（７）と、位置演算器（６）と、位置センサ（４
Ｌ　（４’　）・・・とからなる航走体用位置計測装置
において、時刻Ｔ、における今回の外部位置情報及びシ
ステムの出力位置を記憶する第１及び第２のレジスタ（
４１）　、　（４２）と、時刻Ｔｎ−１における前回の
外部位置情報を記憶する第３のレジスタ（４３）と、上
記第２のレジスタ（４２）の出力位置と上記第１のレジ
スタ（４１）の外部位置情報との差、即ち位置誤差ΔＸ
。

ΔＹを演算出力する位置誤差演算器（４４）と、第１及
び第３のレジスタ（４１）　、　（４３）の出力から時
刻Ｔｎ−１の位置に対する時刻Ｔｎの位置の方位角φｎ
及び両時刻間に航走体が移動した直線距離りを演算する
距離・方位角演算器（４５）と、上記位置誤差演算器及
び上記距離・方位角演算器の出力から上記位置誤差Δｘ
、ΔＹを上記距離りの方向及びそれと直角方向の位置誤
差Δｘ、Δｙに変換する座標変換演算器（４６）と、該
座標変換演算器の位置誤差Δｘ、Δｙと上記距離方位角
演算器の距離出力りとから、方位センサの誤差Δφｎ及
び距離センサ誤差Δ汎を演算するセンサ誤差修正演算器
（４７）とからなるセンサ誤差修正システム（３０）を
設けると共に、上記、方位センサ誤差出力を上記方位セ
ンサの出力に、又上記距離センサ誤差出力を上記距離セ
ンサの出力にそれぞれ加算修正するようになした位置検
出装置が得られる。

〔作　用〕

時刻Ｔ、−、（点ｐ、ｌ−，）における位置センサ（４
１，（４”）・・・の出力を記憶している第３のレジス
タ（４３）及び時刻Ｔｎ（点Ｐ、）における位置センサ
の出力を記憶している第１のレジスタ（４１）の出力を
用いて、距離・方位角演算器（４５）に於て、点ｐｎ−
１から八までの直線距離りと、基準座標に対する線分り
の方位角φイを演算する一方、時刻Ｔｎにおける位置補
正演算器αυの出力を記憶しである第２のレジスタ（４
２）及び第１のレジスタ（４１）の両出力から、位置誤
差演算器（４４）において時刻Ｔゎにおける位置誤差Δ
ｘ、ΔＹを演算する。次に、上記距離・方位角演算器（
４５）の方位出力φｎと上記位置誤差ΔＸ。

ΔＹとから、座標変換演算器（４６）において、距離り
方向の誤差ΔＸと距離りと直交する方向の誤差Δｙとを
演算し、この誤差Δｘ、Δｙ及び距離・方位角演算器（
４５）の出力りとから、 方位誤差　Δφｎ＝Δｙ　／　Ｌ 距離センサ誤差　ΔＳｌ、＝Δｘ　／　Ｌをセンサ誤差
修正演算器（４７）において演算し、方位誤差Δφ□を
方位積分器（７）の出力に、又距離センサ誤差ΔＳｆｉ
を距離センサ（３）の出力に、修正用としてフィードバ
ンクして、角速度センサ及び距離センサの誤差を修正す
る。

〔実施例〕

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

本発明は、第５図に示した従来の位置検出装置、即ち位
置演算器（６）に、第１図に於て破線で囲ったセンサ誤
差修正システム（３０）を付加したものである。このセ
ンサ誤差修正システム（３０）は、位置センサ（４Ｌ　
（４’　）・・・位置補正演算器αυの出力を入力トス
るセンサ誤差演算器（３１）と、このセンサ誤差演算器
（３１）の一方の出力Δφｎを入力とする、方位積分器
（７）及びｓｉｎ／ｃｏｓ発生器（８）間に設けた方位
誤差修正器（３２）と、距離センサ（３）の出力及びセ
ンサ誤差演算器（３１）の他方の出力Δれを入力とする
距離Ｓ／Ｆ修正器（３３）とから構成される。

次に、センサ誤差演算器（３１）の−例を、第２図を参
照して説明する。同図に於て、（４１）は、第１のレジ
スタで、位置センサ（４）の時刻ＴＩ、（点Ｐｆｉ）に
おける搬送車（１）（第４図参照）の最新の位置データ
シ、礼を収納する。（４２）は、第２のレジスタで、時
刻Ｔ。における位置補正演算器αυの位置出力データＸ
、、、　Ｙ、、を収納する。（４３）は、第３のレジス
タで、時刻Ｔｎ、−１（点Ｐア−Ｉ）、即ち時刻Ｔ、。

のすぐ前の位置センサ（４′）の近傍を同一搬送車（１
）が通過した時刻の位置データ父ｎ−１，ｙ、−＋を第
１のレジスタ（４１）より収納する。位置誤差演算器（
４４）は、上記第２及び第１のレジスタ（４２）　、　
（４１）の出力Ｘ、１．　Ｙｌｌ、又７．礼、からΔＸ
　　＝　　Ｘ、ｌ−ＸＩ、・ｆａ）ΔＹ　　＝　　Ｙ、
１−Ｙｌｌ　　　　　　　　　・・・（ｂｌの式に従っ
て、時刻Ｔ。（点へ）に於ける搬送車（１１の正しい位
置に対する位置誤差Δｘ、ΔＹを演算する。

距離・方位角演算器（４５）は、第１及び第３のレジス
タ（４１）　、　（４３）の出力Ｘ、、、Ｙ、、、　Ｘ
ｆｉ−＋、　Ｙ−＋から、搬送車ｆｉｌの時刻Ｔイー１
点（ｐ、−＋）とＴ□（点Ｐ。）との２点間の直線距離
り及び搬送車（１）のＸ方向よりの方位角φ。を次式に
従って演算する。

座標変換演算器（４６）は、位置誤差演算器（４４）の
位置誤差出力Δｘ、ΔＹと、距離・方位角演算器（４５
）の方位角出力φｎとから、次式に従う演算を行って、
搬送車＋１）の点へ−１から点Ｐアに向って引いた線分
り方向の、誤差ΔＸと線分りと直交する方向の誤差Δｙ
に変換する。即ち、 ΔＸ　＝ΔＸｃｏｓφｎ　＋ΔＹｓｉｎφ、　　　　　
　　・（ｅ）Δｙ　−ΔＹｃｏｓφｎ−ΔＸ５ｉｎφ、
　　　　　　　・（ｆｌセンサ誤差修正演算器（４７）
は、上記誤差Δｘ、Δｙと距離・方位角演算器（４５）
の距離出力りとから、次式の演算を行って、方位角誤差
Δφｎと距離センサ誤差（スケールファクタ誤差）Δれ
を次式！ｇ）、　（ｈ）に従って演算すると共に、これ
等を方位誤差修正器（３２）及び距離Ｓ／Ｆ修正器（３
３）に修正信号として出力する。即ち、 Δφゎ＝Δｙ／Ｌ　　　　　・・・（ｇｌΔＳ９−Δｘ
／Ｌ　　　　　・・・（ｈｌ第３図は上述した本発明の
詳細な説明に供する路線図である。

時刻ｔ＝Ｔア−８において、搬送車（１）が点Ｐ。−１
にあって、この位置において、正しい位置に対して方位
誤差Δφイ、距離センサ（３）のスケールファクタ誤差
ΔＳ、ｌを有していたものとし、搬送車（１）が、例え
ば経路Ａを通って、時刻ｔ＝Ｔイで、点ＰＩｌに達した
ものとし、この時、位置センサ（４）の指示している位
置をＱ、　（ｘ、、、、　ｙ、、）とする。又、簡単の
為に、点Ｐｎ−１をＸ、　Ｙ座標の原点（０，０）トす
る。位置センサ（４）の出力位置を（Ｘ、Ｙ）　、真の
位置を（Ｘ、　Ｙ）、真の方位角をφとすれば、となる
。尚、上式でＶは搬送車（１）の速度とする。

方位誤差Δφｎ、スケールファクタ誤差ΔＳｆｉは、共
に微小量とし、ｘ、　Ｘ方向の位置誤差をそれぞれΔｘ
、ΔＹとすれば を用いて近似計算を行えば ΔＸ＝−Δφｆｉｙ＋ｘΔＳ、ｌ・（ｋ）ΔＹ−Δφ、
ＩＸ＋ＹΔれ　　　　　・・・（Ａ）よって、時刻ｔ＝
Ｔ、ｌにおける各値をサフィックスｎをつけて表わせば Δｘ、ｌ＝−Δφｎ・ｙｎ”ｘｎΔＳＩ、・（１１ΔＹ
Ｉ、−Δφ、・Ｘ、ｌ＋ＹｌｌΔれ　　　　　　・・・
（２）即ち、（１）及び（２）式より明らかな如く、ｔ
＝Ｔｎにおける搬送車ｔｌ）の位置誤差ΔＸア、ΔＹ７
は、搬送車（１１の途中のコースによることなく、時刻
Ｔ、ｌ−１とＴ□との間に移動したＸ方向、Ｙ方向の座
標変化のみに依存することになる。ここで、原点Ｐｎ−
１から点Ｐ７に向う線分を考え、その長さをＬとし、且
つＸ軸と線分りとの角度をφイとし、又、線分りの延長
方向をＸ軸、これと直角な方向をｙ軸とΔｙ、、とすれ
ば、［１１，［２１弐よりΔＸ＋＋−ΔＸ１ｌｃｏｓφ
、十ΔＹ、１Ｓｉｎφ、＝ＬΔＳ、　　　・＋３１ΔＶ
ｎ−−ΔＸｌ１ｓｉｎφｎ＋ΔＹｎｃｏｓφ、＝ＬΔφ
、　・１４）Ｌ＝Ｊ太７「王】　　　　　　・・・（５
）φｎ　＝　ｔａｎ−’　（Ｙｌｌ／　Ｘ１１）　　　
　　＝・（６１即ち、ｔ＝Ｔイにおける位置誤差Δｘ７
．ΔＹ１を、点へ−１〜ｐ、間の距離し、方位角φｎを
用いて、線分りの延長方向（Ｘ軸）およびこれと直角な
方向くｙ軸）の誤差ΔＸ？、、Δｙ、、に変換し、これ
等を距離りで除算することにより、方位角誤差Δφ０、
距離センサ（３）のスケールファクタ誤差Δれを求める
ことができる。

即ち、 Δφ９＝Δｙ、ｌ／Ｌ　　　　　　・・・（７）ΔＳｌ
ｌ　−Δｘ、ｌ／　Ｌ　　　　　　−（８）第２図は、
上記の原理を具体化したもので、ΔＸ１ｌｌ　ΔＹ１の
演算を位置誤差演算器（４４）が、（５）。

（６）式の距離し、方位角φ、の演算を距離・方位角演
算器（４５）が、（３）、（４）式の座標変換を座標変
換演算器（４６）が、又、（７）、（８）式の修正量計
算をセンサ誤差修正演算器（４７）がそれぞれ行うよう
になっている。

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

〔発明の効果〕

角速度センサと距離センサとを用いた位置検出装置にお
いて、連続した２個の離散的な外部位置情報を用いて、
航走体の方位誤差を検出するようにしたことにより、方
位誤差に起因する位置誤差を大巾に減少させることがで
き、高精度の位置検出装置を得ることができる。

又、位置情報により方位誤差を修正するようにしたこと
により、低精度低コストの角速度センサの使用が可能と
なる。

更に、位置情報により、距離センサのスケールファクタ
誤差を検出修正するようにしたことにより、高精度の位
置検出装置を得ることができる。

【図面の簡単な説明】

第１図は本発明によるセンサ誤差修正システムを有する
位置検出装置のブロック図、第２図は第１図のセンサ誤
差修正システムのブロック図、第３図は本発明の原理説
明図、第４図は航走体の位置検出装置の外観図、第５図
は従来方式の位置検出装置のブロック図をそれぞれ示す
。 図において（１）は搬送車、（２）は角速度センサ、（
３）は距離センサ、＋４）、　＋４１　’は位置センサ
、（６）は位置演算器、（３０）はセンサ誤差修正シス
テム、（３１）はセンサ誤差演算器、（３２）は方位誤
差修正器、（３３）は距離Ｓ／Ｆ修正器を夫々示す。

Claims (1)

1. 【特許請求の範囲】 距離センサと、方位センサと、位置演算器と、位置セン
   サとからなる航走体用位置計測装置において、 時刻Ｔ＿ｎ、における今回の外部位置情報及びシステム
   の出力位置を記憶する第１及び第２のレジスタと、 時刻Ｔ＿ｎ＿−＿１における前回の外部位置情報を記憶
   する第３のレジスタと、 上記第２のレジスタの出力位置と上記第１のレジスタの
   外部位置情報との差、即ち位置誤差ΔＸ、ΔＹを演算出
   力する位置誤差演算器と、 第１及び第３のレジスタの出力から時刻Ｔ＿ｎ＿−＿１
   の位置に対する時刻Ｔ＿ｎの位置の方位角φ＿ｎ及び両
   時刻間に航走体が移動した直線距離Ｌを演算する距離・
   方位角演算器と、 上記位置誤差演算器及び上記距離・方位角演算器の出力
   から上記位置誤差ΔＸ、ΔＹを上記距離Ｌの方向及びそ
   れと直角方向の位置誤差Δｘ、Δｙに変換する座標変換
   演算器と、 該座標変換演算器の位置誤差Δｘ、Δｙと上記距離・方
   位角演算器の距離出力Ｌとから、方位センサの誤差Δφ
   ＿ｎ及び距離センサ誤差ΔＳ＿ｎを演算するセンサ誤差
   修正演算器とからなるセンサ誤差修正システムを設ける
   と共に、 上記、方位センサ誤差出力を上記方位センサの出力に、
   又上記距離センサ誤差出力を上記距離センサの出力にそ
   れぞれ加算修正するようになしたことを特徴とする位置
   検出装置。