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

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、自走車の位置制御装置に関し、特に、工場内
の無人移動搬送装置、農業および土木機械等の自走車を
、あらかじめ設定された走行コースに従って走行させる
ための、自走車の位置制御装置に関する。

（従来の技術） 従来、上記自走車のような移動体の現在位置を検出する
装置として、移動体で発生された光ビームを、移動体を
中心として円周方向に走査する手段と、移動体とは離れ
た少なくとも３カ所に固定され、入射方向に光を反射す
る光反射手段と、該光反射手段からの反射光を受光する
受光手段とを具備した装置が提案されている（特開昭５
９６７４７６号公報）。

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

ところで、上記システムにおいては、自走車の傾斜や、
振動に起因して基準点に配置された前記光反射手段に光
ビームを照射できない場合があった。このような場合に
は自走車の位置が誤って算出され、その結果、予定され
たコースに沿って自走車を走行させられなくなる。

このように、基準点を見失った場合、見失った基準点の
方位を推定し、該推定された基準点の方位角を使用して
自走車の位置を算出するように構成された検出装置が、
特願昭６３−２６２１９２号に記載されている。

上記、基準点の方位角を推定して自走車の位置を算出す
る検出装置では、基準点の見失い回数が多くなると、推
定による誤茅が蓄積されて、自走車の正確な位置検出が
できなくなるとの配慮から、計数手段で見失い回数を計
数して、該計数された見失い回数が予定の回数を超過し
た時には、例えば自走車の走行を中止させるような構成
をとることも考えられている。

（発明が解決しようとする課題） しかし、例えば自走車が長い段差部分に沿って走行しな
ければ目的の作業をできないような場合は、自走車が比
較的長時間傾斜して走行するようになり、特定の基準点
を連続して見失う場合がある。このような場合には常に
基準点見失い回数が予定の回数を超えるようになるので
、頻繁に自走車の走行が中止され、この中止のたびに作
業者（監視人）が再始動操作を行うのでは作業能率が著
しく低下してしまう。

本発明の目的は、上記従来技術の問題点を解消し、基準
点見失いに起因する自走車の走行中断開度を極力減少さ
せることによって自走車の作業能率を向上させるのに好
適な自走車の位置制御装置を提供することにある。

（課題を解決するための手段および作用）前記の問題点
を解決し、目的を達成するために、本発明は、自走車か
ら離れた位置の少なくとも３カ所の基準点にそれぞれ配
設された光反射手段と、前記自走車に搭載された光ビー
ム発生手段、光ビーム走査手段、および前記光反射゛手
段で反射された光ビームを検出する受光手段と、該受光
手段の受光信号に基づいて自走車および前記基準点間の
距離を測定する手段と、該受光手段の受光信号に基づい
て自走車から見た前記各基準点相互間の開き角を検出す
る手段と、前記基準点の位置情報および前記開き角に基
づいて自走車の位置を算出する第１演算手段と、自走車
から前記基準点のうちの２つの基準点までの距離および
該２つの基準点の位置情報に基づいて自走車の位置を算
出する第２演算手段と、自走車の位置を演算する際に、
基準点が少なくとも３つ検出された場合には前記第１演
算手段の出力を選択し、基準点が２つしか検出できなか
った場合には、前記第２演算手段の出力を選択する選択
手段とを具備した点に特徴がある。

」二記構成を有する本発明では、少なくとも３カ所以上
設けられた基準点のうちの２カ所の基準点しか検出でき
ないような場合でも、自走車から該２カ所の基準点まで
の距離および該基準点の位置情報に基づいて引続き自走
車の位置を検出できるので、自走車の走行を中断する必
要がなく、継続して作業を行わせることができる。

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

第７図は本発明の位置制御装置を搭載した自走車、およ
び該自走車が走行する作業領域周囲の基準点に設けられ
た光反射器の配置状態を示す斜視図である。

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

