JPH01287415A - 自走車の位置制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、自走車の位置制御装置に関し、特に、自動車
、工場内の無人移動搬送装置、農業および土木機械等の
自走車の位置制御装置に関する。

（従来の技術） 従来、上記のような自走車の現在位置を検知する装置と
して、複数箇所に設けられた送信源から発せられた電波
を、自走車に設けられた受信装置で受信し、受信方位な
どから自走車の位置を演算するように構成された位置検
出装置があった。

このような位置検知装置では、電波を発生させるための
送信装置が複数必要とされ、そのために装置全体が高価
なものとなってしまう欠点があった。

これに対し、例えば特開昭５９−６７４７６号公報にお
いて次のような技術が提案されている。

該技術は移動体から発生された光ビームを、該移動体を
中心として円周方向に走査することによって移動体の位
置を検出する装置であって、入射光方向に光を反射する
光反射手段を、移動体とは離れた少なくとも３箇所に固
定し、移動体には光発生手段と、該光発生手段から発生
される光ビームを走査する光ビーム走査手段と、前記光
反射手段の反射光を受信する受光手段とを設けている。

そして、前記受光手段の受光出力に基づいて移動体を中
心とする３つの光反射手段間の開き角を検出し、その検
出した開き角と、あらかじめ設定された光反射手段の位
置情報とに基づいて移動体位置を演算するようにしてい
る。

（発明が解決しようとする課８） しかしながら、上記技術では、あらかじめ設定されてい
る光反射手段の位置情報のわずかな誤差によってもシス
テム全体に大きな悪影響を生ずるため、例えば農作業等
では、野外の作業エリアが変わる毎に、設置する光反射
手段の位置情報、すなわち、前記３つの光反射手段の間
隔および相対角度を作業に先立って予め正確に設定し、
そして、これを制御装置に入力するという作業が必要と
なってくる。このように、広い作業エリアに配置する光
反射手段の間隔や相対角度を、正確に測定し、入力する
というのは極めて大変な作業になるという問題点があっ
た。

本発明の目的は、上記従来技術の問題点を解消し、光反
射手段を移動体の移動領域に適宜設置するだけで移動体
が自らの位置を算出して、移動体の走行方向を制御でき
る自走車の位置制御装置を提供することにある。

（課題を解決するための手段および作用）前記の問題点
を解決するために、本発明は、自走車から発生した光ビ
ームを、該自走車を中心として円周方向に走査すること
により、自走車の位置を検知する自走車の位置制御装置
において、前記自走車とは離れた少なくとも３ｔＩ！所
に設置され、入射光方向に光を反射する光反射手段と、
前記自走車に設けられ、前記光ビームを発生する光ビー
ム発生手段と、前記自走車に設けられ、前記光ビームを
該自走車を中心として円周方向に走査する光ビーム走査
手段と、前記自走車に設けられ、前記光反射手段の反射
光を受光する受光手段と、前記受光手段の受光出力に基
づいて、前記自走車と前記光反射手段との距離を検出す
る手段と、前記自走車から見た前記３箇所に設置された
光反射手段間の開き角検出手段と、前記距離および開き
角に基づいて自走車の位置を原点とする座標軸を基準に
した前記各光反射手段の座標を算出する手段を具備した
点に特徴がある。

上記構成を有する本発明においては、自走車および光反
射手段の距離と、自走車から見た各光反射手段間の開き
角とに基づいて０走車と光反射手段との相対位置が検出
できるので、光反射手段を設置する毎に該光反射手段の
位置を正確に測定し、その結果を制御装置に入力しなく
ても、自走車の位置が検出でき、その検出結果に基づい
て操向制御を行うことができる。

（実施例） 以下に図面を参照して、本発明の一実施例を説明する。

第７図は本発明の制御装置を搭載した自走車、および該
自走車が走行するエリアに配設された光反射器の配置状
態を示す斜視図である。

