JP2739262B2 - 移動体の位置計測装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、いわゆる推測航法によ
って誘導走行される移動体の位置計測装置に関し、特に
推定位置および推定走行方向を正確かつ確実に補正する
ことにより移動体を予定走行路に沿って精度よく走行さ
せることができる位置計測装置に関する。

【０００２】

【従来の技術】従来より、移動体を所定の予定走行路に
沿って誘導走行させる方法として、方向検出器と走行長
検出器の出力に基づき移動体の位置および走行方向を推
定演算し、該推定演算結果に基づき予め教示しておいた
予定走行路上の通過予定地点を通過すべく移動体の走行
を操舵制御する一般的に推測航法と呼ばれる誘導方法が
ある。

【０００３】この方法の欠点は、上記方向検出器として
たとえば角速度センサを用いた場合に検出値に角速度を
積分した際の累積誤差やドリフト誤差が生じたり、走行
長検出器の検出値に路面の凹凸や車輪のスリップ等に起
因する検出誤差が発生したりして、推定演算結果に走行
距離に応じた累積誤差が生じるとともに数値計算に伴う
計算誤差が生じて正確に通過予定地点を通過できないこ
とである。

【０００４】したがって従来より予定走行路に沿って標
識を間欠的に配設し、該標識を認識することによって上
記推定位置および推定走行方向を間欠的に補正すること
が一般的に行われ、種々特許出願されている。

【０００５】この種の技術としては特開昭６１ー２１７
７８７号公報等にみられるごとく、予定走行路の両側に
光反射鏡を間欠的に設けるとともに、移動体の側方から
光ビームを上記光反射鏡に向けて投光するとともに上記
光反射鏡からの反射光を受光する投・受光器を設け、こ
の投・受光器の受光出力および光反射鏡の配設位置等に
基づき移動体の位置および走行方向を補正演算するもの
がある。

【０００６】

【発明が解決しようとする課題】しかし、従来技術で
は、３組以上の投・受光器を移動体に具える必要がある
とともに１回の補正あたり２以上の光反射鏡を必要とす
る。このように多数の光学系を具えたときには装置とし
ての信頼性の低下を招来することになる。そこで、より
少ない光学系を用いて補正を行うことができる装置の開
発が望まれる。

【０００７】また、移動体の位置および走行方向は、投
・受光器の設置位置、光の投光方向、光反射鏡の設置位
置等が既知の値であるとして演算される。したがって、
たとえば光反射鏡の設置位置の測量精度が良くなかった
り、設置の際の取付誤差があったりして、正確な値が得
られないときには演算される移動体の位置および走行方
向の補正精度が劣化することになる。したがって反射鏡
の設置位置が正確に得られなかったり、投・受光器の設
置位置および光の投光方向に誤差がある場合であっても
精度よく移動体の位置および走行方向を補正することが
できる装置の開発が望まれる。また、光反射鏡の設置位
置の測量には多大の労力が必要とされるので、かかる労
力の軽減も望まれる。

【０００８】また、移動体が走行する予定走行路として
は閉ループ状のものがあり、該閉ループの走行路を移動
体が何周も走行する場合がある。ここで、この閉ループ
の走行路上を逐次の位置および方向の情報に基づき移動
体を走行させるときにはスタート地点における移動体の
位置および方向情報が正確でないと１周周回したときの
同スタート地点における移動体の位置および方向は所望
の位置および方向から大幅にずれてきてしまうことにな
る。そこで、移動体が周回するごとにスタート地点にお
いて正確な位置および方向の情報を付与して精度よく移
動体を走行させる必要がある。

【０００９】また、光ビームの投・受光器にあっては、
投光する光ビームのパワーが大きい場合には安全上、問
題があるとともに、設置した光反射鏡以外の光反射物に
よる反射光を受光することになり信頼性上問題がある。
逆に光ビームのパワーを小さくすると反射されるべき反
射鏡において反射光が得られなくなり、これも信頼性上
問題がある。そこで、所定のパワーの光ビームを投光す
ることになるが、この場合も、反射鏡の表面の一部に反
射を妨げる汚れがあり、光ビームがこの汚れの部分に照
射されたときや、移動体がローリング、バウンジング等
により揺動して光ビームが反射鏡に照射されなくなって
しまったときには反射光の受光が不可能になり信頼性の
問題が生じることになる。また、当の反射鏡以外の光反
射物からの反射光を受光するという問題は依然として解
決されない。

【００１０】本発明は、こうした実状に鑑みてなされた
ものであり、移動体の補正位置および補正走行方向を正
確かつ確実に求めるようにして移動体を予定走行路に沿
って精度よく誘導走行させることができる位置計測装置
を提供することをその目的としている。

【００１１】