反射器６ａ〜６ｄは自走車１の作業領域の周囲に配設さ
れる。該反射器６ａ〜６ｄは入射光をその入射方向に反
射する反射面を具備しており、コーナキューブプリズム
等周知の反射手段が使用できる。該反射器６ａ〜６ｄは
操向制御のための自走車１の位置検出の基準点として利
用される。

自走車１に搭載された図示しない制御装置により、前記
回転テーブル４の回転角度に基づいて自走車１の進行方
向に対する各反射器６ａ〜６ｄの方位が算出されると共
に、発光器２から射出された光ビーム２Ｅの位相と、反
射器６ａ〜６ｄから受光器３に戻ってくる反射光２Ｒの
位相との差に基づいて自走車１と各反射器６ａ〜６ｄと
の距離が算出される。

次に、本実施例の制御装置の構成を第１図に示したブロ
ック図に従って説明する。第１図において、発光器２か
ら射出される光ビーム２Ｅは、前記回転テーブル４の回
動方向に走査され、反射器６（６ａ〜６ｄ）によって反
射される。反射器６ａ〜６ｄによって反射された該光ビ
ーム２Ｒは受光器３に入射される。

距離検出部８では、発光器２から射出された光ビーム２
Ｅの位相と、受光器３に戻ってくる反射光２Ｒの位相と
の差に基づいて自走車１および各反射器６ａ〜６ｄ間の
距離が算出される。

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

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

第１演算部１３では、３力所以上の反射器の位置情報と
、角度検出器１０から供給される方位角および開き角と
に基づいて自走車１の位置および進行方向が算出される
。

第２演算部１２では、自走車１から２カ所の反射器まで
の距離と、該２カ所の反射器の位置情報と、角度検出器
１０から供給される方位角とに基づいて自走車１の位置
および進行方向が算出される。

基準点識別部１１では、検出された反射器の個数が検出
され、また該検出された反射器は設置されている反射器
６ａ〜６ｄのうちのどれであるかが識別される。検出さ
れた反射器および検出されなかった（見失った）反射器
の識別は、例えば、次のように行われる。

まず、前回および前々回の走査で検出された反射器の方
位角に基づいて同じ反射器が今回の走査で検出されるべ
き方位角範囲を推定する。そして、推定された方位角範
囲からの反射光を受光した場合は予定の反射器を検出し
たと判断し、該推定方位角範囲からの反射光を受光しな
かった場合は検出されるべき予定の反射器を見失ったと
判断する。

このような手法により、検出された反射器の数および該
反射器の特定を行うことができる。なお、該反射器の識
別処理は、特願昭６３−２６２１９２号および特願昭６
３−３１３５３９号に記載されているので詳細の説明は
省略する。

切換部１７は、検出された基準点の数に応じて基準点識
別部１１から出力される切換信号に応答して切換えられ
る。切換部１７が切換えられると、その切換位置に従っ
て第１演算部１３および第２演算部１２で算出された自
走車１の位置および進行方向を示すデータのいずれか一
方が操舵部１４に供給される。

操舵部１４では、走行コース設定部１６に設定されてい
る走行コースを表すデータと、前記第１演算部１３また
は第２演算部１２で得られた自走車１の位置および進行
方向とに基づいて自走車１を前記走行コースに沿って走
行させるように操舵量が決定される。該操舵量に応じて
操舵モータ１５が駆動され、自走車１が操舵される。操
舵モータ１５の回転量、すなわち自走車１の操舵量は操
舵部１４にフィードバックされる。

駆動制御部１８はエンジン１９の始動・停止、および該
エンジン１９の動力を車輪に伝達するクラッチ２０の動
作を制御する。該駆動制御部１８は、予定の走行コース
と自走車の位置とに基づいて自動的にエンジン１９を始
動・停止させたり、クラッチ２０を断続させたりするこ
とができる。

なお、第１図に示された構成要素のうち、鎖線で示され
た範囲内の部分は、マイクロコンピュータによって構成
することができる。

次に、設置されている反射器のうちの少なくとも３カ所
の反射器が検出されている場合の、自走車１の位置およ
び進行方向を算出する手順を説明する。