同図において、自走車１は例えば芝刈り機等の農作業用
自走車である。該自走車１の上部にはモータ５によって
駆動される回転テーブル４が設けられている。そして、
該回転テーブル４には光ビームを発生する発光器２およ
び該光ビームの反射光を受ける受光器３が搭載されてい
る。前記発光器２は光を発生する発光ダイオードを備え
、受光器３は入射された光を受けて電気的信号に変換す
るフォトダイオードを備えている（共゛に図示しない）
。また、ロータリエンコーダ７は回転テープル４の駆動
軸と連動するように設けられていて、該ロータリエンコ
ーダ７から出力されるパルスを計数することによって、
回転テーブル４の回転角度が検出できる。

６は自走車１の作業エリアの周囲に配設された反射器で
ある。該反射器６は入射した光を、その入射方向に反射
する反射面を具備しており、従来より市販されている、
いわゆるコーナキューブプリズム等が使用できる。

次に、本実施例の制御装置の構成を第１図に示したブロ
ック図に従って説明する。第１図において、発光器２か
ら射出される光ビームは、回転テーブル４の回動方向に
走査され、反射器６によって反射される。反射器６によ
って反射された該光ビームは受光器３に入射される。距
離検出部８では、発光器２から射出された光ビームの位
相と、受光器３に戻ってくる反射光の位相との差に基づ
いて、自走車１および反射器６間の距離が算出される。

カウンタ９では、回転テーブル４の回転に伴ってローク
リエンコーダ７から出力されるパルス数が計数される。

そして、該パルスの計数値は受光器３において反射光を
受光する毎に角度検出部１０に転送される。角度検出部
１０では反射光の受光毎に転送される前記パルスの計数
値に基づいて、自走車１から見た各反射器６間の開き角
が算出される。

基準座標算出部１１では、前記距離検出部８および角度
検出部１０における、前記距離および開き角の算出結果
から、自走車１を原点とする座標系（以下、本明細書で
は原始座標系という）における反射器６の座標が算出さ
れる。基準座標変換部１２では基準座標算出部１１で算
出された座標値に基づき、今度は複数設置されている反
射器６の内のいずれかを原点とする座標系（以下、本明
細書では実行座標系という）に座標変換が行われる。

位置・進行方位演算部１３では自走車１の座標および進
行方位が演算され、その演算結果は操舵部１４に入力さ
れる。操舵部１４では、位置・進行方位演算部１３の演
算結果と、走行コース設定部１６に設定されている走行
コースとが比較され、該比較結果に基づき自走車の前輪
１７に連結された操舵モータ（図示せず）が駆動される
。該操舵モータによる前輪１７の操舵角は自走車１の前
輪に設けられた舵角センサ１５で検出され操舵部１４に
フィードバックされる。

駆動部１８はエンジン１９の始動・停止、および該エン
ジン１９の動力を後輪２１に伝達するクラッチ２０の動
作を制御する。

なお、ＳＷＩ、ＳＷ２は連動して切替わるスイッチであ
り、実行座標系が算出された後はこの実行座標系の中で
、角度検出部１０からの開き角情報のみに基づいて自走
車１の座標位置を算出するように構成されている。

上記構成の本実施例によって自走車１の位置および進行
方位を検出するための基本的原理を説明する。第２図お
よび第３図は、自走車１を原点とする原始座標系を、後
述する手順に従って座標変換した実行座標系における自
走車１および反射器６の位置を示す。

第２図および第３図において、自走車１はＴ点にあって
、該自走車１の作業エリアに配置された反射器６ａ〜６
ｃは、反射器６ｃを原点とし、反射器６ａおよび６ｃを
結ぶ線をＸ軸とするｘ−ｙ座標系で表される。なお、同
図では反射器６ａ〜６ｃはそれぞれＡ、Ｂ、Ｃで示す。

同図かられかるように自走車１の位１ｆＴは、三角形Ａ
ＴＣの外接円上に存在すると同時に、三角形ＢＴＣの外
接円上に存在する。したがって、三角形ＡＴＣおよび三
角形ＢＴＣのそれぞれの外接円ＰおよびＱの２つの交点
を算出することによりて、自走車１の位置が確定できる
。ここで反射器６ｃは原点になっているので、外接円Ｐ
およびＱの他方の交点Ｔを以下の手順に従って算出すれ
ば自走車１の位置は求められる。