【課題を解決するための手段】そこで、この発明の第１
発明では、移動体の移動距離を検出する移動距離検出手
段と前記移動体の走行方向を検出する走行方向検出手段
の出力に基づき前記移動体の位置および走行方向を推定
演算する推定演算手段を有し、該推定演算された前記移
動体の位置および走行方向を間欠的に補正して該補正さ
れた位置および走行方向に基づき前記移動体が予め教示
された予定走行路に沿って走行するよう前記移動体を走
行制御する移動体の位置計測装置において、前記移動体
に設けられ、第１および第２の光ビームを側方に投光す
る第１および第２の投光手段と、前記移動体の予定走行
路の片側に間欠的に配設され、前記第１および第２の投
光手段から投光された第１および第２の光ビームを反射
する光反射手段と、前記移動体に設けられ、前記光反射
手段で反射された第１および第２の光ビームを受光する
受光手段と、前記受光手段の出力と前記移動距離検出手
段の出力とに基づき前記第１および第２の光ビームの一
方を前記受光手段が受光してから他方の光ビームを受光
するまでの前記移動体の移動距離を求め、該移動距離と
前記光反射手段の配設位置と前記第１および第２の光ビ
ームの投光角度と前記推定演算手段による推定走行方向
とに基づき前記移動体の補正位置を演算する補正位置演
算手段と、前記受光手段の出力と前記移動距離検出手段
の出力とに基づき前記第１および第２の光ビームの一方
を前記受光手段が受光してから他方の光ビームを受光す
るまでの前記移動体の移動距離を求め、該移動距離と前
記第１および第２の光ビームの投光角度と前記推定演算
手段による推定位置および推定走行方向とに基づき前記
光反射手段の配設位置を演算する光反射手段位置演算手
段と、前記補正位置演算手段の演算値と該補正位置演算
手段の前回の演算値と前記推定演算手段で演算された前
記移動体の位置とに基づき前記推定演算手段による推定
走行方向の誤差を求め、該誤差と前記推定演算手段によ
る推定走行方向とに基づき前記移動体の補正走行方向を
演算する補正走行方向演算手段とを具え、前記予定走行
路の教示時には、前記移動体を該予定走行路に沿って走
行させ、このとき得られた前記推定演算手段の演算位置
を通過予定地点とするとともに、前記光反射手段位置検
出手段により前記光反射手段の配設位置を検出してお
き、前記移動体を前記予定走行路に沿って走行させると
きには、前記検出された光反射手段の配設位置を前記補
正位置演算手段で用いるデータとするとともに、前記教
示された通過予定地点を前記移動体が走行するように前
記補正位置演算手段と前記補正走行方向演算手段の出力
に基づき前記移動体を走行制御するようにしている。

【００１２】また、この発明の第２発明では、第１発明
の構成において、前記予定走行路は閉ループの走行路で
あり、前記予定走行路の教示時には、前記予定走行路の
スタート地点近傍における２つの光反射手段の配設位置
と前記移動体を前記予定走行路に沿って周回させて前記
スタート地点に戻ってきたときの前記２つの光反射手段
の配設位置とをそれぞれ前記光反射手段位置検出手段に
より検出して、該検出結果と前記周回させて前記スター
ト地点に戻ってきたときの前記推定演算手段による推定
位置とに基づき前記移動体が前記予定走行路を周回した
ときの位置および走行方向のずれを演算し、前記移動体
を前記予定走行路に沿って走行させるときには、前記移
動体が前記スタート地点を通過するたびに前記演算され
たずれに基づき前記移動体の位置および走行方向を補正
するようにしている。

【００１３】また、この発明の第３発明では、第１発明
の構成において、前記第１および第２の投光手段に、前
記光ビームを所定の俯仰角度だけ拡散する手段、または
前記光ビームを所定の俯仰角度だけ走査する手段を、さ
らに設けるようにしている。

【００１４】

【作用】上記第１発明の構成によれば、補正位置演算手
段では第１および第２の光ビーム受光時点間の移動距離
とこれら光ビームの投光角度と光反射手段の配設位置と
さらに推定演算手段で演算された推定走行方向を用いて
移動体の補正位置を演算するようにしている。そして、
上記補正走行方向演算手段では、補正位置演算手段で演
算された補正位置を用いて補正走行方向が演算される。
このように推定演算された走行方向を用いて補正位置を
演算するようにしており、直接走行方向を求める必要が
ないので、光ビームを投光、受光する装置が２組で済む
とともに、光を反射する装置が１つで済み、装置の信頼
性が向上する。

【００１５】さらに上記第１発明の構成によれば、教示
時において計測された推測位置が予定通過地点とされ、
誘導走行時には、上記予定通過地点を辿るように移動体
が走行制御される。ここに、教示時と誘導走行時におい
て同じ距離検出手段および走行方向検出手段を用いてい
るので、これら検出手段の誤差が相殺される。また、教
示時に光反射手段の位置を演算する光反射手段位置演算
手段と、誘導走行時に移動体の補正位置を演算する補正
位置演算手段とでは、同じ光反射手段、投光手段、受光
手段等を用いているので、光反射手段の位置を演算する
際の誤差、光ビームの投光方向の誤差等が教示時と誘導
走行時において相殺される。この結果、移動体は精度よ
く予定走行路に沿って走行される。しかも、移動体を予
定走行路に沿って実際に移動させるだけで教示を行うこ
とができ、予定走行路の通過地点や光反射手段の位置の
測量を行う必要がないので労力が大幅に軽減される。