第４図は、第１演算部１３において自走車１の位置およ
び進行方向の算出手順を説明するための自走車１および
反射器６ａ〜６ｄの位置を示す図である。

１ ２ 同図において、反射器６ａ〜６ｄはそれぞれ基準点Ａ−
Ｄに配置される。自走車１の位置はＴ（ｘ、　　ｙ）で
示され、Ｘ軸に対する自走車１の進行方向はθｆで示さ
れる。同図では説明を簡＋１ｊにするため基準点Ｂを座
標の原点とし、基準点Ｃを通る直線をＸ軸とした。

自走車１の進行方向に対する各基準点Ａ、　　Ｂ。

Ｃ，Ｄの方位角は、θａ、θｂ、θＣ１θｄてそれぞれ
示され、互いに隣接する基準点相互間の開き角は、それ
ぞれα、β、γおよびδて示される。

第１演算部１３に格納され、前記方位角θａ〜θｄ１開
き角α〜δ、および各基準点の位置情報に基いて自走車
１の位置Ｔ（ｘ、ｙ）および進行方向θｆを算出する算
出式を（１）〜（４）に示す。

なお、自走車１の位置Ｔの算出には開き角α〜δのうち
の、検出された２つの開き角が利用され、進行方向θｆ
の算出には方位角θａ〜θｄのうちの、検出された基準
点の方位角が利用される。本実施例では基準点りが検出
されていない例を示す。

したがって、次式では開き角α、βおよび方位角θｂを
利用している。

ｘ−ｘｃｌ（１＋に−ｃｏｔβ）　／　（１＋ｋ　２）
１・・・・・・（１） ｙ−ｋｘ　　　　　　　　　　・・・・・・（２）但し
、ｋ−（ｘｃ−ｘａ−ｙａｅｃｏｔｃｒ）／（ｙａ−ｘ
ａ＊ｃｏｔａ−ｘｃ＊ｃｏｔβ）・・・・・・（３） θｆ−１８０°−（θｂ−ｔａｎ　　’ｋ）・・・・・
・（４） 該算出式を導き出すための手順は特願昭６３１１６６８
９号、特願昭６３−１４９６１９号および特願昭６３−
２０２６９７号において記載されているので詳細の説明
は省略する。

次に、設置されている反射器のうちの２カ所の反射器だ
けが検出されている場合に、自走車１の位置および進行
方向を算出する手順を説明する。

第２図、第３図は、第２演算部１２において自走車１の
位置および進行方向を算出する手順を説明するための自
走車１および反射器６８〜６ｄの位置を示す図である。

同図において、第４図と同符号は同一または同等部分を
示す。本実施例では基準点ＡおよびＢが検出されていな
い（見失った）場合の例を示す。

まず、ここでは基準点Ａ、　Ｂが検出されていないので
、検出されている基準点Ｃを原点とし、基準点りを通る
線を一方の座標軸とする第３図に示した相対座標系で自
走車１の位置検出を行う。基準点ＣおよびＤから自走車
１までの距離をそれぞれｐ　ｃ、　ｉｔ　ｄとする。相
対座標系の座標軸Ｘ「と基準点Ｂを原点とする絶対座標
系のＸ軸とのなす角度を角度θとすると、基準点Ｃを原
点とする相対座標系における基準点りの座ＪＩＡＤｒ（
ＸＤｒ。

ＹＤｒ）は次式で算出される。但し、ＹＤｒ−ＯＸＤｒ
−（ｘｄ−ｘｃ）ｃｏｓθ ＋ｙｄφｓｉｎθ・・・・・・（５） そして、相対座標系上での自走車１の位置Ｔｒ　（Ｘ、
Ｙ）は次式で算出される。

Ｘ＝　（ｉ）ｃ　　−ｐ　ｄ　　＋ＸＤｒ２）２ ／（２・ＸＤｒ）・・・・・・・・・（６）・・・・・
・（７） なお、式（７）によって２つの解が得られるが、いずれ
の解を採用するかの判断は、例えば前回算出時の値に近
い方を採用するというような基準によって行うことがで
きる。