まず、三角形ＡＴＣの外接円Ｐについて、その中心をＰ
とすると、Ｐは線分ＡＣの垂直２等分線上にあり、中心
角と円周角との関係から、＜ｃｐｗ’　−βとなる。

但し、Ｗ′は線分ＣＡの垂直二等分線上の点であり、直
線ＣＡに対し点Ｔと反対側に十分遠くにあるものとする
。

ここで三角形ＣＰＷ（Ｗは線分ＯＡの中点）に着目する
と、 円Ｐの中心の座標は ｉｘａ／２．（ｘａ／２）ｃｏｔａ） 半径はｌｘａ／（２ｓｉｎβ）！となり、外接円Ｐは次
式で表される。

（ｘ−ｘａ／２）２ ＋　（ｙ−（ｘａ／２）ｃｏｔａ）２ ＝　ｔｘａ／　（２ｓ　ｉ　ｎβ））２さらに、鎖式を
整理すると次式が得られる。

ｘ２−ｘａ　＊Ｘ＋ｙ２ −ｘａ”ｙ”ｃｏｔａ−０・・・・・・（１）また、三
角形ＢＴＣの外接円Ｑについて、その中心をＱとすると
、Ｑは線分ＢＣの垂直２等分線上にあり、 乙ＣＱＶ’　−αとなる。

但しＶ′は線分ＣＢの垂直二等分線上の点であり、直線
ＣＢに対し点Ｔと反対側に十分遠くにあるものとする。

ここで三角形ＣＱＶ　（Ｖは線分ＣＢの中点）に着目す
ると、円Ｑの中心の座標は ｔｘｂ／２＋　（ｙｂ／２）ｃｏｔａ。

ｙｂ／２−　（ｘｂ／２）ｃｏｔａｌ 半径はＩ　　ｘｂ”＋ｙｂ”／（２ｓｉｎａ）１となり
、外接円Ｑは次式で表される。

ｘ２−ｘ　（ｘｂ＋ｙｂ＊ｃｏｔａ）＋ｙ２−ｙ　（ｙ
ｂ−ｘｂ−ｃｏｔａ）＝０−−（２）上記（１）、（２
）式から点Ｔの座標（ｘ、　　ｙ）は次式で算出される
。

ｘ−ｘａ　（１＋に−ｃｏｔβ）／　（１＋に２）・・
・・・・（３） ｙ−ｋｘ　　　　　　　・・・・・・・・・・・・・・
・・・・・・・・・・（４）但しに−（ｘａ−ｘｂ−ｙ
ｂ＠ｃｏｔａ）／（ｙｂ−ｘｂ−ｃｏｔａ−ｘａ１１ｃ
ｏｔβ）・・・・・・（５） であり直！ＩＣＴの傾きを表している。

また、自走車１の進行方向は次のようにして算出される
。第３図において、自走車１の進行方向とＸ軸とのなす
角度をθｆとし、該進行方向を基準とした反射器６　ａ
、　　６　ｂ＋　　６　ｃ　（点ａ、　　ｂ、　　ｃ）
までのそれぞれの回転角度をθａ、θｂ、θＣとした場
合、前記線分ＣＴの傾きはｋであるので、θｆ−１８０
°−（θｃ　−ｔ　ａ　ｎ−’ｋ）　・＝（６）次に、
自走車１と反射器６との相対位置を表すための座標系決
定の手順を説明する。

第４図は自走車１と反射Ｈ６の配置図である。

第４図においては、反射器６が４箇所に配置しである例
を示す。反射器６ａ〜６ｄは自走車１の作業エリアに四
角形を形成して配置され、自走車１は任意の位置の点Ｔ
に置かれて作業のため待機している。Ｘ軸およびＹ軸は
点Ｔを原点とする座標軸であり、Ｘ軸およびＹ軸の設定
は任意である。

上記配置において、まず、Ｔ点と各反射器６ａ〜６ｄと
の距、ｌｌｉ　Ｌ　ａ　−Ｌ　ｄを距離検出部８によっ
てａｌ定し、さらに、Ｙ軸を基準とする各反射器６ａ〜
６ｄまでの回転角度φａ〜φｄを角度検出器１０によっ
て測定する。