【００１６】さらに、上記第２発明の構成によれば、教
示時においてスタート地点と１周したときの該スタート
地点における位置および方向のずれが演算される。移動
体が周回するごとにスタート地点において上記ずれだけ
位置および方向を補正することで、ずれが累積されるこ
となくつぎの周回時における誘導走行が精度よくなされ
る。

【００１７】さらに、上記第３発明の構成によれば、光
反射手段の一部が汚れていたとしても他の部分で反射さ
れるとともに、移動体が揺動したとしても光ビームは光
反射手段に確実に照射される。特に光ビームを拡散させ
たときは光ビームが遠くに行けば行くほどパワーが低下
するので、遠くの反射物によって反射されることがなく
なり、より信頼性が向上する。

【００１８】

【００１９】

【実施例】以下、図面を参照して本発明に係る移動体の
位置計測装置の実施例について説明する。

【００２０】図１は実施例装置の構成を示すブロック図
であり、かかる装置は図５に示す移動体２に搭載される
ものとする。移動体２はたとえば無人ダンプトラックで
あり、図４に示す閉ループ状の予定走行路Ｌに沿って走
行される場合を想定している。

【００２１】図１に示すレートジャイロ４は移動体２の
随時の走行方向を検出するために設けられたものであ
り、単位時間あたりの走行方向変化量ΔθをＣＰＵ７に
出力する。一方、移動体２の図示せぬ車輪には同車輪の
回転数を検出するパルスエンコーダ５が付設されてい
る。エンコーダ５は車輪の回転量に応じた数のパルスを
出力し、これをカウンタ６に加える。カウンタ６は上記
パルス数をカウントすることにより移動体２の単位時間
あたりの移動距離（走行長）Δｌを出力し、ＣＰＵ７に
加える。上記レートジャイロ４、カウンタ６の出力Δ
θ、Δｌは後述するように推測航法に基づく移動体２の
位置、方向の推測演算に用いられる。

【００２２】光ビーム投・受光器８、９はそれぞれ図５
に示すように移動体２の進行方向Ｆに対して左側位置３
に配設されている。このうち光ビーム投・受光器８は車
体長手方向に対して垂直方向左に向けてレーザ光Ｌ1 を
投光するとともに、移動体２の予定走行路１の左側に該
走行路１に沿って間欠的に設けられた反射鏡Ｃ1 、Ｃ2
、…、Ｃj-1 、Ｃj 、…Ｃn-1 、Ｃn で反射された反
射光Ｌ1 を受光して、該受光を示す信号をゲート回路１
０に加える。同様に光ビーム投・受光器９は上記レーザ
光Ｌ1 の投光方向に対して車体前方側に角度αだけ傾斜
した方向にレーザ光Ｌ2 を投光するとともに、上記反射
鏡Ｃ1 …で反射された反射光Ｌ2 を受光して、該受光を
示す信号をゲート回路１０に加える。なお、反射鏡Ｃ1
…としては、受けた光ビームを入射方向と同じ方向に反
射する機能を具えているものとする。

【００２３】ここで、ゲート回路１０に入力される受光
信号は後述するように移動体２の位置、方向の補正演算
に用いられる。したがって、移動体２が揺動してしまい
反射鏡Ｃ1 …の受光面から光ビームＬ1 、Ｌ2 が外れて
しまったり、反射鏡Ｃ1 …の表面の汚れの部分に光ビー
ムＬ1 、Ｌ2 が照射された場合には、上記受光信号を得
ることができなくなり、当の位置、方向の補正演算を行
うことができない。

【００２４】そこで、実施例では、反射光の取得を確実
ならしめるために、図７（ａ）、（ｂ）に示すように光
ビームＬ1 、Ｌ2 の投光範囲を上下方向に広げる装置を
具えている。同図（ａ）の装置では、投・受光器８、９
のレーザ光源から射出されたレーザ光Ｌ1 、Ｌ2 がシリ
ンドリカルレンズ１２を介してビームエキスパンドさ
れ、俯仰方向に角度φをもって拡散される。また、同図
（ｂ）の装置では投・受光器８、９のレーザ光源から上
方に射出されたレーザ光Ｌ1 、Ｌ2 がレンズ１３を介し
て多角形のポリゴンミラ１４の側面に照射される。この
ためポリゴンミラ１４の回転Ｄに応じてレーザ光Ｌ1 、
Ｌ2 が俯仰方向に角度φをもって走査される。かかる装
置により反射光が確実に得られ、補正演算を確実に行う
ことができる。特に同図（ａ）のビームエキスパンドの
手法によればビームが遠方に行けば行くほど光の単位面
積あたりのパワーが小さくなるので、反射鏡Ｃ1 …より
も遠方にある他の光反射物によってレーザ光Ｌ1 、Ｌ2
が反射して受光してしまうことが防止され、受光信号の
信頼性をより向上させることができる。

【００２５】さて、ゲート回路１０はフリップフロップ
等からなり、光ビーム投・受光器９から受光信号が加え
られた時点でカウンタ１１によるカウント開始を指示す
る信号をカウンタ１１に出力するとともに、光ビーム投
・受光器８から受光信号が加えられた時点でカウンタ１
１によるカウント終了を指示する信号をカウンタ１１に
出力する。カウンタ１１にはエンコーダ５から出力され
るパルス信号が加えられている。したがって、カウンタ
１１からは反射光Ｌ2 が受光されてから反射光Ｌ1 が受
光される間に移動体２が移動した距離を示す信号が出力
され、これがＣＰＵ７に入力される。