次いで、算出式（８）　、　（７）を用いて得られた座
標（Ｘ、Ｙ）を絶対座標系における座標に変換する。変
換式は次の通りである。

ｘ−Ｘｃｏｓθ−ＹｓＬｎθ−ｘ　ｃ−・・−（８）ｙ
−Ｘｓｉｎθ＋Ｙｃｏｓθ　　　−・・−（９）自走車
１の進行方向θｆは次式によって算出される。

θｆ−１８０°−（θｃ−ｔａｎ　　’ｋ）・・・・・
・（１０） 但し、ｋ　＝　ｙ　／　ｘ 基準点が２ないし３カ所検出されている場合、上記の算
出式を用いて自走車１の位置および進行方向を算出でき
る。さらに、４カ所すべての基準５ ６ 点が検出されている場合には、４カ所すべての基準点に
関するデータに基づいて自走車１の位置および進行方向
を算出する方が、より正確に自走車１の位置および進行
方向を検出できる。

したがって、３力所以上の基準点が検出できた場合には
、３カ所の基準点に関するデータに基づいて自走車１の
位置および進行方向の算出を行ってもよいし、検出され
た基準点の数が３つまたは４つのいずれであるかにより
、３カ所の基準点に関するデータを使用する算出式、お
よび４カ所の基準点に関するデータを使用する算出式の
いずれかを選んで自走車１の位置および進行方向の算出
を行ってもよい。

なお、４カ所すべての基準点に関するデータに基づいて
自走車１の位置および進行方向を算出するための手順お
よび算出式は特願昭６３−２５７９１２号に詳細に記載
されているので、ここではその説明は省略する。

次に、前記演算部１２または１３で算出された自走車１
の位置および進行方向に基づいて行われる自走車１の操
向制御について説明する。第５図は自走車１の走行コー
スと反射器６ａ〜６ｄの配置状態を示す図であり、第２
図と同符号は同一または同等部分を示す。第６図は操向
制御のフローチャートである。

第５図において、点Ｒ（Ｘｒｅｔ、Ｙｒｅｔ）は自走車
１の戻り位置を示し、作業領域２２は座標（Ｘｓ　ｔ、
　Ｙｓ　ｔ）、　（Ｘｓ　ｔ、　Ｙｅ）、　（Ｘｅ。

Ｙｓ　ｔ）、（Ｘｅ、Ｙｅ）で示される点を結ぶ領域の
少し外側の領域である。ここでは自走車１の位置Ｔは（
Ｘｐ、Ｙｐ）で示す。

なお、第５図においては、説明を簡単にするため、作業
区域２２の４辺をＸ軸またはｙ軸に平行にした例を示し
たが、作業区域２２の周囲に反射器６ａ〜６ｄを設ける
ようにさえしてあれば、作業区域２２の各辺の向きおよ
び作業区域２２の形状は任意である。

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

まず、第６図（その１）のステップＳ１では、自走車１
を点Ｒから作業開始位置まで無線操縦等適宜の手段によ
り移動させる。

ステップＳ２では、自走車１を停車させたままで発光器
２．受光器３を回転させ、各基準点Ａ。

Ｂ、　　Ｃ，Ｄの検出を行う。

ステップＳ３では、走行コースのＸ座標ＸｎとしてＸｓ
ｔをセットし、走行コースを決定し、ステップＳ４で自
走車１の走行を開始する。

ステップＳ５では、受光器３が基準点からの反射光を受
光したか否かの判断がなされる。反射光が受光されるま
で該ステップＳ５は繰返される。

反射光が検出されると、ステップＳ６に進んで、基準点
識別処理が実行される。

該基準点識別処理では基準点はすべて検出されたか、ま
た、基準点を見失った場合はどの基準点を見失ったか、
さらに、ある１つの基準点を連続して見失った回数は何
回か等の検出処理がなされる。該基準点識別処理は前記
特願昭６３−２６２１９２号および特願昭６３−３１３
５３９号に記載されている基準点識別処理の手法を適用
することができるので、ここでは説明を省略する。

