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

Abstract

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

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

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

【０００６】

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

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

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

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

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

【００１１】

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

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

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

【００１４】また、この発明の第４発明では、移動体の
位置および走行方向を推定演算する推定演算手段と、前
記移動体の側方から光ビームを投光する投光手段と、前
記移動体の予定走行路に沿って間欠的に配設された光反
射手段と、前記移動体に設けられ、前記光反射手段によ
る反射光を受光する受光手段とを有し、前記受光手段の
出力に基づき前記推定演算手段によって演算された前記
移動体の位置および走行方向を間欠的に補正して、該補
正された位置および走行方向に基づき前記移動体を予定
走行路に沿って走行制御する移動体の位置計測装置にお
いて、前記光ビームを所定の俯仰角度だけ拡散する手段
、または前記光ビームを所定の俯仰角度だけ走査する手
段を前記投光手段に設けるようにしている。

【００１５】

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

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

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

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

【００１９】

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

【００６１】

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

【図面の簡単な説明】

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

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

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

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

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

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

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

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

【符号の説明】

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

Claims (4)

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