【００２６】ここで、推測航法に基づく移動体２の位置
および方向の演算の原理について図６を併せ参照して説
明する。なお、以下小文字でｘ，ｙ、θとあるのは推測
航法に基づく演算値を示し、大文字でＸ、Ｙ、Θとある
のは後述する補正演算に基づく演算値を示すものとす
る。

【００２７】同６図に示すように予め設定された初期位
置Ｐ0 （ｘ0 、ｙ0 ）から移動体２が走行を開始する
と、カウンタ６は単位時間ごとの走行長Δｌ0 、Δｌ1
…を順次出力する。一方、移動体２の初期姿勢角がθ0
であったとすると、ジャイロ４は単位時間ごとの角度変
化量Δθ0 、Δθ1 …を順次出力する。結局、移動体２
の随時の地点Ｐi （ｉ＝１、２、３…）における位置お
よび走行方向（ｘi 、ｙi 、θi ）は下記（１）〜
（３）式のごとく演算される。

【００２８】 同図に示す実線３０は上記（１）〜（３）式によって得
られる位置、方向を時間の経過とともにプロットした移
動体２の走行軌跡を示し、破線３１は移動体２の実際の
位置、方向をプロットした走行軌跡を表している。同図
から明かなように移動体２が長きにわたって走行を続け
るにつれて推測航法に基づく演算値は実際の位置、方向
に対して大きなずれを生じているのがわかる。

【００２９】こうした累積誤差が生じる原因は上記
（１）〜（３）式における演算値にジャイロ４、エンコ
ーダ５による検出誤差が含まれること、および積分計算
上のケタ落ち等に起因する演算誤差が含まれることによ
る。

【００３０】そこで、つぎに、こうした誤差を含む位
置、方向を間欠的に補正する演算処理の原理について説
明する。図５はかかる補正演算を説明するための図であ
り、いま同図に示す反射鏡Ｃ1 …の配設位置は予め測量
等により計測されるか、既知の位置に配設することによ
り既知であるものとする。

【００３１】移動体２としては軌跡１に示すように進行
し、上記反射鏡Ｃ1 …の側方を通過するたびに以下のよ
うな演算を行う。なお、図において一点鎖線で示す１´
は上記推測航法に基づく演算値をプロットした軌跡であ
り、実線で示す１は実際の移動体２の軌跡を示してい
る。

【００３２】すなわち、移動体２が位置（Ｘc/j 、Ｙc/
j ）の反射鏡Ｃj の近傍を通過すると、まず投・受光器
８、９の配設位置３が位置Ｒj に達した時点で光ビーム
投・受光器９から投光されている光Ｌ2が反射鏡Ｃj で
反射され、反射光Ｌ2 が投・受光器９で受光される。つ
ぎに配設位置３が位置Ｑj に達した時点で光ビーム投・
受光器８から同様に投光されている光Ｌ1 が同様に反射
され反射光Ｌ1 が投・受光器８で受光される。この結
果、カウンタ１１は、２点Ｒj 、Ｑj 間の移動距離ａj
を計測する。

【００３３】このため、幾何学的関係から明らかに位置
Ｑj における移動体２の走行方向をΘとすると、上記距
離データａj と反射鏡Ｃj 位置（Ｘc/j 、Ｙc/j ）によ
り次式から移動体２の補正位置Ｑj （Ｘj 、Ｙj ）が得
られる。

【００３４】 Ｘj ＝Ｘc/j ＋ａj ・ｃｏｔα・ｓｉｎΘj …（４） Ｙj ＝Ｙc/j ーａj ・ｃｏｔα・ｃｏｓΘj …（５） ここで走行方向の正しい値Θj が未知であるとしても、
推測航法の演算によって得られる、対応する位置Ｑ´j
における計測値θj の誤差が小さく、かつ反射鏡Ｃj と
位置Ｑj との距離が大きくないので、上記（４）、
（５）式はΘj の代わりに推定演算値θj を用いること
により下記（６）、（７）のごとく近似することができ
る。

【００３５】 Ｘj ＝Ｘc/j ＋ａj ・ｃｏｔα・ｓｉｎθj …（６） Ｙj ＝Ｙc/j ーａj ・ｃｏｔα・ｃｏｓθj …（７） また、推測航法により上記のごとく移動体２の補正位置
Ｑj が演算されると、これと前回の反射鏡Ｃj-1 近傍通
過時の補正位置Ｑj-1 とから、走行方向の推定演算値θ
j の誤差Δθj が下記（８）式のごとく得られる。

【００３６】Δθj ＝角度Ｑj Ｑj-1 Ｑ´j …（８） したがって走行方向の補正値Θj は次式（９）によって
得られる。

【００３７】Θj ＝θj ＋Δθj …（９） また、エンコーダ５のスケールファクタＫl はそれまで
に用いていたＫ´l から次式によって修正される。