ステップＳ７ではある１つの基準点を連続して見失った
回数が予定回数を超過したか否かの判別がなされる。該
ステップＳ７が否定の場合、すなわち、４カ所の基準点
のうちのある１カ所の基準点を予定回数を超えて連続し
て見失ってはいないと判断された場合は、第６図（その
２）のステップＳ８に進んで、４カ所の基準点に関する
データに基づいて自走車１の位置および進行方向を演算
する。

ステップＳ９では、走行コースからのずれ量（ΔＸ＋ｍ
Ｘｐ−Ｘｎ、Δθｆ）を演算し、ステップＳＩＯでは、
前記ずれ量に応じて操舵部１４により操舵角制御を行う
。

ステップＳｌｌでは、自走車１がｙ軸方向において、原
点から遠ざかる方向（行き方向）に走行しているか、原
点に近づく方向（戻り方向）に走行しているかを判断す
る。行き方向であれば、ステップＳ１２において、−行
程が終了したか（Ｙｐ＞Ｙｅ）否かが判断され、戻り方
向であれ９ ０ ば、ステップＳ１３において、−行程終了（Ｙｐ＜Ｙｓ
　ｔ）したか否かを判断する。ステップＳ１２またはＳ
１３において、−行程が終了していないと判断されれば
ステップＳ５に戻る。

ステップＳ１２またはＳ１３において、−行程が終了し
たと判断されれば、次はステップＳ１４において全行程
が終了した（Ｘｐ＞Ｘｅ）か否かの判断を行う。

全行程が終了していなければ、ステップＳ１４からステ
ップＳ１５に移って自走車のＵターン制御を行う。Ｕタ
ーン制御は、前記第１演算部１３゜第２演算部１２で演
算された自走車１の位置情報を操舵部１４にフィードバ
ックする処理によって行われる直線行程の操向制御とは
別の方式で行われる。

すなわち、旋回行程では自走車１の操舵角をあらかじめ
設定された角度に固定して走行させる制御に移行し、各
基準点Ａ、Ｂ、Ｃ，Ｄの方位角のうち、少なくとも１つ
が予定の角度範囲内に合致した時点で、直線行程の操向
制御に戻るようにしている。

ステップ８１６では、ＸｎにＸｎ＋Ｌをセットして次の
走行コースを設定する。次の走行コースが設定されれば
ステップＳ５に戻る。

全行程が終了したならば、戻り位置Ｒ （Ｘｒｅｔ、Ｙｒｅｔ）へ戻って（ステップ５１７）、
走行を停止する（ステップ８１８）。

一方、ステップＳ７が肯定の場合、すなわち、４カ所の
基準点のうちのある１カ所の基準点を予定回数を超えて
連続して見失ったと判断された場合は、ステップＳ１９
に移行する。ステップＳ１９では見失った基準点の数が
１つか２つかの判別を行う。

見失った基準点の数が１つの場合はステップＳ２０に移
行し、第１演算部１３において３カ所の基準点に関する
データに基づいて自走車１の位置および進行方向を算出
し、その後ステップＳ９に進む。

見失った基準点の数が２つの場合は、ステップＳ２１に
移行する。ステップＳ２１では自走車１２ および基準点間の距離を、より正確に測定するために走
行速度を減速する。

走行速度を減速した後、ステップＳ２２に進んで自走車
１から検出された２カ所の基準点までのそれぞれの距離
を測定する。ステップＳ２３では該距離データおよび基
準点の位置情報（座標）に基づき、第２演算部１２に格
納されている算出式を使用して自走車１の位置および進
行方向を算出する。

ステップＳ２４では走行速度を通常の速度に設定し、そ
の後ステップＳ９に進む。