次に、座標算出部１１において、前記距離Ｌａ〜Ｌｄと
回転角度φａ〜φｄとに基づき、点Ｔを原点とするＸ−
Ｙ座標系での各反射器６ａ〜６ｄの座標が算出される。

各反射器６ａ〜６ｄの座標は次の式に従って算出される
。

反射器６　ａ−Ｘａ　−−Ｌａ　争ｓ　ｉ　ｎφａＹａ
−ｍＬａ−ｃｏｓφａ 反射器５　ｂ・・・Ｘｂ　−−Ｌｂ　＊　ｓ　ｉ　ｎφ
ｂＹｂ■Ｌｂ１１ＣＯ５φｂ 反射器５　ｃ・−Ｘｃ−−Ｌ　ｃ　１１ｓ　ｌ　ｎφＣ
Ｙｃ−Ｌｃ　ａｃｏｓφＣ 反射器６　ｄ−Ｘｄ　−−Ｌｄ　＊　ｓ　ｌ　ｎφｄＹ
ｄ＝Ｌｄ−ｃｏｓφｄ Ｘ−Ｙ座標系（原始座標系）での各反射器６ａ〜６ｄの
座標が上式によって算出されたあと、座標変換部１２に
おいて、座標軸を角度θだけ回転させ、かつ、平行移動
させて反射器６ａ〜６ｄのいずれかを原点として実行座
標系に変換する。本実施例では反射器６Ｃを原点とし、
Ｘ軸は反射器６ａを通る。

反射器６Ｃを原点としたｘ−ｙ座標系（実行座標系）で
の各反射器６ａ〜６ｄの座標は次の式で算出される。

反射器６ａ・ｙｃａ−（Ｘａ−Ｘｃ）ｃｏｓθ＋　（Ｙ
ａ−Ｙｃ）ｓ　ｉ　ｎθ ａ−０ 反射器６ｂ−ｘｂ−（Ｘｂ−Ｘｃ）ｃｏｓθ＋　　（Ｙ
ｂ−Ｙｃ）ｓ　ｉ　ｎθ ｙｂ−−（Ｘｂ−Ｘｃ）ｓ　ｉｎθ ＋　　（Ｙｂ−Ｙｃ）ｃｏｓθ 反射器６Ｃ・・・ｘｃ−Ｏ ｃ−Ｑ 反射器６ｄ・・−ｘｄｍ　（Ｘｄ−Ｘｃ）ｃｏｓθ＋　
　（Ｙｄ−Ｙｃ）ｓ　ｉ　ｎθ ｙｄ−−（Ｘｄ−Ｘｃ）ｓ　ｉｎθ ＋　　（Ｙｄ−Ｙｃ）ｃ　ｏ　ｓθ 上記反射器６ａ〜６ｄの座標値から、前記（３）〜（５
）式によって自走車１の位置が算出される。

また、前記（６）式によって自走車１の進行方位が算出
される。

なお、第４図においては、反射器６を４箇所に配置した
例を示したが、この内から任意の３箇所に配置された反
射器６と自走車１との相対位置に基づいて、自走車１の
位置および進行方向が検出される。

すなわち、反射器６と自走車１との相対位置に対応する
、測定精度の良否のパラメータを予め決定しておき、該
パラメータと、反射器６および自走車１の相対位置とを
比較して、より精度の良い位置測定結果が得られるよう
に３ｔＩ所の反射器を選択する。

したがって、この場合は、選択された反射器６に対応す
るように、前記計算式（３）〜（５）は変形して使用さ
れる。

また、第４図に示されるφａ〜φｄはＹ軸を基準にした
反射器までの回転角度であるので、計算式（３）〜（５
）によって自走車の位置と進行方位を算出する場合は自
走車から見た各反射器の開き角αおよびβ換算して演算
が行われる。