【００３８】 Ｋl ＝Ｋ´l ・Ｑj Ｑj-1 ／（Ｑ´j Ｑj-1 ） …（１０） 以上のように移動体２の走行方向θj の計測誤差は
（１）〜（３）式に示した推測航法による演算に対する
影響は大きいが、（４）、（５）式に示した補正演算に
対する影響は小さいので、（６）、（７）式に示すよう
に誤差を含んだ推測演算値θj を用いても、位置を精度
よく計測することができ、さらにこの位置に基づき
（９）式から走行方向を精度よく計測することができ
る。ここに、かかる補正演算は従来のように正しい走行
方向Θj を光ビーム投・受光器の出力から直接検出する
必要がないので、光ビーム投・受光器を２つおよび反射
鏡の数を１つと少なくすることができる。このため、受
光回数が減少し、装置の信頼性が向上する。また、反射
鏡としても従来のように走行路両側に設ける等の必要が
なく、設置場所の制約を小さくすることができるように
なる。

【００３９】ＣＰＵ７は上記説明した推測航法に基づく
推定演算（１）〜（３）および補正演算（６）〜（１
０）を行い、図示せぬステアリング駆動部に対して予定
走行路１に沿って走行させるための舵角指令を出力し
て、移動体２のステアリングを駆動制御する。

【００４０】さて、移動体２としては、予定走行路Ｌが
予め教示され、この教示された予定走行路Ｌ上に沿って
誘導走行される。この予定走行路Ｌの教示の仕方として
は、一般的に、 １）予定走行路Ｌの予定通過地点を構成する各地点を測
量することが考えられるが、実施例では、 ２）移動体２を実際に予定走行路Ｌに沿って走行させた
ときに得られる上記推測航法に基づく演算値
（（（１）、（２）、（３）式）を予定通過地点として
記憶させておくようにしている。

【００４１】また、上記（６）、（７）式による補正位
置の演算は、 ３）予め測量等によって求めておいた反射鏡Ｃ1 …の位
置を用いるものとして説明したが、実施例では、 ４）上記（６）、（７）式を書き換えた下記（１１）、
（１２）式に基づき反射鏡Ｃ1 …の位置を教示時に求め
ておき、この値を自動走行時における補正位置の演算
（（６）、（７）式）に用いるようにしている。

【００４２】さらに実施例では、前述するように閉ルー
プ状の走行路Ｌ（図４）を想定しており、 ５）教示時に、移動体２が閉ループ路のスタート地点を
スタートしてから１周周回して同スタート地点まで戻っ
てきたときの位置、方向のずれを演算しておき、自動走
行時に移動体２が上記スタート地点を通過するたびに上
記ずれだけ位置、方向の補正を行うようにしている。

【００４３】なお、他の実施例として閉ループ状の走行
路でなければ、上記５）を省略する実施も可能である。
また、上記２）、４）の実施の替わりに上記１）、３）
を実施することも可能である。

【００４４】以下、図２、図３を併せ参照してＣＰＵ７
で行われる処理について説明する。 ・ティーチング時 まず、図４に示すようにスタート地点Ｐ0 の直前の地点
ＰF から移動体２を矢印Ｇ方向に走行スタートさせ、予
定走行路Ｌに沿って走行させてやる。なお、図４におい
て１は移動体２が実際に走行する軌跡、１´は推測演算
（（１）〜（２）式）による演算値をプロットした軌跡
であり、Ｃn-1 、Ｃn はスタート地点Ｐ0 の２つ手前の
反射鏡の実際の位置、直前の反射鏡の実際の位置をそれ
ぞれ示している。

【００４５】すると、移動体２が地点Ｐ´´n-1 に到達
した時点で推測演算により位置、方向が（ｘ´´n-1 、
ｙ´´n-1 、θ´´n-1 ）のごとく得られ、これに基づ
き反射鏡Ｃn-1 の位置Ｃ´´n-1 （Ｘ´´c/n-1 、Ｙ´
´c/n-1 ）が、下記（１１）、（１２）式により演算さ
れる。

【００４６】 Ｘ´´c/n-1 ＝ｘ´´n-1 ーａn-1 ・ｃｏｔα・ｓｉｎ
θ´´n-1 …（１１） Ｙ´´c/n-1 ＝ｙ´´n-1 ＋ａn-1 ・ｃｏｔα・ｃｏｓ
θ´´n-1 …（１２） 上記（１１）、（１２）式は（６）、（７）式を書き換
え、反射鏡位置について求めた式である。そして移動体
２が地点Ｐn に到達した時点で反射鏡Ｃn の位置Ｃ´´
n （Ｘ´´c/n 、Ｙ´´c/n ）が上記（１１）、（１
２）式と同様にして得られる（ステップ１０１）。

【００４７】そして、移動体２がスタート地点Ｐ0 に達
した時点以後、推測演算（１）、（２）式により推測位
置Ｐ´1 、Ｐ´2 、Ｐ´3…を順次求め、これらを予定
走行路Ｌの目標通過地点として順次ＣＰＵ７の図示せぬ
メモリに記憶、格納していく（ステップ１０２、１０
３）。