以上の説明のように、本実施例では、基準点を見失った
場合、残りの基準点つまり検出できた基準点の数に応じ
て自走車１の位置および進行方向の算出手法を選択する
ようにした。特に、基準点が２カ所しか検出できなかっ
た場合には、距離測定機能を巧みに利用して、自走車の
走行制御を行い、作業を継続できるようにした。

なお、本実施例では、検出された基準点が２つの場合に
、相対座標系上で自走車の位置を算出し、それを絶対座
標に変換するようにしたが、本発明はこれに限定されず
、検出された基準点の位置情報と該基準点および自走車
間の距離とに基づいて絶対座標系上で直接に自走車の位
置を算出しても良い。例えば、検出された２つの基準点
を中心とし、検出された前記距離を半径とする２つの円
を求め、該円の交点を算出することによって自走車の位
置を検出できる。

（発明の効果） 以上の説明から明らかなように、本発明によれば、次の
ような効果が達成できる。

（１）基準点を見失った場合には、検出された基準点の
数に応じた適当な算出手法によって自走車の位置を検出
して自走車の操向制御が継続できるので、頻繁な走行の
中断が発生しにくくなり、作業効率が向上する。

（２）段差があったり起伏が激しいような作業領域でも
、２カ所の基準点が検出できれば走行作業を継続できる
ので、自走車の適用範囲を拡大できる。

【図面の簡単な説明】

３ ４ 第１図は本発明の一実施例の機能構成を示すブロック図
、第２図、第３図および第４図は自走車と基準点の位置
関係を示す図、第５図は自走車の走行コースと反射器の
配置状態を示す図、第６図は操向制御のフローチャート
、第７図は自走車と反射器の配置状態を示す斜視図であ
る。 １・・・自走車、２・・・発光器、３・・・受光器、６
・・・反射器、７・・・ロータリエンコーダ、８・・・
距離検出部、９・・・カウンタ、１０・・・角度検出部
、１１・・・基準点識別部、１２・・・第２演算部、１
３・・・第１演算部、１４・・・操舵部、１７・・・切
換部

Claims (3)

【特許請求の範囲】
1. （１）自走車から発生した光ビームを、該自走車を中心
   として円周方向に走査することによって基準点を検出し
   、該基準点との相対位置関係から自走車の位置および進
   行方向を検出する自走車の位置制御装置において、 前記自走車とは離れた少なくとも３カ所の基準点に設置
   され、入射方向に光を反射する光反射手段と、 前記自走車に搭載され、前記光ビームを発生する光ビー
   ム発生手段、および該光ビームの反射光を受光する受光
   手段と、 前記光反射手段からの反射光に基づいて前記自走車と各
   基準点との距離を検出する手段と、前記光反射手段から
   の反射光に基づき、自走車から見た各基準点の方位角を
   検出する手段と、少なくとも３カ所に設置された前記各
   基準点の位置情報、および該基準点の相互間の開き角に
   基づいて自走車の位置を算出する第１演算手段と、自走
   車および前記各基準点のうちの２つの基準点間の距離と
   、該２つの基準点の位置情報とに基づいて自走車の位置
   を算出する第２演算手段と、自走車の位置を演算する際
   に、少なくとも３つの基準点が検出された場合には前記
   第１演算手段の出力を選択し、２つの基準点しか検出で
   きなかった場合には、前記第２演算手段の出力を選択す
   る選択手段とを具備したことを特徴とする自走車の位置
   制御装置。
2. （２）前記第２演算手段が、２つの基準点の一方を原点
   とし、該原点および他方の基準点を通る直線を一方の座
   標軸とする相対座標系上での自走車の位置を算出する演
   算手段であって、該相対座標系において算出された自走
   車の位置を別の座標系に座標変換する座標変換手段を具
   備したことを特徴とする請求項１記載の自走車の位置制
   御装置。
3. （３）前記別の座標系が、前記２つの基準点を含む少な
   くとも３つの基準点の１つを原点とし、該原点および他
   の１つの基準点を通る線を一方の座標軸とする絶対座標
   系であることを特徴とする請求項２記載の自走車の位置
   制御装置。