次に、上記手順によって算出された自走車１の位置情報
に基づく、自走車１の操向制御について説明する。第５
図は自走車１の走行コースと反射′ａ６の配置状態を示
す図であり、第６図は操向制御のフローチャートである
。

第５図において、Ａ、Ｂ、Ｃ点は反射器６の配置位置を
示しており、点Ｃを原点とし、点Ｃおよび点Ａを通る線
をＸ軸とする座標系で自走車１の位置および作業エリア
２２を表している。

（Ｘｒｅｔ、Ｙｒｅｔ）は自走車１の戻り位置を示し、
作業エリア２２は座標（Ｘｓ　ｔ、　Ｙｓ　ｔ）、（Ｘ
ｓ　ｔ、Ｙｅ）、（Ｘｅ、Ｙｓ　ｔ）、（Ｘｅ。

Ｙｅ）で示される点を結ぶ領域である。

なお、第５図においては、説明を簡単にするため、作業
エリア２２の４辺をＸ軸またはＹ軸に平行にした例を示
したが、作業エリア２２の周囲に反射？：ｉ６を設ける
ようにさえしてあれば、作業エリア２２の向きは任意で
ある。

第６図のフローチャートに従って制御手順を説明する。

まず、ステップＳ１において、前記位置・進行方位演算
部１３で演算された自走車１の現在位置（Ｘｒｅｔ、Ｙ
ｒｅｔ）と、前記走行コース設定部１６に設定された作
業開始位置の座標（Ｘｓｔ。

Ｙｓｔ）’に基づいて、操舵部１４において自走車１の
前輪１７の操舵量を演算する。

ステップＳ２では、前記操舵量によって決定される方向
に、操舵部１４により前輪１７を操舵し、かつ駆動部１
８によってエンジンを始動させ、クラッチをつないで自
走車１を走行させ、作業開始位置に位置決めさせる。

ステップＳ３では、ＸｎとしてＸｓｔをセットし、走行
コースを決定する。

ステップＳ４で、自走車１の走行を開始させると、自走
車１は自己位ｆｆｆ（Ｘｐ、Ｙｐ）および進行方位θｆ
の演算を行う（ステップＳ５）。

ステップＳ６では走行コースからのずれ量（ΔＸ＝Ｘｐ
−Ｘｎ、Δθｆ）が演算され、ステップＳ７では、前記
ずれ量に応じて操舵部１４により操舵角制御が行われる
。

ステップＳ８では自走車１がｙ軸方向において、原点か
ら遠ざかる方向（行き方向）に走行しているか、原点に
近づく方向（戻り方向）に走行しているかが判断される
。

行き方向であれば、ステップＳ９において、−行程が終
了したか（Ｙｐ＞Ｙｅ）否かが判断され、戻り方向であ
れば、ステップＳＩＯにおいて、−行程終了（Ｙｐ＜Ｙ
ｓ　ｔ）したか否かが判断される。ステップＳ９または
ＳＩＯにおいて、−行程が終了していないと判断されれ
ばステップ８５〜Ｓ８の処理が行われる。

ステップＳ９またはＳＩＯにおいて、−行程が終了した
と判断されれば、次はステップＳｌｌにおいて全行程が
終了した（Ｘｐ＞Ｘｅ）か否かの判断が行われる。

全行程が終了していなければ、ステップＳｌｌからステ
ップＳ１２に移って自走車のＵターン制御が行われ、ス
テップＳ１３では、ＸｎにＸｎ十りがセットされ、次の
走行コースが設定される。次の走行コースが設定されれ
ばステップＳ５に戻って、前記処理が行われる。

全行程が終了したならば、戻り位Ｅ（Ｘｒｅｔ。

Ｙｒｅｔ）へ戻って（ステップ５１４）、走行が停止さ
れる（ステップ５１５）。

なお、ステップＳ１２におけるＵターン制御は、前記位
置・進行方位演算部１３で演算された自走車１の位置情
報を操舵部１４にフィードバックするステップ８５〜Ｓ
７の処理にはよらないで、あらかじめ設定されたプログ
ラムに従って行われる。

すなわち、作業エリア２２内では自走車はフィードバッ
ク制御で操向制御され、方向転換のための制御はプログ
ラム制御によって行われる。