【００４８】走行中、予定走行路Ｌを１周して再びスタ
ート地点に戻ってきたか否かかが判断され（ステップ１
０４）、該判断結果がＮＯの場合はつぎのステップ１０
５に移行され、位置補正地点、つまり反射鏡による反射
光の受光出力に基づく補正演算（（６）、（７）式）を
行うべき地点に到達したか否かが判断される（ステップ
１０５）。該判断結果がＮＯの場合は手順は再び上記ス
テップ１０３に移行される。一方、判断結果がＹＥＳの
場合はつぎのステップ１０６において上記（１１）、
（１２）式と同様にして光ビーム投・受光器８、９の受
光出力に基づき反射鏡Ｃ1 、Ｃ2 …の位置Ｃ´1 、Ｃ´
2 …が順次演算され、手順は再び上記ステップ１０３に
移行される。やがて移動体２が再び反射鏡Ｃn-1 による
位置補正地点に達した時点でそのときの推測演算値Ｐ´
n-1 （ｘ´n-1 、ｙ´n-1 、θ´n-1 ）に基づき反射鏡
Ｃn-1 の位置Ｃ´n-1 （Ｘ´ｃ/n-1、Ｙ´ｃ／n-1 ）が
演算されるとともに、反射鏡Ｃn による位置補地点に達
した時点でそのときの推測演算値Ｐ´n （ｘ´n 、ｙ´
n 、θ´n ）に基づき反射鏡Ｃn の位置Ｃ´n （Ｘ´ｃ
/n、Ｙ´ｃ／n ）が演算される（ステップ１０６）。

【００４９】やがてステップ１０４において移動体２が
終了地点たるスタート地点Ｐ0 に達したと判断された時
点で手順はステップ１０７に移行される。ここで、推測
演算上におけるスタート地点Ｐ0 の位置をＰ´E （ｘ´
E 、ｙ´E 、θ´E ）とする。

【００５０】ステップ１０７では上記ステップ１０１で
演算された教示開始前における２つの直前反射鏡の位置
Ｃ´´n-1 （Ｘ´´c/n-1 、Ｙ´´c/n-1 ）、Ｃ´´n
（Ｘ´´c/n 、Ｙ´´c/n ）とステップ１０６で演算さ
れた同２つの直前反射鏡の位置Ｃ´n-1 （Ｘ´ｃ/n-1、
Ｙ´ｃ／n-1 ）、Ｃ´n （Ｘ´ｃ/n、Ｙ´ｃ／n ）とス
テップ１０３で得られた１周周回したときのスタート地
点位置Ｐ´E （ｘ´E、ｙ´E 、θ´E ）とに基づき下
記（１３）〜（１５）式により推測演算による教示開始
点と終了点との間における推測演算の誤差による位置ず
れΔｘ、Δｙ、方向ずれΔθが求められる。

【００５１】 Δθ＝ａｒｃｔａｎ（（Ｙ´´c/n ーＹ´´c/n-1 ）／（Ｘ´´c/n ー Ｘ´´c/n-1 ））ーａｒｃｔａｎ（（Ｙ´c/n ーＹ´c/n-1 ）／ （Ｘ´c/n ーＸ´c/n-1 ）） …（１３） Δｘ＝（Ｘ´´c/n ーＸ´c/n ）＋（Ｘ´E ーＸ´c/n ）ｃｏｓΔθー （Ｙ´E ーＹ´c/n ）ｓｉｎΔθー（Ｘ´E ーＸ´c/n ） …（１４） Δｙ＝（Ｙ´´c/n ーＹ´c/n ）＋（Ｘ´E ーＸ´c/n ）ｓｉｎΔθー （Ｙ´E ーＹ´c/n ）ｃｏｓΔθー（Ｙ´E ーＹ´c/n ） …（１５） ここで、（１３）式の第１項は２点Ｃ´´n-1 、Ｃ´´
n を結ぶ線分の傾き、第２項は２点Ｃ´n-1 、Ｃ´n を
結ぶ線分の傾きを示している。また、（１４）、（１
５）式における第１項は最終の反射鏡Ｃnの２回にわた
る計測位置の相対的なずれ分を示し、第２項以降はジャ
イロ４に基づく方向θの演算の累積誤差および最終の反
射鏡Ｃｎの計測位置と終了点Ｐ´E との距離によって生
じるずれ分を示している。したがって反射鏡Ｃnとスタ
ート地点Ｐ0 との距離が小さいときには第２項以後を省
略することができる。

【００５２】なお、方向θの演算上の累積誤差が無視す
ることができる場合には、上記（１３）式の演算を省略
することができ、この場合は反射鏡Ｃn-1 、Ｃn のうち
いずれか一方の反射鏡の位置のみを教示開始前と教示終
了時点において計測すればよい。

【００５３】以上にように得られたオフセット（ずれ）
Δθ、Δｘ、Δｙは図示せぬメモリに記憶される（ステ
ップ１０７）。

【００５４】・自動走行時 移動体２を上記教示データＰ´1 、Ｐ´2 …に基づき自
動走行させる場合には、上記演算された反射鏡位置Ｃ´
1 、Ｃ´2 …Ｃ´n-1 、Ｃ´ｎおよびオフセット量Δ
θ、Δｘ、Δｙがメモリから読み出されるとともに、予
定通過地点Ｐ´1、Ｐ´2 …が読み出され（ステップ２
０１）、この通過地点を順次目標地点として移動体２が
操舵制御される（ステップ２０２）。

【００５５】走行中は推測演算（（１）〜（３）式）が
行われるとともに（ステップ２０３）、移動体２が位置
補正地点に到達するごとに（ステップ２０６の判断結果
ＹＥＳ）、推測演算による演算値の補正演算（（６）、
（７）、（９）式）が行われる。そして計測された位
置、方向のデータに基づき前述した態様で移動体２が予
定通過地点Ｐ´1 、Ｐ´2 …を通過するように操舵制御
される。

【００５６】ここに、（６）、（７）式における位置
Ｘ、Ｙの補正演算はステップ２０１で読み出された反射
鏡位置Ｃ´1 、Ｃ´2 …Ｃ´n-1、Ｃ´ｎを用いて行う
ようにしている（ステップ２０４）。

【００５７】やがて、移動体２が終了点、つまり１周周
回して元のスタート地点に戻ってきたと判断されると
（ステップ２０５の判断結果ＹＥＳ）、移動体２を再び
スタート地点から走行させるか否かが判断される（ステ
ップ２０８）。該判断結果がＹＥＳであれば、推測演算
による誤差を補正すべく上記ステップ２０１で読み出さ
れたオフセット量Δｘ，Δｙ、Δθだけ終了点において
計測された位置ｘ、ｙ、θに加算する（ステップ２０
９）。再走行しない場合は（ステップ２０８の判断結果
ＮＯ）、処理は終了する。

【００５８】以上説明したように実施例によれば、ティ
ーチング時において計測された推測位置Ｐ´1 、Ｐ´2
…が予定通過地点とされ、自動走行時には、上記予定通
過地点を辿るように移動体２が走行制御される。ここ
に、ティーチング時と自動走行時において同じジャイロ
４、エンコーダ５を用いているので、誤差に再現性があ
り、センサの検出誤差が相殺される。また、ティーチン
グ時に演算される反射鏡の位置Ｃ´1 、Ｃ´2 …（演算
（１１）、（１２））と、自動走行時に演算される移動
体２の補正位置Ｘ、Ｙ（演算（６）、（７））とでは、
同じ反射鏡Ｃ1 、Ｃ2 …、投・受光器８、９による受光
出力等から得られるので、反射鏡を設置する際の誤差、
光ビームの投光方向の誤差等がティーチング時と自動走
行時において相殺される。この結果、移動体２は精度よ
く予定走行路Ｌに沿って走行される。さらに通過地点や
反射鏡の位置の測量等の労力を要せず、ティーチング時
に移動体２を移動させてやるだけで目標コースを計測で
きたり、移動体２を実際に移動させているので、移動体
２の運動能力に合った軌跡を設定することができるとい
う効果も得られる。

【００５９】さらに推測航法を用いた計測装置では、た
とえば移動体が起伏のある地形を移動する場合のように
移動体の移動が３次元に及ぶ場合には、測量による目標
軌跡と自動走行時の計測値との整合をとるために、たと
えば慣性航法では３次元における移動方向を計測するた
めのセンサを追加し、かつ移動体の移動方向に応じて座
標変換を行う必要があった。しかし実施例のようにティ
ーチング時と自動走行時において同じシステムの計測に
よって得られた演算値を用いた場合には、計測が２次元
だけでも再現性があるので、実際には移動体が移動が３
次元におよんでも精度よく誘導することができるように
なる。

【００６０】また、実施例によれば、ティーチング時に
おいてスタート地点と１周したときの該スタート地点に
おける位置および方向のずれΔｘ，Δｙ、Δθが演算さ
れる。ここにこのずれには再現性がある。そこで移動体
２が周回するごとにスタート地点において上記ずれだけ
位置および方向を補正することで、ずれが累積されるこ
となくつぎの周回時における誘導走行が精度よくなされ
る。なお、実施例では上記ずれをスタート地点直前の２
つの反射鏡位置から求めるようにしているが、スタート
地点直後の２つの反射鏡位置から求める実施も可能であ
る。要はスタート地点近傍の反射鏡であればよくスター
ト地点直前、直後にまたがってもよい。なお、移動体２
から投光される２つの光ビームの投光方向としては、実
施例のものに限定されることなく、図８に示すように垂
直方向に投光される光ビームＬ2 に対して車体後方に角
度βだけ傾斜した方向に光ビームＬ1を投光してもよく
（同図（ａ））、前後に対称な方向に角度γをもって投
光してもよく（同図（ｂ））、車体前方に角度δをもっ
て投光してもよく（同図（ｃ））、また後方に角度εを
もって投光するようにしてもよい（同図（ｄ））。

【００６１】

【発明の効果】以上説明したように本発明によれば、移
動体の補正位置および方向が正確かつ確実に得られるの
で、予定走行路に沿って移動体を精度よく誘導すること
ができるようになるとともに、装置の信頼性を向上させ
ることができる。

【図面の簡単な説明】