なお、上記実施例では原点Ｃ（０，０）に対する作業開
始位置の座標（Ｘｓ　ｔ、　Ｙｓ　ｔ）を、あらかじめ
走行コース設定部１６に設定しておく例を示した。この
他に、自走車１を無線誘導等によって、任意の位置に誘
導し、その位置を作業開始位置（Ｘｓ　ｔ、　Ｙｓ　ｔ
）と定義し、走行を開始させることも可能である。

以上の説明のように、本実施例では、光ビームの発光お
よび受光の位相差を検出して、自走車１と反射器６との
距離および自走車１から見た各反射器６の開き角を検出
し、自走車１の位置を原点とする原始座標系における各
反射器６の座標値を算出するようにした。

そして、前記原始座標系を前記反射器６の内の１つの反
射器を原点とする実行座標系に変換し、該実行座標系に
おける反射器６の座標値に基づいて、自走車１の現在位
置および進行方位を演算している。

その結果、作業エリアに設置された各反射器間の距離が
あらかじめ分かっていなくても、自走車１と反射器６と
の相関位置関係が検出でき、その検出結果に基づいて自
走車１の操向制御を行うことができる。

（発明の効果） 以上の説明から明らかなように、本発明によれば、自走
車による作業が必要とされるエリアが変わって、反射器
をそのつど設置し直さなければならないような場合にも
、作業エリアの周囲に反射器を適宜配置するだけで良い
。したがって、従来のように、反射器を設置する毎に設
置位置を正確に測定し、その結果を制御装置に入力・設
定してから自走車に作業をおこなわせるというような手
間のかかる手順を経なくても良くなり、格段に作業能率
を向上することができる。

【図面の簡単な説明】

第１図は本発明の一実施例を示すブロック図、第２図は
自走車の位置検出の原理説明図、第３図は自走車の進行
方位検出の原理説明図、第４図は座標系決定方法の説明
図、第５図は自走車の走行コースと反射器の配置状態を
示す図、第６図は操向制御のフローチャート、第７図は
自走車と反射器の斜視図である。 １・・・自走車、２・・・発光器、３・・・受光器、４
・・・回転テーブル、５・・・モータ、６・・・反射器
、７・・・ロータリエンコーダ、８・・・距離検出部、
９・・・カウンタ、１０・・・角度検出部、１１・・・
座標算出部、１２・・・座標変換部、１３・・・位置・
進行方位演算部、１４・・・操舵部 代理人　弁理士　平木送入　外１名 第２図 第３図 す軸 第４図 第５図 （Ｘｒｅｔ、Ｙｖｅｔ）

Claims (2)

【特許請求の範囲】
1. （１）自走車から発生した光ビームを、該自走車を中心
   として円周方向に走査することにより、自走車の位置を
   検知する自走車の位置制御装置において、前記自走車と
   は離れた少なくとも３箇所に設置され、入射光方向に光
   を反射する光反射手段と、前記自走車に設けられ、前記
   光ビームを発生する光ビーム発生手段と、前記自走車に
   設けられ、前記光ビームを該自走車を中心として円周方
   向に走査する光ビーム走査手段と、前記自走車に設けら
   れ、前記光反射手段の反射光を受光する受光手段と、前
   記受光手段の受光出力に基づいて、前記自走車と前記光
   反射手段との距離を検出する手段と、前記自走車から見
   た前記３箇所に設置された光反射手段間の開き角検出手
   段と、前記距離および開き角に基づいて自走車の位置を
   原点とする座標系における前記各光反射手段の座標を算
   出する手段を具備したことを特徴とする自走車の位置制
   御装置。
2. （２）前記自走車の位置を原点とする座標系を、前記反
   射手段の１つを原点とする座標軸を基準に座標変換させ
   る座標変換手段を具備した請求項１記載の自走車の位置
   制御装置。（３）反射手段のうちの１つを原点とし、残
   りの反射手段のうちの１つを通る直線を、少なくとも一
   方の座標軸とするように座標変換することを特徴とする
   請求項２記載の自走車の位置制御装置。