【図１】図１は本発明に係る移動体の位置計測装置の実
施例の構成を示すブロック図である。

【図２】図２は実施例の処理手順を示すフローチャート
である。

【図３】図３は実施例の処理手順を示すフローチャート
である。

【図４】図４は実施例の予定走行路を示す図で、閉ルー
プ走行路におけるスタート地点と終了地点とのずれの演
算を説明するために用いた図である。

【図５】図５は移動体が走行する様子を示す図で、位置
および方向の補正演算を説明するために用いた図であ
る。

【図６】図６は推測航法に基づく位置および方向の推定
演算を説明するために用いた図である。

【図７】図７（ａ）、（ｂ）は図１に示す光ビーム投・
受光器の具体的構成を説明するために用いた図である。

【図８】図８（ａ）、（ｂ）、（ｃ）、（ｄ）は光ビー
ム投・受光器の投光方向の例を例示した図である。

【符号の説明】

２ 移動体 ４ レートジャイロ ５ エンコーダ ６ カウンタ ７ ＣＰＵ ８ 光ビーム投・受光器 ９ 光ビーム投・受光器 １１ カウンタ Ｌ 予定走行路

Claims (3)

(57)【特許請求の範囲】
1. 【請求項１】 移動体の移動距離を検出する移動距
   離検出手段と前記移動体の走行方向を検出する走行方向
   検出手段の出力に基づき前記移動体の位置および走行方
   向を推定演算する推定演算手段を有し、該推定演算され
   た前記移動体の位置および走行方向を間欠的に補正して
   該補正された位置および走行方向に基づき前記移動体が
   予め教示された予定走行路に沿って走行するよう前記移
   動体を走行制御する移動体の位置計測装置において、 前記移動体に設けられ、第１および第２の光ビームを側
   方に投光する第１および第２の投光手段と、 前記移動体の予定走行路の片側に間欠的に配設され、前
   記第１および第２の投光手段から投光された第１および
   第２の光ビームを反射する光反射手段と、 前記移動体に設けられ、前記光反射手段で反射された第
   １および第２の光ビームを受光する受光手段と、 前記受光手段の出力と前記移動距離検出手段の出力とに
   基づき前記第１および第２の光ビームの一方を前記受光
   手段が受光してから他方の光ビームを受光するまでの前
   記移動体の移動距離を求め、該移動距離と前記光反射手
   段の配設位置と前記第１および第２の光ビームの投光角
   度と前記推定演算手段による推定走行方向とに基づき前
   記移動体の補正位置を演算する補正位置演算手段と、 前記受光手段の出力と前記移動距離検出手段の出力とに
   基づき前記第１および第２の光ビームの一方を前記受光
   手段が受光してから他方の光ビームを受光するまでの前
   記移動体の移動距離を求め、該移動距離と前記第１およ
   び第２の光ビームの投光角度と前記推定演算手段による
   推定位置および推定走行方向とに基づき前記光反射手段
   の配設位置を演算する光反射手段位置演算手段と、 前記補正位置演算手段の演算値と該補正位置演算手段の
   前回の演算値と前記推定演算手段で演算された前記移動
   体の位置とに基づき前記推定演算手段による推定走行方
   向の誤差を求め、該誤差と前記推定演算手段による推定
   走行方向とに基づき前記移動体の補正走行方向を演算す
   る補正走行方向演算手段とを具え、 前記予定走行路の教示時には、前記移動体を該予定走行
   路に沿って走行させ、 このとき得られた前記推定演算手段の演算位置を通過予
   定地点とするとともに、前記光反射手段位置検出手段に
   より前記光反射手段の配設位置を検出しておき、 前記移動体を前記予定走行路に沿って走行させるときに
   は、前記検出された光反射手段の配設位置を前記補正位
   置演算手段で用いるデータとするとともに、前記教示さ
   れた通過予定地点を前記移動体が走行するように前記補
   正位置演算手段と前記補正走行方向演算手段の出力に基
   づき前記移動体を走行制御するようにした移動体の位置
   計測装置。
2. 【請求項２】 前記予定走行路は閉ループの走行路
   であり、 前記予定走行路の教示時には、前記予定走行路のスター
   ト地点近傍における２つの光反射手段の配設位置と前記
   移動体を前記予定走行路に沿って周回させて前記スター
   ト地点に戻ってきたときの前記２つの光反射手段の配設
   位置とをそれぞれ前記光反射手段位置検出手段により検
   出して、該検出結果と前記周回させて前記スタート地点
   に戻ってきたときの前記推定演算手段による推定位置と
   に基づき前記移動体が前記予定走行路を周回したときの
   位置および走行方向のずれを演算し、 前記移動体を前記予定走行路に沿って走行させるときに
   は、前記移動体が前記スタート地点を通過するたびに前
   記演算されたずれに基づき前記移動体の位置および走行
   方向を補正するようにした請求項１記載の移動体の位置
   計測装置。
3. 【請求項３】 前記第１および第２の投光手段に、 前記光ビームを所定の俯仰角度だけ拡散する手段、また
   は前記光ビームを所定の俯仰角度だけ走査する手段を、
   さらに設けるようにした請求項１記載の移動体の位置計
   測装置。